diff --git a/.circleci/config.yml b/.circleci/config.yml index ef14bf0d66..8f60f876b4 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -1,18 +1,27 @@ version: 2.1 -orbs: - codecov: codecov/codecov@1.0.5 -references: - common_environment: &common_environment - ROS_WS: "/opt/ros_ws" - UNDERLAY_WS: "/opt/underlay_ws" - OVERLAY_WS: "/opt/overlay_ws" - CCACHE_LOGFILE: "/tmp/ccache.log" - CCACHE_MAXSIZE: "200M" - MAKEFLAGS: "-j 1 -l 2" - RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED: "1" - RETEST_UNTIL_PASS: "2" +_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: @@ -23,11 +32,17 @@ references: 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 >>-v11\ + -{{ arch }}\ + -{{ .Branch }}\ + -{{ .Environment.CIRCLE_PR_NUMBER }}\ + -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" + - "<< parameters.key >>-v11\ + -{{ arch }}\ + -main\ + -\ + -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" save_to_cache: description: "Save To Cache" parameters: @@ -43,14 +58,18 @@ references: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v1\ + key: "<< parameters.key >>-v11\ -{{ 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 + - << parameters.path >>/test_results when: << parameters.when >> install_dependencies: description: "Install Dependencies" @@ -65,12 +84,20 @@ references: 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 + 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 + + 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 + rosdep update --rosdistro $ROS_DISTRO dependencies=$( rosdep install -q -y \ --from-paths src \ @@ -82,9 +109,9 @@ references: 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: @@ -101,32 +128,78 @@ references: 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: | - if [ -d install ] && [ ! -f 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 - echo "Skipping Build" - else - . << parameters.underlay >>/install/setup.sh - rm -rf build install log - colcon build \ - --symlink-install \ - --mixin << parameters.mixins >> - rm -f build_failed + 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 >> @@ -137,31 +210,90 @@ references: 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_UNPASSED=$( + colcon list \ + --names-only \ + --packages-skip-test-passed \ + | xargs) + echo TEST_UNPASSED: $TEST_UNPASSED + + 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_UNPASSED" ] || \ + [ -n "$TEST_FAILURES" ] || \ + [ -n "$TEST_INVALID" ] + then + TEST_PACKAGES=$( + colcon list \ + --names-only \ + --packages-above \ + $TEST_UNPASSED \ + $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 + colcon clean packages --yes \ + --packages-select ${TEST_PACKAGES} \ + --base-select build \ + --clean-match \ + "*.gcda" + + . install/setup.sh set -o xtrace colcon test \ --packages-select ${TEST_PACKAGES} - colcon test \ - --packages-select ${TEST_PACKAGES} \ - --packages-select-test-failures \ - --retest-until-pass ${RETEST_UNTIL_PASS} \ - --ctest-args --test-regex "test_.*" 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,29 +301,27 @@ references: 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 + +_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/* on_checkout: &on_checkout checkout: @@ -200,34 +330,21 @@ references: run: name: Post Checkout command: | + cp $OVERLAY_WS/src/navigation2/.circleci/defaults.yaml $COLCON_DEFAULTS_FILE if ! cmp \ - $OVERLAY_WS/src/navigation2/tools/ros2_dependencies.repos \ - $UNDERLAY_WS/ros2_dependencies.repos >/dev/null 2>&1 + $OVERLAY_WS/src/navigation2/tools/underlay.repos \ + $UNDERLAY_WS/underlay.repos >/dev/null 2>&1 then - echo "Cleaning Underlay" - rm -rf $UNDERLAY_WS/* - cp $OVERLAY_WS/src/navigation2/tools/ros2_dependencies.repos \ - $UNDERLAY_WS/ros2_dependencies.repos - mkdir -p $UNDERLAY_WS/src + echo "Importing Underlay" + cp $OVERLAY_WS/src/navigation2/tools/underlay.repos \ + $UNDERLAY_WS/underlay.repos vcs import $UNDERLAY_WS/src \ - < $UNDERLAY_WS/ros2_dependencies.repos + < $UNDERLAY_WS/underlay.repos fi install_underlay_dependencies: &install_underlay_dependencies 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 @@ -252,18 +369,11 @@ references: 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: ~/.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 @@ -271,9 +381,18 @@ references: command: src/navigation2/tools/code_coverage_report.bash ci when: always upload_overlay_coverage: &upload_overlay_coverage - codecov/upload: - file: lcov/project_coverage.info - flags: project + run: + name: Upload Code Coverage + working_directory: /opt/overlay_ws + command: | + 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" \ + -n "$RMW_IMPLEMENTATION" \ + -Z || echo 'Codecov upload failed' when: always commands: @@ -288,18 +407,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: @@ -310,6 +423,9 @@ commands: - *restore_overlay_workspace test_build: description: "Test Build" + parameters: + cache_test: + type: boolean steps: - *test_overlay_workspace report_coverage: @@ -318,94 +434,84 @@ commands: - *collect_overlay_coverage - *upload_overlay_coverage +_environments: + common_environment: &common_environment + ROS_WS: "/opt/ros_ws" + UNDERLAY_WS: "/opt/underlay_ws" + OVERLAY_WS: "/opt/overlay_ws" + UNDERLAY_MIXINS: "release ccache lld" + CCACHE_LOGFILE: "/tmp/ccache.log" + CCACHE_MAXSIZE: "200M" + 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:master.debug - 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:master.release + - image: ghcr.io/ros-planning/navigation2:main working_directory: /opt/overlay_ws environment: <<: *common_environment CACHE_NONCE: "Release" - OVERLAY_MIXINS: "release ccache" - UNDERLAY_MIXINS: "release ccache" + OVERLAY_MIXINS: "release ccache coverage-gcc lld" + +_jobs: + job_test: &job_test + parameters: + cache_test: + type: boolean + default: false + rmw: + default: "rmw_cyclonedds_cpp" + type: string + parallelism: 1 + environment: + RMW_IMPLEMENTATION: << parameters.rmw >> jobs: - debug_build: &debug_build - executor: debug_exec + release_build: &release_build + executor: release_exec steps: - checkout_source - setup_dependencies - build_source - release_build: - executor: release_exec - <<: *debug_build - debug_test: - executor: debug_exec - parallelism: 1 - steps: - - restore_build - - test_build - - report_coverage release_test: &release_test + <<: *job_test executor: release_exec - parallelism: 1 steps: - restore_build - - test_build - test_rmw_connext_cpp: - <<: *release_test - environment: - RMW_IMPLEMENTATION: "rmw_connext_cpp" - test_rmw_cyclonedds_cpp: - <<: *release_test - environment: - RMW_IMPLEMENTATION: "rmw_cyclonedds_cpp" - test_rmw_fastrtps_cpp: - <<: *release_test - environment: - RMW_IMPLEMENTATION: "rmw_fastrtps_cpp" + - test_build: + cache_test: << parameters.cache_test >> + - report_coverage workflows: version: 2 build_and_test: jobs: - - debug_build - - debug_test: - requires: - - debug_build - release_build - release_test: requires: - release_build + cache_test: true nightly: jobs: - - debug_build - - debug_test: - requires: - - debug_build - release_build - # - test_rmw_connext_cpp: - # requires: - # - release_build - - test_rmw_cyclonedds_cpp: - requires: - - release_build - - test_rmw_fastrtps_cpp: + - release_test: requires: - release_build + matrix: + parameters: + rmw: + - rmw_connextdds + - rmw_cyclonedds_cpp + - rmw_fastrtps_cpp triggers: - schedule: - cron: "0 0 * * *" + cron: "0 13 * * *" filters: branches: only: - - master + - main diff --git a/.circleci/defaults.yaml b/.circleci/defaults.yaml new file mode 100644 index 0000000000..e0d946f78e --- /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/debug/Dockerfile deleted file mode 120000 index 36c49d2a30..0000000000 --- a/.dockerhub/debug/Dockerfile +++ /dev/null @@ -1 +0,0 @@ -../../Dockerfile \ No newline at end of file diff --git a/.dockerhub/debug/dummy.Dockerfile b/.dockerhub/debug/dummy.Dockerfile deleted file mode 100644 index 121ee27a36..0000000000 --- a/.dockerhub/debug/dummy.Dockerfile +++ /dev/null @@ -1,12 +0,0 @@ -# This is a dummy Dockerfile for setting repository links on Docker Hub. -# Build rules on Docker Hub can trigger whenever the base image updates. -# Base images are specified in the FROM: directive in the tracked Dockerfile. -# However, build args are used by the build hooks to adjust the base image -# so a single Dockerfile can be reused for multiple CI jobs, reducing maintenance. -# To re-enable repository linking when using build args in the FROM: directive, -# this dummy Dockerfile explicitly conveys the base image/repo to link against -# while build rules that target this still use the same hook in this directory. -# Note: This only works for non-official images. - -FROM osrf/ros2:nightly -RUN echo "This is a dummy Dockerfile." \ No newline at end of file diff --git a/.dockerhub/debug/hooks/build b/.dockerhub/debug/hooks/build deleted file mode 100755 index 310fbbd87b..0000000000 --- a/.dockerhub/debug/hooks/build +++ /dev/null @@ -1,15 +0,0 @@ -#!/bin/bash -set -ex - -export FROM_IMAGE=osrf/ros2:nightly -export FAIL_ON_BUILD_FAILURE="" -export UNDERLAY_MIXINS="release ccache" -export OVERLAY_MIXINS="debug ccache coverage-gcc" - -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/.dockerhub/devel/Dockerfile b/.dockerhub/devel/Dockerfile deleted file mode 120000 index 36c49d2a30..0000000000 --- a/.dockerhub/devel/Dockerfile +++ /dev/null @@ -1 +0,0 @@ -../../Dockerfile \ No newline at end of file diff --git a/.dockerhub/devel/hooks/build b/.dockerhub/devel/hooks/build deleted file mode 100755 index 093a78ec02..0000000000 --- a/.dockerhub/devel/hooks/build +++ /dev/null @@ -1,11 +0,0 @@ -#!/bin/bash -set -ex - -export ROS_DISTRO=${SOURCE_BRANCH%"-devel"} -export FROM_IMAGE=ros:${ROS_DISTRO} - -docker build \ - --tag ${IMAGE_NAME} \ - --build-arg FROM_IMAGE \ - --build-arg FAIL_ON_BUILD_FAILURE="" \ - --file ./Dockerfile ../../. diff --git a/.dockerhub/release/Dockerfile b/.dockerhub/release/Dockerfile deleted file mode 120000 index 36c49d2a30..0000000000 --- a/.dockerhub/release/Dockerfile +++ /dev/null @@ -1 +0,0 @@ -../../Dockerfile \ No newline at end of file diff --git a/.dockerhub/release/dummy.Dockerfile b/.dockerhub/release/dummy.Dockerfile deleted file mode 100644 index 81a7ca1cba..0000000000 --- a/.dockerhub/release/dummy.Dockerfile +++ /dev/null @@ -1,12 +0,0 @@ -# This is a dummy Dockerfile for repository links on Docker Hub. -# Build rules on Docker Hub can trigger whenever the base image updates. -# Base images are specified in the FROM: directive in the tracked Dockerfile. -# However, build args are used by the build hooks to adjust the base image -# so a single Dockerfile can be reused for multiple CI jobs, reducing maintenance. -# To re-enable repository linking when using build args in the FROM: directive, -# this dummy Dockerfile explicitly conveys the base image/repo to link against -# while build rules that target this still use the same hook in this directory. -# Note: This only works for non-official images. - -FROM osrf/ros2:nightly-rmw-nonfree -RUN echo "This is a dummy Dockerfile." \ No newline at end of file diff --git a/.dockerhub/release/hooks/build b/.dockerhub/release/hooks/build deleted file mode 100755 index 5f9dd7e757..0000000000 --- a/.dockerhub/release/hooks/build +++ /dev/null @@ -1,15 +0,0 @@ -#!/bin/bash -set -ex - -export FROM_IMAGE=osrf/ros2:nightly -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/.dockerignore b/.dockerignore index ad7d6a52f5..5e31fef9d5 100644 --- a/.dockerignore +++ b/.dockerignore @@ -1,10 +1,11 @@ ################################################################################ # Repo -.circleci/* -.git/* +.circleci/ +.github/ +.git/ +.github/ .dockerignore .gitignore **Dockerfile **.Dockerfile -codecov.yml diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index da6919c3e5..630c2a0226 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -35,3 +35,12 @@ * 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 features **OR** changes to existing behaviors are reflected in the tuning 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/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000000..3017359824 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,14 @@ +version: 2 +updates: + - package-ecosystem: "docker" + directory: "/" + schedule: + interval: "daily" + commit-message: + prefix: "🐳" + - package-ecosystem: "github-actions" + directory: "/" + schedule: + interval: "daily" + commit-message: + prefix: "🛠️" diff --git a/.github/mergify.yml b/.github/mergify.yml new file mode 100644 index 0000000000..3713a4262a --- /dev/null +++ b/.github/mergify.yml @@ -0,0 +1,73 @@ +pull_request_rules: + - name: backport to galactic at reviewers discretion + conditions: + - base=main + - "label=backport-galactic" + actions: + backport: + branches: + - galactic + + - name: backport to foxy at reviewers discretion + conditions: + - base=main + - "label=backport-foxy" + actions: + backport: + branches: + - foxy-devel + + - name: delete head branch after merge + conditions: + - merged + actions: + delete_head_branch: + + - name: ask to resolve conflict + conditions: + - conflict + - author!=mergify + actions: + comment: + message: This pull request is in conflict. Could you fix it @{{author}}? + + - name: development targets main branch + conditions: + - base!=main + - author!=SteveMacenski + - author!=mergify + actions: + comment: + message: | + @{{author}}, all pull requests must be targeted towards the `main` development branch. + Once merged into `main`, it is possible to backport to @{{base}}, but it must be in `main` + to have these changes reflected into new distributions. + + - name: Main build failures + conditions: + - base=main + - or: + - "check-failure=ci/circleci: debug_build" + - "check-failure=ci/circleci: release_build" + actions: + comment: + message: | + @{{author}}, your PR has failed to build. Please check CI outputs and resolve issues. + You may need to rebase or pull in `main` due to API changes (or your contribution genuinely fails). + + - name: Removed maintainer checklist + conditions: + - "-body~=^.*#### For Maintainers: .*$" + - author!=SteveMacenski + - author!=mergify + actions: + comment: + message: | + @{{author}}, please properly fill in PR template in the future. @stevemacenski, use this instead. + - [ ] 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 features **OR** changes to existing behaviors are reflected in the tuning 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/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml new file mode 100644 index 0000000000..9358ea9629 --- /dev/null +++ b/.github/workflows/update_ci_image.yaml @@ -0,0 +1,110 @@ +--- +name: Update CI Image + +on: + schedule: + # 7am UTC, 12am PDT + - cron: '0 7 * * *' + push: + branches: + - main + paths: + - '**/package.xml' + - '**/*.repos' + - 'Dockerfile' + +jobs: + check_ci_files: + name: Check CI Files + runs-on: ubuntu-latest + outputs: + trigger: ${{ steps.check.outputs.trigger }} + no_cache: ${{ steps.check.outputs.no_cache }} + steps: + - name: "Check package updates" + id: check + if: github.event_name == 'push' + run: | + echo "::set-output name=trigger::true" + echo "::set-output name=no_cache::false" + check_ci_image: + name: Check CI Image + if: github.event_name == 'schedule' + needs: check_ci_files + runs-on: ubuntu-latest + outputs: + trigger: ${{ steps.check.outputs.trigger }} + no_cache: ${{ steps.check.outputs.no_cache }} + container: + image: ghcr.io/ros-planning/navigation2:main + steps: + - name: "Check apt updates" + id: check + 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}" \ + > upgrade.log + cat upgrade.log + cat upgrade.log \ + | grep "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" \ + && echo "::set-output name=trigger::false" \ + || echo "::set-output name=trigger::true" + echo "::set-output name=no_cache::true" + rebuild_ci_image: + name: Rebuild CI Image + if: always() + needs: + - check_ci_files + - check_ci_image + runs-on: ubuntu-latest + steps: + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v2 + - name: Login to Docker Hub + uses: docker/login-action@v2 + with: + registry: ghcr.io + username: ${{ github.repository_owner }} + password: ${{ secrets.GITHUB_TOKEN }} + - name: Set build config + id: config + run: | + timestamp=$(date --utc +%Y%m%d%H%M%S) + echo "::set-output name=timestamp::${timestamp}" + + no_cache=false + if [ "${{needs.check_ci_files.outputs.no_cache}}" == 'true' ] || \ + [ "${{needs.check_ci_image.outputs.no_cache}}" == 'true' ] + then + no_cache=true + fi + echo "::set-output name=no_cache::${no_cache}" + + trigger=false + if [ "${{needs.check_ci_files.outputs.trigger}}" == 'true' ] || \ + [ "${{needs.check_ci_image.outputs.trigger}}" == 'true' ] + then + trigger=true + fi + echo "::set-output name=trigger::${trigger}" + - name: Build and push + if: steps.config.outputs.trigger == 'true' + id: docker_build + uses: docker/build-push-action@v3 + with: + pull: true + push: true + no-cache: ${{ steps.config.outputs.no_cache }} + cache-from: type=registry,ref=ghcr.io/ros-planning/navigation2:main + cache-to: type=inline + target: builder + tags: | + ghcr.io/ros-planning/navigation2:main + ghcr.io/ros-planning/navigation2:main-${{ steps.config.outputs.timestamp }} + - name: Image digest + if: steps.config.outputs.trigger == 'true' + run: echo ${{ steps.docker_build.outputs.digest }} diff --git a/.gitignore b/.gitignore index 173558e8c0..21696be872 100644 --- a/.gitignore +++ b/.gitignore @@ -44,5 +44,13 @@ install # Python artifacts __pycache__/ *.py[cod] +.ipynb_checkpoints sphinx_doc/_build + +# CLion artifacts +.idea +cmake-build-debug/ + +# doxygen docs +doc/html/ diff --git a/Dockerfile b/Dockerfile index 0767205c60..155f5aefbb 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,12 +1,12 @@ # This dockerfile can be configured via --build-arg # 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 UNDERLAY_MIXINS="debug ccache lld" +# export OVERLAY_MIXINS="debug ccache coverage-gcc lld" # docker build -t nav2:latest \ # --build-arg UNDERLAY_MIXINS \ # --build-arg OVERLAY_MIXINS ./ -ARG FROM_IMAGE=osrf/ros2:nightly +ARG FROM_IMAGE=ros:rolling ARG UNDERLAY_WS=/opt/underlay_ws ARG OVERLAY_WS=/opt/overlay_ws @@ -16,9 +16,8 @@ FROM $FROM_IMAGE AS cacher # clone underlay source ARG UNDERLAY_WS WORKDIR $UNDERLAY_WS/src -COPY ./tools/ros2_dependencies.repos ../ -RUN vcs import ./ < ../ros2_dependencies.repos && \ - find ./ -name ".git" | xargs rm -rf +COPY ./tools/underlay.repos ../ +RUN vcs import ./ < ../underlay.repos # copy overlay source ARG OVERLAY_WS @@ -27,24 +26,48 @@ 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 --with-new-pkgs && \ + apt-get install -y \ ccache \ lcov \ + lld \ + 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@a937541bfc496c7a267db7ee9d6cceca61e470ca \ + git+https://github.com/ruffsl/colcon-clean.git@a7f1074d1ebc1a54a6508625b117974f2672f2a9 \ && rosdep update \ + && colcon mixin update \ + && colcon metadata 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 && \ @@ -58,47 +81,53 @@ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ # build underlay source COPY --from=cacher $UNDERLAY_WS ./ -ARG UNDERLAY_MIXINS="release ccache" -ARG FAIL_ON_BUILD_FAILURE=True +ARG UNDERLAY_MIXINS="release ccache lld" +ARG CCACHE_DIR="$UNDERLAY_WS/.ccache" RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ + colcon cache lock && \ colcon build \ --symlink-install \ --mixin $UNDERLAY_MIXINS \ - --event-handlers console_direct+ \ - || touch build_failed && \ - if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \ - exit 1; \ - fi + --event-handlers console_direct+ # 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" +ARG OVERLAY_MIXINS="release ccache lld" +ARG CCACHE_DIR="$OVERLAY_WS/.ccache" RUN . $UNDERLAY_WS/install/setup.sh && \ + colcon cache lock && \ colcon build \ --symlink-install \ - --mixin $OVERLAY_MIXINS \ - || touch build_failed && \ - if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \ - exit 1; \ - fi + --mixin $OVERLAY_MIXINS # 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 + +# test overlay build +ARG RUN_TESTS +ARG FAIL_ON_TEST_FAILURE=True +RUN if [ -n "$RUN_TESTS" ]; then \ + . install/setup.sh && \ + colcon test && \ + colcon test-result \ + || ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \ + fi diff --git a/Doxyfile b/Doxyfile index 834f11b1a1..aa34c7acb2 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 a235e822a1..702ffd08ba 100644 --- a/README.md +++ b/README.md @@ -1,16 +1,24 @@ -# Navigation 2 -[![Docker Build](https://github.com/umdlife/navigation2/actions/workflows/dockerhub-action.yaml/badge.svg?event=schedule)](https://github.com/umdlife/navigation2/actions/workflows/dockerhub-action.yaml) +# Nav2 +[![GitHub Workflow Status](https://github.com/ros-planning/navigation2/actions/workflows/update_ci_image.yaml/badge.svg)](https://github.com/ros-planning/navigation2/actions/workflows/update_ci_image.yaml) +[![codecov](https://codecov.io/gh/ros-planning/navigation2/branch/main/graph/badge.svg)](https://codecov.io/gh/ros-planning/navigation2) + +

+ +

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) +- [Container Images for Building Nav2](https://github.com/orgs/ros-planning/packages/container/package/navigation2) - [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 @@ -30,33 +38,51 @@ please cite this work in your papers! } ``` + If you use our work on VSLAM and formal comparisons for service robot needs, please cite the paper: + + - A. Merzlyakov, S. Macenski. [**A Comparison of Modern General-Purpose Visual SLAM Approaches**](https://arxiv.org/abs/2107.07589). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2021. + + ```bibtex + @InProceedings{vslamComparison2021, + title = {A Comparison of Modern General-Purpose Visual SLAM Approaches}, + author = {Merzlyakov, Alexey and Macenski, Steven}, + year = {2021}, + booktitle = {2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + pdf = {https://arxiv.org/abs/2107.07589} + } +``` + ## Build Status -| Service | Dashing | Eloquent | Foxy | Master | +| Service | Foxy | Galactic | Main | +| :---: | :---: | :---: | :---: | +| 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) | + +If you use the navigation framework, an algorithm from this repository, or ideas from it +please cite this work in your papers! + +| Package | Foxy Source | Foxy Debian | Galactic Source | Galactic Debian | | :---: | :---: | :---: | :---: | :---: | -| 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/Edev__navigation2__ubuntu_bionic_amd64/badge/icon)](http://build.ros2.org/job/Edev__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 | -| Circle CI | N/A | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-planning/navigation2/tree/master.svg?style=svg)](https://circleci.com/gh/ros-planning/navigation2/tree/master) | - - -| 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/) | +| 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 202b5fdbbc..b30da12897 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/debian/create_debians.sh b/debian/create_debians.sh index 726d4d9cec..427c3c3631 100755 --- a/debian/create_debians.sh +++ b/debian/create_debians.sh @@ -11,8 +11,8 @@ rosdep update &> /dev/null # Package list to create and install debian. # Watch out the order to avoid missing dependencies while creating the debian. PACKAGE_LIST_UNDERLAY=( - BehaviorTree.CPP \ - angles/angles + BehaviorTree/BehaviorTree.CPP \ + ros/angles/angles ) for PACKAGE in ${PACKAGE_LIST_UNDERLAY[@]}; do @@ -21,8 +21,8 @@ for PACKAGE in ${PACKAGE_LIST_UNDERLAY[@]}; do # We have to go to the ROS package parent directory cd /opt/underlay_ws/src/$PACKAGE; - bloom-generate rosdebian --ros-distro foxy &> /dev/null - debian/rules "binary --parallel" &> /dev/null + bloom-generate rosdebian --ros-distro humble + debian/rules "binary --parallel --with autoreconf " cd .. DEB_FILE=$(find *.deb); @@ -53,7 +53,15 @@ PACKAGE_LIST=( navigation2/nav2_amcl \ navigation2/nav2_planner \ navigation2/nav2_navfn_planner \ - navigation2/nav2_controller + navigation2/nav2_dwb_controller/nav_2d_msgs \ + navigation2/nav2_dwb_controller/nav_2d_utils \ + navigation2/nav2_controller \ + navigation2/nav2_dwb_controller/dwb_msgs \ + navigation2/nav2_dwb_controller/dwb_core \ + navigation2/nav2_dwb_controller/dwb_plugins \ + navigation2/nav2_dwb_controller/costmap_queue \ + navigation2/nav2_dwb_controller/dwb_critics \ + navigation2/nav2_dwb_controller/nav2_dwb_controller ) for PACKAGE in ${PACKAGE_LIST[@]}; do @@ -62,8 +70,8 @@ for PACKAGE in ${PACKAGE_LIST[@]}; do # We have to go to the ROS package parent directory cd $PACKAGE; - bloom-generate rosdebian --ros-distro foxy &> /dev/null - debian/rules "binary --parallel" &> /dev/null + bloom-generate rosdebian --ros-distro humble + debian/rules "binary --parallel --with autoreconf " cd .. DEB_FILE=$(find *.deb); @@ -76,4 +84,4 @@ for PACKAGE in ${PACKAGE_LIST[@]}; do done -echo "Complete!" +echo "Complete!" \ No newline at end of file diff --git a/doc/design/CostmapFilters_design.pdf b/doc/design/CostmapFilters_design.pdf new file mode 100644 index 0000000000..c65866ba14 Binary files /dev/null and b/doc/design/CostmapFilters_design.pdf differ diff --git a/doc/nav2_logo.png b/doc/nav2_logo.png new file mode 100644 index 0000000000..4c2f656a99 Binary files /dev/null and b/doc/nav2_logo.png differ diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md deleted file mode 100644 index ada0e743f9..0000000000 --- a/doc/parameters/param_list.md +++ /dev/null @@ -1,790 +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_speed_controller_bt_node", "nav2_truncate_path_action_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_rotate_action_bt_node", "nav2_translate_action_bt_node", "nav2_is_battery_low_condition_bt_node", "nav2_goal_updater_node_bt_node", "nav2_navigate_to_pose_action_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"] | 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 `plugin_names`, 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 `plugin_names`, 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 `plugin_names`, 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 `plugin_names`. - -| 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 `plugin_names` -* ``: 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 `plugin_names` parameter and `"nav2_costmap_2d::VoxelLayer"` is appended to `plugin_types` 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 | - -# 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) | - -**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 | -| ``.shorten_transformed_plan | true | Determines whether we will pass the full plan on to the critics | -| ``.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 | - -# map_server - -## map_server - -| Parameter | Default | Description | -| ----------| --------| ------------| -| save_map_timeout | 2000 | Timeout to attempt to save map with (ms) | -| 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_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 | - -# 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_ms | 5000 | Maximum planning time in ms | -| ``.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 | - -# 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 | -| ---------- | ------- | ----------- | -| position | N/A | Position | -| orientation | N/A | Orientation | -| 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/process/PreReleaseChecklist.md b/doc/process/PreReleaseChecklist.md index 4affa47d36..01b5c14c62 100644 --- a/doc/process/PreReleaseChecklist.md +++ b/doc/process/PreReleaseChecklist.md @@ -24,14 +24,14 @@ There is a docker file to do that, so run sudo docker build -t nav2:full_ros_build --build-arg ROS2_BRANCH=dashing --build-arg http_proxy=http://myproxy.example.com:80 --build-arg https_proxy=http://myproxy.example.com:80 -f Dockerfile.full_ros_build ./ ``` -ROS2_BRANCH should be the release you are targeting or just `master` if you want -to compare against ROS2 master. +ROS2_BRANCH should be the release you are targeting or just `main` if you want +to compare against ROS2 main. ### Ensure all dependencies are released. We want to ensure the correct version of all our dependencies have been released to the branch we are targeting. To do that, we skip the -`ros2_dependencies.repos` install step and rely solely on rosdep to install +`underlay.repos` install step and rely solely on rosdep to install everything. There is a dockerfile to do that as well, so run @@ -41,7 +41,7 @@ sudo docker build -t nav2:rosdep_only_build --build-arg ROS2_BRANCH=dashing --bu ``` As before, ROS2_BRANCH is the branch you are targeting. In this case, there is -no master option. We can only run this dockerfile against a set of released +no main option. We can only run this dockerfile against a set of released packages. ### Ensure the test suite passes diff --git a/doc/use_cases/README.md b/doc/use_cases/README.md index 14ea423c0e..47d6ee5688 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/minimal_repos.txt b/minimal_repos.txt index c69320f64b..bf4bab3c42 100644 --- a/minimal_repos.txt +++ b/minimal_repos.txt @@ -13,4 +13,5 @@ nav2_amcl nav2_rviz_plugins nav2_navfn_planner geographic_msgs -nav2_controller \ No newline at end of file +nav2_controller +nav2_dwb_controller \ No newline at end of file diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt index be83000bd4..3a5836ab2d 100644 --- a/nav2_amcl/CMakeLists.txt +++ b/nav2_amcl/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(message_filters REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) @@ -15,6 +16,7 @@ find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_msgs REQUIRED) +find_package(pluginlib REQUIRED) nav2_package() @@ -43,6 +45,7 @@ add_library(${library_name} SHARED ) target_include_directories(${library_name} PRIVATE src/include) + if(HAVE_DRAND48) target_compile_definitions(${library_name} PRIVATE "HAVE_DRAND48") endif() @@ -50,6 +53,7 @@ endif() set(dependencies rclcpp rclcpp_lifecycle + rclcpp_components message_filters tf2_geometry_msgs geometry_msgs @@ -60,6 +64,7 @@ set(dependencies tf2 nav2_util nav2_msgs + pluginlib ) ament_target_dependencies(${executable_name} @@ -75,9 +80,11 @@ ament_target_dependencies(${library_name} ) target_link_libraries(${library_name} - map_lib motions_lib sensors_lib + map_lib pf_lib sensors_lib ) +rclcpp_components_register_nodes(${library_name} "nav2_amcl::AmclNode") + install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -103,5 +110,5 @@ endif() ament_export_include_directories(include) ament_export_libraries(${library_name} pf_lib sensors_lib motions_lib map_lib) ament_export_dependencies(${dependencies}) - +pluginlib_export_plugin_description_file(nav2_amcl plugins.xml) ament_package() diff --git a/nav2_amcl/README.md b/nav2_amcl/README.md index cf398fba82..7215500919 100644 --- a/nav2_amcl/README.md +++ b/nav2_amcl/README.md @@ -1,19 +1,4 @@ # AMCL -Adaptive Monte Carlo Localization (AMCL) is a probabilistic localization module which estimates the position and orientation (i.e. Pose) of a robot in a given known map. +Adaptive Monte Carlo Localization (AMCL) is a probabilistic localization module which estimates the position and orientation (i.e. Pose) of a robot in a given known map using a 2D laser scanner. This is largely a refactored port from ROS 1 without any algorithmic changes. -## Overview -Currently, the AMCL module in ROS 2 Navigation System is a direct port from [ROS1 AMCL](http://wiki.ros.org/amcl) package with some minor code re-factoring. The direct port includes all of ROS1 functionalities except running from Bag files. - -## Added Feature -AutoLocalization - is implemented by utilizing AMCL's `global_localization` service request and Behavior Tree (BT). This enables initial pose estimation capability on differential type robot. - - On startup of the navigation stack, the initial robot pose needs to be sent to AMCL otherwise AMCL initializes its filter state to default values with a particle cloud centered around (0,0,0). If the initial pose of the robot is not known and the robot is outside of default particle cloud, particles may not converge when robot moves. - -With the AutoLocalization branch of BT, first the `global_localization` service of AMCL is called to randomly disperse all particles through the free space in the map. Once particles are dispersed, the robot rotates, backs up, and, if necessary, rotates again to localize itself. The full implementation is described in the AutoLocalization section of [BT Navigator](https://github.com/ros-planning/navigation2/blob/master/nav2_bt_navigator/README.md) - -**Warning**: AutoLocalization actuates robot; currently, obstacle avoidance has not been integrated into this feature. The user is advised to not use this feature on a physical robot for safety reasons. As of now, this feature should only be used in simulations. - -## Future Plan -* Running from Ros bag -* Extending AMCL to work with different type of Sensors -* Improving overall localization performance with fusing data from different sensors such as IMU, Sonar, Radar, Laser, Depth camera, and etc. +See the [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-amcl.html) for more details about configurable settings and their meanings. diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 772a63dd51..43b7d97ad4 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" @@ -41,6 +40,7 @@ #include "std_srvs/srv/empty.hpp" #include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_listener.h" +#include "pluginlib/class_loader.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-parameter" @@ -52,25 +52,65 @@ namespace nav2_amcl { - +/* + * @class AmclNode + * @brief ROS wrapper for AMCL + */ class AmclNode : public nav2_util::LifecycleNode { public: - AmclNode(); + /* + * @brief AMCL constructor + * @param options Additional options to control creation of the node. + */ + explicit AmclNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /* + * @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; + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + // Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't // respond until we're in the active state std::atomic active_{false}; + // Dedicated callback group and executor for services and subscriptions in AmclNode, + // in order to isolate TF timer used in message filter. + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr executor_thread_; + // Pose hypothesis typedef struct { @@ -80,22 +120,44 @@ 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}; amcl_hyp_t * initial_pose_hyp_; - std::recursive_mutex configuration_mutex_; + std::recursive_mutex mutex_; rclcpp::Subscription::ConstSharedPtr map_sub_; #if NEW_UNIFORM_SAMPLING static std::vector> free_space_indices; #endif // Transforms + /* + * @brief Initialize required ROS transformations + */ void initTransforms(); std::shared_ptr tf_broadcaster_; std::shared_ptr tf_listener_; @@ -103,29 +165,46 @@ class AmclNode : public nav2_util::LifecycleNode bool sent_first_transform_{false}; bool latest_tf_valid_{false}; tf2::Transform latest_tf_; - 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_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 +212,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 +224,20 @@ class AmclNode : public nav2_util::LifecycleNode std::atomic force_update_{false}; // Odometry + /* + * @brief Initialize odometry + */ void initOdometry(); - std::unique_ptr motion_model_; + std::shared_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]; + pluginlib::ClassLoader plugin_loader_{"nav2_amcl", + "nav2_amcl::MotionModel"}; + /* + * @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 +246,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}; std::mutex pf_mutex_; @@ -166,41 +261,74 @@ 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_; - 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}; @@ -211,7 +339,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 d3f8332c5b..84de30f78d 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/map/map.hpp b/nav2_amcl/include/nav2_amcl/map/map.hpp index 4b8fba66c4..c9557efa0b 100644 --- a/nav2_amcl/include/nav2_amcl/map/map.hpp +++ b/nav2_amcl/include/nav2_amcl/map/map.hpp @@ -87,15 +87,6 @@ map_t * map_alloc(void); // Destroy a map void map_free(map_t * map); -// Get the cell at the given point -map_cell_t * map_get_cell(map_t * map, double ox, double oy, double oa); - -// Load an occupancy map -int map_load_occ(map_t * map, const char * filename, double scale, int negate); - -// Load a wifi signal strength map -// int map_load_wifi(map_t *map, const char *filename, int index); - // Update the cspace distances void map_update_cspace(map_t * map, double max_occ_dist); diff --git a/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp b/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp new file mode 100644 index 0000000000..2d17360204 --- /dev/null +++ b/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp @@ -0,0 +1,47 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#ifndef NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_ +#define NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_ + +#include +#include +#include +#include "nav2_amcl/motion_model/motion_model.hpp" +#include "nav2_amcl/angleutils.hpp" + + +namespace nav2_amcl +{ + +class DifferentialMotionModel : public nav2_amcl::MotionModel +{ +public: + virtual void initialize( + double alpha1, double alpha2, double alpha3, double alpha4, + double alpha5); + virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta); + +private: + double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_; +}; +} // namespace nav2_amcl +#endif // NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_ 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 77fc388914..4cd1c6a879 100644 --- a/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp +++ b/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp @@ -18,50 +18,45 @@ #define NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_ #include +#include + #include "nav2_amcl/pf/pf.hpp" #include "nav2_amcl/pf/pf_pdf.hpp" namespace nav2_amcl { +/** + * @class nav2_amcl::MotionModel + * @brief An abstract motion model class + */ class MotionModel { public: virtual ~MotionModel() = default; - virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta) = 0; - - static MotionModel * createMotionModel( - std::string & type, double alpha1, double alpha2, - double alpha3, double alpha4, double alpha5); -}; - -class OmniMotionModel : public MotionModel -{ -public: - OmniMotionModel(double alpha1, double alpha2, double alpha3, double alpha4, double alpha5); - void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta); -private: - double alpha1_; - double alpha2_; - double alpha3_; - double alpha4_; - double alpha5_; -}; - -class DifferentialMotionModel : public MotionModel -{ -public: - DifferentialMotionModel(double alpha1, double alpha2, double alpha3, double alpha4); - void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta); - -private: - double alpha1_; - double alpha2_; - double alpha3_; - double alpha4_; + /** + * @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 + */ + virtual void initialize( + double alpha1, double alpha2, double alpha3, double alpha4, + double alpha5) = 0; + + /** + * @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; }; - } // namespace nav2_amcl #endif // NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_ diff --git a/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp b/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp new file mode 100644 index 0000000000..ae586d4d6c --- /dev/null +++ b/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp @@ -0,0 +1,47 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#ifndef NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_ +#define NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_ + +#include +#include +#include +#include "nav2_amcl/motion_model/motion_model.hpp" +#include "nav2_amcl/angleutils.hpp" + + +namespace nav2_amcl +{ + +class OmniMotionModel : public nav2_amcl::MotionModel +{ +public: + virtual void initialize( + double alpha1, double alpha2, double alpha3, double alpha4, + double alpha5); + virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta); + +private: + double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_; +}; +} // namespace nav2_amcl +#endif // NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_ diff --git a/nav2_amcl/include/nav2_amcl/pf/eig3.hpp b/nav2_amcl/include/nav2_amcl/pf/eig3.hpp index ae0d698b17..ea53e1e870 100644 --- a/nav2_amcl/include/nav2_amcl/pf/eig3.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/eig3.hpp @@ -1,3 +1,23 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ /* Eigen-decomposition for symmetric 3x3 real matrices. Public domain, copied from the public domain Java library JAMA. */ diff --git a/nav2_amcl/include/nav2_amcl/pf/pf.hpp b/nav2_amcl/include/nav2_amcl/pf/pf.hpp index 114d7dfafb..afcb9daafb 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf.hpp @@ -153,7 +153,7 @@ void pf_init(pf_t * pf, pf_vector_t mean, pf_matrix_t cov); void pf_init_model(pf_t * pf, pf_init_model_fn_t init_fn, void * init_data); // Update the filter with some new action -void pf_update_action(pf_t * pf, pf_action_model_fn_t action_fn, void * action_data); +// void pf_update_action(pf_t * pf, pf_action_model_fn_t action_fn, void * action_data); // Update the filter with some new sensor observation void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data); @@ -162,7 +162,7 @@ void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_d void pf_update_resample(pf_t * pf); // Compute the CEP statistics (mean and variance). -void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var); +// void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var); // Compute the statistics for a particular cluster. Returns 0 if // there is no such cluster. @@ -181,7 +181,7 @@ void pf_draw_samples(pf_t * pf, struct _rtk_fig_t * fig, int max_samples); void pf_draw_hist(pf_t * pf, struct _rtk_fig_t * fig); // Draw the CEP statistics -void pf_draw_cep_stats(pf_t * pf, struct _rtk_fig_t * fig); +// void pf_draw_cep_stats(pf_t * pf, struct _rtk_fig_t * fig); // Draw the cluster statistics void pf_draw_cluster_stats(pf_t * pf, struct _rtk_fig_t * fig); diff --git a/nav2_amcl/include/nav2_amcl/pf/pf_kdtree.hpp b/nav2_amcl/include/nav2_amcl/pf/pf_kdtree.hpp index 7950b34629..e6ea8a4a20 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf_kdtree.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf_kdtree.hpp @@ -91,7 +91,7 @@ extern void pf_kdtree_insert(pf_kdtree_t * self, pf_vector_t pose, double value) extern void pf_kdtree_cluster(pf_kdtree_t * self); // Determine the probability estimate for the given pose -extern double pf_kdtree_get_prob(pf_kdtree_t * self, pf_vector_t pose); +// extern double pf_kdtree_get_prob(pf_kdtree_t * self, pf_vector_t pose); // Determine the cluster label for the given pose extern int pf_kdtree_get_cluster(pf_kdtree_t * self, pf_vector_t pose); diff --git a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp index 207ce3d988..c3b406cfd4 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp @@ -52,13 +52,13 @@ typedef struct pf_vector_t pf_vector_zero(); // Check for NAN or INF in any component -int pf_vector_finite(pf_vector_t a); +// int pf_vector_finite(pf_vector_t a); // Print a vector -void pf_vector_fprintf(pf_vector_t s, FILE * file, const char * fmt); +// void pf_vector_fprintf(pf_vector_t s, FILE * file, const char * fmt); // Simple vector addition -pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b); +// pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b); // Simple vector subtraction pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b); @@ -67,17 +67,17 @@ pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b); pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b); // Transform from global to local coords (a - b) -pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b); +// pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b); // Return a zero matrix pf_matrix_t pf_matrix_zero(); // Check for NAN or INF in any component -int pf_matrix_finite(pf_matrix_t a); +// int pf_matrix_finite(pf_matrix_t a); // Print a matrix -void pf_matrix_fprintf(pf_matrix_t s, FILE * file, const char * fmt); +// void pf_matrix_fprintf(pf_matrix_t s, FILE * file, const char * fmt); // Compute the matrix inverse. Will also return the determinant, // which should be checked for underflow (indicated singular matrix). diff --git a/nav2_amcl/include/nav2_amcl/portable_utils.hpp b/nav2_amcl/include/nav2_amcl/portable_utils.hpp new file mode 100644 index 0000000000..13d8a795c0 --- /dev/null +++ b/nav2_amcl/include/nav2_amcl/portable_utils.hpp @@ -0,0 +1,41 @@ +// Copyright (c) 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_AMCL__PORTABLE_UTILS_HPP_ +#define NAV2_AMCL__PORTABLE_UTILS_HPP_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef HAVE_DRAND48 +// Some system (e.g., Windows) doesn't come with drand48(), srand48(). +// Use rand, and srand for such system. +static double drand48(void) +{ + return ((double)rand()) / RAND_MAX;// NOLINT +} + +static void srand48(long int seedval)// NOLINT +{ + srand(seedval); +} +#endif + +#ifdef __cplusplus +} +#endif + +#endif // NAV2_AMCL__PORTABLE_UTILS_HPP_ diff --git a/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp b/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp index 2b5beaea93..edc4124128 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 ff27677865..3ff62b6870 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 0.4.7 + 1.1.0

amcl is a probabilistic localization system for a robot moving in @@ -35,7 +35,7 @@ nav2_msgs launch_ros launch_testing - + pluginlib ament_lint_common ament_lint_auto diff --git a/nav2_amcl/plugins.xml b/nav2_amcl/plugins.xml new file mode 100644 index 0000000000..8a52d81696 --- /dev/null +++ b/nav2_amcl/plugins.xml @@ -0,0 +1,8 @@ + + + This is a diff plugin. + + + This is a omni plugin. + + diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 3dea8569c2..a28b4927a3 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" @@ -48,17 +48,18 @@ #include "tf2/utils.h" #pragma GCC diagnostic pop -#include "portable_utils.h" +#include "nav2_amcl/portable_utils.hpp" using namespace std::placeholders; +using rcl_interfaces::msg::ParameterType; using namespace std::chrono_literals; namespace nav2_amcl { using nav2_util::geometry_utils::orientationAroundZAxis; -AmclNode::AmclNode() -: nav2_util::LifecycleNode("amcl", "", true) +AmclNode::AmclNode(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("amcl", "", options) { RCLCPP_INFO(get_logger(), "Creating"); @@ -174,7 +175,7 @@ AmclNode::AmclNode() "resample_interval", rclcpp::ParameterValue(1), "Number of filter updates required before resampling"); - add_parameter("robot_model_type", rclcpp::ParameterValue(std::string("differential"))); + add_parameter("robot_model_type", rclcpp::ParameterValue("nav2_amcl::DifferentialMotionModel")); add_parameter( "save_pose_rate", rclcpp::ParameterValue(0.5), @@ -221,53 +222,36 @@ AmclNode::AmclNode() add_parameter( "map_topic", rclcpp::ParameterValue("map"), "Topic to subscribe to in order to receive the map to localize on"); + + add_parameter( + "first_map_only", rclcpp::ParameterValue(false), + "Set this to true, when you want to load a new map published from the map_server"); } AmclNode::~AmclNode() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); - + callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); initParameters(); initTransforms(); + initParticleFilter(); + initLaserScan(); initMessageFilters(); initPubSub(); initServices(); initOdometry(); - initParticleFilter(); - initLaserScan(); - + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, get_node_base_interface()); + executor_thread_ = std::make_unique(executor_); return nav2_util::CallbackReturn::SUCCESS; } -void -AmclNode::waitForTransforms() -{ - std::string tf_error; - - RCLCPP_INFO(get_logger(), "Checking that transform thread is ready"); - - while (rclcpp::ok() && - !tf_buffer_->canTransform( - global_frame_id_, odom_frame_id_, tf2::TimePointZero, - transform_tolerance_, &tf_error)) - { - RCLCPP_INFO( - get_logger(), "Timed out waiting for transform from %s to %s" - " to become available, tf error: %s", - odom_frame_id_.c_str(), global_frame_id_.c_str(), tf_error.c_str()); - - // The error string will accumulate and errors will typically be the same, so the last - // will do for the warning above. Reset the string here to avoid accumulation. - tf_error.clear(); - } -} - nav2_util::CallbackReturn AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/) { @@ -275,14 +259,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 @@ -304,6 +282,16 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/) handleInitialPose(last_published_pose_); } + auto node = shared_from_this(); + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &AmclNode::dynamicParametersCallback, + this, std::placeholders::_1)); + + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -316,9 +304,14 @@ 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(); + // reset dynamic parameter handler + dyn_params_handler_.reset(); + + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -327,6 +320,8 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); + executor_thread_.reset(); + // Get rid of the inputs first (services and message filter input), so we // don't continue to process incoming messages global_loc_srv_.reset(); @@ -337,8 +332,10 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) laser_scan_sub_.reset(); // Map - map_free(map_); - map_ = nullptr; + if (map_ != NULL) { + map_free(map_); + map_ = nullptr; + } first_map_received_ = false; free_space_indices.resize(0); @@ -349,7 +346,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // PubSub pose_pub_.reset(); - particlecloud_pub_.reset(); particle_cloud_pub_.reset(); // Odometry @@ -394,26 +390,6 @@ AmclNode::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -void -AmclNode::checkLaserReceived() -{ - if (last_laser_received_ts_.nanoseconds() == 0) { - RCLCPP_WARN( - get_logger(), "Laser scan has not been received" - " (and thus no pose updates have been published)." - " Verify that data is being published on the %s topic.", scan_topic_.c_str()); - return; - } - - rclcpp::Duration d = now() - last_laser_received_ts_; - if (d.nanoseconds() * 1e-9 > laser_check_interval_.count()) { - RCLCPP_WARN( - get_logger(), "No laser scan received (and thus no pose updates have been published) for %f" - " seconds. Verify that data is being published on the %s topic.", - d.nanoseconds() * 1e-9, scan_topic_.c_str()); - } -} - bool AmclNode::checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time last_time) { @@ -505,7 +481,7 @@ AmclNode::globalLocalizationCallback( const std::shared_ptr/*req*/, std::shared_ptr/*res*/) { - std::lock_guard lock(pf_mutex_); + std::lock_guard cfl(mutex_); RCLCPP_INFO(get_logger(), "Initializing with uniform distribution"); @@ -531,7 +507,7 @@ AmclNode::nomotionUpdateCallback( void AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - std::lock_guard lock(pf_mutex_); + std::lock_guard cfl(mutex_); RCLCPP_INFO(get_logger(), "initialPoseReceived"); @@ -566,6 +542,7 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha void AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg) { + std::lock_guard cfl(mutex_); // In case the client sent us a pose estimate in the past, integrate the // intervening odometric change. geometry_msgs::msg::TransformStamped tx_odom; @@ -630,7 +607,7 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg) void AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) { - std::lock_guard lock(pf_mutex_); + std::lock_guard cfl(mutex_); // Since the sensor data is continually being published by the simulator or robot, // we don't want our callbacks to fire until we're in the active state @@ -874,19 +851,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)); } @@ -941,7 +914,7 @@ AmclNode::publishAmclPose( if (!initial_pose_is_known_) { if (checkElapsedTime(2s, last_time_printed_msg_)) { RCLCPP_WARN( - get_logger(), "ACML cannot publish a pose or update the transform. " + get_logger(), "AMCL cannot publish a pose or update the transform. " "Please set the initial pose..."); last_time_printed_msg_ = now(); } @@ -1103,7 +1076,7 @@ AmclNode::initParameters() get_parameter("z_max", z_max_); get_parameter("z_rand", z_rand_); get_parameter("z_short", z_short_); - get_parameter("first_map_only_", first_map_only_); + get_parameter("first_map_only", first_map_only_); get_parameter("always_reset_initial_pose", always_reset_initial_pose_); get_parameter("scan_topic", scan_topic_); get_parameter("map_topic", map_topic_); @@ -1118,6 +1091,25 @@ AmclNode::initParameters() last_time_printed_msg_ = now(); // Semantic checks + if (laser_likelihood_max_dist_ < 0) { + RCLCPP_WARN( + get_logger(), "You've set laser_likelihood_max_dist to be negtive," + " this isn't allowed so it will be set to default value 2.0."); + laser_likelihood_max_dist_ = 2.0; + } + if (max_particles_ < 0) { + RCLCPP_WARN( + get_logger(), "You've set max_particles to be negtive," + " this isn't allowed so it will be set to default value 2000."); + max_particles_ = 2000; + } + + if (min_particles_ < 0) { + RCLCPP_WARN( + get_logger(), "You've set min_particles to be negtive," + " this isn't allowed so it will be set to default value 500."); + min_particles_ = 500; + } if (min_particles_ > max_particles_) { RCLCPP_WARN( @@ -1126,11 +1118,218 @@ AmclNode::initParameters() max_particles_ = min_particles_; } + if (resample_interval_ <= 0) { + RCLCPP_WARN( + get_logger(), "You've set resample_interval to be zero or negtive," + " this isn't allowed so it will be set to default value to 1."); + resample_interval_ = 1; + } + if (always_reset_initial_pose_) { initial_pose_is_known_ = false; } } +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +AmclNode::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard cfl(mutex_); + rcl_interfaces::msg::SetParametersResult result; + double save_pose_rate; + double tmp_tol; + + int max_particles = max_particles_; + int min_particles = min_particles_; + + bool reinit_pf = false; + bool reinit_odom = false; + bool reinit_laser = false; + bool reinit_map = false; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "alpha1") { + alpha1_ = parameter.as_double(); + reinit_odom = true; + } else if (param_name == "alpha2") { + alpha2_ = parameter.as_double(); + reinit_odom = true; + } else if (param_name == "alpha3") { + alpha3_ = parameter.as_double(); + reinit_odom = true; + } else if (param_name == "alpha4") { + alpha4_ = parameter.as_double(); + reinit_odom = true; + } else if (param_name == "alpha5") { + alpha5_ = parameter.as_double(); + reinit_odom = true; + } else if (param_name == "beam_skip_distance") { + beam_skip_distance_ = parameter.as_double(); + reinit_laser = true; + } else if (param_name == "beam_skip_error_threshold") { + beam_skip_error_threshold_ = parameter.as_double(); + reinit_laser = true; + } else if (param_name == "beam_skip_threshold") { + beam_skip_threshold_ = parameter.as_double(); + reinit_laser = true; + } else if (param_name == "lambda_short") { + lambda_short_ = parameter.as_double(); + reinit_laser = true; + } else if (param_name == "laser_likelihood_max_dist") { + laser_likelihood_max_dist_ = parameter.as_double(); + reinit_laser = true; + } else if (param_name == "laser_max_range") { + laser_max_range_ = parameter.as_double(); + reinit_laser = true; + } else if (param_name == "laser_min_range") { + laser_min_range_ = parameter.as_double(); + reinit_laser = true; + } else if (param_name == "pf_err") { + pf_err_ = parameter.as_double(); + reinit_pf = true; + } else if (param_name == "pf_z") { + pf_z_ = parameter.as_double(); + reinit_pf = true; + } else if (param_name == "recovery_alpha_fast") { + alpha_fast_ = parameter.as_double(); + reinit_pf = true; + } else if (param_name == "recovery_alpha_slow") { + alpha_slow_ = parameter.as_double(); + reinit_pf = true; + } else if (param_name == "save_pose_rate") { + save_pose_rate = parameter.as_double(); + save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); + } else if (param_name == "sigma_hit") { + sigma_hit_ = parameter.as_double(); + reinit_laser = true; + } else if (param_name == "transform_tolerance") { + tmp_tol = parameter.as_double(); + transform_tolerance_ = tf2::durationFromSec(tmp_tol); + reinit_laser = true; + } else if (param_name == "update_min_a") { + a_thresh_ = parameter.as_double(); + } else if (param_name == "update_min_d") { + d_thresh_ = parameter.as_double(); + } else if (param_name == "z_hit") { + z_hit_ = parameter.as_double(); + reinit_laser = true; + } else if (param_name == "z_max") { + z_max_ = parameter.as_double(); + reinit_laser = true; + } else if (param_name == "z_rand") { + z_rand_ = parameter.as_double(); + reinit_laser = true; + } else if (param_name == "z_short") { + z_short_ = parameter.as_double(); + reinit_laser = true; + } + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == "base_frame_id") { + base_frame_id_ = parameter.as_string(); + } else if (param_name == "global_frame_id") { + global_frame_id_ = parameter.as_string(); + } else if (param_name == "map_topic") { + map_topic_ = parameter.as_string(); + reinit_map = true; + } else if (param_name == "laser_model_type") { + sensor_model_type_ = parameter.as_string(); + reinit_laser = true; + } else if (param_name == "odom_frame_id") { + odom_frame_id_ = parameter.as_string(); + reinit_laser = true; + } else if (param_name == "scan_topic") { + scan_topic_ = parameter.as_string(); + reinit_laser = true; + } else if (param_name == "robot_model_type") { + robot_model_type_ = parameter.as_string(); + reinit_odom = true; + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == "do_beamskip") { + do_beamskip_ = parameter.as_bool(); + reinit_laser = true; + } else if (param_name == "tf_broadcast") { + tf_broadcast_ = parameter.as_bool(); + } else if (param_name == "set_initial_pose") { + set_initial_pose_ = parameter.as_bool(); + } else if (param_name == "first_map_only") { + first_map_only_ = parameter.as_bool(); + } + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == "max_beams") { + max_beams_ = parameter.as_int(); + reinit_laser = true; + } else if (param_name == "max_particles") { + max_particles_ = parameter.as_int(); + reinit_pf = true; + } else if (param_name == "min_particles") { + min_particles_ = parameter.as_int(); + reinit_pf = true; + } else if (param_name == "resample_interval") { + resample_interval_ = parameter.as_int(); + } + } + } + + // Checking if the minimum particles is greater than max_particles_ + if (min_particles_ > max_particles_) { + RCLCPP_ERROR( + this->get_logger(), + "You've set min_particles to be greater than max particles," + " this isn't allowed."); + // sticking to the old values + max_particles_ = max_particles; + min_particles_ = min_particles; + result.successful = false; + return result; + } + + // Re-initialize the particle filter + if (reinit_pf) { + if (pf_ != NULL) { + pf_free(pf_); + pf_ = NULL; + } + initParticleFilter(); + } + + // Re-initialize the odometry + if (reinit_odom) { + motion_model_.reset(); + initOdometry(); + } + + // Re-initialize the lasers and it's filters + if (reinit_laser) { + lasers_.clear(); + lasers_update_.clear(); + frame_to_laser_.clear(); + laser_scan_connection_.disconnect(); + laser_scan_sub_.reset(); + + initMessageFilters(); + } + + // Re-initialize the map + if (reinit_map) { + map_sub_.reset(); + map_sub_ = create_subscription( + map_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&AmclNode::mapReceived, this, std::placeholders::_1)); + } + + result.successful = true; + return result; +} + void AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { @@ -1145,7 +1344,7 @@ AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) void AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg) { - std::lock_guard cfl(configuration_mutex_); + std::lock_guard cfl(mutex_); RCLCPP_INFO( get_logger(), "Received a %d X %d map @ %.3f m/pix", @@ -1232,13 +1431,14 @@ AmclNode::initTransforms() RCLCPP_INFO(get_logger(), "initTransforms"); // Initialize transform listener and broadcaster - tf_buffer_ = std::make_shared(rclcpp_node_->get_clock()); + tf_buffer_ = std::make_shared(get_clock()); auto timer_interface = std::make_shared( - rclcpp_node_->get_node_base_interface(), - rclcpp_node_->get_node_timers_interface()); + get_node_base_interface(), + get_node_timers_interface(), + callback_group_); tf_buffer_->setCreateTimerInterface(timer_interface); tf_listener_ = std::make_shared(*tf_buffer_); - tf_broadcaster_ = std::make_shared(rclcpp_node_); + tf_broadcaster_ = std::make_shared(shared_from_this()); sent_first_transform_ = false; latest_tf_valid_ = false; @@ -1248,11 +1448,18 @@ AmclNode::initTransforms() void AmclNode::initMessageFilters() { - laser_scan_sub_ = std::make_unique>( - rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data); + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; + laser_scan_sub_ = std::make_unique>( + shared_from_this(), scan_topic_, rmw_qos_profile_sensor_data, sub_opt); laser_scan_filter_ = std::make_unique>( - *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_); + *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, + get_node_logging_interface(), + get_node_clock_interface(), + transform_tolerance_); + laser_scan_connection_ = laser_scan_filter_->registerCallback( std::bind( @@ -1265,10 +1472,6 @@ AmclNode::initPubSub() { RCLCPP_INFO(get_logger(), "initPubSub"); - particlecloud_pub_ = create_publisher( - "particlecloud", - rclcpp::SensorDataQoS()); - particle_cloud_pub_ = create_publisher( "particle_cloud", rclcpp::SensorDataQoS()); @@ -1321,9 +1524,8 @@ AmclNode::initOdometry() init_cov_[2] = last_published_pose_.pose.covariance[35]; } - motion_model_ = std::unique_ptr( - nav2_amcl::MotionModel::createMotionModel( - robot_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_)); + motion_model_ = plugin_loader_.createSharedInstance(robot_model_type_); + motion_model_->initialize(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_); latest_odom_pose_ = geometry_msgs::msg::PoseStamped(); } @@ -1365,3 +1567,10 @@ AmclNode::initLaserScan() } } // namespace nav2_amcl + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_amcl::AmclNode) diff --git a/nav2_amcl/src/include/portable_utils.h b/nav2_amcl/src/include/portable_utils.h deleted file mode 100644 index 1577e6875b..0000000000 --- a/nav2_amcl/src/include/portable_utils.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef PORTABLE_UTILS_H -#define PORTABLE_UTILS_H - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef HAVE_DRAND48 -// Some system (e.g., Windows) doesn't come with drand48(), srand48(). -// Use rand, and srand for such system. -static double drand48(void) -{ - return ((double)rand()) / RAND_MAX; -} - -static void srand48(long int seedval) -{ - srand(seedval); -} -#endif - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/nav2_amcl/src/map/CMakeLists.txt b/nav2_amcl/src/map/CMakeLists.txt index 602629f9fd..9e965f6695 100644 --- a/nav2_amcl/src/map/CMakeLists.txt +++ b/nav2_amcl/src/map/CMakeLists.txt @@ -1,6 +1,5 @@ add_library(map_lib SHARED map.c - map_store.c map_range.c map_draw.c map_cspace.cpp diff --git a/nav2_amcl/src/map/map.c b/nav2_amcl/src/map/map.c index b619208202..a4528cc663 100644 --- a/nav2_amcl/src/map/map.c +++ b/nav2_amcl/src/map/map.c @@ -63,22 +63,3 @@ void map_free(map_t * map) free(map->cells); free(map); } - - -// Get the cell at the given point -map_cell_t * map_get_cell(map_t * map, double ox, double oy, double oa) -{ - (void)oa; - int i, j; - map_cell_t * cell; - - i = MAP_GXWX(map, ox); - j = MAP_GYWY(map, oy); - - if (!MAP_VALID(map, i, j)) { - return NULL; - } - - cell = map->cells + MAP_INDEX(map, i, j); - return cell; -} diff --git a/nav2_amcl/src/map/map_cspace.cpp b/nav2_amcl/src/map/map_cspace.cpp index 6dca9e8964..ce66baf1a5 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_amcl/src/map/map_store.c b/nav2_amcl/src/map/map_store.c deleted file mode 100644 index 6199263d99..0000000000 --- a/nav2_amcl/src/map/map_store.c +++ /dev/null @@ -1,203 +0,0 @@ -/* - * Player - One Hell of a Robot Server - * Copyright (C) 2000 Brian Gerkey & Kasper Stoy - * gerkey@usc.edu kaspers@robotics.usc.edu - * - * This library is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * This library is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with this library; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - */ -/************************************************************************** - * Desc: Global map storage functions - * Author: Andrew Howard - * Date: 6 Feb 2003 - * CVS: $Id: map_store.c 2951 2005-08-19 00:48:20Z gerkey $ -**************************************************************************/ - -#include -#include -#include -#include -#include - -#include "nav2_amcl/map/map.hpp" - - -//////////////////////////////////////////////////////////////////////////// -// Load an occupancy grid -int map_load_occ(map_t * map, const char * filename, double scale, int negate) -{ - FILE * file; - char magic[3]; - int i, j; - int ch, occ; - int width, height, depth; - map_cell_t * cell; - - // Open file - file = fopen(filename, "r"); - if (file == NULL) { - fprintf(stderr, "%s: %s\n", strerror(errno), filename); - return -1; - } - - // Read ppm header - - if ((fscanf(file, "%2s \n", magic) != 1) || (strcmp(magic, "P5") != 0)) { - fprintf(stderr, "incorrect image format; must be PGM/binary"); - fclose(file); - return -1; - } - - // Ignore comments - while ((ch = fgetc(file)) == '#') { - while (fgetc(file) != '\n') {}} - ungetc(ch, file); - - // Read image dimensions - if (fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3) { - fprintf(stderr, "Failed ot read image dimensions"); - fclose(file); - return -1; - } - - // Allocate space in the map - if (map->cells == NULL) { - map->scale = scale; - map->size_x = width; - map->size_y = height; - map->cells = calloc(width * height, sizeof(map->cells[0])); - } else { - if (width != map->size_x || height != map->size_y) { - // PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions"); - fclose(file); - return -1; - } - } - - // Read in the image - for (j = height - 1; j >= 0; j--) { - for (i = 0; i < width; i++) { - ch = fgetc(file); - - // Black-on-white images - if (!negate) { - if (ch < depth / 4) { - occ = +1; - } else if (ch > 3 * depth / 4) { - occ = -1; - } else { - occ = 0; - } - } else { // White-on-black images - if (ch < depth / 4) { - occ = -1; - } else if (ch > 3 * depth / 4) { - occ = +1; - } else { - occ = 0; - } - } - - if (!MAP_VALID(map, i, j)) { - continue; - } - cell = map->cells + MAP_INDEX(map, i, j); - cell->occ_state = occ; - } - } - - fclose(file); - - return 0; -} - - -//////////////////////////////////////////////////////////////////////////// -// Load a wifi signal strength map -/* -int map_load_wifi(map_t *map, const char *filename, int index) -{ - FILE *file; - char magic[3]; - int i, j; - int ch, level; - int width, height, depth; - map_cell_t *cell; - - // Open file - file = fopen(filename, "r"); - if (file == NULL) - { - fprintf(stderr, "%s: %s\n", strerror(errno), filename); - return -1; - } - - // Read ppm header - fscanf(file, "%10s \n", magic); - if (strcmp(magic, "P5") != 0) - { - fprintf(stderr, "incorrect image format; must be PGM/binary"); - return -1; - } - - // Ignore comments - while ((ch = fgetc(file)) == '#') - while (fgetc(file) != '\n'); - ungetc(ch, file); - - // Read image dimensions - fscanf(file, " %d %d \n %d \n", &width, &height, &depth); - - // Allocate space in the map - if (map->cells == NULL) - { - map->size_x = width; - map->size_y = height; - map->cells = calloc(width * height, sizeof(map->cells[0])); - } - else - { - if (width != map->size_x || height != map->size_y) - { - //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions"); - return -1; - } - } - - // Read in the image - for (j = height - 1; j >= 0; j--) - { - for (i = 0; i < width; i++) - { - ch = fgetc(file); - - if (!MAP_VALID(map, i, j)) - continue; - - if (ch == 0) - level = 0; - else - level = ch * 100 / 255 - 100; - - cell = map->cells + MAP_INDEX(map, i, j); - cell->wifi_levels[index] = level; - } - } - - fclose(file); - - return 0; -} -*/ diff --git a/nav2_amcl/src/motion_model/CMakeLists.txt b/nav2_amcl/src/motion_model/CMakeLists.txt index 6322ad9870..f1c2e35413 100644 --- a/nav2_amcl/src/motion_model/CMakeLists.txt +++ b/nav2_amcl/src/motion_model/CMakeLists.txt @@ -1,10 +1,10 @@ add_library(motions_lib SHARED omni_motion_model.cpp differential_motion_model.cpp - motion_model.cpp ) target_link_libraries(motions_lib pf_lib) ament_target_dependencies(motions_lib + pluginlib nav2_util ) diff --git a/nav2_amcl/src/motion_model/differential_motion_model.cpp b/nav2_amcl/src/motion_model/differential_motion_model.cpp index ecdc6146f9..228b66057a 100644 --- a/nav2_amcl/src/motion_model/differential_motion_model.cpp +++ b/nav2_amcl/src/motion_model/differential_motion_model.cpp @@ -19,25 +19,21 @@ * */ - -#include -#include -#include - -#include "nav2_amcl/motion_model/motion_model.hpp" -#include "nav2_amcl/angleutils.hpp" +#include "nav2_amcl/motion_model/differential_motion_model.hpp" namespace nav2_amcl { -DifferentialMotionModel::DifferentialMotionModel( - double alpha1, double alpha2, double alpha3, - double alpha4) +void +DifferentialMotionModel::initialize( + double alpha1, double alpha2, double alpha3, double alpha4, + double alpha5) { alpha1_ = alpha1; alpha2_ = alpha2; alpha3_ = alpha3; alpha4_ = alpha4; + alpha5_ = alpha5; } void @@ -116,3 +112,6 @@ DifferentialMotionModel::odometryUpdate( } } // namespace nav2_amcl + +#include +PLUGINLIB_EXPORT_CLASS(nav2_amcl::DifferentialMotionModel, nav2_amcl::MotionModel) diff --git a/nav2_amcl/src/motion_model/motion_model.cpp b/nav2_amcl/src/motion_model/motion_model.cpp deleted file mode 100644 index f4a8f384db..0000000000 --- a/nav2_amcl/src/motion_model/motion_model.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// This library is free software; you can redistribute it and/or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 2.1 of the License, or (at your option) any later version. -// -// This library is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// Lesser General Public License for more details. -// -// You should have received a copy of the GNU Lesser General Public -// License along with this library; if not, write to the Free Software -// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - -#include "nav2_amcl/motion_model/motion_model.hpp" - -#include - -namespace nav2_amcl -{ - -MotionModel * -MotionModel::createMotionModel( - std::string & type, double alpha1, double alpha2, - double alpha3, double alpha4, double alpha5) -{ - if (type == "differential") { - return new DifferentialMotionModel(alpha1, alpha2, alpha3, alpha4); - } else if (type == "omnidirectional") { - return new OmniMotionModel(alpha1, alpha2, alpha3, alpha4, alpha5); - } - - return nullptr; -} - -} // namespace nav2_amcl diff --git a/nav2_amcl/src/motion_model/omni_motion_model.cpp b/nav2_amcl/src/motion_model/omni_motion_model.cpp index 639e74ece8..969fb7f0d6 100644 --- a/nav2_amcl/src/motion_model/omni_motion_model.cpp +++ b/nav2_amcl/src/motion_model/omni_motion_model.cpp @@ -19,17 +19,13 @@ * */ -#include -#include -#include - -#include "nav2_amcl/motion_model/motion_model.hpp" -#include "nav2_amcl/angleutils.hpp" +#include "nav2_amcl/motion_model/omni_motion_model.hpp" namespace nav2_amcl { -OmniMotionModel::OmniMotionModel( +void +OmniMotionModel::initialize( double alpha1, double alpha2, double alpha3, double alpha4, double alpha5) { @@ -41,7 +37,9 @@ OmniMotionModel::OmniMotionModel( } void -OmniMotionModel::odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta) +OmniMotionModel::odometryUpdate( + pf_t * pf, const pf_vector_t & pose, + const pf_vector_t & delta) { // Compute the new sample poses pf_sample_set_t * set; @@ -91,3 +89,6 @@ OmniMotionModel::odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_ve } } // namespace nav2_amcl + +#include +PLUGINLIB_EXPORT_CLASS(nav2_amcl::OmniMotionModel, nav2_amcl::MotionModel) diff --git a/nav2_amcl/src/pf/eig3.c b/nav2_amcl/src/pf/eig3.c index a87354dbc8..a5a84ae5df 100644 --- a/nav2_amcl/src/pf/eig3.c +++ b/nav2_amcl/src/pf/eig3.c @@ -1,3 +1,23 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ /* Eigen decomposition code for symmetric 3x3 matrices, copied from the public domain Java Matrix library JAMA. */ @@ -13,11 +33,6 @@ static int n = 3; #endif -static double hypot2(double x, double y) -{ - return sqrt(x * x + y * y); -} - // Symmetric Householder reduction to tridiagonal form. static void tred2(double V[n][n], double d[n], double e[n]) @@ -177,7 +192,7 @@ static void tql2(double V[n][n], double d[n], double e[n]) g = d[l]; p = (d[l + 1] - g) / (2.0 * e[l]); - r = hypot2(p, 1.0); + r = hypot(p, 1.0); if (p < 0) { r = -r; } @@ -205,7 +220,7 @@ static void tql2(double V[n][n], double d[n], double e[n]) s2 = s; g = c * e[i]; h = c * p; - r = hypot2(p, e[i]); + r = hypot(p, e[i]); e[i + 1] = s * r; s = e[i] / r; c = p / r; diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index 851e63b64e..63d71b99bd 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -35,7 +35,7 @@ #include "nav2_amcl/pf/pf_pdf.hpp" #include "nav2_amcl/pf/pf_kdtree.hpp" -#include "portable_utils.h" +#include "nav2_amcl/portable_utils.hpp" // Compute the required number of samples, given that there are k bins @@ -238,14 +238,14 @@ int pf_update_converged(pf_t * pf) } // Update the filter with some new action -void pf_update_action(pf_t * pf, pf_action_model_fn_t action_fn, void * action_data) -{ - pf_sample_set_t * set; +// void pf_update_action(pf_t * pf, pf_action_model_fn_t action_fn, void * action_data) +// { +// pf_sample_set_t * set; - set = pf->sets + pf->current_set; +// set = pf->sets + pf->current_set; - (*action_fn)(action_data, set); -} +// (*action_fn)(action_data, set); +// } // Update the filter with some new sensor observation void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data) @@ -597,36 +597,36 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) // Compute the CEP statistics (mean and variance). -void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var) -{ - int i; - double mn, mx, my, mrr; - pf_sample_set_t * set; - pf_sample_t * sample; - - set = pf->sets + pf->current_set; - - mn = 0.0; - mx = 0.0; - my = 0.0; - mrr = 0.0; - - for (i = 0; i < set->sample_count; i++) { - sample = set->samples + i; - - mn += sample->weight; - mx += sample->weight * sample->pose.v[0]; - my += sample->weight * sample->pose.v[1]; - mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0]; - mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1]; - } - - mean->v[0] = mx / mn; - mean->v[1] = my / mn; - mean->v[2] = 0.0; - - *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn)); -} +// void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var) +// { +// int i; +// double mn, mx, my, mrr; +// pf_sample_set_t * set; +// pf_sample_t * sample; + +// set = pf->sets + pf->current_set; + +// mn = 0.0; +// mx = 0.0; +// my = 0.0; +// mrr = 0.0; + +// for (i = 0; i < set->sample_count; i++) { +// sample = set->samples + i; + +// mn += sample->weight; +// mx += sample->weight * sample->pose.v[0]; +// my += sample->weight * sample->pose.v[1]; +// mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0]; +// mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1]; +// } + +// mean->v[0] = mx / mn; +// mean->v[1] = my / mn; +// mean->v[2] = 0.0; + +// *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn)); +// } // Get the statistics for a particular cluster. diff --git a/nav2_amcl/src/pf/pf_draw.c b/nav2_amcl/src/pf/pf_draw.c index d3a9fb10dd..b8e833f513 100644 --- a/nav2_amcl/src/pf/pf_draw.c +++ b/nav2_amcl/src/pf/pf_draw.c @@ -83,17 +83,17 @@ void pf_draw_hist(pf_t * pf, rtk_fig_t * fig) // Draw the CEP statistics -void pf_draw_cep_stats(pf_t * pf, rtk_fig_t * fig) -{ - pf_vector_t mean; - double var; +// void pf_draw_cep_stats(pf_t * pf, rtk_fig_t * fig) +// { +// pf_vector_t mean; +// double var; - pf_get_cep_stats(pf, &mean, &var); - var = sqrt(var); +// pf_get_cep_stats(pf, &mean, &var); +// var = sqrt(var); - rtk_fig_color(fig, 0, 0, 1); - rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0); -} +// rtk_fig_color(fig, 0, 0, 1); +// rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0); +// } // Draw the cluster statistics void pf_draw_cluster_stats(pf_t * pf, rtk_fig_t * fig) diff --git a/nav2_amcl/src/pf/pf_kdtree.c b/nav2_amcl/src/pf/pf_kdtree.c index cce02bdedf..7b27f2f0f5 100644 --- a/nav2_amcl/src/pf/pf_kdtree.c +++ b/nav2_amcl/src/pf/pf_kdtree.c @@ -144,21 +144,21 @@ void pf_kdtree_insert(pf_kdtree_t * self, pf_vector_t pose, double value) //////////////////////////////////////////////////////////////////////////////// // Determine the probability estimate for the given pose. TODO: this // should do a kernel density estimate rather than a simple histogram. -double pf_kdtree_get_prob(pf_kdtree_t * self, pf_vector_t pose) -{ - int key[3]; - pf_kdtree_node_t * node; - - key[0] = floor(pose.v[0] / self->size[0]); - key[1] = floor(pose.v[1] / self->size[1]); - key[2] = floor(pose.v[2] / self->size[2]); - - node = pf_kdtree_find_node(self, self->root, key); - if (node == NULL) { - return 0.0; - } - return node->value; -} +// double pf_kdtree_get_prob(pf_kdtree_t * self, pf_vector_t pose) +// { +// int key[3]; +// pf_kdtree_node_t * node; + +// key[0] = floor(pose.v[0] / self->size[0]); +// key[1] = floor(pose.v[1] / self->size[1]); +// key[2] = floor(pose.v[2] / self->size[2]); + +// node = pf_kdtree_find_node(self, self->root, key); +// if (node == NULL) { +// return 0.0; +// } +// return node->value; +// } //////////////////////////////////////////////////////////////////////////////// diff --git a/nav2_amcl/src/pf/pf_pdf.c b/nav2_amcl/src/pf/pf_pdf.c index 858bea7df8..98a7748ffa 100644 --- a/nav2_amcl/src/pf/pf_pdf.c +++ b/nav2_amcl/src/pf/pf_pdf.c @@ -34,7 +34,7 @@ #include "nav2_amcl/pf/pf_pdf.hpp" -#include "portable_utils.h" +#include "nav2_amcl/portable_utils.hpp" // Random number generator seed value static unsigned int pf_pdf_seed; diff --git a/nav2_amcl/src/pf/pf_vector.c b/nav2_amcl/src/pf/pf_vector.c index 29e183749e..a7a5cd39c6 100644 --- a/nav2_amcl/src/pf/pf_vector.c +++ b/nav2_amcl/src/pf/pf_vector.c @@ -47,45 +47,45 @@ pf_vector_t pf_vector_zero() } -// Check for NAN or INF in any component -int pf_vector_finite(pf_vector_t a) -{ - int i; +// // Check for NAN or INF in any component +// int pf_vector_finite(pf_vector_t a) +// { +// int i; - for (i = 0; i < 3; i++) { - if (!isfinite(a.v[i])) { - return 0; - } - } +// for (i = 0; i < 3; i++) { +// if (!isfinite(a.v[i])) { +// return 0; +// } +// } - return 1; -} +// return 1; +// } // Print a vector -void pf_vector_fprintf(pf_vector_t a, FILE * file, const char * fmt) -{ - int i; +// void pf_vector_fprintf(pf_vector_t a, FILE * file, const char * fmt) +// { +// int i; - for (i = 0; i < 3; i++) { - fprintf(file, fmt, a.v[i]); - fprintf(file, " "); - } - fprintf(file, "\n"); -} +// for (i = 0; i < 3; i++) { +// fprintf(file, fmt, a.v[i]); +// fprintf(file, " "); +// } +// fprintf(file, "\n"); +// } -// Simple vector addition -pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b) -{ - pf_vector_t c; +// // Simple vector addition +// pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b) +// { +// pf_vector_t c; - c.v[0] = a.v[0] + b.v[0]; - c.v[1] = a.v[1] + b.v[1]; - c.v[2] = a.v[2] + b.v[2]; +// c.v[0] = a.v[0] + b.v[0]; +// c.v[1] = a.v[1] + b.v[1]; +// c.v[2] = a.v[2] + b.v[2]; - return c; -} +// return c; +// } // Simple vector subtraction @@ -115,18 +115,18 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b) } -// Transform from global to local coords (a - b) -pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b) -{ - pf_vector_t c; +// // Transform from global to local coords (a - b) +// pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b) +// { +// pf_vector_t c; - c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]); - c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]); - c.v[2] = a.v[2] - b.v[2]; - c.v[2] = atan2(sin(c.v[2]), cos(c.v[2])); +// c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]); +// c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]); +// c.v[2] = a.v[2] - b.v[2]; +// c.v[2] = atan2(sin(c.v[2]), cos(c.v[2])); - return c; -} +// return c; +// } // Return a zero matrix @@ -145,36 +145,36 @@ pf_matrix_t pf_matrix_zero() } -// Check for NAN or INF in any component -int pf_matrix_finite(pf_matrix_t a) -{ - int i, j; +// // Check for NAN or INF in any component +// int pf_matrix_finite(pf_matrix_t a) +// { +// int i, j; - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - if (!isfinite(a.m[i][j])) { - return 0; - } - } - } +// for (i = 0; i < 3; i++) { +// for (j = 0; j < 3; j++) { +// if (!isfinite(a.m[i][j])) { +// return 0; +// } +// } +// } - return 1; -} +// return 1; +// } // Print a matrix -void pf_matrix_fprintf(pf_matrix_t a, FILE * file, const char * fmt) -{ - int i, j; - - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - fprintf(file, fmt, a.m[i][j]); - fprintf(file, " "); - } - fprintf(file, "\n"); - } -} +// void pf_matrix_fprintf(pf_matrix_t a, FILE * file, const char * fmt) +// { +// int i, j; + +// for (i = 0; i < 3; i++) { +// for (j = 0; j < 3; j++) { +// fprintf(file, fmt, a.m[i][j]); +// fprintf(file, " "); +// } +// fprintf(file, "\n"); +// } +// } /* diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index c92b06228f..0569c58bc9 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -54,12 +54,36 @@ 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_controller_cancel_bt_node SHARED plugins/action/controller_cancel_node.cpp) +list(APPEND plugin_libs nav2_controller_cancel_bt_node) + +add_library(nav2_wait_cancel_bt_node SHARED plugins/action/wait_cancel_node.cpp) +list(APPEND plugin_libs nav2_wait_cancel_bt_node) + +add_library(nav2_spin_cancel_bt_node SHARED plugins/action/spin_cancel_node.cpp) +list(APPEND plugin_libs nav2_spin_cancel_bt_node) + +add_library(nav2_back_up_cancel_bt_node SHARED plugins/action/back_up_cancel_node.cpp) +list(APPEND plugin_libs nav2_back_up_cancel_bt_node) + +add_library(nav2_drive_on_heading_cancel_bt_node SHARED plugins/action/drive_on_heading_cancel_node.cpp) +list(APPEND plugin_libs nav2_drive_on_heading_cancel_bt_node) + +add_library(nav2_smooth_path_action_bt_node SHARED plugins/action/smooth_path_action.cpp) +list(APPEND plugin_libs nav2_smooth_path_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) add_library(nav2_back_up_action_bt_node SHARED plugins/action/back_up_action.cpp) list(APPEND plugin_libs nav2_back_up_action_bt_node) +add_library(nav2_drive_on_heading_bt_node SHARED plugins/action/drive_on_heading_action.cpp) +list(APPEND plugin_libs nav2_drive_on_heading_bt_node) + add_library(nav2_spin_action_bt_node SHARED plugins/action/spin_action.cpp) list(APPEND plugin_libs nav2_spin_action_bt_node) @@ -78,12 +102,21 @@ list(APPEND plugin_libs nav2_transform_available_condition_bt_node) add_library(nav2_goal_reached_condition_bt_node SHARED plugins/condition/goal_reached_condition.cpp) list(APPEND plugin_libs nav2_goal_reached_condition_bt_node) +add_library(nav2_globally_updated_goal_condition_bt_node SHARED plugins/condition/globally_updated_goal_condition.cpp) +list(APPEND plugin_libs nav2_globally_updated_goal_condition_bt_node) + add_library(nav2_goal_updated_condition_bt_node SHARED plugins/condition/goal_updated_condition.cpp) list(APPEND plugin_libs nav2_goal_updated_condition_bt_node) +add_library(nav2_is_path_valid_condition_bt_node SHARED plugins/condition/is_path_valid_condition.cpp) +list(APPEND plugin_libs nav2_is_path_valid_condition_bt_node) + add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp) list(APPEND plugin_libs nav2_time_expired_condition_bt_node) +add_library(nav2_path_expiring_timer_condition SHARED plugins/condition/path_expiring_timer_condition.cpp) +list(APPEND plugin_libs nav2_path_expiring_timer_condition) + add_library(nav2_distance_traveled_condition_bt_node SHARED plugins/condition/distance_traveled_condition.cpp) list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node) @@ -108,21 +141,48 @@ list(APPEND plugin_libs nav2_speed_controller_bt_node) add_library(nav2_truncate_path_action_bt_node SHARED plugins/action/truncate_path_action.cpp) list(APPEND plugin_libs nav2_truncate_path_action_bt_node) +add_library(nav2_truncate_path_local_action_bt_node SHARED plugins/action/truncate_path_local_action.cpp) +list(APPEND plugin_libs nav2_truncate_path_local_action_bt_node) + add_library(nav2_goal_updater_node_bt_node SHARED plugins/decorator/goal_updater_node.cpp) list(APPEND plugin_libs nav2_goal_updater_node_bt_node) +add_library(nav2_path_longer_on_approach_bt_node SHARED plugins/decorator/path_longer_on_approach.cpp) +list(APPEND plugin_libs nav2_path_longer_on_approach_bt_node) + add_library(nav2_recovery_node_bt_node SHARED plugins/control/recovery_node.cpp) 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) + +add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp) +list(APPEND plugin_libs nav2_goal_updated_controller_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 2a9cfac472..db761cb31d 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -1,11 +1,15 @@ # nav2_behavior_tree +This module is used by the nav2_bt_navigator to implement a ROS2 node that executes navigation Behavior Trees for either navigation or autonomy systems. The nav2_behavior_tree module uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) for the core Behavior Tree processing. + The nav2_behavior_tree module provides: -* A C++ template class for integrating ROS2 actions into Behavior Trees, +* A C++ template class for easily integrating ROS2 actions and services into Behavior Trees, * Navigation-specific behavior tree nodes, and -* a generic BehaviorTreeEngine class that simplifies the integration of BT processing into ROS2 nodes. +* a generic BehaviorTreeEngine class that simplifies the integration of BT processing into ROS2 nodes for navigation or higher-level autonomy applications. + +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://navigation.ros.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin. -This module is used by the nav2_bt_navigator to implement a ROS2 node that executes navigation Behavior Trees. The nav2_behavior_tree module uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) for the core Behavior Tree processing. +See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. ## The bt_action_node Template and the Behavior Tree Engine @@ -59,26 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine. -## Navigation-Specific Behavior Tree Nodes - -The nav2_behavior_tree package provides several navigation-specific nodes that are pre-registered and can be included in Behavior Trees. - -| BT Node | Type | Description | -|----------|:-------------|------| -| 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. | -| 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. | -| TransformAvailable | Condition | Checks if a TF transform is available. Returns failure if it cannot be found. Once found, it will always return success. Useful for initial condition checks. | -| 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. | -| 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. | -| 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. | -| 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.| - 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 b1189a6ba6..7ca0ac70ad 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 61816a9950..bc48d821af 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,37 +29,64 @@ 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); - void addGrootMonitoring( - BT::Tree * tree, - uint16_t publisher_port, - uint16_t server_port, - uint16_t max_msg_per_second = 25); - - 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: 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 6d36ce91e0..ed2b7886c1 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,19 +26,38 @@ 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, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) { - node_ = config().blackboard->get("node"); - std::string node_namespace; - node_namespace = node_->get_namespace(); + 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_); // Get the required items from the blackboard server_timeout_ = @@ -52,6 +71,8 @@ class BtActionNode : public BT::ActionNodeBase if (getInput("server_name", remapped_action_name)) { action_name_ = remapped_action_name; } + std::string node_namespace; + node_namespace = node_->get_namespace(); // Append namespace to the action name if(node_namespace != "/") { action_name_ = node_namespace + "/" + action_name_; @@ -59,7 +80,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; @@ -68,19 +89,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 = { @@ -92,6 +120,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({}); @@ -100,39 +132,57 @@ 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 - virtual void on_wait_for_result() + /** + * @brief Function to perform some user-defined operation after a timeout + * waiting for a result that hasn't been received yet. Also provides access to + * the latest feedback message from the action server. Feedback will be nullptr + * in subsequent calls to this function if no new feedback is received while waiting for a result. + * @param feedback shared_ptr to latest feedback message, nullptr if no new feedback was received + */ + virtual void on_wait_for_result(std::shared_ptr/*feedback*/) { } - // 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 @@ -143,53 +193,108 @@ class BtActionNode : public BT::ActionNodeBase // user defined callback on_tick(); - on_new_goal_received(); + send_new_goal(); } - // The following code corresponds to the "RUNNING" loop - if (rclcpp::ok() && !goal_result_available_) { - // user defined callback. May modify the value of "goal_updated_" - on_wait_for_result(); - - auto goal_status = goal_handle_->get_status(); - if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || - goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) - { - goal_updated_ = false; - on_new_goal_received(); + try { + // 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 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; + } } - rclcpp::spin_some(node_); + // The following code corresponds to the "RUNNING" loop + if (rclcpp::ok() && !goal_result_available_) { + // user defined callback. May modify the value of "goal_updated_" + on_wait_for_result(feedback_); + + // reset feedback to avoid stale information + feedback_.reset(); + + auto goal_status = goal_handle_->get_status(); + if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || + goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) + { + goal_updated_ = false; + 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; + } + } - // check if, after invoking spin_some(), we finally received the result - if (!goal_result_available_) { - // Yield this Action, returning RUNNING - return BT::NodeStatus::RUNNING; + callback_group_executor_.spin_some(); + + // check if, after invoking spin_some(), we finally received the result + if (!goal_result_available_) { + // Yield this Action, returning RUNNING + return BT::NodeStatus::RUNNING; + } + } + } catch (const std::runtime_error & e) { + if (e.what() == std::string("send_goal failed") || + e.what() == std::string("Goal was rejected by the action server")) + { + // Action related failure that should not fail the tree, but the node + return BT::NodeStatus::FAILURE; + } else { + // Internal exception to propogate to the tree + throw e; } } + 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( @@ -202,6 +307,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 @@ -209,7 +318,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 @@ -217,13 +331,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) @@ -232,21 +356,59 @@ class BtActionNode : public BT::ActionNodeBase result_ = result; } }; + send_goal_options.feedback_callback = + [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) { + feedback_ = feedback; + }; - 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(); + } - if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) - { + /** + * @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; + } + + 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; @@ -265,12 +427,25 @@ class BtActionNode : public BT::ActionNodeBase typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; typename rclcpp_action::ClientGoalHandle::WrappedResult result_; + // To handle feedback from action server + std::shared_ptr feedback_; + // 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 0000000000..7558080347 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -0,0 +1,245 @@ +// 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 + */ + const 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_; + + // 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 0000000000..360e8f24d6 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -0,0 +1,241 @@ +// 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); + } +} + +template +BtActionServer::~BtActionServer() +{} + +template +bool BtActionServer::on_configure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Name client node after action name + std::string client_node_name = action_name_; + std::replace(client_node_name.begin(), client_node_name.end(), '/', '_'); + // 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()) + client_node_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 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_.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; + } + + // 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; + 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, rc); + + 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_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp new file mode 100644 index 0000000000..b48478ecc4 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -0,0 +1,166 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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_CANCEL_ACTION_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp_v3/action_node.h" +#include "nav2_util/node_utils.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_behavior_tree/bt_conversions.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief Abstract class representing an action for cancelling BT node + * @tparam ActionT Type of action + */ +template +class BtCancelActionNode : public BT::ActionNodeBase +{ +public: + /** + * @brief A nav2_behavior_tree::BtCancelActionNode 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 + */ + BtCancelActionNode( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) + : 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 + server_timeout_ = + config().blackboard->template get("server_timeout"); + getInput("server_timeout", server_timeout_); + + std::string remapped_action_name; + if (getInput("server_name", remapped_action_name)) { + action_name_ = remapped_action_name; + } + createActionClient(action_name_); + + // Give the derive class a chance to do any initialization + RCLCPP_DEBUG( + node_->get_logger(), "\"%s\" BtCancelActionNode initialized", + xml_tag_name.c_str()); + } + + BtCancelActionNode() = delete; + + virtual ~BtCancelActionNode() + { + } + + /** + * @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, callback_group_); + + // Make sure the server is actually there before continuing + RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); + action_client_->wait_for_action_server(); + } + + /** + * @brief Any subclass of BtCancelActionNode 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 = { + BT::InputPort("server_name", "Action server name"), + BT::InputPort("server_timeout") + }; + basic.insert(addition.begin(), addition.end()); + + return basic; + } + + void halt() + { + } + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts({}); + } + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override + { + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(BT::NodeStatus::RUNNING); + + // Cancel all the goals specified before 10ms from current time + // to avoid async communication error + + rclcpp::Time goal_expiry_time = node_->now() - std::chrono::milliseconds(10); + + auto future_cancel = action_client_->async_cancel_goals_before(goal_expiry_time); + + if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( + node_->get_logger(), + "Failed to cancel the action server for %s", action_name_.c_str()); + return BT::NodeStatus::FAILURE; + } + return BT::NodeStatus::SUCCESS; + } + +protected: + std::string action_name_; + typename std::shared_ptr> action_client_; + + // 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 canceled + std::chrono::milliseconds server_timeout_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_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 58fc402671..c109c4f0e2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp @@ -15,9 +15,13 @@ #ifndef NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_ #define NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_ +#include + +#include "rclcpp/time.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" namespace BT { @@ -26,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) { @@ -42,10 +51,15 @@ 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) { - // three real numbers separated by semicolons + // four real numbers separated by semicolons auto parts = BT::splitString(key, ';'); if (parts.size() != 4) { throw std::runtime_error("invalid number of fields for orientation attribute)"); @@ -59,6 +73,38 @@ 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) +{ + // 7 real numbers separated by semicolons + auto parts = BT::splitString(key, ';'); + if (parts.size() != 9) { + throw std::runtime_error("invalid number of fields for PoseStamped attribute)"); + } else { + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString(parts[0])); + pose_stamped.header.frame_id = BT::convertFromString(parts[1]); + pose_stamped.pose.position.x = BT::convertFromString(parts[2]); + pose_stamped.pose.position.y = BT::convertFromString(parts[3]); + pose_stamped.pose.position.z = BT::convertFromString(parts[4]); + pose_stamped.pose.orientation.x = BT::convertFromString(parts[5]); + pose_stamped.pose.orientation.y = BT::convertFromString(parts[6]); + pose_stamped.pose.orientation.z = BT::convertFromString(parts[7]); + pose_stamped.pose.orientation.w = BT::convertFromString(parts[8]); + return pose_stamped; + } +} + +/** + * @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 c13a2e47fe..02e395f69d 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,42 +26,60 @@ 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->get("node"); - std::string node_namespace; - node_namespace = node_->get_namespace(); + 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_); + std::string node_namespace; + node_namespace = node_->get_namespace(); // Append namespace to the action name if(node_namespace != "/") { service_name_ = node_namespace + "/" + 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()); } @@ -72,8 +90,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 = { @@ -85,50 +107,106 @@ 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_).share(); + 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 Function to perform some user-defined operation upon successful + * completion of the service. Could put a value on the blackboard. + * @param response can be used to get the result of the service call in the BT Node. + * @return BT::NodeStatus Returns SUCCESS by default, user may override to return another value + */ + virtual BT::NodeStatus on_completion(std::shared_ptr/*response*/) + { + return BT::NodeStatus::SUCCESS; + } + + /** + * @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; + BT::NodeStatus status = on_completion(future_result_.get()); + return status; + } + + 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; @@ -143,10 +221,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 e49dec78ab..4481198972 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,22 +23,39 @@ 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( { BT::InputPort("backup_dist", 0.15, "Distance to backup"), - BT::InputPort("backup_speed", 0.025, "Speed at which to backup") + BT::InputPort("backup_speed", 0.025, "Speed at which to backup"), + BT::InputPort("time_allowance", 10.0, "Allowed time for reversing") }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp new file mode 100644 index 0000000000..d59bbff5a5 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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__BACK_UP_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/back_up.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp + */ +class BackUpCancel : public BtCancelActionNode +{ +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 + */ + BackUpCancel( + const std::string & xml_tag_name, + const std::string & action_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 providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_ 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 28d8b02584..0bb8ad0bb2 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 0000000000..f9f233c233 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -0,0 +1,80 @@ +// 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", "", + "Mapped name to the planner plugin type to use"), + }); + } +}; + +} // 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 fc881a1d19..105b52fbf6 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,30 +24,50 @@ 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("planner_id", ""), + BT::InputPort( + "start", "Start pose of the path if overriding current robot pose"), + BT::InputPort( + "planner_id", "", + "Mapped name to the planner plugin type to use"), }); } - -private: - bool first_time_{true}; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_cancel_node.hpp new file mode 100644 index 0000000000..a9c742df31 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_cancel_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/follow_path.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowPath + */ +class ControllerCancel : public BtCancelActionNode +{ +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 + */ + ControllerCancel( + const std::string & xml_tag_name, + const std::string & action_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 providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_ 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 0000000000..968750a05e --- /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/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp new file mode 100644 index 0000000000..5632a7551e --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp @@ -0,0 +1,60 @@ +// 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. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_ + +#include + +#include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_msgs/action/drive_on_heading.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::DriveOnHeading + */ +class DriveOnHeadingAction : public BtActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::DriveOnHeadingAction + * @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 + */ + DriveOnHeadingAction( + const std::string & xml_tag_name, + const std::string & action_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 providedBasicPorts( + { + BT::InputPort("dist_to_travel", 0.15, "Distance to travel"), + BT::InputPort("speed", 0.025, "Speed at which to travel"), + BT::InputPort("time_allowance", 10.0, "Allowed time for driving on heading") + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp new file mode 100644 index 0000000000..a2f13a94fe --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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__DRIVE_ON_HEADING_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/drive_on_heading.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::DriveOnHeading + */ +class DriveOnHeadingCancel : public BtCancelActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::DriveOnHeadingCancel + * @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 + */ + DriveOnHeadingCancel( + const std::string & xml_tag_name, + const std::string & action_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 providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_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 951c948c8e..a97993c773 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 @@ -16,6 +16,7 @@ #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_ #include +#include #include "nav2_msgs/action/follow_path.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -23,24 +24,47 @@ 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; - void on_wait_for_result() override; + /** + * @brief Function to perform some user-defined operation after a timeout + * waiting for a result that hasn't been received yet + * @param feedback shared_ptr to latest feedback message + */ + void on_wait_for_result( + std::shared_ptr feedback) 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 0000000000..11f0d0f423 --- /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 0000000000..5234dedaec --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -0,0 +1,66 @@ +// 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"), + BT::InputPort("behavior_tree", "Behavior tree to run"), + }); + } +}; + +} // 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 be48c54a3f..e8c37af738 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,23 +25,38 @@ 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( { - BT::InputPort("position", "Position"), - BT::InputPort("orientation", "Orientation") + BT::InputPort("goal", "Destination to plan to"), + BT::InputPort("behavior_tree", "Behavior tree to run"), }); } }; 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 0000000000..3aa9e6573e --- /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 c241687261..8d90327f20 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 0000000000..05c01ed58f --- /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/smooth_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp new file mode 100644 index 0000000000..982f4161ea --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2021 RoboTech Vision +// 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__PLUGINS__ACTION__SMOOTH_PATH_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTH_PATH_ACTION_HPP_ + +#include + +#include "nav2_msgs/action/smooth_path.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::SmoothPath + */ +class SmoothPathAction : public nav2_behavior_tree::BtActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::SmoothPathAction + * @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 + */ + SmoothPathAction( + 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( + "smoothed_path", + "Path smoothed by SmootherServer node"), + BT::OutputPort("smoothing_duration", "Time taken to smooth path"), + BT::OutputPort( + "was_completed", "True if smoothing was not interrupted by time limit"), + BT::InputPort("unsmoothed_path", "Path to be smoothed"), + BT::InputPort("max_smoothing_duration", 3.0, "Maximum smoothing duration"), + BT::InputPort( + "check_for_collisions", false, + "If true collision check will be performed after smoothing"), + BT::InputPort("smoother_id", ""), + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTH_PATH_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 4120cad9c5..76496ba7ed 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,23 +23,44 @@ 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( { - BT::InputPort("spin_dist", 1.57, "Spin distance") + BT::InputPort("spin_dist", 1.57, "Spin distance"), + BT::InputPort("time_allowance", 10.0, "Allowed time for spinning"), + BT::InputPort("is_recovery", true, "True if recovery") }); } + +private: + bool is_recovery_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_cancel_node.hpp new file mode 100644 index 0000000000..0e747fa70d --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_cancel_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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__SPIN_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/spin.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait + */ +class SpinCancel : public BtCancelActionNode +{ +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 + */ + SpinCancel( + const std::string & xml_tag_name, + const std::string & action_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 providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_ 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 f76b7df4d5..2604bcfea1 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/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp new file mode 100644 index 0000000000..113e0d83e7 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp @@ -0,0 +1,124 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// 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__TRUNCATE_PATH_LOCAL_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_LOCAL_ACTION_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" + +#include "behaviortree_cpp_v3/action_node.h" +#include "tf2_ros/buffer.h" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ActionNodeBase to shorten path to some distance around robot + */ +class TruncatePathLocal : public BT::ActionNodeBase +{ +public: + /** + * @brief A nav2_behavior_tree::TruncatePathLocal constructor + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + TruncatePathLocal( + 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("input_path", "Original Path"), + BT::OutputPort( + "output_path", "Path truncated to a certain distance around robot"), + BT::InputPort( + "distance_forward", 8.0, + "Distance in forward direction"), + BT::InputPort( + "distance_backward", 4.0, + "Distance in backward direction"), + BT::InputPort( + "robot_frame", "base_link", + "Robot base frame id"), + BT::InputPort( + "transform_tolerance", 0.2, + "Transform lookup tolerance"), + BT::InputPort( + "pose", "Manually specified pose to be used" + "if overriding current robot pose"), + BT::InputPort( + "angular_distance_weight", 0.0, + "Weight of angular distance relative to positional distance when finding which path " + "pose is closest to robot. Not applicable on paths without orientations assigned"), + BT::InputPort( + "max_robot_pose_search_dist", std::numeric_limits::infinity(), + "Maximum forward integrated distance along the path (starting from the last detected pose) " + "to bound the search for the closest pose to the robot. When set to infinity (default), " + "whole path is searched every time"), + }; + } + +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; + + /** + * @brief Get either specified input pose or robot pose in path frame + * @param path_frame_id Frame ID of path + * @param pose Output pose + * @return True if succeeded + */ + bool getRobotPose(std::string path_frame_id, geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief A custom pose distance method which takes angular distance into account + * in addition to spatial distance (to improve picking a correct pose near cusps and loops) + * @param pose1 Distance is computed between this pose and pose2 + * @param pose2 Distance is computed between this pose and pose1 + * @param angular_distance_weight Weight of angular distance relative to spatial distance + * (1.0 means that 1 radian of angular distance corresponds to 1 meter of spatial distance) + */ + static double poseDistance( + const geometry_msgs::msg::PoseStamped & pose1, + const geometry_msgs::msg::PoseStamped & pose2, + const double angular_distance_weight); + + std::shared_ptr tf_buffer_; + + nav_msgs::msg::Path path_; + nav_msgs::msg::Path::_poses_type::iterator closest_pose_detection_begin_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_LOCAL_ACTION_HPP_ 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 a6e753da2e..f452d24d32 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/action/wait_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp new file mode 100644 index 0000000000..1545e9da2e --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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__WAIT_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/wait.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait + */ +class WaitCancel : public BtCancelActionNode +{ +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 + */ + WaitCancel( + const std::string & xml_tag_name, + const std::string & action_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 providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_ 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 7bd9b8627a..6373a5600c 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/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp new file mode 100644 index 0000000000..64f77f1473 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp @@ -0,0 +1,73 @@ +// Copyright (c) 2021 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "behaviortree_cpp_v3/condition_node.h" +#include "geometry_msgs/msg/pose_stamped.hpp" + + +namespace nav2_behavior_tree +{ +/** + * @brief A BT::ConditionNode that returns SUCCESS when goal is + * updated on the blackboard and FAILURE otherwise + */ +class GloballyUpdatedGoalCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::GloballyUpdatedGoalCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + GloballyUpdatedGoalCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + GloballyUpdatedGoalCondition() = 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 {}; + } + +private: + bool first_time; + rclcpp::Node::SharedPtr node_; + geometry_msgs::msg::PoseStamped goal_; + std::vector goals_; +}; + +} // namespace nav2_behavior_tree + + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_ 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 fc69d53d33..a4d41437d8 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 45a6704ef9..1e8c9712c1 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 9bed7e6ac4..2bb7d995b8 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 0bc30cd563..5c4753cf9a 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_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp new file mode 100644 index 0000000000..a04b1263f4 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -0,0 +1,75 @@ +// Copyright (c) 2021 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp_v3/condition_node.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/srv/is_path_valid.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that returns SUCCESS when the IsPathValid + * service returns true and FAILURE otherwise + */ +class IsPathValidCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::IsPathValidCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + IsPathValidCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsPathValidCondition() = 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 { + BT::InputPort("path", "Path to Check"), + BT::InputPort("server_timeout") + }; + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Client::SharedPtr client_; + // The timeout value while waiting for a responce from the + // is path valid service + std::chrono::milliseconds server_timeout_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_ 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 e46f03cccc..565f261100 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/path_expiring_timer_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp new file mode 100644 index 0000000000..fb2f42f3d4 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp @@ -0,0 +1,73 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp_v3/condition_node.h" +#include "nav_msgs/msg/path.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that returns SUCCESS every time a specified + * time period passes and FAILURE otherwise + */ +class PathExpiringTimerCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::PathExpiringTimerCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + PathExpiringTimerCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + PathExpiringTimerCondition() = 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 { + BT::InputPort("seconds", 1.0, "Seconds"), + BT::InputPort("path") + }; + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Time start_; + nav_msgs::msg::Path prev_path_; + double period_; + bool first_time_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_ 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 ec40b9db43..9f8c47afab 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 1a624a4cd2..64572e21b7 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 666d119164..0384381d2a 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 331a19d807..89c0cfa300 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 8ebc2ab26c..8c374ce6af 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 299833f555..2d571597ea 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_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp new file mode 100644 index 0000000000..4ac0ab44ee --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -0,0 +1,68 @@ +// 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. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp_v3/decorator_node.h" + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::DecoratorNode that ticks its child if the goal was updated + */ +class GoalUpdatedController : public BT::DecoratorNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::GoalUpdatedController + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ + GoalUpdatedController( + 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 goal_was_updated_; + geometry_msgs::msg::PoseStamped goal_; + std::vector goals_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_ 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 1aba0ccd89..49dfbc1a0c 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/path_longer_on_approach.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp new file mode 100644 index 0000000000..6e41516434 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp @@ -0,0 +1,113 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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__PATH_LONGER_ON_APPROACH_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__PATH_LONGER_ON_APPROACH_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "behaviortree_cpp_v3/decorator_node.h" +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::DecoratorNode that ticks its child everytime when the length of + * the new path is smaller than the old one by the length given by the user. + */ +class PathLongerOnApproach : public BT::DecoratorNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::PathLongerOnApproach + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ + PathLongerOnApproach( + 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 { + BT::InputPort("path", "Planned Path"), + BT::InputPort( + "prox_len", 3.0, + "Proximity length (m) for the path to be longer on approach"), + BT::InputPort( + "length_factor", 2.0, + "Length multiplication factor to check if the path is significantly longer"), + }; + } + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + +private: + /** + * @brief Checks if the global path is updated + * @param new_path new path to the goal + * @param old_path current path to the goal + * @return whether the path is updated for the current goal + */ + bool isPathUpdated( + nav_msgs::msg::Path & new_path, + nav_msgs::msg::Path & old_path); + + /** + * @brief Checks if the robot is in the goal proximity + * @param old_path current path to the goal + * @param prox_leng proximity length from the goal + * @return whether the robot is in the goal proximity + */ + bool isRobotInGoalProximity( + nav_msgs::msg::Path & old_path, + double & prox_leng); + + /** + * @brief Checks if the new path is longer + * @param new_path new path to the goal + * @param old_path current path to the goal + * @param length_factor multipler for path length check + * @return whether the new path is longer + */ + bool isNewPathLonger( + nav_msgs::msg::Path & new_path, + nav_msgs::msg::Path & old_path, + double & length_factor); + +private: + nav_msgs::msg::Path new_path_; + nav_msgs::msg::Path old_path_; + double prox_len_ = std::numeric_limits::max(); + double length_factor_ = std::numeric_limits::max(); + rclcpp::Node::SharedPtr node_; + bool first_time_ = true; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__PATH_LONGER_ON_APPROACH_HPP_ 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 01cb268766..201f4868cf 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 0000000000..c16ef63efa --- /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 5af0429117..15322ef85b 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 0000000000..d71a57bc04 --- /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 8697adf1e2..72a09fa3eb 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -1,7 +1,7 @@ @@ -10,41 +10,165 @@ Distance to backup Speed at which to backup + Allowed time for reversing + Service name + Server timeout + + + + Distance to travel + Speed at which to travel + Allowed time for reversing + Service name + Server timeout + + + + Service name to cancel the controller server + Server timeout + + + + Service name to cancel the backup behavior + Server timeout + + + + Service name to cancel the drive on heading behavior + Server timeout + + + + Service name to cancel the spin behavior + Server timeout + + + + Service name to cancel the wait behavior + Server timeout Service name + Server timeout Destination to plan to + Start pose of the path if overriding current robot pose + Mapped name to the planner plugin type to use + Service name + Server timeout + Path created by ComputePathToPose node + + + + Destinations to plan through + Start pose of the path if overriding current robot pose + Service name + Server timeout + Mapped name to the planner plugin type to use Path created by ComputePathToPose node - + + + + Input goals to remove if passed + Radius tolerance on a goal to consider it passed + Global frame + Robot base frame + Set of goals after removing any passed + + + + + Path to be smoothed + Maximum smoothing duration + Bool if collision check should be performed + Smoothed path + Smoothing duration + True if smoothing was not interrupted by time limit Path to follow + Goal checker + Service name + Server timeout - Orientation - Position + Goal + Service name + Server timeout + Behavior tree to run + + + + Goals + Service name + Server timeout + Behavior tree to run + + + + Service name + Server timeout + + + + Distance before goal to truncate + Path to truncate + Truncated path to utilize + + + + Distance in forward direction + Distance in backward direction + Robot base frame id + Transform lookup tolerance + Manually specified pose to be used if overriding current robot pose + Weight of angular distance relative to positional distance when finding which path pose is closest to robot. Not applicable on paths without orientations assigned + Maximum forward integrated distance along the path (starting from the last detected pose) to bound the search for the closest pose to the robot. When set to infinity (default), whole path is searched every time + 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 + Allowed time for spinning + Service name + Server timeout Wait time + Service name + Server timeout Destination + Reference frame + Robot base frame @@ -56,6 +180,38 @@ + + + + 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 + + + + + Time to check if expired + Check if path has been updated to enable timer reset + + + + + + + Path to validate + Server timeout + + @@ -72,6 +228,16 @@ Distance + Reference frame + Robot base frame + + + + + + + Original goal in + Output goal set by subscription @@ -87,5 +253,14 @@ Duration (secs) for velocity smoothing filter + + Planned Path + Proximity length (m) for the path to be longer on approach + Length multiplication factor to check if the path is significantly longer + + + + + diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 604c3b5c28..81e24a8cb3 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.4.7 + 1.1.0 TODO Michael Jeronimo Carlos Orduno @@ -50,6 +50,8 @@ ament_lint_common ament_lint_auto + ament_cmake_gtest + test_msgs ament_cmake diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index c1eec645fb..b72afaf810 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -12,59 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__BACK_UP_ACTION_HPP_ -#define NAV2_BEHAVIOR_TREE__BACK_UP_ACTION_HPP_ - #include #include -#include -#include "nav2_behavior_tree/bt_action_node.hpp" -#include "nav2_msgs/action/back_up.hpp" +#include "nav2_behavior_tree/plugins/action/back_up_action.hpp" namespace nav2_behavior_tree { -class BackUpAction : public BtActionNode +BackUpAction::BackUpAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) { -public: - BackUpAction( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf) - : BtActionNode(xml_tag_name, action_name, conf) - { - double dist; - getInput("backup_dist", dist); - double speed; - getInput("backup_speed", speed); - - // silently fix, vector direction determined by distance sign - if (speed < 0.0) { - speed *= -1.0; - } - - // Populate the input message - goal_.target.x = dist; - goal_.target.y = 0.0; - goal_.target.z = 0.0; - goal_.speed = speed; - } - - void on_tick() override - { - increment_recovery_count(); - } + double dist; + getInput("backup_dist", dist); + double speed; + getInput("backup_speed", speed); + double time_allowance; + getInput("time_allowance", time_allowance); + + // Populate the input message + goal_.target.x = dist; + goal_.target.y = 0.0; + goal_.target.z = 0.0; + goal_.speed = speed; + goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); +} - static BT::PortsList providedPorts() - { - return providedBasicPorts( - { - BT::InputPort("backup_dist", -0.15, "Distance to backup"), - BT::InputPort("backup_speed", 0.025, "Speed at which to backup") - }); - } -}; +void BackUpAction::on_tick() +{ + increment_recovery_count(); +} } // namespace nav2_behavior_tree @@ -75,10 +55,8 @@ BT_REGISTER_NODES(factory) [](const std::string & name, const BT::NodeConfiguration & config) { return std::make_unique( - name, "back_up", config); + name, "backup", config); }; factory.registerBuilder("BackUp", builder); } - -#endif // NAV2_BEHAVIOR_TREE__BACK_UP_ACTION_HPP_ diff --git a/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp b/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp new file mode 100644 index 0000000000..3baa1ffb1e --- /dev/null +++ b/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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/back_up_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +BackUpCancel::BackUpCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // 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, "backup", config); + }; + + factory.registerBuilder( + "CancelBackUp", builder); +} diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index aa269bdbcc..08d2b00632 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 0000000000..f699b9fc0b --- /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 f4f551eb58..83b29dfe3a 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,19 +30,16 @@ 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() { setOutput("path", result_.result->path); - - if (first_time_) { - first_time_ = false; - } else { - config().blackboard->set("path_updated", true); - } return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp b/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp new file mode 100644 index 0000000000..966ee01e10 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +ControllerCancel::ControllerCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // 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, "follow_path", config); + }; + + factory.registerBuilder( + "CancelControl", builder); +} 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 0000000000..7d77adbee3 --- /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/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp new file mode 100644 index 0000000000..81a93a6362 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -0,0 +1,57 @@ +// 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. + +#include +#include + +#include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp" + +namespace nav2_behavior_tree +{ + +DriveOnHeadingAction::DriveOnHeadingAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) +{ + double dist; + getInput("dist_to_travel", dist); + double speed; + getInput("speed", speed); + double time_allowance; + getInput("time_allowance", time_allowance); + + // Populate the input message + goal_.target.x = dist; + goal_.target.y = 0.0; + goal_.target.z = 0.0; + goal_.speed = speed; + goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); +} + +} // 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, "drive_on_heading", config); + }; + + factory.registerBuilder("DriveOnHeading", builder); +} diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp new file mode 100644 index 0000000000..b29de63df5 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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/drive_on_heading_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +DriveOnHeadingCancel::DriveOnHeadingCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // 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, "drive_on_heading", config); + }; + + factory.registerBuilder( + "CancelDriveOnHeading", builder); +} diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 62823f4032..0fc0dc5b57 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -26,25 +26,42 @@ FollowPathAction::FollowPathAction( const BT::NodeConfiguration & conf) : BtActionNode(xml_tag_name, action_name, conf) { - config().blackboard->set("path_updated", false); } 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() +void FollowPathAction::on_wait_for_result( + std::shared_ptr/*feedback*/) { - // Check if the goal has been updated - if (config().blackboard->get("path_updated")) { - // Reset the flag in the blackboard - config().blackboard->set("path_updated", false); + // Grab the new path + nav_msgs::msg::Path new_path; + getInput("path", new_path); - // Grab the new goal and set the flag so that we send the new goal to + // Check if it is not same with the current one + if (goal_.path != new_path) { // the action server on the next loop iteration - getInput("path", goal_.path); + 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; } } 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 0000000000..1a2e9c703b --- /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 0000000000..693fbfa146 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -0,0 +1,56 @@ +// 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; + } + getInput("behavior_tree", goal_.behavior_tree); +} + +} // 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/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp index f779a682ad..82cdab44f8 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -30,19 +30,13 @@ NavigateToPoseAction::NavigateToPoseAction( void NavigateToPoseAction::on_tick() { - // Use the position and orientation fields from the XML attributes to initialize the goal - geometry_msgs::msg::Point position; - geometry_msgs::msg::Quaternion orientation; - - if (!getInput("position", position) || !getInput("orientation", orientation)) { + if (!getInput("goal", goal_.pose)) { RCLCPP_ERROR( node_->get_logger(), - "NavigateToPoseAction: position or orientation not provided"); + "NavigateToPoseAction: goal not provided"); return; } - - goal_.pose.pose.position = position; - goal_.pose.pose.orientation = orientation; + getInput("behavior_tree", goal_.behavior_tree); } } // namespace nav2_behavior_tree 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 0000000000..ccb8518412 --- /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 0000000000..99568f933f --- /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/action/smooth_path_action.cpp b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp new file mode 100644 index 0000000000..bfbc841ff1 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp @@ -0,0 +1,64 @@ +// Copyright (c) 2021 RoboTech Vision +// 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. + +#include +#include + +#include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp" + +namespace nav2_behavior_tree +{ + +SmoothPathAction::SmoothPathAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) +{ +} + +void SmoothPathAction::on_tick() +{ + getInput("unsmoothed_path", goal_.path); + getInput("smoother_id", goal_.smoother_id); + double max_smoothing_duration; + getInput("max_smoothing_duration", max_smoothing_duration); + goal_.max_smoothing_duration = rclcpp::Duration::from_seconds(max_smoothing_duration); + getInput("check_for_collisions", goal_.check_for_collisions); +} + +BT::NodeStatus SmoothPathAction::on_success() +{ + setOutput("smoothed_path", result_.result->path); + setOutput("smoothing_duration", rclcpp::Duration(result_.result->smoothing_duration).seconds()); + setOutput("was_completed", result_.result->was_completed); + 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, "smooth_path", config); + }; + + factory.registerBuilder( + "SmoothPath", builder); +} diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index ad668232c6..73dc8c589f 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -12,49 +12,35 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__SPIN_ACTION_HPP_ -#define NAV2_BEHAVIOR_TREE__SPIN_ACTION_HPP_ - #include #include -#include -#include "nav2_behavior_tree/bt_action_node.hpp" -#include "nav2_msgs/action/spin.hpp" -#include "geometry_msgs/msg/quaternion.hpp" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "nav2_behavior_tree/plugins/action/spin_action.hpp" namespace nav2_behavior_tree { -class SpinAction : public BtActionNode +SpinAction::SpinAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) { -public: - SpinAction( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf) - : BtActionNode(xml_tag_name, action_name, conf) - { - double dist; - getInput("spin_dist", dist); - goal_.target_yaw = dist; - } + double dist; + getInput("spin_dist", dist); + double time_allowance; + getInput("time_allowance", time_allowance); + goal_.target_yaw = dist; + goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + getInput("is_recovery", is_recovery_); +} - void on_tick() override - { +void SpinAction::on_tick() +{ + if (is_recovery_) { increment_recovery_count(); } - - static BT::PortsList providedPorts() - { - return providedBasicPorts( - { - BT::InputPort("spin_dist", 1.57, "Spin distance") - }); - } -}; +} } // namespace nav2_behavior_tree @@ -69,5 +55,3 @@ BT_REGISTER_NODES(factory) factory.registerBuilder("Spin", builder); } - -#endif // NAV2_BEHAVIOR_TREE__SPIN_ACTION_HPP_ diff --git a/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp b/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp new file mode 100644 index 0000000000..62ddbc4719 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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/spin_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +SpinCancel::SpinCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // 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, "spin", config); + }; + + factory.registerBuilder( + "CancelSpin", builder); +} diff --git a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp new file mode 100644 index 0000000000..934f2b86bb --- /dev/null +++ b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2021 RoboTech Vision +// +// 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 "behaviortree_cpp_v3/decorator_node.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "tf2_ros/create_timer_ros.h" + +#include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp" + +namespace nav2_behavior_tree +{ + +TruncatePathLocal::TruncatePathLocal( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf) +{ + tf_buffer_ = + config().blackboard->template get>( + "tf_buffer"); +} + +inline BT::NodeStatus TruncatePathLocal::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + double distance_forward, distance_backward; + geometry_msgs::msg::PoseStamped pose; + double angular_distance_weight; + double max_robot_pose_search_dist; + + getInput("distance_forward", distance_forward); + getInput("distance_backward", distance_backward); + getInput("angular_distance_weight", angular_distance_weight); + getInput("max_robot_pose_search_dist", max_robot_pose_search_dist); + + bool path_pruning = std::isfinite(max_robot_pose_search_dist); + nav_msgs::msg::Path new_path; + getInput("input_path", new_path); + if (!path_pruning || new_path != path_) { + path_ = new_path; + closest_pose_detection_begin_ = path_.poses.begin(); + } + + if (!getRobotPose(path_.header.frame_id, pose)) { + return BT::NodeStatus::FAILURE; + } + + if (path_.poses.empty()) { + setOutput("output_path", path_); + return BT::NodeStatus::SUCCESS; + } + + auto closest_pose_detection_end = path_.poses.end(); + if (path_pruning) { + closest_pose_detection_end = nav2_util::geometry_utils::first_after_integrated_distance( + closest_pose_detection_begin_, path_.poses.end(), max_robot_pose_search_dist); + } + + // find the closest pose on the path + auto current_pose = nav2_util::geometry_utils::min_by( + closest_pose_detection_begin_, closest_pose_detection_end, + [&pose, angular_distance_weight](const geometry_msgs::msg::PoseStamped & ps) { + return poseDistance(pose, ps, angular_distance_weight); + }); + + if (path_pruning) { + closest_pose_detection_begin_ = current_pose; + } + + // expand forwards to extract desired length + auto forward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance( + current_pose, path_.poses.end(), distance_forward); + + // expand backwards to extract desired length + // Note: current_pose + 1 is used because reverse iterator points to a cell before it + auto backward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance( + std::reverse_iterator(current_pose + 1), path_.poses.rend(), distance_backward); + + nav_msgs::msg::Path output_path; + output_path.header = path_.header; + output_path.poses = std::vector( + backward_pose_it.base(), forward_pose_it); + setOutput("output_path", output_path); + + return BT::NodeStatus::SUCCESS; +} + +inline bool TruncatePathLocal::getRobotPose( + std::string path_frame_id, geometry_msgs::msg::PoseStamped & pose) +{ + if (!getInput("pose", pose)) { + std::string robot_frame; + if (!getInput("robot_frame", robot_frame)) { + RCLCPP_ERROR( + config().blackboard->get("node")->get_logger(), + "Neither pose nor robot_frame specified for %s", name().c_str()); + return false; + } + double transform_tolerance; + getInput("transform_tolerance", transform_tolerance); + if (!nav2_util::getCurrentPose( + pose, *tf_buffer_, path_frame_id, robot_frame, transform_tolerance)) + { + RCLCPP_WARN( + config().blackboard->get("node")->get_logger(), + "Failed to lookup current robot pose for %s", name().c_str()); + return false; + } + } + return true; +} + +double +TruncatePathLocal::poseDistance( + const geometry_msgs::msg::PoseStamped & pose1, + const geometry_msgs::msg::PoseStamped & pose2, + const double angular_distance_weight) +{ + double dx = pose1.pose.position.x - pose2.pose.position.x; + double dy = pose1.pose.position.y - pose2.pose.position.y; + // taking angular distance into account in addition to spatial distance + // (to improve picking a correct pose near cusps and loops) + tf2::Quaternion q1; + tf2::convert(pose1.pose.orientation, q1); + tf2::Quaternion q2; + tf2::convert(pose2.pose.orientation, q2); + double da = angular_distance_weight * std::abs(q1.angleShortestPath(q2)); + return std::sqrt(dx * dx + dy * dy + da * da); +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) { + factory.registerNodeType( + "TruncatePathLocal"); +} diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index 268a1f7e9b..d2b0e6484b 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -12,58 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__WAIT_ACTION_HPP_ -#define NAV2_BEHAVIOR_TREE__WAIT_ACTION_HPP_ - #include #include -#include -#include "nav2_behavior_tree/bt_action_node.hpp" -#include "nav2_msgs/action/wait.hpp" +#include "nav2_behavior_tree/plugins/action/wait_action.hpp" namespace nav2_behavior_tree { -class WaitAction : public BtActionNode +WaitAction::WaitAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) { -public: - WaitAction( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf) - : BtActionNode(xml_tag_name, action_name, conf) - { - int duration; - getInput("wait_duration", duration); - if (duration <= 0) { - RCLCPP_WARN( - node_->get_logger(), "Wait duration is negative or zero " - "(%i). Setting to positive.", duration); - duration *= -1; - } - - goal_.time.sec = duration; + int duration; + getInput("wait_duration", duration); + if (duration <= 0) { + RCLCPP_WARN( + node_->get_logger(), "Wait duration is negative or zero " + "(%i). Setting to positive.", duration); + duration *= -1; } - void on_tick() override - { - increment_recovery_count(); - } + goal_.time.sec = duration; +} - // Any BT node that accepts parameters must provide a requiredNodeParameters method - static BT::PortsList providedPorts() - { - return providedBasicPorts( - { - BT::InputPort("wait_duration", 1, "Wait time") - }); - } -}; +void WaitAction::on_tick() +{ + increment_recovery_count(); +} } // namespace nav2_behavior_tree - #include "behaviortree_cpp_v3/bt_factory.h" BT_REGISTER_NODES(factory) { @@ -75,5 +56,3 @@ BT_REGISTER_NODES(factory) factory.registerBuilder("Wait", builder); } - -#endif // NAV2_BEHAVIOR_TREE__WAIT_ACTION_HPP_ diff --git a/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp b/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp new file mode 100644 index 0000000000..9365b7e06f --- /dev/null +++ b/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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/wait_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +WaitCancel::WaitCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // 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, "wait", config); + }; + + factory.registerBuilder( + "CancelWait", builder); +} diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp new file mode 100644 index 0000000000..b6afc0d5a4 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -0,0 +1,61 @@ +// Copyright (c) 2021 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp" + +namespace nav2_behavior_tree +{ + +GloballyUpdatedGoalCondition::GloballyUpdatedGoalCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + first_time(true) +{ + node_ = config().blackboard->get("node"); +} + +BT::NodeStatus GloballyUpdatedGoalCondition::tick() +{ + if (first_time) { + first_time = false; + config().blackboard->get>("goals", goals_); + config().blackboard->get("goal", goal_); + return BT::NodeStatus::FAILURE; + } + + 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; + } + + return BT::NodeStatus::FAILURE; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GlobalUpdatedGoal"); +} diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index a09b9f81df..34930bb582 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 @@ -23,19 +23,24 @@ GoalUpdatedCondition::GoalUpdatedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf) -{ -} +{} 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 4aad6d1676..b8630261e6 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_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp new file mode 100644 index 0000000000..4a02dede9a --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -0,0 +1,61 @@ +// Copyright (c) 2021 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp" +#include +#include +#include + +namespace nav2_behavior_tree +{ + +IsPathValidCondition::IsPathValidCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf) +{ + node_ = config().blackboard->get("node"); + client_ = node_->create_client("is_path_valid"); + + server_timeout_ = config().blackboard->template get("server_timeout"); + getInput("server_timeout", server_timeout_); +} + +BT::NodeStatus IsPathValidCondition::tick() +{ + nav_msgs::msg::Path path; + getInput("path", path); + + auto request = std::make_shared(); + + request->path = path; + auto result = client_->async_send_request(request); + + if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (result.get()->is_valid) { + return BT::NodeStatus::SUCCESS; + } + } + return BT::NodeStatus::FAILURE; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsPathValid"); +} diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp index 185f331dc9..211254dafe 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/condition/path_expiring_timer_condition.cpp b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp new file mode 100644 index 0000000000..e018e02535 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp @@ -0,0 +1,75 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "behaviortree_cpp_v3/condition_node.h" + +#include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp" + +namespace nav2_behavior_tree +{ + +PathExpiringTimerCondition::PathExpiringTimerCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + period_(1.0), + first_time_(true) +{ + getInput("seconds", period_); + node_ = config().blackboard->get("node"); +} + +BT::NodeStatus PathExpiringTimerCondition::tick() +{ + if (first_time_) { + getInput("path", prev_path_); + first_time_ = false; + start_ = node_->now(); + return BT::NodeStatus::FAILURE; + } + + // Grab the new path + nav_msgs::msg::Path path; + getInput("path", path); + + // Reset timer if the path has been updated + if (prev_path_ != path) { + prev_path_ = path; + start_ = node_->now(); + } + + // Determine how long its been since we've started this iteration + auto elapsed = node_->now() - start_; + + // Now, get that in seconds + auto seconds = elapsed.seconds(); + + if (seconds < period_) { + return BT::NodeStatus::FAILURE; + } + + start_ = node_->now(); // Reset the timer + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("PathExpiringTimer"); +} diff --git a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp index 9b506c685f..640c3ed7b4 100644 --- a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp +++ b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp @@ -69,6 +69,7 @@ BT::NodeStatus PipelineSequence::tick() void PipelineSequence::halt() { BT::ControlNode::halt(); + last_child_ticked_ = 0; } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index bfe6ac6ae3..42fc8076be 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -47,19 +47,21 @@ BT::NodeStatus RecoveryNode::tick() switch (child_status) { case BT::NodeStatus::SUCCESS: { - retry_count_ = 0; + // reset node and return success when first child returns success halt(); return BT::NodeStatus::SUCCESS; } case BT::NodeStatus::FAILURE: { - // tick second child if (retry_count_ < number_of_retries_) { + // halt first child and tick second child in next iteration + ControlNode::haltChild(0); current_child_idx_++; break; } else { - ControlNode::haltChildren(); + // reset node and return failure when max retries has been exceeded + halt(); return BT::NodeStatus::FAILURE; } } @@ -71,8 +73,7 @@ BT::NodeStatus RecoveryNode::tick() default: { - halt(); - return BT::NodeStatus::FAILURE; + throw BT::LogicError("A child node must never return IDLE"); } } // end switch @@ -80,16 +81,16 @@ BT::NodeStatus RecoveryNode::tick() switch (child_status) { case BT::NodeStatus::SUCCESS: { + // halt second child, increment recovery count, and tick first child in next iteration + ControlNode::haltChild(1); retry_count_++; current_child_idx_--; - ControlNode::haltChildren(); } break; case BT::NodeStatus::FAILURE: { - current_child_idx_--; - retry_count_ = 0; + // reset node and return failure if second child fails halt(); return BT::NodeStatus::FAILURE; } @@ -101,13 +102,13 @@ BT::NodeStatus RecoveryNode::tick() default: { - halt(); - return BT::NodeStatus::FAILURE; + throw BT::LogicError("A child node must never return IDLE"); } } // end switch } } // end while loop - retry_count_ = 0; + + // reset node and return failure halt(); return BT::NodeStatus::FAILURE; } @@ -115,8 +116,8 @@ BT::NodeStatus RecoveryNode::tick() void RecoveryNode::halt() { ControlNode::halt(); - current_child_idx_ = 0; retry_count_ = 0; + current_child_idx_ = 0; } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp new file mode 100644 index 0000000000..f8a2d8cefc --- /dev/null +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -0,0 +1,88 @@ +// 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. + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "behaviortree_cpp_v3/decorator_node.h" +#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp" + + +namespace nav2_behavior_tree +{ + +GoalUpdatedController::GoalUpdatedController( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::DecoratorNode(name, conf) +{ +} + +BT::NodeStatus GoalUpdatedController::tick() +{ + if (status() == BT::NodeStatus::IDLE) { + // Reset since we're starting a new iteration of + // the goal updated controller (moving from IDLE to RUNNING) + + config().blackboard->get>("goals", goals_); + config().blackboard->get("goal", goal_); + + goal_was_updated_ = true; + } + + setStatus(BT::NodeStatus::RUNNING); + + 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; + goal_was_updated_ = true; + } + + // The child gets ticked the first time through and any time the goal has + // changed or preempted. In addition, once the child begins to run, it is ticked each time + // 'til completion + if ((child_node_->status() == BT::NodeStatus::RUNNING) || goal_was_updated_) { + goal_was_updated_ = false; + const BT::NodeStatus child_state = child_node_->executeTick(); + + switch (child_state) { + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + + case BT::NodeStatus::SUCCESS: + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::FAILURE: + default: + return BT::NodeStatus::FAILURE; + } + } + + return status(); +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GoalUpdatedController"); +} diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 5279613498..febaa7de64 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/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp new file mode 100644 index 0000000000..d4b40b0964 --- /dev/null +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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_util/geometry_utils.hpp" + +#include "nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp" + +namespace nav2_behavior_tree +{ + +PathLongerOnApproach::PathLongerOnApproach( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::DecoratorNode(name, conf) +{ + node_ = config().blackboard->get("node"); +} + +bool PathLongerOnApproach::isPathUpdated( + nav_msgs::msg::Path & new_path, + nav_msgs::msg::Path & old_path) +{ + return new_path != old_path && old_path.poses.size() != 0 && + new_path.poses.size() != 0 && + old_path.poses.back() == new_path.poses.back(); +} + +bool PathLongerOnApproach::isRobotInGoalProximity( + nav_msgs::msg::Path & old_path, + double & prox_leng) +{ + return nav2_util::geometry_utils::calculate_path_length(old_path, 0) < prox_leng; +} + +bool PathLongerOnApproach::isNewPathLonger( + nav_msgs::msg::Path & new_path, + nav_msgs::msg::Path & old_path, + double & length_factor) +{ + return nav2_util::geometry_utils::calculate_path_length(new_path, 0) > + length_factor * nav2_util::geometry_utils::calculate_path_length( + old_path, 0); +} + +inline BT::NodeStatus PathLongerOnApproach::tick() +{ + getInput("path", new_path_); + getInput("prox_len", prox_len_); + getInput("length_factor", length_factor_); + + if (status() == BT::NodeStatus::IDLE) { + // Reset the starting point since we're starting a new iteration of + // PathLongerOnApproach (moving from IDLE to RUNNING) + first_time_ = true; + } + + setStatus(BT::NodeStatus::RUNNING); + + // Check if the path is updated and valid, compare the old and the new path length, + // given the goal proximity and check if the new path is longer + if (isPathUpdated(new_path_, old_path_) && isRobotInGoalProximity(old_path_, prox_len_) && + isNewPathLonger(new_path_, old_path_, length_factor_) && !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: + old_path_ = new_path_; + return BT::NodeStatus::SUCCESS; + case BT::NodeStatus::FAILURE: + old_path_ = new_path_; + return BT::NodeStatus::FAILURE; + default: + old_path_ = new_path_; + return BT::NodeStatus::FAILURE; + } + } + old_path_ = new_path_; + first_time_ = false; + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("PathLongerOnApproach"); +} 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 0000000000..84f0fadfbb --- /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 b93b88d7b6..26d96de77a 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 93841615d9..7bfc003cf1 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; @@ -76,29 +83,14 @@ BehaviorTreeEngine::createTreeFromFile( return factory_.createTreeFromFile(file_path, blackboard); } -void -BehaviorTreeEngine::addGrootMonitoring( - BT::Tree * tree, - uint16_t publisher_port, - uint16_t server_port, - uint16_t max_msg_per_second) -{ - // This logger publish status changes using ZeroMQ. Used by Groot - groot_monitor_ = std::make_unique( - *tree, max_msg_per_second, publisher_port, - server_port); -} - -void -BehaviorTreeEngine::resetGrootMonitor() -{ - groot_monitor_.reset(); -} - // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state void BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node) { + if (!root_node) { + return; + } + // this halt signal should propagate through the entire tree. root_node->halt(); diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 3f05ae112d..3c6e595ba7 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -1,17 +1,65 @@ +find_package(test_msgs REQUIRED) + +ament_add_gtest(test_bt_action_node test_bt_action_node.cpp) +ament_target_dependencies(test_bt_action_node ${dependencies} test_msgs) + +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}) + +ament_add_gtest(test_action_back_up_action test_back_up_action.cpp) +target_link_libraries(test_action_back_up_action nav2_back_up_action_bt_node) +ament_target_dependencies(test_action_back_up_action ${dependencies}) + +ament_add_gtest(test_action_drive_on_heading test_drive_on_heading_action.cpp) +target_link_libraries(test_action_drive_on_heading nav2_drive_on_heading_bt_node) +ament_target_dependencies(test_action_drive_on_heading ${dependencies}) + +ament_add_gtest(test_action_wait_action test_wait_action.cpp) +target_link_libraries(test_action_wait_action nav2_wait_action_bt_node) +ament_target_dependencies(test_action_wait_action ${dependencies}) + +ament_add_gtest(test_action_controller_cancel_action test_controller_cancel_node.cpp) +target_link_libraries(test_action_controller_cancel_action nav2_controller_cancel_bt_node) +ament_target_dependencies(test_action_controller_cancel_action ${dependencies}) + +ament_add_gtest(test_action_wait_cancel_action test_wait_cancel_node.cpp) +target_link_libraries(test_action_wait_cancel_action nav2_wait_cancel_bt_node) +ament_target_dependencies(test_action_wait_cancel_action ${dependencies}) + +ament_add_gtest(test_action_spin_cancel_action test_spin_cancel_node.cpp) +target_link_libraries(test_action_spin_cancel_action nav2_spin_cancel_bt_node) +ament_target_dependencies(test_action_spin_cancel_action ${dependencies}) + + +ament_add_gtest(test_action_back_up_cancel_action test_back_up_cancel_node.cpp) +target_link_libraries(test_action_back_up_cancel_action nav2_back_up_cancel_bt_node) +ament_target_dependencies(test_action_back_up_cancel_action ${dependencies}) + +ament_add_gtest(test_action_drive_on_heading_cancel_action test_drive_on_heading_cancel_node.cpp) +target_link_libraries(test_action_drive_on_heading_cancel_action nav2_drive_on_heading_cancel_bt_node) +ament_target_dependencies(test_action_drive_on_heading_cancel_action ${dependencies}) + ament_add_gtest(test_action_clear_costmap_service test_clear_costmap_service.cpp) target_link_libraries(test_action_clear_costmap_service nav2_clear_costmap_service_bt_node) ament_target_dependencies(test_action_clear_costmap_service ${dependencies}) -ament_add_gtest( - test_action_reinitialize_global_localization_service test_reinitialize_global_localization_service.cpp) -target_link_libraries( - test_action_reinitialize_global_localization_service nav2_reinitialize_global_localization_service_bt_node) +ament_add_gtest(test_action_reinitialize_global_localization_service test_reinitialize_global_localization_service.cpp) +target_link_libraries(test_action_reinitialize_global_localization_service nav2_reinitialize_global_localization_service_bt_node) ament_target_dependencies(test_action_reinitialize_global_localization_service ${dependencies}) ament_add_gtest(test_action_compute_path_to_pose_action test_compute_path_to_pose_action.cpp) 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_smooth_path_action test_smooth_path_action.cpp) +target_link_libraries(test_action_smooth_path_action nav2_smooth_path_action_bt_node) +ament_target_dependencies(test_action_smooth_path_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}) @@ -20,6 +68,30 @@ 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_truncate_path_local_action test_truncate_path_local_action.cpp) +target_link_libraries(test_truncate_path_local_action nav2_truncate_path_local_action_bt_node) +ament_target_dependencies(test_truncate_path_local_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 8906d03d7f..db97020212 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,8 +64,10 @@ 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("path_updated", false); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -142,7 +154,40 @@ TEST_F(BackUpActionTestFixture, test_tick) 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::RUNNING); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + 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); +} + +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(); diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp new file mode 100644 index 0000000000..5bef6868e5 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -0,0 +1,171 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelBackUpServer : public TestActionServer +{ +public: + CancelBackUpServer() + : TestActionServer("back_up") + {} + +protected: + void execute( + const typename std::shared_ptr> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // BackUping here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelBackUpActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_back_up_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)); + client_ = rclcpp_action::create_client( + node_, "back_up"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "back_up", config); + }; + + factory_->registerBuilder("CancelBackUp", 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_; + static std::shared_ptr> client_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelBackUpActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelBackUpActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelBackUpActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelBackUpActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelBackUpActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelBackUpActionTestFixture::tree_ = nullptr; + +TEST_F(CancelBackUpActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::BackUp::Goal(); + + // Setting target pose + goal_msg.target.x = 0.5; + + // BackUping for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is infact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and back_up on new thread + CancelBackUpActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelBackUpActionTestFixture::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_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp new file mode 100644 index 0000000000..d95cbd4a84 --- /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 c5a65c52a1..f10177ecff 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,8 +49,10 @@ 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("path_updated", false); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -107,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); @@ -120,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 0000000000..122c764621 --- /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 dd05931537..5e35a1981d 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,8 +70,10 @@ 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("path_updated", false); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = @@ -129,48 +136,112 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) goal.pose.position.x = 1.0; config_->blackboard->set("goal", goal); - // first tick should send the goal to our server - EXPECT_EQ(config_->blackboard->get("path_updated"), false); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + // 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()->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(), 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 + goal.pose.position.x = -2.5; + config_->blackboard->set("goal", goal); + + 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(), 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(); } - // not set to true after the first goal - EXPECT_EQ(config_->blackboard->get("path_updated"), false); + // 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(), 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, 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 + // set new goal and new start goal.pose.position.x = -2.5; + start.pose.position.x = -1.5; config_->blackboard->set("goal", goal); - - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, -2.5); + config_->blackboard->set("start", start); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } - // path is updated on new goal - EXPECT_EQ(config_->blackboard->get("path_updated"), true); + 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_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp new file mode 100644 index 0000000000..3ca86a832b --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp @@ -0,0 +1,168 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/controller_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelControllerServer : public TestActionServer +{ +public: + CancelControllerServer() + : TestActionServer("follow_path") + {} + +protected: + void execute( + const typename std::shared_ptr> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // waiting here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelControllerActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_control_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)); + client_ = rclcpp_action::create_client( + node_, "follow_path"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "follow_path", config); + }; + + factory_->registerBuilder("CancelControl", 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_; + static std::shared_ptr> client_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelControllerActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelControllerActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelControllerActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelControllerActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelControllerActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelControllerActionTestFixture::tree_ = nullptr; + +TEST_F(CancelControllerActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::FollowPath::Goal(); + + // Waiting for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is infact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + CancelControllerActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelControllerActionTestFixture::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_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp new file mode 100644 index 0000000000..64ce713888 --- /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_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp new file mode 100644 index 0000000000..31f9af0427 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -0,0 +1,211 @@ +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp" + +class DriveOnHeadingActionServer : public TestActionServer +{ +public: + DriveOnHeadingActionServer() + : TestActionServer("drive_on_heading") + {} + +protected: + void execute( + const typename std::shared_ptr> + goal_handle) + override + { + nav2_msgs::action::DriveOnHeading::Result::SharedPtr result = + std::make_shared(); + bool return_success = getReturnSuccess(); + if (return_success) { + goal_handle->succeed(result); + } else { + goal_handle->abort(result); + } + } +}; + +class DriveOnHeadingActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("drive_on_heading_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, "drive_on_heading", config); + }; + + factory_->registerBuilder("DriveOnHeading", 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 DriveOnHeadingActionTestFixture::node_ = nullptr; +std::shared_ptr +DriveOnHeadingActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * DriveOnHeadingActionTestFixture::config_ = nullptr; +std::shared_ptr DriveOnHeadingActionTestFixture::factory_ = nullptr; +std::shared_ptr DriveOnHeadingActionTestFixture::tree_ = nullptr; + +TEST_F(DriveOnHeadingActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("dist_to_travel"), 0.15); + EXPECT_EQ(tree_->rootNode()->getInput("speed"), 0.025); + EXPECT_EQ(tree_->rootNode()->getInput("time_allowance"), 10.0); + + xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("dist_to_travel"), 2.0); + EXPECT_EQ(tree_->rootNode()->getInput("speed"), 0.26); +} + +TEST_F(DriveOnHeadingActionTestFixture, test_tick) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + auto goal = action_server_->getCurrentGoal(); + EXPECT_EQ(goal->target.x, 2.0); + EXPECT_EQ(goal->speed, 0.26f); +} + +TEST_F(DriveOnHeadingActionTestFixture, test_failure) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setReturnSuccess(false); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); + + 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); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + DriveOnHeadingActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(DriveOnHeadingActionTestFixture::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_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp new file mode 100644 index 0000000000..d0eb2b5bd5 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp @@ -0,0 +1,175 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelDriveOnHeadingServer : public TestActionServer +{ +public: + CancelDriveOnHeadingServer() + : TestActionServer("drive_on_heading_cancel") + {} + +protected: + void execute( + const typename std::shared_ptr> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // DriveOnHeadingCancel here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelDriveOnHeadingTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_drive_on_heading_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)); + client_ = rclcpp_action::create_client( + node_, "drive_on_heading_cancel"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "drive_on_heading_cancel", config); + }; + + factory_->registerBuilder( + "CancelDriveOnHeading", + 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_; + static std::shared_ptr> client_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelDriveOnHeadingTestFixture::node_ = nullptr; +std::shared_ptr +CancelDriveOnHeadingTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelDriveOnHeadingTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelDriveOnHeadingTestFixture::config_ = nullptr; +std::shared_ptr +CancelDriveOnHeadingTestFixture::factory_ = nullptr; +std::shared_ptr CancelDriveOnHeadingTestFixture::tree_ = nullptr; + +TEST_F(CancelDriveOnHeadingTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client + ::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::DriveOnHeading::Goal(); + + // Setting target pose + goal_msg.target.x = 0.5; + + // DriveOnHeadingCancel for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is infact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and drive on new thread + CancelDriveOnHeadingTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelDriveOnHeadingTestFixture::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_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 2bd07f4232..1325070e00 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,8 +63,10 @@ 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("path_updated", false); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = @@ -119,9 +121,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) )"; - config_->blackboard->set("path_updated", true); tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - EXPECT_EQ(config_->blackboard->get("path_updated"), false); // set new path on blackboard nav_msgs::msg::Path path; @@ -129,18 +129,17 @@ TEST_F(FollowPathActionTestFixture, test_tick) path.poses[0].pose.position.x = 1.0; config_->blackboard->set("path", path); - // first tick should send the goal to our server - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(tree_->rootNode()->getInput("controller_id"), std::string("FollowPath")); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, 1.0); - EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath")); - // 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("controller_id"), std::string("FollowPath")); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath")); // halt node so another goal can be sent tree_->rootNode()->halt(); @@ -150,17 +149,13 @@ TEST_F(FollowPathActionTestFixture, test_tick) path.poses[0].pose.position.x = -2.5; config_->blackboard->set("path", path); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, -2.5); - config_->blackboard->set("path_updated", true); - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } - // path is updated on new goal - EXPECT_EQ(config_->blackboard->get("path_updated"), false); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, -2.5); } int main(int argc, char ** argv) 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 0000000000..cc0b574ff9 --- /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 0000000000..c62e15798e --- /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 c57ca23bce..19b96d6f68 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,8 +64,10 @@ 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("path_updated", false); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = @@ -116,7 +118,7 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) R"( - + )"; @@ -124,15 +126,14 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) geometry_msgs::msg::PoseStamped pose; - // first tick should send the goal to our server - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); - // 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()->pose, pose); // halt node so another goal can be sent tree_->rootNode()->halt(); @@ -141,16 +142,13 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) // set new goal pose.pose.position.x = -2.5; pose.pose.orientation.x = 1.0; - config_->blackboard->set("position", pose.pose.position); - config_->blackboard->set("orientation", pose.pose.orientation); - - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); + config_->blackboard->set("goal", pose); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } + EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); } 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 0000000000..baf856de64 --- /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 03078d384d..1dbf1a7e6d 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,8 +49,10 @@ 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("path_updated", false); config_->blackboard->set("initial_pose_received", false); factory_->registerNodeType( 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 0000000000..4dc971a2ca --- /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_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp new file mode 100644 index 0000000000..5bbf108c8f --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -0,0 +1,180 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2021 RoboTech Vision +// +// 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 "nav_msgs/msg/path.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp" + +class SmoothPathActionServer : public TestActionServer +{ +public: + SmoothPathActionServer() + : TestActionServer("smooth_path") + {} + +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 SmoothPathActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("smooth_path_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, "smooth_path", config); + }; + + factory_->registerBuilder( + "SmoothPath", 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 SmoothPathActionTestFixture::node_ = nullptr; +std::shared_ptr +SmoothPathActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * SmoothPathActionTestFixture::config_ = nullptr; +std::shared_ptr SmoothPathActionTestFixture::factory_ = nullptr; +std::shared_ptr SmoothPathActionTestFixture::tree_ = nullptr; + +TEST_F(SmoothPathActionTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + nav_msgs::msg::Path path; + + // 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()->path, path); + + // halt node so another goal can be sent + tree_->rootNode()->halt(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // set new goal + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = -2.5; + pose.pose.orientation.x = 1.0; + path.poses.push_back(pose); + config_->blackboard->set("unsmoothed_path", path); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + nav_msgs::msg::Path path_empty; + EXPECT_NE(path_empty, path); + EXPECT_EQ(action_server_->getCurrentGoal()->path, path); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + SmoothPathActionTestFixture::action_server_ = + std::make_shared(); + + std::thread server_thread([]() { + rclcpp::spin(SmoothPathActionTestFixture::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_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index e24b3b251e..332d514941 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,8 +64,10 @@ 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("path_updated", false); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -140,7 +152,39 @@ TEST_F(SpinActionTestFixture, test_tick) 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::RUNNING); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + 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); } diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp new file mode 100644 index 0000000000..7909067381 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -0,0 +1,171 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelSpinServer : public TestActionServer +{ +public: + CancelSpinServer() + : TestActionServer("spin") + {} + +protected: + void execute( + const typename std::shared_ptr> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // Spining here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelSpinActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_spin_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)); + client_ = rclcpp_action::create_client( + node_, "spin"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "spin", config); + }; + + factory_->registerBuilder("CancelSpin", 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_; + static std::shared_ptr> client_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelSpinActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelSpinActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelSpinActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelSpinActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelSpinActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelSpinActionTestFixture::tree_ = nullptr; + +TEST_F(CancelSpinActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::Spin::Goal(); + + // Setting target yaw + goal_msg.target_yaw = 1.57; + + // Spining for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is infact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + CancelSpinActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelSpinActionTestFixture::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_truncate_path_local_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp new file mode 100644 index 0000000000..4947c6bc8c --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp @@ -0,0 +1,458 @@ +// Copyright (c) 2021 RoboTech Vision +// +// 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 "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_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp" + + +class TruncatePathLocalTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() override + { + bt_node_ = std::make_shared( + "truncate_path_local", *config_); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + try { + factory_->registerBuilder( + "TruncatePathLocal", builder); + } catch (BT::BehaviorTreeException const &) { + // ignoring multiple registrations of TruncatePathLocal + } + } + + void TearDown() override + { + bt_node_.reset(); + tree_.reset(); + } + + static geometry_msgs::msg::PoseStamped poseMsg(double x, double y, double orientation) + { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(orientation); + return pose; + } + + nav_msgs::msg::Path createLoopCrossingTestPath() + { + nav_msgs::msg::Path path; + path.header.stamp = node_->now(); + path.header.frame_id = "map"; + + // this is a loop to make it harder for robot to find the proper closest pose + path.poses.push_back(poseMsg(-0.3, -1.2, -M_PI * 3 / 2)); + // the position is closest to robot but orientation is different + path.poses.push_back(poseMsg(-0.3, 0.0, -M_PI * 3 / 2)); + path.poses.push_back(poseMsg(-0.5, 1.0, -M_PI)); + path.poses.push_back(poseMsg(-1.5, 1.0, -M_PI / 2)); + path.poses.push_back(poseMsg(-1.5, 0.0, 0.0)); + + // this is the correct path section for the first match + path.poses.push_back(poseMsg(-0.5, 0.0, 0.0)); + path.poses.push_back(poseMsg(0.4, 0.0, 0.0)); + path.poses.push_back(poseMsg(1.5, 0.0, 0.0)); + + // this is a loop to make it harder for robot to find the proper closest pose + path.poses.push_back(poseMsg(1.5, 1.0, M_PI / 2)); + path.poses.push_back(poseMsg(0.5, 1.0, M_PI)); + // the position is closest to robot but orientation is different + path.poses.push_back(poseMsg(0.3, 0.0, M_PI * 3 / 2)); + path.poses.push_back(poseMsg(0.3, -1.0, M_PI * 3 / 2)); + + return path; + } + +protected: + static std::shared_ptr bt_node_; + static std::shared_ptr tree_; +}; + +std::shared_ptr TruncatePathLocalTestFixture::bt_node_ = + nullptr; +std::shared_ptr TruncatePathLocalTestFixture::tree_ = nullptr; + +TEST_F(TruncatePathLocalTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create path and set it on blackboard + nav_msgs::msg::Path path = createLoopCrossingTestPath(); + EXPECT_EQ(path.poses.size(), 12u); + + config_->blackboard->set("path", path); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + + nav_msgs::msg::Path truncated_path; + config_->blackboard->get("truncated_path", truncated_path); + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_NE(path, truncated_path); + ASSERT_GE(truncated_path.poses.size(), 1u); + EXPECT_EQ(truncated_path.poses.size(), 3u); + EXPECT_EQ(truncated_path.poses.front().pose.position.x, -0.5); + EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0); + EXPECT_EQ(truncated_path.poses.back().pose.position.x, 1.5); + EXPECT_EQ(truncated_path.poses.back().pose.position.y, 0.0); + + ///////////////////////////////////////// + // should match the first loop crossing + config_->blackboard->set("pose", poseMsg(0.0, 0.0, M_PI / 2)); + + tree_->haltTree(); + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + config_->blackboard->get("truncated_path", truncated_path); + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_NE(path, truncated_path); + ASSERT_GE(truncated_path.poses.size(), 1u); + EXPECT_EQ(truncated_path.poses.size(), 2u); + EXPECT_EQ(truncated_path.poses.front().pose.position.x, -0.3); + EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0); + EXPECT_EQ(truncated_path.poses.back().pose.position.x, -0.5); + EXPECT_EQ(truncated_path.poses.back().pose.position.y, 1.0); + + ///////////////////////////////////////// + // should match the last loop crossing + config_->blackboard->set("pose", poseMsg(0.0, 0.0, -M_PI / 2)); + + tree_->haltTree(); + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + config_->blackboard->get("truncated_path", truncated_path); + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_NE(path, truncated_path); + ASSERT_GE(truncated_path.poses.size(), 1u); + EXPECT_EQ(truncated_path.poses.size(), 2u); + EXPECT_EQ(truncated_path.poses.front().pose.position.x, 0.3); + EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0); + EXPECT_EQ(truncated_path.poses.back().pose.position.x, 0.3); + EXPECT_EQ(truncated_path.poses.back().pose.position.y, -1.0); + + SUCCEED(); +} + +TEST_F(TruncatePathLocalTestFixture, test_success_on_empty_path) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create path and set it on blackboard + nav_msgs::msg::Path path; + path.header.stamp = node_->now(); + path.header.frame_id = "map"; + + config_->blackboard->set("path", path); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + nav_msgs::msg::Path truncated_path; + config_->blackboard->get("truncated_path", truncated_path); + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(path, truncated_path); + SUCCEED(); +} + +TEST_F(TruncatePathLocalTestFixture, test_failure_on_no_pose) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create path and set it on blackboard + nav_msgs::msg::Path path; + path.header.stamp = node_->now(); + path.header.frame_id = "map"; + + config_->blackboard->set("path", path); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + nav_msgs::msg::Path truncated_path; + config_->blackboard->get("truncated_path", truncated_path); + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); + SUCCEED(); +} + +TEST_F(TruncatePathLocalTestFixture, test_failure_on_invalid_robot_frame) +{ + // 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 + nav_msgs::msg::Path path = createLoopCrossingTestPath(); + EXPECT_EQ(path.poses.size(), 12u); + + config_->blackboard->set("path", path); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + nav_msgs::msg::Path truncated_path; + config_->blackboard->get("truncated_path", truncated_path); + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); + SUCCEED(); +} + +TEST_F(TruncatePathLocalTestFixture, test_path_pruning) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create path and set it on blackboard + nav_msgs::msg::Path path = createLoopCrossingTestPath(); + nav_msgs::msg::Path truncated_path; + + config_->blackboard->set("path", path); + + ///////////////////////////////////////// + // should match the first loop crossing + config_->blackboard->set("pose", poseMsg(0.0, 0.0, 0.0)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + config_->blackboard->get("truncated_path", truncated_path); + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_NE(path, truncated_path); + ASSERT_GE(truncated_path.poses.size(), 1u); + EXPECT_EQ(truncated_path.poses.size(), 2u); + EXPECT_EQ(truncated_path.poses.front().pose.position.x, -0.3); + EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0); + EXPECT_EQ(truncated_path.poses.back().pose.position.x, -0.5); + EXPECT_EQ(truncated_path.poses.back().pose.position.y, 1.0); + + ///////////////////////////////////////// + // move along the path to leave the first loop crossing behind + config_->blackboard->set("pose", poseMsg(-1.5, 1.0, 0.0)); + // tick until node succeeds + tree_->haltTree(); + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + // this truncated_path is not interesting, let's proceed to the second loop crossing + + ///////////////////////////////////////// + // should match the second loop crossing + config_->blackboard->set("pose", poseMsg(0.0, 0.0, 0.0)); + // tick until node succeeds + tree_->haltTree(); + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + config_->blackboard->get("truncated_path", truncated_path); + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_NE(path, truncated_path); + ASSERT_GE(truncated_path.poses.size(), 1u); + EXPECT_EQ(truncated_path.poses.size(), 3u); + EXPECT_EQ(truncated_path.poses.front().pose.position.x, -0.5); + EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0); + EXPECT_EQ(truncated_path.poses.back().pose.position.x, 1.5); + EXPECT_EQ(truncated_path.poses.back().pose.position.y, 0.0); + + ///////////////////////////////////////// + // move along the path to leave the second loop crossing behind + config_->blackboard->set("pose", poseMsg(1.5, 1.0, 0.0)); + // tick until node succeeds + tree_->haltTree(); + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + // this truncated_path is not interesting, let's proceed to the last loop crossing + + ///////////////////////////////////////// + // should match the last loop crossing + config_->blackboard->set("pose", poseMsg(0.0, 0.0, 0.0)); + // tick until node succeeds + tree_->haltTree(); + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + config_->blackboard->get("truncated_path", truncated_path); + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_NE(path, truncated_path); + ASSERT_GE(truncated_path.poses.size(), 1u); + EXPECT_EQ(truncated_path.poses.size(), 2u); + EXPECT_EQ(truncated_path.poses.front().pose.position.x, 0.3); + EXPECT_EQ(truncated_path.poses.front().pose.position.y, 0.0); + EXPECT_EQ(truncated_path.poses.back().pose.position.x, 0.3); + EXPECT_EQ(truncated_path.poses.back().pose.position.y, -1.0); + + SUCCEED(); +} + +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_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index b1023c4723..05cd388d7a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -55,8 +55,10 @@ 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("path_updated", false); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -141,7 +143,12 @@ TEST_F(WaitActionTestFixture, test_tick) 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::RUNNING); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); EXPECT_EQ(rclcpp::Duration(action_server_->getCurrentGoal()->time).seconds(), 5.0); } diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp new file mode 100644 index 0000000000..9e72f1413c --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -0,0 +1,171 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelWaitServer : public TestActionServer +{ +public: + CancelWaitServer() + : TestActionServer("wait") + {} + +protected: + void execute( + const typename std::shared_ptr> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // waiting here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelWaitActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_wait_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)); + client_ = rclcpp_action::create_client( + node_, "wait"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "wait", config); + }; + + factory_->registerBuilder("CancelWait", 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_; + static std::shared_ptr> client_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelWaitActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelWaitActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelWaitActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelWaitActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelWaitActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelWaitActionTestFixture::tree_ = nullptr; + +TEST_F(CancelWaitActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::Wait::Goal(); + + // Setting a waiting time for 5 Seconds. + goal_msg.time.sec = 5; + + // Waiting for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is infact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + CancelWaitActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelWaitActionTestFixture::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/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index b1350f7453..21fd0b2940 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -6,6 +6,10 @@ ament_add_gtest(test_condition_time_expired test_time_expired.cpp) target_link_libraries(test_condition_time_expired nav2_time_expired_condition_bt_node) ament_target_dependencies(test_condition_time_expired ${dependencies}) +ament_add_gtest(test_condition_path_expiring_timer test_path_expiring_timer.cpp) +target_link_libraries(test_condition_path_expiring_timer nav2_path_expiring_timer_condition) +ament_target_dependencies(test_condition_time_expired ${dependencies}) + ament_add_gtest(test_condition_goal_reached test_goal_reached.cpp) target_link_libraries(test_condition_goal_reached nav2_goal_reached_condition_bt_node) ament_target_dependencies(test_condition_goal_reached ${dependencies}) @@ -14,6 +18,10 @@ ament_add_gtest(test_condition_goal_updated test_goal_updated.cpp) target_link_libraries(test_condition_goal_updated nav2_goal_updated_condition_bt_node) ament_target_dependencies(test_condition_goal_updated ${dependencies}) +ament_add_gtest(test_condition_globally_updated_goal test_globally_updated_goal.cpp) +target_link_libraries(test_condition_globally_updated_goal nav2_globally_updated_goal_condition_bt_node) +ament_target_dependencies(test_condition_globally_updated_goal ${dependencies}) + ament_add_gtest(test_condition_initial_pose_received test_initial_pose_received.cpp) target_link_libraries(test_condition_initial_pose_received nav2_initial_pose_received_condition_bt_node) ament_target_dependencies(test_condition_initial_pose_received ${dependencies}) @@ -29,3 +37,7 @@ ament_target_dependencies(test_condition_is_stuck ${dependencies}) ament_add_gtest(test_condition_is_battery_low test_is_battery_low.cpp) target_link_libraries(test_condition_is_battery_low nav2_is_battery_low_condition_bt_node) ament_target_dependencies(test_condition_is_battery_low ${dependencies}) + +ament_add_gtest(test_condition_is_path_valid test_is_path_valid.cpp) +target_link_libraries(test_condition_is_path_valid nav2_is_path_valid_condition_bt_node) +ament_target_dependencies(test_condition_is_path_valid ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp b/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp new file mode 100644 index 0000000000..d41a6d424c --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp @@ -0,0 +1,73 @@ +// Copyright (c) 2021 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/robot_utils.hpp" + +#include "../../test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp" + +class GloballyUpdatedGoalConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + bt_node_ = std::make_shared( + "globally_updated_goal", *config_); + } + + void TearDown() + { + bt_node_.reset(); + } + +protected: + static std::shared_ptr bt_node_; +}; + +std::shared_ptr +GloballyUpdatedGoalConditionTestFixture::bt_node_ = nullptr; + +TEST_F(GloballyUpdatedGoalConditionTestFixture, test_behavior) +{ + geometry_msgs::msg::PoseStamped goal; + config_->blackboard->set("goal", goal); + + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + 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/condition/test_is_path_valid.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp new file mode 100644 index 0000000000..8984aac8c1 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp @@ -0,0 +1,126 @@ +// Copyright (c) 2021 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/robot_utils.hpp" +#include "../../test_service.hpp" + +#include "../../test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class IsPathValidService : public TestService +{ +public: + IsPathValidService() + : TestService("is_path_valid") + {} + + virtual void handle_service( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)request; + response->is_valid = true; + } +}; + +class IsPathValidTestFixture : public ::testing::Test +{ +public: + void SetUp() + { + node_ = std::make_shared("test_is_path_valid_condition"); + factory_ = std::make_shared(); + config_ = new BT::NodeConfiguration(); + config_->blackboard = BT::Blackboard::create(); + config_->blackboard->set("node", node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(10)); + factory_->registerNodeType("IsPathValid"); + } + + void TearDown() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + server_.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_; +}; + +std::shared_ptr IsPathValidTestFixture::server_ = nullptr; +rclcpp::Node::SharedPtr IsPathValidTestFixture::node_ = nullptr; +BT::NodeConfiguration * IsPathValidTestFixture::config_ = nullptr; +std::shared_ptr IsPathValidTestFixture::factory_ = nullptr; +std::shared_ptr IsPathValidTestFixture::tree_ = nullptr; + +TEST_F(IsPathValidTestFixture, test_behavior) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + std::this_thread::sleep_for(500ms); + + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize service and spin on new thread + IsPathValidTestFixture::server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(IsPathValidTestFixture::server_); + }); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp new file mode 100644 index 0000000000..b88fc4d634 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp @@ -0,0 +1,99 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/robot_utils.hpp" + +#include "../../test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class PathExpiringTimerConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + node_ = std::make_shared("test_path_expiring_condition"); + config_ = new BT::NodeConfiguration(); + config_->blackboard = BT::Blackboard::create(); + config_->blackboard->set("node", node_); + bt_node_ = std::make_shared( + "time_expired", *config_); + } + + void TearDown() + { + delete config_; + config_ = nullptr; + bt_node_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static std::shared_ptr bt_node_; + static BT::NodeConfiguration * config_; +}; + +rclcpp::Node::SharedPtr PathExpiringTimerConditionTestFixture::node_ = nullptr; +std::shared_ptr +PathExpiringTimerConditionTestFixture::bt_node_ = nullptr; +BT::NodeConfiguration * PathExpiringTimerConditionTestFixture::config_ = nullptr; + +TEST_F(PathExpiringTimerConditionTestFixture, test_behavior) +{ + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + for (int i = 0; i < 20; ++i) { + rclcpp::sleep_for(500ms); + if (i % 2) { + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + } else { + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + } + } + + // place a new path on the blackboard to reset the timer + nav_msgs::msg::Path path; + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 1.0; + path.poses.push_back(pose); + + config_->blackboard->set("path", path); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + rclcpp::sleep_for(1500ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); +} + +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/condition/test_transform_available.cpp b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp index dc80035f67..a24dcbf530 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp @@ -44,8 +44,10 @@ 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("path_updated", false); config_->blackboard->set("initial_pose_received", false); } diff --git a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp index 8356a0064f..608d8f587e 100644 --- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp @@ -26,21 +26,31 @@ class RecoveryDummy : public nav2_behavior_tree::DummyNode public: BT::NodeStatus tick() override { - if (++ticks_ > num_failures_ && num_failures_ != -1) { + if (ticks_ == num_success_) { setStatus(BT::NodeStatus::SUCCESS); + } else if (ticks_ == num_failure_) { + setStatus(BT::NodeStatus::FAILURE); } + ticks_++; return status(); } - void setMaxFailures(int max_failures) + void returnSuccessOn(int tick) { - num_failures_ = max_failures; + num_success_ = tick; + ticks_ = 0; + } + + void returnFailureOn(int tick) + { + num_failure_ = tick; ticks_ = 0; } private: int ticks_{0}; - int num_failures_{-1}; + int num_success_{-1}; + int num_failure_{-1}; }; class RecoveryNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture @@ -92,10 +102,10 @@ TEST_F(RecoveryNodeTestFixture, test_running) TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child) { first_child_->changeStatus(BT::NodeStatus::IDLE); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::IDLE); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); } TEST_F(RecoveryNodeTestFixture, test_success_one_retry) @@ -105,7 +115,7 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry) EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); // first child fails, second child succeeds, then first child succeeds (one retry) - first_child_->setMaxFailures(1); + first_child_->returnSuccessOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); @@ -125,6 +135,7 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); // first child fails, second child succeeds, then first child fails (one retry) + first_child_->returnFailureOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); diff --git a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt index ad53c5d1d1..ed6504a682 100644 --- a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt @@ -13,3 +13,15 @@ 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}) + +ament_add_gtest(test_goal_updated_controller test_goal_updated_controller.cpp) +target_link_libraries(test_goal_updated_controller nav2_goal_updated_controller_bt_node) +ament_target_dependencies(test_goal_updated_controller ${dependencies}) + +ament_add_gtest(test_decorator_path_longer_on_approach test_path_longer_on_approach.cpp) +target_link_libraries(test_decorator_path_longer_on_approach nav2_path_longer_on_approach_bt_node) +ament_target_dependencies(test_decorator_path_longer_on_approach ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp new file mode 100644 index 0000000000..d8e2a24fb0 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -0,0 +1,108 @@ +// 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/goal_updated_controller.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + // Setting fake goals on blackboard + geometry_msgs::msg::PoseStamped goal1; + goal1.header.stamp = node_->now(); + std::vector poses1; + poses1.push_back(goal1); + config_->blackboard->set("goal", goal1); + config_->blackboard->set>("goals", poses1); + + bt_node_ = std::make_shared( + "goal_updated_controller", *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 +GoalUpdatedControllerTestFixture::bt_node_ = nullptr; +std::shared_ptr +GoalUpdatedControllerTestFixture::dummy_node_ = nullptr; + +TEST_F(GoalUpdatedControllerTestFixture, test_behavior) +{ + // Creating updated fake-goals + geometry_msgs::msg::PoseStamped goal2; + goal2.header.stamp = node_->now(); + std::vector poses2; + poses2.push_back(goal2); + + // starting in idle + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); + + // tick for the first time, dummy node should be ticked + 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 updated goal, dummy node should be ticked + config_->blackboard->set("goal", goal2); + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + + // tick again without update, dummy node should not be ticked + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + + // tick again with updated goals, dummy node should be ticked + config_->blackboard->set>("goals", poses2); + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); +} + +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_path_longer_on_approach.cpp b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp new file mode 100644 index 0000000000..7a71f0a74b --- /dev/null +++ b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp @@ -0,0 +1,163 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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 "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" + +#include "../../test_behavior_tree_fixture.hpp" +#include "../../test_dummy_tree_node.hpp" +#include "nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class PathLongerOnApproachTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("path_longer_on_approach_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( + "PathLongerOnApproach", 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 PathLongerOnApproachTestFixture::node_ = nullptr; + +BT::NodeConfiguration * PathLongerOnApproachTestFixture::config_ = nullptr; +std::shared_ptr PathLongerOnApproachTestFixture::factory_ = nullptr; +std::shared_ptr PathLongerOnApproachTestFixture::tree_ = nullptr; + +TEST_F(PathLongerOnApproachTestFixture, test_tick) +{ + // Success test + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // set new path on blackboard + nav_msgs::msg::Path new_path; + new_path.poses.resize(10); + for (unsigned int i = 0; i < new_path.poses.size(); i++) { + // Assuming distance between waypoints to be 1.5m + new_path.poses[i].pose.position.x = 1.5 * i; + } + config_->blackboard->set("path", new_path); + + tree_->rootNode()->executeTick(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Failure test + // create tree + xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // set old path on blackboard + nav_msgs::msg::Path old_path; + old_path.poses.resize(5); + for (unsigned int i = 1; i <= old_path.poses.size(); i++) { + // Assuming distance between waypoints to be 3.0m + old_path.poses[i - 1].pose.position.x = 3.0 * i; + } + config_->blackboard->set("path", old_path); + tree_->rootNode()->executeTick(); + + // set new path on blackboard + new_path.poses.resize(11); + for (unsigned int i = 0; i <= new_path.poses.size(); i++) { + // Assuming distance between waypoints to be 1.5m + new_path.poses[i].pose.position.x = 1.5 * i; + } + config_->blackboard->set("path", new_path); + tree_->rootNode()->executeTick(); + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); +} + +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/decorator/test_single_trigger_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp new file mode 100644 index 0000000000..5b34129057 --- /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 ddfef77c38..7e9d50c9c3 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 2c7891edb6..d532a7da82 100644 --- a/nav2_behavior_tree/test/test_action_server.hpp +++ b/nav2_behavior_tree/test/test_action_server.hpp @@ -48,6 +48,21 @@ class TestActionServer : public rclcpp::Node return current_goal_; } + void setReturnSuccess(bool return_success) + { + return_success_ = return_success; + } + + bool getReturnSuccess(void) + { + return return_success_; + } + + bool isGoalCancelled() + { + return goal_cancelled_; + } + protected: virtual rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID &, @@ -60,6 +75,7 @@ class TestActionServer : public rclcpp::Node virtual rclcpp_action::CancelResponse handle_cancel( const typename std::shared_ptr>) { + goal_cancelled_ = true; return rclcpp_action::CancelResponse::ACCEPT; } @@ -77,6 +93,8 @@ class TestActionServer : public rclcpp::Node private: typename rclcpp_action::Server::SharedPtr action_server_; std::shared_ptr current_goal_; + bool return_success_ = true; + bool goal_cancelled_ = false; }; #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 c6583c9d47..2a377f9caa 100644 --- a/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp +++ b/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp @@ -51,8 +51,10 @@ 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("path_updated", false); config_->blackboard->set("initial_pose_received", false); transform_handler_->activate(); diff --git a/nav2_behavior_tree/test/test_bt_conversions.cpp b/nav2_behavior_tree/test/test_bt_conversions.cpp index 0659233ef4..ab67d5e52f 100644 --- a/nav2_behavior_tree/test/test_bt_conversions.cpp +++ b/nav2_behavior_tree/test/test_bt_conversions.cpp @@ -20,6 +20,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "nav2_behavior_tree/bt_conversions.hpp" @@ -161,6 +162,81 @@ TEST(QuaternionPortTest, test_correct_syntax) EXPECT_EQ(value.w, 0.7); } +TEST(PoseStampedPortTest, test_wrong_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>("PoseStampedPort"); + auto tree = factory.createTreeFromText(xml_txt); + + geometry_msgs::msg::PoseStamped value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); + EXPECT_EQ(value.header.frame_id, ""); + EXPECT_EQ(value.pose.position.x, 0.0); + EXPECT_EQ(value.pose.position.y, 0.0); + EXPECT_EQ(value.pose.position.z, 0.0); + EXPECT_EQ(value.pose.orientation.x, 0.0); + EXPECT_EQ(value.pose.orientation.y, 0.0); + EXPECT_EQ(value.pose.orientation.z, 0.0); + EXPECT_EQ(value.pose.orientation.w, 1.0); + + xml_txt = + R"( + + + + + )"; + + tree = factory.createTreeFromText(xml_txt); + tree.rootNode()->getInput("test", value); + EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); + EXPECT_EQ(value.header.frame_id, ""); + EXPECT_EQ(value.pose.position.x, 0.0); + EXPECT_EQ(value.pose.position.y, 0.0); + EXPECT_EQ(value.pose.position.z, 0.0); + EXPECT_EQ(value.pose.orientation.x, 0.0); + EXPECT_EQ(value.pose.orientation.y, 0.0); + EXPECT_EQ(value.pose.orientation.z, 0.0); + EXPECT_EQ(value.pose.orientation.w, 1.0); +} + +TEST(PoseStampedPortTest, test_correct_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>("PoseStampedPort"); + auto tree = factory.createTreeFromText(xml_txt); + + tree = factory.createTreeFromText(xml_txt); + geometry_msgs::msg::PoseStamped value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); + EXPECT_EQ(value.header.frame_id, "map"); + EXPECT_EQ(value.pose.position.x, 1.0); + EXPECT_EQ(value.pose.position.y, 2.0); + EXPECT_EQ(value.pose.position.z, 3.0); + EXPECT_EQ(value.pose.orientation.x, 4.0); + EXPECT_EQ(value.pose.orientation.y, 5.0); + EXPECT_EQ(value.pose.orientation.z, 6.0); + EXPECT_EQ(value.pose.orientation.w, 7.0); +} + TEST(MillisecondsPortTest, test_correct_syntax) { std::string xml_txt = diff --git a/nav2_amcl/CHANGELOG.rst b/nav2_behaviors/CHANGELOG.rst similarity index 100% rename from nav2_amcl/CHANGELOG.rst rename to nav2_behaviors/CHANGELOG.rst diff --git a/nav2_recoveries/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt similarity index 58% rename from nav2_recoveries/CMakeLists.txt rename to nav2_behaviors/CMakeLists.txt index 96b6bf65e3..6b0d4d1bd2 100644 --- a/nav2_recoveries/CMakeLists.txt +++ b/nav2_behaviors/CMakeLists.txt @@ -1,11 +1,12 @@ cmake_minimum_required(VERSION 3.5) -project(nav2_recoveries) +project(nav2_behaviors) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) find_package(nav2_util REQUIRED) @@ -24,13 +25,14 @@ include_directories( include ) -set(library_name recoveries_server_core) -set(executable_name recoveries_server) +set(library_name behavior_server_core) +set(executable_name behavior_server) set(dependencies rclcpp rclcpp_action rclcpp_lifecycle + rclcpp_components std_msgs nav2_util nav2_behavior_tree @@ -45,53 +47,49 @@ set(dependencies ) # plugins -add_library(nav2_spin_recovery SHARED +add_library(nav2_spin_behavior SHARED plugins/spin.cpp ) -ament_target_dependencies(nav2_spin_recovery +ament_target_dependencies(nav2_spin_behavior ${dependencies} ) -# prevent pluginlib from using boost -target_compile_definitions(nav2_spin_recovery PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +add_library(nav2_wait_behavior SHARED +plugins/wait.cpp +) -add_library(nav2_backup_recovery SHARED - plugins/back_up.cpp +ament_target_dependencies(nav2_wait_behavior +${dependencies} ) -ament_target_dependencies(nav2_backup_recovery - ${dependencies} +add_library(nav2_drive_on_heading_behavior SHARED + plugins/drive_on_heading.cpp ) -# prevent pluginlib from using boost -target_compile_definitions(nav2_backup_recovery PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +ament_target_dependencies(nav2_drive_on_heading_behavior + ${dependencies} +) -add_library(nav2_wait_recovery SHARED - plugins/wait.cpp +add_library(nav2_back_up_behavior SHARED + plugins/back_up.cpp ) -ament_target_dependencies(nav2_wait_recovery +ament_target_dependencies(nav2_back_up_behavior ${dependencies} ) -# prevent pluginlib from using boost -target_compile_definitions(nav2_wait_recovery PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - -pluginlib_export_plugin_description_file(nav2_core recovery_plugin.xml) +pluginlib_export_plugin_description_file(nav2_core behavior_plugin.xml) # Library -add_library(${library_name} - src/recovery_server.cpp +add_library(${library_name} SHARED + src/behavior_server.cpp ) ament_target_dependencies(${library_name} ${dependencies} ) -# prevent pluginlib from using boost -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - # Executable add_executable(${executable_name} src/main.cpp @@ -103,11 +101,13 @@ ament_target_dependencies(${executable_name} ${dependencies} ) +rclcpp_components_register_nodes(${library_name} "behavior_server::BehaviorServer") install(TARGETS ${library_name} - nav2_backup_recovery - nav2_spin_recovery - nav2_wait_recovery + nav2_spin_behavior + nav2_wait_behavior + nav2_drive_on_heading_behavior + nav2_back_up_behavior ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -121,7 +121,7 @@ install(DIRECTORY include/ DESTINATION include/ ) -install(FILES recovery_plugin.xml +install(FILES behavior_plugin.xml DESTINATION share/${PROJECT_NAME} ) @@ -134,10 +134,10 @@ endif() ament_export_include_directories(include) ament_export_libraries(${library_name} - nav2_backup_recovery - nav2_spin_recovery - nav2_wait_recovery + nav2_spin_behavior + nav2_wait_behavior + nav2_drive_on_heading_behavior + nav2_back_up_behavior ) -ament_export_definitions("PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - +ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_behaviors/README.md b/nav2_behaviors/README.md new file mode 100644 index 0000000000..5ecd14af5c --- /dev/null +++ b/nav2_behaviors/README.md @@ -0,0 +1,15 @@ +# Behaviors + +The `nav2_behaviors` package implements a task server for executing behaviors. + +The package defines: +- A `TimedBehavior` template which is used as a base class to implement specific timed behavior action server - but not required. +- The `Backup`, `DriveOnHeading`, `Spin` and `Wait` behaviors. + +The only required class a behavior must derive from is the `nav2_core/behavior.hpp` class, which implements the pluginlib interface the behavior server will use to dynamically load your behavior. The `nav2_behaviors/timed_behavior.hpp` derives from this class and implements a generic action server for a timed behavior behavior (e.g. calls an implmentation function on a regular time interval to compute a value) but **this is not required** if it is not helpful. A behavior does not even need to be an action if you do not wish, it may be a service or other interface. However, most motion and behavior primitives are probably long-running and make sense to be modeled as actions, so the provided `timed_behavior.hpp` helps in managing the complexity to simplify new behavior development, described more below. + +The value of the centralized behavior server is to **share resources** amongst several behaviors that would otherwise be independent nodes. Subscriptions to TF, costmaps, and more can be quite heavy and add non-trivial compute costs to a robot system. By combining these independent behaviors into a single server, they may share these resources while retaining complete independence in execution and interface. + +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_behavior_plugin.html). + +See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. diff --git a/nav2_behaviors/behavior_plugin.xml b/nav2_behaviors/behavior_plugin.xml new file mode 100644 index 0000000000..5989f566a9 --- /dev/null +++ b/nav2_behaviors/behavior_plugin.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp similarity index 58% rename from nav2_recoveries/include/nav2_recoveries/recovery_server.hpp rename to nav2_behaviors/include/nav2_behaviors/behavior_server.hpp index 26c497d458..ea7a2d4cfa 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp @@ -23,49 +23,77 @@ #include "tf2_ros/create_timer_ros.h" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav2_core/recovery.hpp" +#include "nav2_core/behavior.hpp" -#ifndef NAV2_RECOVERIES__RECOVERY_SERVER_HPP_ -#define NAV2_RECOVERIES__RECOVERY_SERVER_HPP_ +#ifndef NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_ +#define NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_ -namespace recovery_server +namespace behavior_server { -class RecoveryServer : public nav2_util::LifecycleNode +/** + * @class behavior_server::BehaviorServer + * @brief An server hosting a map of behavior plugins + */ +class BehaviorServer : public nav2_util::LifecycleNode { public: - RecoveryServer(); - ~RecoveryServer(); + /** + * @brief A constructor for behavior_server::BehaviorServer + * @param options Additional options to control creation of the node. + */ + explicit BehaviorServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~BehaviorServer(); - bool loadRecoveryPlugins(); + /** + * @brief Loads behavior plugins from parameter file + * @return bool if successfully loaded the plugins + */ + bool loadBehaviorPlugins(); 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_; std::shared_ptr transform_listener_; // Plugins - std::vector> recoveries_; - pluginlib::ClassLoader plugin_loader_; + pluginlib::ClassLoader plugin_loader_; + std::vector> behaviors_; std::vector default_ids_; std::vector default_types_; - std::vector recovery_ids_; - std::vector recovery_types_; + std::vector behavior_ids_; + std::vector behavior_types_; // Utilities std::unique_ptr costmap_sub_; std::unique_ptr footprint_sub_; std::shared_ptr collision_checker_; - - double transform_tolerance_; }; -} // namespace recovery_server +} // namespace behavior_server -#endif // NAV2_RECOVERIES__RECOVERY_SERVER_HPP_ +#endif // NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_ diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp similarity index 62% rename from nav2_recoveries/include/nav2_recoveries/recovery.hpp rename to nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 7edb732d00..d696607feb 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_RECOVERIES__RECOVERY_HPP_ -#define NAV2_RECOVERIES__RECOVERY_HPP_ +#ifndef NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_ +#define NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_ #include #include @@ -29,9 +29,13 @@ #include "geometry_msgs/msg/twist.hpp" #include "nav2_util/simple_action_server.hpp" #include "nav2_util/robot_utils.hpp" -#include "nav2_core/recovery.hpp" +#include "nav2_core/behavior.hpp" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" +#include "tf2/utils.h" +#pragma GCC diagnostic pop -namespace nav2_recoveries +namespace nav2_behaviors { enum class Status : int8_t @@ -43,20 +47,27 @@ enum class Status : int8_t using namespace std::chrono_literals; //NOLINT +/** + * @class nav2_behaviors::Behavior + * @brief An action server Behavior base class implementing the action server and basic factory. + */ template -class Recovery : public nav2_core::Recovery +class TimedBehavior : public nav2_core::Behavior { public: - using ActionServer = nav2_util::SimpleActionServer; + using ActionServer = nav2_util::SimpleActionServer; - Recovery() + /** + * @brief A TimedBehavior constructor + */ + TimedBehavior() : action_server_(nullptr), cycle_frequency_(10.0), enabled_(false) { } - virtual ~Recovery() + virtual ~TimedBehavior() { } @@ -69,7 +80,7 @@ class Recovery : public nav2_core::Recovery // This is the method derived classes should mainly implement // and will be called cyclically while it returns RUNNING. // Implement the behavior such that it runs some unit of work on each call - // and provides a status. The recovery will finish once SUCCEEDED is returned + // and provides a status. The Behavior will finish once SUCCEEDED is returned // It's up to the derived class to define the final commanded velocity. virtual Status onCycleUpdate() = 0; @@ -85,33 +96,39 @@ class Recovery : public nav2_core::Recovery { } + // configure the server on lifecycle setup void configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & name, std::shared_ptr tf, std::shared_ptr collision_checker) override { - RCLCPP_INFO(parent->get_logger(), "Configuring %s", name.c_str()); - node_ = parent; - recovery_name_ = name; + auto node = node_.lock(); + + logger_ = node->get_logger(); + + RCLCPP_INFO(logger_, "Configuring %s", name.c_str()); + + behavior_name_ = name; tf_ = tf; - node_->get_parameter("cycle_frequency", cycle_frequency_); - node_->get_parameter("global_frame", global_frame_); - node_->get_parameter("robot_base_frame", robot_base_frame_); - node_->get_parameter("transform_tolerance", transform_tolerance_); + node->get_parameter("cycle_frequency", cycle_frequency_); + node->get_parameter("global_frame", global_frame_); + node->get_parameter("robot_base_frame", robot_base_frame_); + node->get_parameter("transform_tolerance", transform_tolerance_); action_server_ = std::make_shared( - node_, recovery_name_, - std::bind(&Recovery::execute, this)); + node, behavior_name_, + std::bind(&TimedBehavior::execute, this)); collision_checker_ = collision_checker; - vel_pub_ = node_->template create_publisher("cmd_vel", 1); + vel_pub_ = node->template create_publisher("cmd_vel", 1); onConfigure(); } + // Cleanup server on lifecycle transition void cleanup() override { action_server_.reset(); @@ -119,23 +136,28 @@ class Recovery : public nav2_core::Recovery onCleanup(); } + // Activate server on lifecycle transition void activate() override { - RCLCPP_INFO(node_->get_logger(), "Activating %s", recovery_name_.c_str()); + RCLCPP_INFO(logger_, "Activating %s", behavior_name_.c_str()); vel_pub_->on_activate(); + action_server_->activate(); enabled_ = true; } + // Deactivate server on lifecycle transition void deactivate() override { vel_pub_->on_deactivate(); + action_server_->deactivate(); enabled_ = false; } protected: - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; - std::string recovery_name_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + + std::string behavior_name_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_pub_; std::shared_ptr action_server_; std::shared_ptr collision_checker_; @@ -150,25 +172,42 @@ class Recovery : public nav2_core::Recovery // Clock rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; + // Logger + rclcpp::Logger logger_{rclcpp::get_logger("nav2_behaviors")}; + + // Main execution callbacks for the action server implementation calling the Behavior's + // onRun and cycle functions to execute a specific behavior void execute() { - RCLCPP_INFO(node_->get_logger(), "Attempting %s", recovery_name_.c_str()); + RCLCPP_INFO(logger_, "Attempting %s", behavior_name_.c_str()); if (!enabled_) { - RCLCPP_WARN(node_->get_logger(), "Called while inactive, ignoring request."); + RCLCPP_WARN( + logger_, + "Called while inactive, ignoring request."); return; } if (onRun(action_server_->get_current_goal()) != Status::SUCCEEDED) { - RCLCPP_INFO(node_->get_logger(), "Initial checks failed for %s", recovery_name_.c_str()); + RCLCPP_INFO( + logger_, + "Initial checks failed for %s", behavior_name_.c_str()); action_server_->terminate_current(); return; } // Log a message every second - auto timer = node_->create_wall_timer( - 1s, - [&]() {RCLCPP_INFO(node_->get_logger(), "%s running...", recovery_name_.c_str());}); + { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + auto timer = node->create_wall_timer( + 1s, + [&]() + {RCLCPP_INFO(logger_, "%s running...", behavior_name_.c_str());}); + } auto start_time = steady_clock_.now(); @@ -179,19 +218,19 @@ class Recovery : public nav2_core::Recovery while (rclcpp::ok()) { if (action_server_->is_cancel_requested()) { - RCLCPP_INFO(node_->get_logger(), "Canceling %s", recovery_name_.c_str()); + RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str()); stopRobot(); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->terminate_all(result); return; } - // TODO(orduno) #868 Enable preempting a Recovery on-the-fly without stopping + // TODO(orduno) #868 Enable preempting a Behavior on-the-fly without stopping if (action_server_->is_preempt_requested()) { RCLCPP_ERROR( - node_->get_logger(), "Received a preemption request for %s," + logger_, "Received a preemption request for %s," " however feature is currently not implemented. Aborting and stopping.", - recovery_name_.c_str()); + behavior_name_.c_str()); stopRobot(); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->terminate_current(result); @@ -200,13 +239,15 @@ class Recovery : public nav2_core::Recovery switch (onCycleUpdate()) { case Status::SUCCEEDED: - RCLCPP_INFO(node_->get_logger(), "%s completed successfully", recovery_name_.c_str()); + RCLCPP_INFO( + logger_, + "%s completed successfully", behavior_name_.c_str()); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->succeeded_current(result); return; case Status::FAILED: - RCLCPP_WARN(node_->get_logger(), "%s failed", recovery_name_.c_str()); + RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str()); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->terminate_current(result); return; @@ -220,6 +261,7 @@ class Recovery : public nav2_core::Recovery } } + // Stop the robot with a commanded velocity void stopRobot() { auto cmd_vel = std::make_unique(); @@ -231,6 +273,6 @@ class Recovery : public nav2_core::Recovery } }; -} // namespace nav2_recoveries +} // namespace nav2_behaviors -#endif // NAV2_RECOVERIES__RECOVERY_HPP_ +#endif // NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_ diff --git a/nav2_recoveries/package.xml b/nav2_behaviors/package.xml similarity index 93% rename from nav2_recoveries/package.xml rename to nav2_behaviors/package.xml index 62f2f79540..5a8e871984 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_behaviors/package.xml @@ -1,8 +1,8 @@ - nav2_recoveries - 0.4.7 + nav2_behaviors + 1.1.0 TODO Carlos Orduno Steve Macenski @@ -43,6 +43,6 @@ ament_cmake - + diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp new file mode 100644 index 0000000000..90a69a686e --- /dev/null +++ b/nav2_behaviors/plugins/back_up.cpp @@ -0,0 +1,50 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "back_up.hpp" + +namespace nav2_behaviors +{ + +Status BackUp::onRun(const std::shared_ptr command) +{ + if (command->target.y != 0.0 || command->target.z != 0.0) { + RCLCPP_INFO( + logger_, + "Backing up in Y and Z not supported, will only move in X."); + return Status::FAILED; + } + + // Silently ensure that both the speed and direction are negative. + command_x_ = -std::fabs(command->target.x); + command_speed_ = -std::fabs(command->speed); + command_time_allowance_ = command->time_allowance; + + end_time_ = steady_clock_.now() + command_time_allowance_; + + if (!nav2_util::getCurrentPose( + initial_pose_, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_ERROR(logger_, "Initial robot pose is not available."); + return Status::FAILED; + } + + return Status::SUCCEEDED; +} + +} // namespace nav2_behaviors + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_behaviors::BackUp, nav2_core::Behavior) diff --git a/nav2_behaviors/plugins/back_up.hpp b/nav2_behaviors/plugins/back_up.hpp new file mode 100644 index 0000000000..f892c5b0e2 --- /dev/null +++ b/nav2_behaviors/plugins/back_up.hpp @@ -0,0 +1,35 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_ +#define NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_ + +#include + +#include "drive_on_heading.hpp" +#include "nav2_msgs/action/back_up.hpp" + +using BackUpAction = nav2_msgs::action::BackUp; + + +namespace nav2_behaviors +{ +class BackUp : public DriveOnHeading +{ +public: + Status onRun(const std::shared_ptr command) override; +}; +} + +#endif // NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_ diff --git a/nav2_behaviors/plugins/drive_on_heading.cpp b/nav2_behaviors/plugins/drive_on_heading.cpp new file mode 100644 index 0000000000..dae6ed83a1 --- /dev/null +++ b/nav2_behaviors/plugins/drive_on_heading.cpp @@ -0,0 +1,26 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "drive_on_heading.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_msgs/action/back_up.hpp" + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_behaviors::DriveOnHeading<>, nav2_core::Behavior) diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/plugins/drive_on_heading.hpp new file mode 100644 index 0000000000..65ab8f1157 --- /dev/null +++ b/nav2_behaviors/plugins/drive_on_heading.hpp @@ -0,0 +1,218 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_ +#define NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_ + +#include +#include +#include + +#include "nav2_behaviors/timed_behavior.hpp" +#include "nav2_msgs/action/drive_on_heading.hpp" +#include "nav2_msgs/action/back_up.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_behaviors +{ + +/** + * @class nav2_behaviors::DriveOnHeading + * @brief An action server Behavior for spinning in + */ +template +class DriveOnHeading : public TimedBehavior +{ +public: + /** + * @brief A constructor for nav2_behaviors::DriveOnHeading + */ + DriveOnHeading() + : TimedBehavior(), + feedback_(std::make_shared()) + { + } + + + ~DriveOnHeading() + {} + + /** + * @brief Initialization to run behavior + * @param command Goal to execute + * @return Status of behavior + */ + Status onRun(const std::shared_ptr command) override + { + if (command->target.y != 0.0 || command->target.z != 0.0) { + RCLCPP_INFO( + this->logger_, + "DrivingOnHeading in Y and Z not supported, will only move in X."); + return Status::FAILED; + } + + // Ensure that both the speed and direction have the same sign + if (!((command->target.x > 0.0) == (command->speed > 0.0)) ) { + RCLCPP_ERROR(this->logger_, "Speed and command sign did not match"); + return Status::FAILED; + } + + command_x_ = command->target.x; + command_speed_ = command->speed; + command_time_allowance_ = command->time_allowance; + + end_time_ = this->steady_clock_.now() + command_time_allowance_; + + if (!nav2_util::getCurrentPose( + initial_pose_, *this->tf_, this->global_frame_, this->robot_base_frame_, + this->transform_tolerance_)) + { + RCLCPP_ERROR(this->logger_, "Initial robot pose is not available."); + return Status::FAILED; + } + + return Status::SUCCEEDED; + } + + /** + * @brief Loop function to run behavior + * @return Status of behavior + */ + Status onCycleUpdate() + { + rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now(); + if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { + this->stopRobot(); + RCLCPP_WARN( + this->logger_, + "Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading"); + return Status::FAILED; + } + + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *this->tf_, this->global_frame_, this->robot_base_frame_, + this->transform_tolerance_)) + { + RCLCPP_ERROR(this->logger_, "Current robot pose is not available."); + return Status::FAILED; + } + + double diff_x = initial_pose_.pose.position.x - current_pose.pose.position.x; + double diff_y = initial_pose_.pose.position.y - current_pose.pose.position.y; + double distance = hypot(diff_x, diff_y); + + feedback_->distance_traveled = distance; + this->action_server_->publish_feedback(feedback_); + + if (distance >= std::fabs(command_x_)) { + this->stopRobot(); + return Status::SUCCEEDED; + } + + // TODO(mhpanah): cmd_vel value should be passed as a parameter + auto cmd_vel = std::make_unique(); + cmd_vel->linear.y = 0.0; + cmd_vel->angular.z = 0.0; + cmd_vel->linear.x = command_speed_; + + geometry_msgs::msg::Pose2D pose2d; + pose2d.x = current_pose.pose.position.x; + pose2d.y = current_pose.pose.position.y; + pose2d.theta = tf2::getYaw(current_pose.pose.orientation); + + if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) { + this->stopRobot(); + RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading"); + return Status::FAILED; + } + + this->vel_pub_->publish(std::move(cmd_vel)); + + return Status::RUNNING; + } + +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) + { + // Simulate ahead by simulate_ahead_time_ in this->cycle_frequency_ increments + int cycle_count = 0; + double sim_position_change; + const double diff_dist = abs(command_x_) - distance; + const int max_cycle_count = static_cast(this->cycle_frequency_ * simulate_ahead_time_); + geometry_msgs::msg::Pose2D init_pose = pose2d; + bool fetch_data = true; + + while (cycle_count < max_cycle_count) { + sim_position_change = cmd_vel->linear.x * (cycle_count / this->cycle_frequency_); + 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.) { + break; + } + + if (!this->collision_checker_->isCollisionFree(pose2d, fetch_data)) { + return false; + } + fetch_data = false; + } + return true; + } + + /** + * @brief Configuration of behavior action + */ + void onConfigure() override + { + auto node = this->node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + nav2_util::declare_parameter_if_not_declared( + node, + "simulate_ahead_time", rclcpp::ParameterValue(2.0)); + node->get_parameter("simulate_ahead_time", simulate_ahead_time_); + } + + double min_linear_vel_; + double max_linear_vel_; + double linear_acc_lim_; + + geometry_msgs::msg::PoseStamped initial_pose_; + double command_x_; + double command_speed_; + rclcpp::Duration command_time_allowance_{0, 0}; + rclcpp::Time end_time_; + double simulate_ahead_time_; + + typename ActionT::Feedback::SharedPtr feedback_; +}; + +} // namespace nav2_behaviors + +#endif // NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_ diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp similarity index 74% rename from nav2_recoveries/plugins/spin.cpp rename to nav2_behaviors/plugins/spin.cpp index 103623b15c..641f59064e 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -26,16 +26,16 @@ #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; -namespace nav2_recoveries +namespace nav2_behaviors { Spin::Spin() -: Recovery(), +: TimedBehavior(), feedback_(std::make_shared()), prev_yaw_(0.0) { @@ -47,25 +47,30 @@ Spin::~Spin() void Spin::onConfigure() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + nav2_util::declare_parameter_if_not_declared( - node_, + node, "simulate_ahead_time", rclcpp::ParameterValue(2.0)); - node_->get_parameter("simulate_ahead_time", simulate_ahead_time_); + node->get_parameter("simulate_ahead_time", simulate_ahead_time_); nav2_util::declare_parameter_if_not_declared( - node_, + node, "max_rotational_vel", rclcpp::ParameterValue(1.0)); - node_->get_parameter("max_rotational_vel", max_rotational_vel_); + node->get_parameter("max_rotational_vel", max_rotational_vel_); nav2_util::declare_parameter_if_not_declared( - node_, + node, "min_rotational_vel", rclcpp::ParameterValue(0.4)); - node_->get_parameter("min_rotational_vel", min_rotational_vel_); + node->get_parameter("min_rotational_vel", min_rotational_vel_); nav2_util::declare_parameter_if_not_declared( - node_, + node, "rotational_acc_lim", rclcpp::ParameterValue(3.2)); - node_->get_parameter("rotational_acc_lim", rotational_acc_lim_); + node->get_parameter("rotational_acc_lim", rotational_acc_lim_); } Status Spin::onRun(const std::shared_ptr command) @@ -75,7 +80,7 @@ Status Spin::onRun(const std::shared_ptr command) current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + RCLCPP_ERROR(logger_, "Current robot pose is not available."); return Status::FAILED; } @@ -84,19 +89,32 @@ Status Spin::onRun(const std::shared_ptr command) cmd_yaw_ = command->target_yaw; RCLCPP_INFO( - node_->get_logger(), "Turning %0.2f for spin recovery.", + logger_, "Turning %0.2f for spin behavior.", cmd_yaw_); + + command_time_allowance_ = command->time_allowance; + end_time_ = steady_clock_.now() + command_time_allowance_; + return Status::SUCCEEDED; } Status Spin::onCycleUpdate() { + rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { + stopRobot(); + RCLCPP_WARN( + logger_, + "Exceeded time allowance before reaching the Spin goal - Exiting Spin"); + return Status::FAILED; + } + geometry_msgs::msg::PoseStamped current_pose; if (!nav2_util::getCurrentPose( current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + RCLCPP_ERROR(logger_, "Current robot pose is not available."); return Status::FAILED; } @@ -114,7 +132,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; } @@ -132,8 +150,8 @@ Status Spin::onCycleUpdate() if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) { stopRobot(); - RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting Spin"); - return Status::SUCCEEDED; + RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin"); + return Status::FAILED; } vel_pub_->publish(std::move(cmd_vel)); @@ -151,6 +169,7 @@ bool Spin::isCollisionFree( double sim_position_change; const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); geometry_msgs::msg::Pose2D init_pose = pose2d; + bool fetch_data = true; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_); @@ -161,14 +180,15 @@ bool Spin::isCollisionFree( break; } - if (!collision_checker_->isCollisionFree(pose2d)) { + if (!collision_checker_->isCollisionFree(pose2d, fetch_data)) { return false; } + fetch_data = false; } return true; } -} // namespace nav2_recoveries +} // namespace nav2_behaviors #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(nav2_recoveries::Spin, nav2_core::Recovery) +PLUGINLIB_EXPORT_CLASS(nav2_behaviors::Spin, nav2_core::Behavior) diff --git a/nav2_recoveries/plugins/spin.hpp b/nav2_behaviors/plugins/spin.hpp similarity index 57% rename from nav2_recoveries/plugins/spin.hpp rename to nav2_behaviors/plugins/spin.hpp index cf1d6b7d04..7174c91f46 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_behaviors/plugins/spin.hpp @@ -12,32 +12,60 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_RECOVERIES__PLUGINS__SPIN_HPP_ -#define NAV2_RECOVERIES__PLUGINS__SPIN_HPP_ +#ifndef NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_ +#define NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_ #include #include #include -#include "nav2_recoveries/recovery.hpp" +#include "nav2_behaviors/timed_behavior.hpp" #include "nav2_msgs/action/spin.hpp" #include "geometry_msgs/msg/quaternion.hpp" -namespace nav2_recoveries +namespace nav2_behaviors { using SpinAction = nav2_msgs::action::Spin; -class Spin : public Recovery +/** + * @class nav2_behaviors::Spin + * @brief An action server behavior for spinning in + */ +class Spin : public TimedBehavior { public: + /** + * @brief A constructor for nav2_behaviors::Spin + */ Spin(); ~Spin(); + /** + * @brief Initialization to run behavior + * @param command Goal to execute + * @return Status of behavior + */ Status onRun(const std::shared_ptr command) override; + + /** + * @brief Configuration of behavior action + */ void onConfigure() override; + + /** + * @brief Loop function to run behavior + * @return Status of behavior + */ 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, @@ -52,8 +80,10 @@ class Spin : public Recovery double prev_yaw_; double relative_yaw_; double simulate_ahead_time_; + rclcpp::Duration command_time_allowance_{0, 0}; + rclcpp::Time end_time_; }; -} // namespace nav2_recoveries +} // namespace nav2_behaviors -#endif // NAV2_RECOVERIES__PLUGINS__SPIN_HPP_ +#endif // NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_ diff --git a/nav2_recoveries/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp similarity index 85% rename from nav2_recoveries/plugins/wait.cpp rename to nav2_behaviors/plugins/wait.cpp index e9a27b4dda..7ca00e2abc 100644 --- a/nav2_recoveries/plugins/wait.cpp +++ b/nav2_behaviors/plugins/wait.cpp @@ -18,11 +18,11 @@ #include "wait.hpp" -namespace nav2_recoveries +namespace nav2_behaviors { Wait::Wait() -: Recovery(), +: TimedBehavior(), feedback_(std::make_shared()) { } @@ -44,7 +44,7 @@ Status Wait::onCycleUpdate() auto time_left = std::chrono::duration_cast(wait_end_ - current_point).count(); - feedback_->time_left = rclcpp::Duration(time_left); + feedback_->time_left = rclcpp::Duration(rclcpp::Duration::from_nanoseconds(time_left)); action_server_->publish_feedback(feedback_); if (time_left > 0) { @@ -54,7 +54,7 @@ Status Wait::onCycleUpdate() } } -} // namespace nav2_recoveries +} // namespace nav2_behaviors #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(nav2_recoveries::Wait, nav2_core::Recovery) +PLUGINLIB_EXPORT_CLASS(nav2_behaviors::Wait, nav2_core::Behavior) diff --git a/nav2_recoveries/plugins/wait.hpp b/nav2_behaviors/plugins/wait.hpp similarity index 60% rename from nav2_recoveries/plugins/wait.hpp rename to nav2_behaviors/plugins/wait.hpp index 34364ce174..38e85fd14a 100644 --- a/nav2_recoveries/plugins/wait.hpp +++ b/nav2_behaviors/plugins/wait.hpp @@ -12,28 +12,44 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_RECOVERIES__PLUGINS__WAIT_HPP_ -#define NAV2_RECOVERIES__PLUGINS__WAIT_HPP_ +#ifndef NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_ +#define NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_ #include #include #include -#include "nav2_recoveries/recovery.hpp" +#include "nav2_behaviors/timed_behavior.hpp" #include "nav2_msgs/action/wait.hpp" -namespace nav2_recoveries +namespace nav2_behaviors { using WaitAction = nav2_msgs::action::Wait; -class Wait : public Recovery +/** + * @class nav2_behaviors::Wait + * @brief An action server behavior for waiting a fixed duration + */ +class Wait : public TimedBehavior { public: + /** + * @brief A constructor for nav2_behaviors::Wait + */ Wait(); ~Wait(); + /** + * @brief Initialization to run behavior + * @param command Goal to execute + * @return Status of behavior + */ Status onRun(const std::shared_ptr command) override; + /** + * @brief Loop function to run behavior + * @return Status of behavior + */ Status onCycleUpdate() override; protected: @@ -41,6 +57,6 @@ class Wait : public Recovery WaitAction::Feedback::SharedPtr feedback_; }; -} // namespace nav2_recoveries +} // namespace nav2_behaviors -#endif // NAV2_RECOVERIES__PLUGINS__WAIT_HPP_ +#endif // NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_ diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_behaviors/src/behavior_server.cpp similarity index 54% rename from nav2_recoveries/src/recovery_server.cpp rename to nav2_behaviors/src/behavior_server.cpp index bd78ae85f8..d6f7568c40 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -17,16 +17,19 @@ #include #include #include "nav2_util/node_utils.hpp" -#include "nav2_recoveries/recovery_server.hpp" +#include "nav2_behaviors/behavior_server.hpp" -namespace recovery_server +namespace behavior_server { -RecoveryServer::RecoveryServer() -: LifecycleNode("recoveries_server", "", true), - plugin_loader_("nav2_core", "nav2_core::Recovery"), - default_ids_{"spin", "backup", "wait"}, - default_types_{"nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"} +BehaviorServer::BehaviorServer(const rclcpp::NodeOptions & options) +: LifecycleNode("behavior_server", "", options), + plugin_loader_("nav2_core", "nav2_core::Behavior"), + default_ids_{"spin", "backup", "drive_on_heading", "wait"}, + default_types_{"nav2_behaviors/Spin", + "nav2_behaviors/BackUp", + "nav2_behaviors/DriveOnHeading", + "nav2_behaviors/Wait"} { declare_parameter( "costmap_topic", @@ -35,7 +38,14 @@ RecoveryServer::RecoveryServer() "footprint_topic", rclcpp::ParameterValue(std::string("local_costmap/published_footprint"))); declare_parameter("cycle_frequency", rclcpp::ParameterValue(10.0)); - declare_parameter("recovery_plugins", default_ids_); + declare_parameter("behavior_plugins", default_ids_); + + get_parameter("behavior_plugins", behavior_ids_); + if (behavior_ids_ == default_ids_) { + for (size_t i = 0; i < default_ids_.size(); ++i) { + declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); + } + } declare_parameter( "global_frame", @@ -49,12 +59,13 @@ RecoveryServer::RecoveryServer() } -RecoveryServer::~RecoveryServer() +BehaviorServer::~BehaviorServer() { + behaviors_.clear(); } nav2_util::CallbackReturn -RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -65,30 +76,22 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_->setCreateTimerInterface(timer_interface); transform_listener_ = std::make_shared(*tf_); - std::string costmap_topic, footprint_topic; + std::string costmap_topic, footprint_topic, robot_base_frame; + double transform_tolerance; this->get_parameter("costmap_topic", costmap_topic); this->get_parameter("footprint_topic", footprint_topic); - this->get_parameter("transform_tolerance", transform_tolerance_); + this->get_parameter("transform_tolerance", transform_tolerance); + this->get_parameter("robot_base_frame", robot_base_frame); costmap_sub_ = std::make_unique( shared_from_this(), costmap_topic); footprint_sub_ = std::make_unique( - shared_from_this(), footprint_topic, 1.0); + shared_from_this(), footprint_topic, *tf_, robot_base_frame, transform_tolerance); - std::string global_frame, robot_base_frame; - get_parameter("global_frame", global_frame); - get_parameter("robot_base_frame", robot_base_frame); collision_checker_ = std::make_shared( - *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), - global_frame, robot_base_frame, transform_tolerance_); + *costmap_sub_, *footprint_sub_, this->get_name()); - get_parameter("recovery_plugins", recovery_ids_); - if (recovery_ids_ == default_ids_) { - for (size_t i = 0; i < default_ids_.size(); ++i) { - declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); - } - } - recovery_types_.resize(recovery_ids_.size()); - if (!loadRecoveryPlugins()) { + behavior_types_.resize(behavior_ids_.size()); + if (!loadBehaviorPlugins()) { return nav2_util::CallbackReturn::FAILURE; } @@ -97,22 +100,22 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) bool -RecoveryServer::loadRecoveryPlugins() +BehaviorServer::loadBehaviorPlugins() { auto node = shared_from_this(); - for (size_t i = 0; i != recovery_ids_.size(); i++) { - recovery_types_[i] = nav2_util::get_plugin_type_param(node, recovery_ids_[i]); + for (size_t i = 0; i != behavior_ids_.size(); i++) { + behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]); try { RCLCPP_INFO( - get_logger(), "Creating recovery plugin %s of type %s", - recovery_ids_[i].c_str(), recovery_types_[i].c_str()); - recoveries_.push_back(plugin_loader_.createUniqueInstance(recovery_types_[i])); - recoveries_.back()->configure(node, recovery_ids_[i], tf_, collision_checker_); + get_logger(), "Creating behavior plugin %s of type %s", + behavior_ids_[i].c_str(), behavior_types_[i].c_str()); + behaviors_.push_back(plugin_loader_.createUniqueInstance(behavior_types_[i])); + behaviors_.back()->configure(node, behavior_ids_[i], tf_, collision_checker_); } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL( - get_logger(), "Failed to create recovery %s of type %s." - " Exception: %s", recovery_ids_[i].c_str(), recovery_types_[i].c_str(), + get_logger(), "Failed to create behavior %s of type %s." + " Exception: %s", behavior_ids_[i].c_str(), behavior_types_[i].c_str(), ex.what()); return false; } @@ -122,41 +125,47 @@ RecoveryServer::loadRecoveryPlugins() } nav2_util::CallbackReturn -RecoveryServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - std::vector>::iterator iter; - for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) { + std::vector>::iterator iter; + for (iter = behaviors_.begin(); iter != behaviors_.end(); ++iter) { (*iter)->activate(); } + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } nav2_util::CallbackReturn -RecoveryServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); - std::vector>::iterator iter; - for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) { + std::vector>::iterator iter; + for (iter = behaviors_.begin(); iter != behaviors_.end(); ++iter) { (*iter)->deactivate(); } + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } nav2_util::CallbackReturn -RecoveryServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - std::vector>::iterator iter; - for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) { + std::vector>::iterator iter; + for (iter = behaviors_.begin(); iter != behaviors_.end(); ++iter) { (*iter)->cleanup(); } - recoveries_.clear(); + behaviors_.clear(); transform_listener_.reset(); tf_.reset(); footprint_sub_.reset(); @@ -167,10 +176,17 @@ RecoveryServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -RecoveryServer::on_shutdown(const rclcpp_lifecycle::State &) +BehaviorServer::on_shutdown(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Shutting down"); return nav2_util::CallbackReturn::SUCCESS; } -} // end namespace recovery_server +} // end namespace behavior_server + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(behavior_server::BehaviorServer) diff --git a/nav2_recoveries/src/main.cpp b/nav2_behaviors/src/main.cpp similarity index 87% rename from nav2_recoveries/src/main.cpp rename to nav2_behaviors/src/main.cpp index 23dea6b90f..0b06ea9f17 100644 --- a/nav2_recoveries/src/main.cpp +++ b/nav2_behaviors/src/main.cpp @@ -16,13 +16,13 @@ #include #include -#include "nav2_recoveries/recovery_server.hpp" +#include "nav2_behaviors/behavior_server.hpp" #include "rclcpp/rclcpp.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto recoveries_node = std::make_shared(); + auto recoveries_node = std::make_shared(); rclcpp::spin(recoveries_node->get_node_base_interface()); rclcpp::shutdown(); diff --git a/nav2_behaviors/test/CMakeLists.txt b/nav2_behaviors/test/CMakeLists.txt new file mode 100644 index 0000000000..9418db1571 --- /dev/null +++ b/nav2_behaviors/test/CMakeLists.txt @@ -0,0 +1,7 @@ +ament_add_gtest(test_behaviors + test_behaviors.cpp +) + +ament_target_dependencies(test_behaviors + ${dependencies} +) diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_behaviors/test/test_behaviors.cpp similarity index 78% rename from nav2_recoveries/test/test_recoveries.cpp rename to nav2_behaviors/test/test_behaviors.cpp index 6f5c83912e..de1253cdc9 100644 --- a/nav2_recoveries/test/test_recoveries.cpp +++ b/nav2_behaviors/test/test_behaviors.cpp @@ -23,30 +23,30 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "nav2_recoveries/recovery.hpp" -#include "nav2_msgs/action/dummy_recovery.hpp" +#include "nav2_behaviors/timed_behavior.hpp" +#include "nav2_msgs/action/dummy_behavior.hpp" -using nav2_recoveries::Recovery; -using nav2_recoveries::Status; -using RecoveryAction = nav2_msgs::action::DummyRecovery; -using ClientGoalHandle = rclcpp_action::ClientGoalHandle; +using nav2_behaviors::TimedBehavior; +using nav2_behaviors::Status; +using BehaviorAction = nav2_msgs::action::DummyBehavior; +using ClientGoalHandle = rclcpp_action::ClientGoalHandle; using namespace std::chrono_literals; -// A recovery for testing the base class +// A behavior for testing the base class -class DummyRecovery : public Recovery +class DummyBehavior : public TimedBehavior { public: - DummyRecovery() - : Recovery(), + DummyBehavior() + : TimedBehavior(), initialized_(false) {} - ~DummyRecovery() {} + ~DummyBehavior() {} - Status onRun(const std::shared_ptr goal) override + Status onRun(const std::shared_ptr goal) override { - // A normal recovery would catch the command and initialize + // A normal behavior would catch the command and initialize initialized_ = false; command_ = goal->command.data; start_time_ = std::chrono::system_clock::now(); @@ -63,7 +63,7 @@ class DummyRecovery : public Recovery Status onCycleUpdate() override { - // A normal recovery would set the robot in motion in the first call + // A normal behavior would set the robot in motion in the first call // and check for robot states on subsequent calls to check if the movement // was completed. @@ -92,17 +92,17 @@ class DummyRecovery : public Recovery // Define a test class to hold the context for the tests -class RecoveryTest : public ::testing::Test +class BehaviorTest : public ::testing::Test { protected: - RecoveryTest() {SetUp();} - ~RecoveryTest() {} + BehaviorTest() {SetUp();} + ~BehaviorTest() {} void SetUp() { node_lifecycle_ = std::make_shared( - "LifecycleRecoveryTestNode", rclcpp::NodeOptions()); + "LifecycleBehaviorTestNode", rclcpp::NodeOptions()); node_lifecycle_->declare_parameter( "costmap_topic", rclcpp::ParameterValue(std::string("local_costmap/costmap_raw"))); @@ -125,21 +125,21 @@ class RecoveryTest : public ::testing::Test node_lifecycle_, costmap_topic); std::shared_ptr footprint_sub_ = std::make_shared( - node_lifecycle_, footprint_topic, 1.0); + node_lifecycle_, footprint_topic, *tf_buffer_); std::shared_ptr collision_checker_ = std::make_shared( - *costmap_sub_, *footprint_sub_, *tf_buffer_, - node_lifecycle_->get_name(), "odom"); + *costmap_sub_, *footprint_sub_, + node_lifecycle_->get_name()); - recovery_ = std::make_shared(); - recovery_->configure(node_lifecycle_, "Recovery", tf_buffer_, collision_checker_); - recovery_->activate(); + behavior_ = std::make_shared(); + behavior_->configure(node_lifecycle_, "Behavior", tf_buffer_, collision_checker_); + behavior_->activate(); - client_ = rclcpp_action::create_client( + client_ = rclcpp_action::create_client( node_lifecycle_->get_node_base_interface(), node_lifecycle_->get_node_graph_interface(), node_lifecycle_->get_node_logging_interface(), - node_lifecycle_->get_node_waitables_interface(), "Recovery"); + node_lifecycle_->get_node_waitables_interface(), "Behavior"); std::cout << "Setup complete." << std::endl; } @@ -152,7 +152,7 @@ class RecoveryTest : public ::testing::Test return false; } - auto goal = RecoveryAction::Goal(); + auto goal = BehaviorAction::Goal(); goal.command.data = command; auto future_goal = client_->async_send_goal(goal); @@ -195,58 +195,58 @@ class RecoveryTest : public ::testing::Test } std::shared_ptr node_lifecycle_; - std::shared_ptr recovery_; - std::shared_ptr> client_; - std::shared_ptr> goal_handle_; + std::shared_ptr behavior_; + std::shared_ptr> client_; + std::shared_ptr> goal_handle_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; }; // Define the tests -TEST_F(RecoveryTest, testingSuccess) +TEST_F(BehaviorTest, testingSuccess) { ASSERT_TRUE(sendCommand("Testing success")); EXPECT_EQ(getOutcome(), Status::SUCCEEDED); SUCCEED(); } -TEST_F(RecoveryTest, testingFailureOnRun) +TEST_F(BehaviorTest, testingFailureOnRun) { ASSERT_TRUE(sendCommand("Testing failure on run")); EXPECT_EQ(getOutcome(), Status::FAILED); SUCCEED(); } -TEST_F(RecoveryTest, testingFailureOnInit) +TEST_F(BehaviorTest, testingFailureOnInit) { ASSERT_TRUE(sendCommand("Testing failure on init")); EXPECT_EQ(getOutcome(), Status::FAILED); SUCCEED(); } -TEST_F(RecoveryTest, testingSequentialFailures) +TEST_F(BehaviorTest, testingSequentialFailures) { ASSERT_TRUE(sendCommand("Testing failure on run")); EXPECT_EQ(getOutcome(), Status::FAILED); SUCCEED(); } -TEST_F(RecoveryTest, testingTotalElapsedTimeIsGratherThanZeroIfStarted) +TEST_F(BehaviorTest, testingTotalElapsedTimeIsGratherThanZeroIfStarted) { ASSERT_TRUE(sendCommand("Testing success")); EXPECT_GT(getResult().result->total_elapsed_time.sec, 0.0); SUCCEED(); } -TEST_F(RecoveryTest, testingTotalElapsedTimeIsZeroIfFailureOnInit) +TEST_F(BehaviorTest, testingTotalElapsedTimeIsZeroIfFailureOnInit) { ASSERT_TRUE(sendCommand("Testing failure on init")); EXPECT_EQ(getResult().result->total_elapsed_time.sec, 0.0); SUCCEED(); } -TEST_F(RecoveryTest, testingTotalElapsedTimeIsZeroIfFailureOnRun) +TEST_F(BehaviorTest, testingTotalElapsedTimeIsZeroIfFailureOnRun) { ASSERT_TRUE(sendCommand("Testing failure on run")); EXPECT_EQ(getResult().result->total_elapsed_time.sec, 0.0); diff --git a/nav2_bringup/bringup/CHANGELOG.rst b/nav2_bringup/CHANGELOG.rst similarity index 100% rename from nav2_bringup/bringup/CHANGELOG.rst rename to nav2_bringup/CHANGELOG.rst diff --git a/nav2_bringup/bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt similarity index 100% rename from nav2_bringup/bringup/CMakeLists.txt rename to nav2_bringup/CMakeLists.txt diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md new file mode 100644 index 0000000000..574a817ecb --- /dev/null +++ b/nav2_bringup/README.md @@ -0,0 +1,17 @@ +# nav2_bringup + +The `nav2_bringup` package is an example bringup system for Nav2 applications. + +This is a very flexible example for nav2 bringup that can be modified for different maps/robots/hardware/worlds/etc. It is our expectation for an application specific robot system that you're mirroring `nav2_bringup` package and modifying it for your specific maps/robots/bringup needs. This is an applied and working demonstration for the default system bringup with many options that can be easily modified. + +Usual robot stacks will have a `_nav` package with config/bringup files and this is that for the general case to base a specific robot system off of. + +Dynamically composed bringup (based on [ROS2 Composition](https://docs.ros.org/en/galactic/Tutorials/Composition.html)) is optional for users. It can be used to compose all Nav2 nodes in a single process instead of launching these nodes separately, which is useful for embedded systems users that need to make optimizations due to harsh resource constraints. Dynamically composed bringup is used by default, but can be disabled by using the launch argument `use_composition:=False`. + +* Some discussions about performance improvement of composed bringup could be found here: https://discourse.ros.org/t/nav2-composition/22175. + +To use, please see the Nav2 [Getting Started Page](https://navigation.ros.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://navigation.ros.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. + +Note: +* gazebo should be started with both libgazebo_ros_init.so and libgazebo_ros_factory.so to work correctly. +* spawn_entity node could not remap /tf and /tf_static to tf and tf_static in the launch file yet, used only for multi-robot situations. Instead it should be done as remapping argument /tf:=tf /tf_static:=tf_static under ros2 tag in each plugin which publishs transforms in the SDF file. It is essential to differentiate the tf's of the different robot. diff --git a/nav2_bringup/bringup/README.md b/nav2_bringup/bringup/README.md deleted file mode 100644 index 525b65df3a..0000000000 --- a/nav2_bringup/bringup/README.md +++ /dev/null @@ -1,136 +0,0 @@ -# nav2_bringup - -The `nav2_bringup` package is an example bringup system for Navigation2 applications. - -### Pre-requisites: -* [Install ROS 2](https://index.ros.org/doc/ros2/Installation/Dashing/) -* Install Navigation2 - - ```sudo apt install ros--navigation2``` - -* Install Navigation2 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 -### Pre-requisites: - -* [Install Gazebo](http://gazebosim.org/tutorials?tut=install_ubuntu&cat=install) -* gazebo_ros_pkgs for ROS2 installed on the system - - ```sudo apt-get install ros--gazebo*``` -* A Gazebo world for simulating the robot ([Gazebo tutorials](http://gazebosim.org/tutorials?tut=quick_start)) -* A map of that world saved to a map.pgm and map.yaml ([ROS Navigation Tutorials](https://github.com/ros-planning/navigation2/tree/master/doc/use_cases)) - -### Terminal 1: Launch Gazebo - -Example: See [turtlebot3_gazebo models](https://github.com/ROBOTIS-GIT/turtlebot3_simulations/tree/ros2/turtlebot3_gazebo/models) for details - -```bash -export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH: -gazebo --verbose -s libgazebo_ros_init.so -``` - -### Terminal 2: Launch your robot specific transforms - -Example: See [turtlebot3_gazebo](https://github.com/ROBOTIS-GIT/turtlebot3_simulations/tree/ros2/turtlebot3_gazebo) for details - -```bash -source /opt/ros/dashing/setup.bash -export TURTLEBOT3_MODEL=waffle -ros2 launch turtlebot3_bringup turtlebot3_state_publisher.launch.py use_sim_time:=True -``` - -### Terminal 3: Launch Navigation2 - -```bash -source /opt/ros/dashing/setup.bash -ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True autostart:=True \ -map:= -``` - -### Terminal 4: Run RViz with Navigation2 config file - -```bash -source /opt/ros/dashing/setup.bash -ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/launch/nav2_default_view.rviz -``` - -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. -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: -* Add "RobotModel", set "Description Source" with "File", set "Description File" with the name of the urdf file for your robot (example: turtlebot3_burger.urdf)" - -### Advanced: single-terminal launch - -A convenience file is provided to launch Gazebo, RVIZ and Navigation2 using a single command: - -```bash -ros2 launch nav2_bringup tb3_simulation_launch.py -``` - -Where `` can used to replace any of the default options, for example: - -``` -world:= -map:= -rviz_config_file:= -simulator:= -bt_xml_file:= -``` - - -Before running the command make sure you are sourcing the `ROS2` workspace, setting the path to the Gazebo model and defining the TB3 robot model to use. - -```bash -source -export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH: -export TURTLEBOT3_MODEL=waffle -``` - -Also, a file for launching **two** robots with **independent** navigation stacks is provided: - -```bash -ros2 launch nav2_bringup multi_tb3_simulation_launch.py -``` - - -## Launch Navigation2 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. - -* Learn more about how to use Navigation 2 with SLAM to create maps; - - - [Navigation 2 with SLAM](https://github.com/ros-planning/navigation2/blob/master/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_ - -* Publish all the transforms from your robot from base_link to base_scan - - -### Terminal 1 : Launch Navigation2 using your map.yaml - -```bash -source /opt/ros/dashing/setup.bash -ros2 launch nav2_bringup bringup_launch.py map:= map_type:=occupancy -``` - -### Terminal 2 : Launch RVIZ - -```bash -source /opt/ros/dashing/setup.bash -ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/launch/nav2_default_view.rviz -``` - -In RVIZ: -* Make sure all transforms from odom are present. (odom->base_link->base_scan) -* Localize the robot using “2D Pose Estimate” button. -* Send the robot a goal pose using “2D Nav Goal” button. diff --git a/nav2_bringup/bringup/launch/localization_launch.py b/nav2_bringup/bringup/launch/localization_launch.py deleted file mode 100644 index e058bba8c1..0000000000 --- a/nav2_bringup/bringup/launch/localization_launch.py +++ /dev/null @@ -1,107 +0,0 @@ -# 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. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from nav2_common.launch import RewrittenYaml - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('nav2_bringup') - - namespace = LaunchConfiguration('namespace') - map_yaml_file = LaunchConfiguration('map') - use_sim_time = LaunchConfiguration('use_sim_time') - autostart = LaunchConfiguration('autostart') - params_file = LaunchConfiguration('params_file') - lifecycle_nodes = ['map_server', 'amcl'] - - # 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')] - - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'use_sim_time': use_sim_time, - 'yaml_filename': map_yaml_file} - - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) - - return LaunchDescription([ - # Set env var to print messages to stdout immediately - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - - DeclareLaunchArgument( - 'namespace', default_value='', - description='Top-level namespace'), - - DeclareLaunchArgument( - 'map', - default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), - description='Full path to map yaml file to load'), - - 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'), - - Node( - package='nav2_map_server', - executable='map_server', - name='map_server', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_amcl', - executable='amcl', - name='amcl', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_localization', - output='screen', - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]) - ]) diff --git a/nav2_bringup/bringup/launch/spawn_tb3_launch.py b/nav2_bringup/bringup/launch/spawn_tb3_launch.py deleted file mode 100644 index af0d100372..0000000000 --- a/nav2_bringup/bringup/launch/spawn_tb3_launch.py +++ /dev/null @@ -1,36 +0,0 @@ -# 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. - -from launch import LaunchDescription - -import launch.actions -import launch_ros.actions - - -def generate_launch_description(): - - return LaunchDescription([ - # TODO(orduno) might not be necessary to have it's own package - launch_ros.actions.Node( - package='nav2_gazebo_spawner', - executable='nav2_gazebo_spawner', - output='screen', - arguments=[ - '--robot_name', launch.substitutions.LaunchConfiguration('robot_name'), - '--robot_namespace', launch.substitutions.LaunchConfiguration('robot_name'), - '--turtlebot_type', launch.substitutions.LaunchConfiguration('turtlebot_type'), - '-x', launch.substitutions.LaunchConfiguration('x_pose'), - '-y', launch.substitutions.LaunchConfiguration('y_pose'), - '-z', launch.substitutions.LaunchConfiguration('z_pose')]), - ]) diff --git a/nav2_bringup/bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py similarity index 68% rename from nav2_bringup/bringup/launch/bringup_launch.py rename to nav2_bringup/launch/bringup_launch.py index 4e200fcde2..bd47147227 100644 --- a/nav2_bringup/bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -22,7 +22,9 @@ from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node from launch_ros.actions import PushRosNamespace +from nav2_common.launch import RewrittenYaml def generate_launch_description(): @@ -37,8 +39,29 @@ 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') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') + + # 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')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'yaml_filename': map_yaml_file} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') @@ -72,29 +95,40 @@ 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_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Whether to use composed bringup') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + # Specify the actions bringup_cmd_group = GroupAction([ PushRosNamespace( condition=IfCondition(use_namespace), namespace=namespace), + Node( + condition=IfCondition(use_composition), + name='nav2_container', + package='rclcpp_components', + executable='component_container_isolated', + parameters=[configured_params, {'autostart': autostart}], + remappings=remappings, + output='screen'), + IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), condition=IfCondition(slam), launch_arguments={'namespace': namespace, 'use_sim_time': use_sim_time, 'autostart': autostart, + 'use_respawn': use_respawn, 'params_file': params_file}.items()), IncludeLaunchDescription( @@ -106,7 +140,9 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file, - 'use_lifecycle_mgr': 'false'}.items()), + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container'}.items()), IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), @@ -114,9 +150,9 @@ 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()), + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container'}.items()), ]) # Create the launch description and populate @@ -133,7 +169,8 @@ 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) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_use_respawn_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py new file mode 100644 index 0000000000..218debd918 --- /dev/null +++ b/nav2_bringup/launch/localization_launch.py @@ -0,0 +1,180 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + map_yaml_file = LaunchConfiguration('map') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + use_respawn = LaunchConfiguration('use_respawn') + + lifecycle_nodes = ['map_server', 'amcl'] + + # 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')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'yaml_filename': map_yaml_file} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + description='Full path to map yaml file to load') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_amcl', + executable='amcl', + name='amcl', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_localization', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='map_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_amcl', + plugin='nav2_amcl::AmclNode', + name='amcl', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_localization', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_map_yaml_cmd) + 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_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + + # Add the actions to launch all of the localiztion nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py b/nav2_bringup/launch/multi_tb3_simulation_launch.py similarity index 81% rename from nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py rename to nav2_bringup/launch/multi_tb3_simulation_launch.py index 4612223f18..d91c5ce063 100644 --- a/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/multi_tb3_simulation_launch.py @@ -13,11 +13,11 @@ # limitations under the License. """ -Example for spawing multiple robots in Gazebo. +Example for spawning multiple robots in Gazebo. This is an example on how to create a launch file for spawning multiple robots into Gazebo and launch multiple instances of the navigation stack, each controlling one robot. -The robots co-exist on a shared environment and are controlled by independent nav stacks +The robots co-exist on a shared environment and are controlled by independent nav stacks. """ import os @@ -39,8 +39,10 @@ def generate_launch_description(): # Names and poses of the robots robots = [ - {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01}, - {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01}] + {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01, + 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}, + {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01, + 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}] # Simulation settings world = LaunchConfiguration('world') @@ -49,7 +51,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 +83,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') @@ -108,30 +102,16 @@ def generate_launch_description(): default_value='True', description='Whether to start RVIZ') - # Start Gazebo with plugin providing the robot spawing service + # Start Gazebo with plugin providing the robot spawning service start_gazebo_cmd = ExecuteProcess( - cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', world], + cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', world], output='screen') - # Define commands for spawing the robots into Gazebo - spawn_robots_cmds = [] - for robot in robots: - spawn_robots_cmds.append( - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', - 'spawn_tb3_launch.py')), - launch_arguments={ - 'x_pose': TextSubstitution(text=str(robot['x_pose'])), - 'y_pose': TextSubstitution(text=str(robot['y_pose'])), - 'z_pose': TextSubstitution(text=str(robot['z_pose'])), - 'robot_name': robot['name'], - 'turtlebot_type': TextSubstitution(text='waffle') - }.items())) - # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: - params_file = LaunchConfiguration(robot['name'] + '_params_file') + params_file = LaunchConfiguration(f"{robot['name']}_params_file") group = GroupAction([ IncludeLaunchDescription( @@ -152,12 +132,18 @@ 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', 'headless': 'False', - 'use_robot_state_pub': use_robot_state_pub}.items()), + 'use_robot_state_pub': use_robot_state_pub, + 'x_pose': TextSubstitution(text=str(robot['x_pose'])), + 'y_pose': TextSubstitution(text=str(robot['y_pose'])), + 'z_pose': TextSubstitution(text=str(robot['z_pose'])), + 'roll': TextSubstitution(text=str(robot['roll'])), + 'pitch': TextSubstitution(text=str(robot['pitch'])), + 'yaw': TextSubstitution(text=str(robot['yaw'])), + 'robot_name':TextSubstitution(text=robot['name']), }.items()), LogInfo( condition=IfCondition(log_settings), @@ -168,9 +154,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 +176,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) @@ -202,9 +184,6 @@ def generate_launch_description(): # Add the actions to start gazebo, robots and simulations ld.add_action(start_gazebo_cmd) - for spawn_robot_cmd in spawn_robots_cmds: - ld.add_action(spawn_robot_cmd) - for simulation_instance_cmd in nav_instances_cmds: ld.add_action(simulation_instance_cmd) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py new file mode 100644 index 0000000000..fec21d1f0b --- /dev/null +++ b/nav2_bringup/launch/navigation_launch.py @@ -0,0 +1,238 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + 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') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + use_respawn = LaunchConfiguration('use_respawn') + + lifecycle_nodes = ['controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower'] + + # 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')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'autostart': autostart} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + 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}]), + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_bringup/bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py similarity index 98% rename from nav2_bringup/bringup/launch/rviz_launch.py rename to nav2_bringup/launch/rviz_launch.py index 3126b11348..9f123b81eb 100644 --- a/nav2_bringup/bringup/launch/rviz_launch.py +++ b/nav2_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/launch/slam_launch.py similarity index 68% rename from nav2_bringup/bringup/launch/slam_launch.py rename to nav2_bringup/launch/slam_launch.py index a32f68094b..6c3b1c1bd3 100644 --- a/nav2_bringup/bringup/launch/slam_launch.py +++ b/nav2_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(): @@ -30,6 +31,7 @@ def generate_launch_description(): params_file = LaunchConfiguration('params_file') use_sim_time = LaunchConfiguration('use_sim_time') autostart = LaunchConfiguration('autostart') + use_respawn = LaunchConfiguration('use_respawn') # Variables lifecycle_nodes = ['map_saver'] @@ -62,33 +64,53 @@ 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') + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + # 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', + respawn=use_respawn, + respawn_delay=2.0, 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 @@ -96,12 +118,14 @@ def generate_launch_description(): ld.add_action(declare_params_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_autostart_cmd) - - # Running SLAM Toolbox - ld.add_action(start_slam_toolbox_cmd) + ld.add_action(declare_use_respawn_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/launch/tb3_simulation_launch.py similarity index 69% rename from nav2_bringup/bringup/launch/tb3_simulation_launch.py rename to nav2_bringup/launch/tb3_simulation_launch.py index ce843a8adb..b4dfd3bd16 100644 --- a/nav2_bringup/bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -38,8 +38,9 @@ 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') + use_composition = LaunchConfiguration('use_composition') + use_respawn = LaunchConfiguration('use_respawn') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') @@ -48,6 +49,14 @@ def generate_launch_description(): use_rviz = LaunchConfiguration('use_rviz') headless = LaunchConfiguration('headless') world = LaunchConfiguration('world') + pose = {'x': LaunchConfiguration('x_pose', default='-2.00'), + 'y': LaunchConfiguration('y_pose', default='-0.50'), + 'z': LaunchConfiguration('z_pose', default='0.01'), + 'R': LaunchConfiguration('roll', default='0.00'), + 'P': LaunchConfiguration('pitch', default='0.00'), + 'Y': LaunchConfiguration('yaw', default='0.00')} + robot_name = LaunchConfiguration('robot_name') + robot_sdf = LaunchConfiguration('robot_sdf') # 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 @@ -76,7 +85,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 +99,22 @@ 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_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Whether to use composed bringup') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + 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( @@ -122,7 +134,7 @@ def generate_launch_description(): declare_simulator_cmd = DeclareLaunchArgument( 'headless', - default_value='False', + default_value='True', description='Whether to execute gzclient)') declare_world_cmd = DeclareLaunchArgument( @@ -130,22 +142,36 @@ 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'), - default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'), + # worlds/turtlebot3_worlds/waffle.model') + default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), description='Full path to world model file to load') + declare_robot_name_cmd = DeclareLaunchArgument( + 'robot_name', + default_value='turtlebot3_waffle', + description='name of the robot') + + declare_robot_sdf_cmd = DeclareLaunchArgument( + 'robot_sdf', + default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'), + description='Full path to robot sdf file to spawn the robot in gazebo') + # Specify the actions start_gazebo_server_cmd = ExecuteProcess( condition=IfCondition(use_simulator), - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', world], + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', world], 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') urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() start_robot_state_publisher_cmd = Node( condition=IfCondition(use_robot_state_pub), @@ -154,27 +180,41 @@ def generate_launch_description(): name='robot_state_publisher', namespace=namespace, output='screen', - parameters=[{'use_sim_time': use_sim_time}], - remappings=remappings, - arguments=[urdf]) + parameters=[{'use_sim_time': use_sim_time, + 'robot_description': robot_description}], + remappings=remappings) + + start_gazebo_spawner_cmd = Node( + package='gazebo_ros', + executable='spawn_entity.py', + output='screen', + arguments=[ + '-entity', robot_name, + '-file', robot_sdf, + '-robot_namespace', namespace, + '-x', pose['x'], '-y', pose['y'], '-z', pose['z'], + '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y']]) 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', + launch_arguments={'namespace': namespace, + 'use_namespace': use_namespace, '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()) + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn}.items()) # Create the launch description and populate ld = LaunchDescription() @@ -186,8 +226,8 @@ 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_use_composition_cmd) ld.add_action(declare_rviz_config_file_cmd) ld.add_action(declare_use_simulator_cmd) @@ -195,10 +235,14 @@ def generate_launch_description(): ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_simulator_cmd) ld.add_action(declare_world_cmd) + ld.add_action(declare_robot_name_cmd) + ld.add_action(declare_robot_sdf_cmd) + ld.add_action(declare_use_respawn_cmd) # Add any conditioned actions ld.add_action(start_gazebo_server_cmd) ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_gazebo_spawner_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) diff --git a/nav2_bringup/bringup/maps/turtlebot3_world.pgm b/nav2_bringup/maps/turtlebot3_world.pgm similarity index 100% rename from nav2_bringup/bringup/maps/turtlebot3_world.pgm rename to nav2_bringup/maps/turtlebot3_world.pgm diff --git a/nav2_bringup/bringup/maps/turtlebot3_world.yaml b/nav2_bringup/maps/turtlebot3_world.yaml similarity index 100% rename from nav2_bringup/bringup/maps/turtlebot3_world.yaml rename to nav2_bringup/maps/turtlebot3_world.yaml 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 deleted file mode 100644 index 05cc3c6dbc..0000000000 --- a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py +++ /dev/null @@ -1,115 +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. - -"""Script used to spawn a robot in a generic position.""" - -import argparse -import os -import xml.etree.ElementTree as ET - -from ament_index_python.packages import get_package_share_directory -from gazebo_msgs.srv import SpawnEntity -import rclpy - - -def main(): - # Get input arguments from user - parser = argparse.ArgumentParser(description='Spawn Robot into Gazebo with navigation2') - 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', - help='ROS namespace to apply to the tf and plugins') - parser.add_argument('-x', type=float, default=0, - help='the x component of the initial position [meters]') - parser.add_argument('-y', type=float, default=0, - help='the y component of the initial position [meters]') - parser.add_argument('-z', type=float, default=0, - help='the z component of the initial position [meters]') - parser.add_argument('-k', '--timeout', type=float, default=10.0, - help="Seconds to wait. Block until the future is complete if negative. Don't wait if 0.") - - group = parser.add_mutually_exclusive_group(required=True) - group.add_argument('-t', '--turtlebot_type', type=str, - choices=['waffle', 'burger']) - group.add_argument('-s', '--sdf', type=str, - help="the path to the robot's model file (sdf)") - - args, unknown = parser.parse_known_args() - - # Start node - rclpy.init() - node = rclpy.create_node('entity_spawner') - - node.get_logger().info( - 'Creating Service client to connect to `/spawn_entity`') - client = node.create_client(SpawnEntity, '/spawn_entity') - - node.get_logger().info('Connecting to `/spawn_entity` service...') - if not client.service_is_ready(): - client.wait_for_service() - node.get_logger().info('...connected!') - - node.get_logger().info('spawning `{}` on namespace `{}` at {}, {}, {}'.format( - args.robot_name, args.robot_namespace, args.x, args.y, args.z)) - - # Get path to the robot's sdf file - if args.turtlebot_type is not None: - sdf_file_path = os.path.join( - get_package_share_directory('turtlebot3_gazebo'), 'models', - 'turtlebot3_{}'.format(args.turtlebot_type), 'model.sdf') - else: - sdf_file_path = args.sdf - - # We need to remap the transform (/tf) topic so each robot has its own. - # We do this by adding `ROS argument entries` to the sdf file for - # each plugin broadcasting a transform. These argument entries provide the - # remapping rule, i.e. /tf -> //tf - tree = ET.parse(sdf_file_path) - root = tree.getroot() - for plugin in root.iter('plugin'): - # TODO(orduno) Handle case if an sdf file from non-turtlebot is provided - if 'turtlebot3_diff_drive' in plugin.attrib.values(): - # The only plugin we care for now is 'diff_drive' which is - # 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' - - # Set data for request - request = SpawnEntity.Request() - request.name = args.robot_name - request.xml = ET.tostring(root, encoding='unicode') - request.robot_namespace = args.robot_namespace - request.initial_pose.position.x = float(args.x) - request.initial_pose.position.y = float(args.y) - request.initial_pose.position.z = float(args.z) - - node.get_logger().info('Sending service request to `/spawn_entity`') - future = client.call_async(request) - rclpy.spin_until_future_complete(node, future, args.timeout) - if future.result() is not None: - print('response: %r' % future.result()) - else: - raise RuntimeError( - 'exception while calling service: %r' % future.exception()) - - node.get_logger().info('Done! Shutting down node.') - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml deleted file mode 100644 index c69941fb2a..0000000000 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - nav2_gazebo_spawner - 0.4.7 - Package for spawning a robot model into Gazebo for navigation2 - lkumarbe - lkumarbe - Apache-2.0 - - rclpy - std_msgs - - ament_lint_auto - ament_lint_common - ament_copyright - python3-pytest - - - ament_python - - diff --git a/nav2_bringup/nav2_gazebo_spawner/setup.cfg b/nav2_bringup/nav2_gazebo_spawner/setup.cfg deleted file mode 100644 index 3edf46fbb6..0000000000 --- a/nav2_bringup/nav2_gazebo_spawner/setup.cfg +++ /dev/null @@ -1,6 +0,0 @@ -[develop] -script-dir=$base/lib/nav2_gazebo_spawner -[install] -install-scripts=$base/lib/nav2_gazebo_spawner -[tool:pytest] -junit_family=xunit2 diff --git a/nav2_bringup/nav2_gazebo_spawner/setup.py b/nav2_bringup/nav2_gazebo_spawner/setup.py deleted file mode 100644 index c71c075e74..0000000000 --- a/nav2_bringup/nav2_gazebo_spawner/setup.py +++ /dev/null @@ -1,22 +0,0 @@ -from setuptools import setup - -PACKAGE_NAME = 'nav2_gazebo_spawner' - -setup( - name=PACKAGE_NAME, - version='0.3.0', - packages=[PACKAGE_NAME], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + PACKAGE_NAME]), - ('share/' + PACKAGE_NAME, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'nav2_gazebo_spawner = nav2_gazebo_spawner.nav2_gazebo_spawner:main', - ], - }, -) diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/package.xml similarity index 90% rename from nav2_bringup/bringup/package.xml rename to nav2_bringup/package.xml index d34bd54d0a..685ae9a44c 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,8 +2,8 @@ nav2_bringup - 0.4.7 - Bringup scripts and configurations for the navigation2 stack + 1.1.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/params/nav2_multirobot_params_1.yaml similarity index 72% rename from nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml rename to nav2_bringup/params/nav2_multirobot_params_1.yaml index 4d1e964381..fcdf39205f 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -26,7 +26,7 @@ amcl: recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 - robot_model_type: "differential" + robot_model_type: "nav2_amcl::DifferentialMotionModel" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true @@ -53,27 +53,57 @@ 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_smooth_path_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_back_up_action_bt_node + - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node - nav2_initial_pose_received_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_truncate_path_local_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_path_expiring_timer_condition - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_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 + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -87,7 +117,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: "goal_checker" controller_plugins: ["FollowPath"] # Progress checker parameters @@ -168,6 +198,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 @@ -191,6 +225,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 @@ -206,6 +244,7 @@ map_server: ros__parameters: use_sim_time: True yaml_filename: "turtlebot3_world.yaml" + save_map_timeout: 5.0 planner_server: ros__parameters: @@ -221,21 +260,27 @@ planner_server_rclcpp_node: ros__parameters: use_sim_time: True -recoveries_server: +smoother_server: + ros__parameters: + use_sim_time: True + +behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] spin: - plugin: "nav2_recoveries/Spin" + plugin: "nav2_behaviors/Spin" backup: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors/Wait" global_frame: odom robot_base_frame: base_link - transform_timeout: 0.1 + transform_tolerance: 0.1 use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 @@ -245,3 +290,13 @@ recoveries_server: robot_state_publisher: ros__parameters: use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml similarity index 72% rename from nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml rename to nav2_bringup/params/nav2_multirobot_params_2.yaml index 39d1ff0e45..5562fe7f0e 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -26,7 +26,7 @@ amcl: recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 - robot_model_type: "differential" + robot_model_type: "nav2_amcl::DifferentialMotionModel" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true @@ -53,27 +53,57 @@ 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_smooth_path_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_back_up_action_bt_node + - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node - nav2_initial_pose_received_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_truncate_path_local_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_path_expiring_timer_condition - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_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 + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -87,7 +117,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: "goal_checker" controller_plugins: ["FollowPath"] # Progress checker parameters @@ -168,6 +198,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 @@ -191,6 +225,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 @@ -206,6 +244,7 @@ map_server: ros__parameters: use_sim_time: True yaml_filename: "turtlebot3_world.yaml" + save_map_timeout: 5.0 planner_server: ros__parameters: @@ -221,21 +260,27 @@ planner_server_rclcpp_node: ros__parameters: use_sim_time: True -recoveries_server: +smoother_server: + ros__parameters: + use_sim_time: True + +behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] spin: - plugin: "nav2_recoveries/Spin" + plugin: "nav2_behaviors/Spin" backup: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors/Wait" global_frame: odom robot_base_frame: base_link - transform_timeout: 0.1 + transform_tolerance: 0.1 use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 @@ -245,3 +290,13 @@ recoveries_server: robot_state_publisher: ros__parameters: use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml new file mode 100644 index 0000000000..864429df14 --- /dev/null +++ b/nav2_bringup/params/nav2_params.yaml @@ -0,0 +1,349 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + 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_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_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_truncate_path_local_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_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_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 + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +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 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #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 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + 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 + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + 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 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz similarity index 93% rename from nav2_bringup/bringup/rviz/nav2_default_view.rviz rename to nav2_bringup/rviz/nav2_default_view.rviz index c07e3e0cba..d2a4b5e136 100644 --- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -62,6 +62,9 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: "" + Mass Properties: + Inertia: false + Mass: false Name: RobotModel TF Prefix: "" Update Interval: 0 @@ -156,10 +159,11 @@ Visualization Manager: Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 - Style: Flat Squares + Style: Points Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort Value: /scan @@ -193,6 +197,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Best Effort Value: /mobile_base/sensors/bumper_pointcloud @@ -214,24 +219,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: @@ -258,9 +258,16 @@ Visualization Manager: Topic: Depth: 1 Durability Policy: Transient Local + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -286,6 +293,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /plan @@ -298,7 +306,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 @@ -317,6 +325,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /global_costmap/voxel_marked_cloud @@ -331,6 +340,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /global_costmap/published_footprint @@ -348,9 +358,16 @@ Visualization Manager: Topic: Depth: 1 Durability Policy: Transient Local + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -376,6 +393,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /local_plan @@ -400,6 +418,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /local_costmap/published_footprint @@ -412,7 +431,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 @@ -431,6 +450,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /local_costmap/voxel_marked_cloud @@ -482,6 +502,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /intel_realsense_r200_depth/points @@ -515,6 +536,9 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile diff --git a/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/rviz/nav2_namespaced_view.rviz similarity index 96% rename from nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz rename to nav2_bringup/rviz/nav2_namespaced_view.rviz index 2a024f75b9..57b2d7bf74 100644 --- a/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz +++ b/nav2_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: @@ -309,6 +304,18 @@ Visualization Manager: Value: true Enabled: true Name: Controller + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/nav2_bringup/bringup/urdf/turtlebot3_waffle.urdf b/nav2_bringup/urdf/turtlebot3_waffle.urdf similarity index 92% rename from nav2_bringup/bringup/urdf/turtlebot3_waffle.urdf rename to nav2_bringup/urdf/turtlebot3_waffle.urdf index c92483449e..f56bd6d479 100644 --- a/nav2_bringup/bringup/urdf/turtlebot3_waffle.urdf +++ b/nav2_bringup/urdf/turtlebot3_waffle.urdf @@ -60,7 +60,7 @@ - + @@ -92,7 +92,7 @@ - + @@ -124,7 +124,7 @@ - + @@ -209,7 +209,7 @@ - + @@ -240,7 +240,7 @@ - + diff --git a/nav2_bringup/bringup/worlds/waffle.model b/nav2_bringup/worlds/waffle.model similarity index 89% rename from nav2_bringup/bringup/worlds/waffle.model rename to nav2_bringup/worlds/waffle.model index 0cd59d56b0..3d983ef545 100644 --- a/nav2_bringup/bringup/worlds/waffle.model +++ b/nav2_bringup/worlds/waffle.model @@ -1,57 +1,7 @@ - - - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - - - - 1000.0 - 0.001 - 1 - - - quick - 150 - 0 - 1.400000 - 1 - - - 0.00001 - 0.2 - 2000.000000 - 0.01000 - - - - - - 1 - - model://turtlebot3_world - - - - -2.0 -0.5 0.01 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 @@ -83,7 +33,7 @@ -0.064 0 0 0 0 0 - model://turtlebot3_waffle/meshes/waffle_base.dae + model://turtlebot3_common/meshes/waffle_base.dae 0.001 0.001 0.001 @@ -137,6 +87,7 @@ + false ~/out:=imu @@ -173,7 +124,7 @@ -0.064 0 0.121 0 0 0 - model://turtlebot3_waffle/meshes/lds.dae + model://turtlebot3_common/meshes/lds.dae 0.001 0.001 0.001 @@ -266,7 +217,7 @@ 0.0 0.144 0.023 0 0 0 - model://turtlebot3_waffle/meshes/tire.dae + model://turtlebot3_common/meshes/tire.dae 0.001 0.001 0.001 @@ -324,7 +275,7 @@ 0.0 -0.144 0.023 0 0 0 - model://turtlebot3_waffle/meshes/tire.dae + model://turtlebot3_common/meshes/tire.dae 0.001 0.001 0.001 @@ -499,6 +450,9 @@ + + /tf:=tf 30 @@ -539,6 +493,4 @@ - - diff --git a/nav2_bringup/bringup/worlds/world_only.model b/nav2_bringup/worlds/world_only.model similarity index 100% rename from nav2_bringup/bringup/worlds/world_only.model rename to nav2_bringup/worlds/world_only.model diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index 22d8f4884c..e36fb0de03 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) @@ -14,6 +15,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,15 +32,11 @@ 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 rclcpp_lifecycle + rclcpp_components std_msgs geometry_msgs nav2_behavior_tree @@ -47,9 +45,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} ) @@ -60,6 +65,8 @@ ament_target_dependencies(${library_name} ${dependencies} ) +rclcpp_components_register_nodes(${library_name} "nav2_bt_navigator::BtNavigator") + install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md index f0864df97a..8dd51a6c6e 100644 --- a/nav2_bt_navigator/README.md +++ b/nav2_bt_navigator/README.md @@ -1,144 +1,9 @@ # 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 and NavigateThroughPoses task interfaces. 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. -## Overview - -The BT Navigator receives a goal pose and navigates the robot to the specified destination. To do so, the module reads an XML description of the Behavior Tree from a file, as specified by a Node parameter, and passes that to a generic [BehaviorTreeEngine class](../nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp) which uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) to dynamically create and execute the BT. - -## Specifying an input XML file - -The BtNavigator node has a parameter, *bt_xml_filename*, that can be specified using a ROS2 parameters YAML file, like this: - -``` -BtNavigator: - ros__parameters: - bt_xml_filename: -``` - -Using the XML filename as a parameter makes it easy to change or extend the logic used for navigation. Once can simply update the XML description for the BT and the BtNavigator task server will use the new description. - -## 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. - -## 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). -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) - -[navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) implements basic navigation by continuously computing and updating the path at a rate of 1Hz. The default controller, the nav2_dwb_controller, implements path following at a rate of 10Hz. - -![Navigation with time based replanning](./doc/navigate_w_replanning_time.png) - -### Navigate with Replanning (distace-based) - -[navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml) implements basic navigation by continuously computing and updating the path after every 1 meter distance traveled by the robot. - -![Navigation with distance based replanning](./doc/navigate_w_replanning_distance.png) - -#### Navigate with replanning is composed of the following custom decorator, condition, control and action nodes: - -#### Control Nodes -* PipelineSequence: This is a variant of a Sequence Node. When this node is ticked, -it will tick the first child till it succeeds. Then it will tick the first two -children till the second one succeeds. Then it will tick the first three till the -third succeeds and so on, till there are no more children. This will return RUNNING, -till the last child succeeds, at which time it also returns SUCCESS. If any child -returns FAILURE, all nodes are halted and this node returns FAILURE. -* RoundRobin: This is a custom control node introduced to the Behavior Tree. When this node is ticked, it will tick the first child until it returns SUCCESS or FAILURE. If the child returns either SUCCESS or FAILURE, it will tick its next child. Once the node reaches the last child, it will restart ticking from the first child. The main difference between the RoundRobin node versus the Sequence node is that when a child returns FAILURE the RoundRobin node will tick the next child but in the Sequence node, it will return FAILURE. - -* Recovery: This is a control flow type 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. The second child is responsible for recovery actions such as re-initializing system or other recovery behaviors. If the recovery behaviors are succeeded, then the first child will be executed again. The user can specify how many times the recovery actions should be taken before returning failure. The figure below depicts a simple recovery node. - -

- -

-
- - -#### 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. - -* DistanceController: A custom control flow node, which controls the tick rate based on the distance traveled. This custom node has only one child. The user can set the distance after which the planner should replan a new path. This node returns RUNNING when it is not ticking its child. Currently, in navigation, the `DistanceController` is used to tick the `ComputePathToPose` and `GoalReached` node after every 0.5 meters. - -* SpeedController: A custom control flow node, which controls the tick rate based on the current speed. This decorator offers the most flexibility as the user can set the minimum/maximum tick rate which is adjusted according to the current speed. - -* 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. - -#### Action Nodes -* ComputePathToPose: When this node is ticked, the goal will be placed on the blackboard which will be shared to the Behavior tree. The bt action node would then utilizes the action server to send a request to the global planner to recompute the global path. Once the global path is recomputed, the result will be sent back via action server and then the updated path will be placed on the blackboard. The `planner` parameter will tell the planning server which of the loaded planning plugins to utilize, in case of desiring different parameters, planners, or behaviors. The name of the planner should correspond with the high level task it accomplishes and align with the `planner_ids` name given to it in the planner server. If no planner name is provided, it will use the only planner in the planner server when only one is available. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-navigator.html) for additional parameter descriptions, as well as the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. -The graphical version of this Behavior Tree: - -

- -

-
- -The navigate with replanning BT first ticks the `RateController` node which specifies how frequently the `GoalReached` and `ComputePathToPose` should be invoked. Then the `GoalReached` nodes check the distance to the goal to determine if the `ComputePathToPose` should be ticked or not. The `ComputePathToPose` gets the incoming goal pose from the blackboard, computes the path and puts the result back on the blackboard, where `FollowPath` picks it up. Each time a new path is computed, the blackboard gets updated and then `FollowPath` picks up the new goal to compute a control effort for. `controller_id` will specify the type of control effort you'd like to compute such as `FollowPath` `FollowPathSlow` `FollowPathExactly`, etc. - - -* TruncatePath: A custom control node, which modifies a path making it shorter. It removes parts of the path closer than a distance to the goal pose. The resulting last pose of the path orientates the robot to the original goal pose. - -### 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. - -

- -

-
- - -This tree is currently our default tree in the stack and the xml file is located here: [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml). - - -#### Halting recoveries on navigation goal (Preemption reference design) -In general, the recovery behaviours and any other long running process should be stopped when the navigation goal is issued (e.g. preemption). In the default tree in the stack, this behaviour is accomplished using a condition node checking the global navigation goal and a reactive fallback controller node: - -

- -

- -This way, the recovery actions can be interrupted if a new goal is sent to the bt_navigator. Adding other condition nodes to this structure, it is possible to halt the recoveries in other cases (e.g, giving a time limit for their execution). This is the recommended design pattern for preempting a node or tree branch under specific conditions such as a new navigation request. Please notice that the order of definition of the nodes in the xml file can alter the behaviour of this structure, and that all conditions should be placed before the recovery behaviour node or branch. - - -#### 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. - -![Navigate following a dynamic point to a certain distance](./doc/follow_point.png) - -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: +## Overview -![Legend](./doc/legend.png) +The BT Navigator receives a goal pose and navigates the robot to the specified destination(s). To do so, the module reads an XML description of the Behavior Tree from a file, as specified by a Node parameter, and passes that to a generic [BehaviorTreeEngine class](../nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp) which uses the [Behavior-Tree.CPP library](https://github.com/BehaviorTree/BehaviorTree.CPP) to dynamically create and execute the BT. The BT XML can also be specified on a per-task basis so that your robot may have many different types of navigation or autonomy behaviors on a per-task basis. diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml new file mode 100644 index 0000000000..f4ed518444 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 0000000000..1378863cdf --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml similarity index 66% rename from nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml rename to nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 9b1311a391..2396b844ae 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -1,7 +1,8 @@ @@ -20,12 +21,15 @@ - - - + + + + + - + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml new file mode 100644 index 0000000000..6b5883bfe7 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml new file mode 100644 index 0000000000..e60a2e7751 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 746ca53d8d..0000000000 --- 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_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml new file mode 100644 index 0000000000..46d18fe654 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml new file mode 100644 index 0000000000..e921db5557 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file 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 8bc4b7f05a..049e78794b 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/behavior_trees/odometry_calibration.xml b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml new file mode 100644 index 0000000000..49000e14fa --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + 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 ef50b05451..0000000000 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 7bcb6ab7b5..0000000000 Binary files a/nav2_bt_navigator/doc/auto_localization.png and /dev/null differ diff --git a/nav2_bt_navigator/doc/parallel_w_goal_patience_and_recovery.png b/nav2_bt_navigator/doc/parallel_w_goal_patience_and_recovery.png new file mode 100644 index 0000000000..e91b283c7e Binary files /dev/null and b/nav2_bt_navigator/doc/parallel_w_goal_patience_and_recovery.png differ diff --git a/nav2_bt_navigator/doc/parallel_w_recovery.png b/nav2_bt_navigator/doc/parallel_w_recovery.png index ac1022a599..e67f3f77a5 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 c0f67c7d5e..0000000000 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 775a71bc25..0000000000 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 5c87558a2e..0406c8735e 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -19,18 +19,18 @@ #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 "nav2_util/odometry_utils.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 "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 @@ -41,8 +41,9 @@ class BtNavigator : public nav2_util::LifecycleNode public: /** * @brief A constructor for nav2_bt_navigator::BtNavigator class + * @param options Additional options to control creation of the node. */ - BtNavigator(); + explicit BtNavigator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief A destructor for nav2_bt_navigator::BtNavigator class */ @@ -83,68 +84,24 @@ 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_; - - /** - * @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); + // To handle all the BT related execution + std::unique_ptr> pose_navigator_; + std::unique_ptr> + poses_navigator_; + nav2_bt_navigator::NavigatorMuxer plugin_muxer_; - 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::shared_ptr odom_smoother_; // Metrics for feedback - rclcpp::Time start_time_; std::string robot_frame_; std::string global_frame_; double transform_tolerance_; + std::string odom_topic_; + + // 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 0000000000..e53bdce6ab --- /dev/null +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp @@ -0,0 +1,338 @@ +// 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 "nav2_util/odometry_utils.hpp" +#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 + * @param odom_smoother Object to get current smoothed robot's speed + * @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, + std::shared_ptr odom_smoother) + { + 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, std::placeholders::_2)); + + 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, odom_smoother) && ok; + } + + /** + * @brief Activation 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 Deactivation 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; + + /** + * @brief Get the action server + * @return Action server pointer + */ + std::unique_ptr> & getActionServer() + { + return bt_action_server_; + } + +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; + } + + bool goal_accepted = goalReceived(goal); + + if (goal_accepted) { + plugin_muxer_->startNavigating(getName()); + } + + return goal_accepted; + } + + /** + * @brief An intermediate completion function to mux navigators + */ + void onCompletion( + typename ActionT::Result::SharedPtr result, + const nav2_behavior_tree::BtStatus final_bt_status) + { + plugin_muxer_->stopNavigating(getName()); + goalCompleted(result, final_bt_status); + } + + /** + * @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, + const nav2_behavior_tree::BtStatus final_bt_status) = 0; + + /** + * @param Method to configure resources. + */ + virtual bool configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/, + std::shared_ptr/*odom_smoother*/) + { + return true; + } + + /** + * @brief Method to cleanup resources. + */ + virtual bool cleanup() {return true;} + + /** + * @brief Method to activate any threads involved in execution. + */ + virtual bool activate() {return true;} + + /** + * @brief Method to deactivate 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 0000000000..d6e218d860 --- /dev/null +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -0,0 +1,120 @@ +// 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 + * @param odom_smoother Object to get current smoothed robot's speed + */ + bool configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr node, + std::shared_ptr odom_smoother) 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 + * @param final_bt_status Resulting status of the behavior tree execution that may be + * referenced while populating the result. + */ + void goalCompleted( + typename ActionT::Result::SharedPtr result, + const nav2_behavior_tree::BtStatus final_bt_status) 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::shared_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 0000000000..20230aa10e --- /dev/null +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -0,0 +1,136 @@ +// 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 + * @param odom_smoother Object to get current smoothed robot's speed + */ + bool configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr node, + std::shared_ptr odom_smoother) 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 + * @param final_bt_status Resulting status of the behavior tree execution that may be + * referenced while populating the result. + */ + void goalCompleted( + typename ActionT::Result::SharedPtr result, + const nav2_behavior_tree::BtStatus final_bt_status) 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::shared_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 c517d6c9e5..0000000000 --- a/nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp +++ /dev/null @@ -1,48 +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::SharedPtr & 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::Node::SharedPtr ros_node_; - 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 3528045247..e3287b118d 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.7 + 1.1.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 e44e64887e..24f5d7f253 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -14,60 +14,72 @@ #include "nav2_bt_navigator/bt_navigator.hpp" -#include #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) +BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("bt_navigator", "", options) { 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_smooth_path_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_back_up_action_bt_node", + "nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node", + "nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition", "nav2_distance_traveled_condition_bt_node", - "nav2_rotate_action_bt_node", - "nav2_translate_action_bt_node", + "nav2_single_trigger_bt_node", + "nav2_goal_updated_controller_bt_node", "nav2_is_battery_low_condition_bt_node", - "nav2_goal_updater_node_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", + "nav2_controller_cancel_bt_node", + "nav2_path_longer_on_approach_bt_node" + "nav2_wait_cancel_bt_node", + "nav2_spin_cancel_bt_node", + "nav2_back_up_cancel_bt_node" + "nav2_drive_on_heading_cancel_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")); @@ -80,7 +92,6 @@ BtNavigator::BtNavigator() BtNavigator::~BtNavigator() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn @@ -88,17 +99,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()); @@ -106,89 +106,39 @@ 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), false); - - // 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(); + odom_topic_ = get_parameter("odom_topic").as_string(); - // 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("path_updated", false); // 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_); + // Odometry smoother object for getting current speed + odom_smoother_ = std::make_shared(shared_from_this(), 0.3, odom_topic_); - if (!loadBehaviorTree(default_bt_xml_filename_)) { - RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str()); + if (!pose_navigator_->on_configure( + shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_)) + { return nav2_util::CallbackReturn::FAILURE; } - 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 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_, odom_smoother_)) + { + 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; + return nav2_util::CallbackReturn::SUCCESS; } nav2_util::CallbackReturn @@ -196,7 +146,12 @@ BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - action_server_->activate(); + if (!poses_navigator_->on_activate() || !pose_navigator_->on_activate()) { + return nav2_util::CallbackReturn::FAILURE; + } + + // create bond connection + createBond(); return nav2_util::CallbackReturn::SUCCESS; } @@ -206,7 +161,12 @@ 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(); return nav2_util::CallbackReturn::SUCCESS; } @@ -216,23 +176,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; @@ -245,114 +198,11 @@ 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 == "" ? 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); -} +} // namespace nav2_bt_navigator -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); -} +#include "rclcpp_components/register_node_macro.hpp" -} // namespace nav2_bt_navigator +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_bt_navigator::BtNavigator) 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 0000000000..ebd1cd4e77 --- /dev/null +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -0,0 +1,219 @@ +// 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, + std::shared_ptr odom_smoother) +{ + start_time_ = rclcpp::Time(0); + auto node = parent_node.lock(); + + if (!node->has_parameter("goals_blackboard_id")) { + 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_ = odom_smoother; + + 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"); + node->declare_parameter( + "default_nav_through_poses_bt_xml", + pkg_share_dir + + "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml"); + } + + 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*/, + const nav2_behavior_tree::BtStatus /*final_bt_status*/) +{ +} + +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 0000000000..82727af5e2 --- /dev/null +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -0,0 +1,228 @@ +// 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, + std::shared_ptr odom_smoother) +{ + start_time_ = rclcpp::Time(0); + auto node = parent_node.lock(); + + if (!node->has_parameter("goal_blackboard_id")) { + 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_ = odom_smoother; + + 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"); + node->declare_parameter( + "default_nav_to_pose_bt_xml", + pkg_share_dir + + "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"); + } + + 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*/, + const nav2_behavior_tree::BtStatus /*final_bt_status*/) +{ +} + +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 5b248014e8..0000000000 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ /dev/null @@ -1,67 +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::SharedPtr & ros_node, const BT::Tree & tree) -: StatusChangeLogger(tree.rootNode()), ros_node_(ros_node) -{ - log_pub_ = ros_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( - ros_node_->get_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_.size() > 0) { - auto log_msg = std::make_unique(); - log_msg->timestamp = ros_node_->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 ee8555f799..b00d30c17b 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/has_node_params.py b/nav2_common/nav2_common/launch/has_node_params.py index 21b8a32f9e..92f96790b5 100644 --- a/nav2_common/nav2_common/launch/has_node_params.py +++ b/nav2_common/nav2_common/launch/has_node_params.py @@ -18,44 +18,43 @@ import yaml import launch +import sys # delete this class HasNodeParams(launch.Substitution): - """ - Substitution that checks if a param file contains parameters for a node. + """ + Substitution that checks if a param file contains parameters for a node - Used in launch system - """ + Used in launch system + """ - def __init__(self, - source_file: launch.SomeSubstitutionsType, - node_name: Text) -> None: - super().__init__() - """ + 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 """ - # import here to avoid loop - from launch.utilities import normalize_to_list_of_substitutions - self.__source_file = normalize_to_list_of_substitutions(source_file) - self.__node_name = node_name + 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 + @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 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')) + 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" + 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 d2bbada1c3..4b5c82e757 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 40332a6427..4e08efbf9d 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -117,7 +117,7 @@ def substitute_params(self, yaml, param_rewrites): for path in yaml_paths: if path in param_rewrites: # this is an absolute path (ex. 'key.keyA.keyB.val') - rewrite_val = param_rewrites[path] + rewrite_val = self.convert(param_rewrites[path]) yaml_keys = path.split('.') yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val) diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 753275f3bd..2213c09104 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.7 + 1.1.0 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt new file mode 100644 index 0000000000..0d0cb497ae --- /dev/null +++ b/nav2_constrained_smoother/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_constrained_smoother) + +set(CMAKE_BUILD_TYPE Release) # significant Ceres optimization speedup + +find_package(ament_cmake REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_common REQUIRED) +find_package(angles REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(pluginlib REQUIRED) +find_package(Ceres REQUIRED COMPONENTS SuiteSparse) + +set(CMAKE_CXX_STANDARD 17) + +nav2_package() + +set(library_name nav2_constrained_smoother) + +include_directories( + include + ${CERES_INCLUDES} +) + +set(dependencies + angles + rclcpp + rclcpp_action + nav2_msgs + nav2_costmap_2d + nav2_util + nav2_core + pluginlib +) + +add_library(${library_name} SHARED src/constrained_smoother.cpp) +target_link_libraries(${library_name} ${CERES_LIBRARIES}) +# prevent pluginlib from using boost +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +ament_target_dependencies(${library_name} ${dependencies}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + +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) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) + +pluginlib_export_plugin_description_file(nav2_core nav2_constrained_smoother.xml) + +ament_package() diff --git a/nav2_constrained_smoother/README.md b/nav2_constrained_smoother/README.md new file mode 100644 index 0000000000..8359e5d3d7 --- /dev/null +++ b/nav2_constrained_smoother/README.md @@ -0,0 +1,49 @@ +# Constrained Smoother + +A smoother plugin for `nav2_smoother` based on the original deprecated smoother in `nav2_smac_planner` by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) and put into operational state by [**RoboTech Vision**](https://robotechvision.com/). Suitable for applications which need planned global path to be pushed away from obstacles and/or for Reeds-Shepp motion models. + +See documentation on navigation.ros.org: https://navigation.ros.org/configuration/packages/configuring-constrained-smoother.html + + +Example of configuration (see indoor_navigation package of this repo for a full launch configuration): + +``` +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["SmoothPath"] + + SmoothPath: + plugin: "nav2_constrained_smoother/ConstrainedSmoother" + reversing_enabled: true # whether to detect forward/reverse direction and cusps. Should be set to false for paths without orientations assigned + path_downsampling_factor: 3 # every n-th node of the path is taken. Useful for speed-up + path_upsampling_factor: 1 # 0 - path remains downsampled, 1 - path is upsampled back to original granularity using cubic bezier, 2... - more upsampling + keep_start_orientation: true # whether to prevent the start orientation from being smoothed + keep_goal_orientation: true # whether to prevent the gpal orientation from being smoothed + minimum_turning_radius: 0.40 # minimum turning radius the robot can perform. Can be set to 0.0 (or w_curve can be set to 0.0 with the same effect) for diff-drive/holonomic robots + w_curve: 30.0 # weight to enforce minimum_turning_radius + w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight + w_smooth: 15000.0 # weight to maximize smoothness of path + w_cost: 0.015 # weight to steer robot away from collision and cost + + # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes) + # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification + w_cost_cusp_multiplier: 3.0 # option to have higher weight during forward/reverse direction change which is often accompanied with dangerous rotations + cusp_zone_length: 2.5 # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight eqals w_cost*w_cost_cusp_multiplier) + + # Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...] + # IMPORTANT: Requires much higher number of iterations to actually improve the path. Uncomment only if you really need it (highly elongated/asymmetric robots) + # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification + # cost_check_points: [-0.185, 0.0, 1.0] + + optimizer: + max_iterations: 70 # max iterations of smoother + debug_optimizer: false # print debug info + gradient_tol: 5e3 + fn_tol: 1.0e-15 + param_tol: 1.0e-20 +``` + +Note: Smoothing paths which contain multiple subsequent poses at one point (e.g. in-place rotations from Smac lattice planners) is currently not supported + +Note: Constrained Smoother is recommended to be used on a path with a bounded length. TruncatePathLocal BT Node can be used for extracting a relevant path section around robot (in combination with DistanceController to achieve periodicity) diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp new file mode 100644 index 0000000000..bfd564772e --- /dev/null +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp @@ -0,0 +1,104 @@ +// Copyright (c) 2021 RoboTech Vision +// 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_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_ +#define NAV2_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_ + +#include +#include +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_constrained_smoother/smoother.hpp" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_constrained_smoother +{ + +/** + * @class nav2_constrained_smoother::ConstrainedSmoother + * @brief Regulated pure pursuit controller plugin + */ +class ConstrainedSmoother : public nav2_core::Smoother +{ +public: + /** + * @brief Constructor for nav2_constrained_smoother::ConstrainedSmoother + */ + ConstrainedSmoother() = default; + + /** + * @brief Destrructor for nav2_constrained_smoother::ConstrainedSmoother + */ + ~ConstrainedSmoother() override = default; + + /** + * @brief Configure smoother parameters and member variables + * @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, std::shared_ptr tf, + std::shared_ptr costmap_sub, + std::shared_ptr footprint_sub) 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 Method to smooth given path + * + * @param path In-out path to be optimized + * @param max_time Maximum duration smoothing should take + * @return Smoothed path + */ + bool smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) override; + +protected: + std::shared_ptr tf_; + std::string plugin_name_; + std::shared_ptr costmap_sub_; + rclcpp::Logger logger_ {rclcpp::get_logger("ConstrainedSmoother")}; + + std::unique_ptr smoother_; + SmootherParams smoother_params_; + OptimizerParams optimizer_params_; +}; + +} // namespace nav2_constrained_smoother + +#endif // NAV2_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_ diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp new file mode 100644 index 0000000000..36c313cb09 --- /dev/null +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/options.hpp @@ -0,0 +1,202 @@ +// Copyright (c) 2021 RoboTech Vision +// 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 NAV2_CONSTRAINED_SMOOTHER__OPTIONS_HPP_ +#define NAV2_CONSTRAINED_SMOOTHER__OPTIONS_HPP_ + +#include +#include +#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "ceres/ceres.h" + +namespace nav2_constrained_smoother +{ + +/** + * @struct nav2_smac_planner::SmootherParams + * @brief Parameters for the smoother cost function + */ +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(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string("."); + + // Smoother params + double minimum_turning_radius; + nav2_util::declare_parameter_if_not_declared( + node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4)); + node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius); + max_curvature = 1.0f / minimum_turning_radius; + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_curve", rclcpp::ParameterValue(30.0)); + node->get_parameter(local_name + "w_curve", curvature_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_cost", rclcpp::ParameterValue(0.015)); + node->get_parameter(local_name + "w_cost", costmap_weight); + double cost_cusp_multiplier; + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_cost_cusp_multiplier", rclcpp::ParameterValue(3.0)); + node->get_parameter(local_name + "w_cost_cusp_multiplier", cost_cusp_multiplier); + cusp_costmap_weight = costmap_weight * cost_cusp_multiplier; + nav2_util::declare_parameter_if_not_declared( + node, local_name + "cusp_zone_length", rclcpp::ParameterValue(2.5)); + node->get_parameter(local_name + "cusp_zone_length", cusp_zone_length); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_dist", rclcpp::ParameterValue(0.0)); + node->get_parameter(local_name + "w_dist", distance_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_smooth", rclcpp::ParameterValue(15000.0)); + node->get_parameter(local_name + "w_smooth", smooth_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "cost_check_points", rclcpp::ParameterValue(std::vector())); + node->get_parameter(local_name + "cost_check_points", cost_check_points); + if (cost_check_points.size() % 3 != 0) { + RCLCPP_ERROR( + rclcpp::get_logger( + "constrained_smoother"), + "cost_check_points parameter must contain values as follows: " + "[x1, y1, weight1, x2, y2, weight2, ...]"); + throw std::runtime_error("Invalid parameter: cost_check_points"); + } + // normalize check point weights so that their sum == 1.0 + double check_point_weights_sum = 0.0; + for (size_t i = 2u; i < cost_check_points.size(); i += 3) { + check_point_weights_sum += cost_check_points[i]; + } + for (size_t i = 2u; i < cost_check_points.size(); i += 3) { + cost_check_points[i] /= check_point_weights_sum; + } + nav2_util::declare_parameter_if_not_declared( + node, local_name + "path_downsampling_factor", rclcpp::ParameterValue(1)); + node->get_parameter(local_name + "path_downsampling_factor", path_downsampling_factor); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "path_upsampling_factor", rclcpp::ParameterValue(1)); + node->get_parameter(local_name + "path_upsampling_factor", path_upsampling_factor); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "reversing_enabled", rclcpp::ParameterValue(true)); + node->get_parameter(local_name + "reversing_enabled", reversing_enabled); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "keep_goal_orientation", rclcpp::ParameterValue(true)); + node->get_parameter(local_name + "keep_goal_orientation", keep_goal_orientation); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "keep_start_orientation", rclcpp::ParameterValue(true)); + node->get_parameter(local_name + "keep_start_orientation", keep_start_orientation); + } + + double smooth_weight{0.0}; + double costmap_weight{0.0}; + double cusp_costmap_weight{0.0}; + double cusp_zone_length{0.0}; + double distance_weight{0.0}; + double curvature_weight{0.0}; + double max_curvature{0.0}; + double max_time{10.0}; // adjusted by action goal, not by parameters + int path_downsampling_factor{1}; + int path_upsampling_factor{1}; + bool reversing_enabled{true}; + bool keep_goal_orientation{true}; + bool keep_start_orientation{true}; + std::vector cost_check_points{}; +}; + +/** + * @struct nav2_smac_planner::OptimizerParams + * @brief Parameters for the ceres optimizer + */ +struct OptimizerParams +{ + OptimizerParams() + : debug(false), + max_iterations(50), + param_tol(1e-8), + fn_tol(1e-6), + gradient_tol(1e-10) + { + } + + /** + * @brief Get params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".optimizer."); + + // Optimizer params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "linear_solver_type", rclcpp::ParameterValue("SPARSE_NORMAL_CHOLESKY")); + node->get_parameter(local_name + "linear_solver_type", linear_solver_type); + if (solver_types.find(linear_solver_type) == solver_types.end()) { + std::stringstream valid_types_str; + for (auto type = solver_types.begin(); type != solver_types.end(); type++) { + if (type != solver_types.begin()) { + valid_types_str << ", "; + } + valid_types_str << type->first; + } + RCLCPP_ERROR( + rclcpp::get_logger("constrained_smoother"), + "Invalid linear_solver_type. Valid values are %s", valid_types_str.str().c_str()); + throw std::runtime_error("Invalid parameter: linear_solver_type"); + } + nav2_util::declare_parameter_if_not_declared( + node, local_name + "param_tol", rclcpp::ParameterValue(1e-15)); + node->get_parameter(local_name + "param_tol", param_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "fn_tol", rclcpp::ParameterValue(1e-7)); + node->get_parameter(local_name + "fn_tol", fn_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "gradient_tol", rclcpp::ParameterValue(1e-10)); + node->get_parameter(local_name + "gradient_tol", gradient_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_iterations", rclcpp::ParameterValue(100)); + node->get_parameter(local_name + "max_iterations", max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "debug_optimizer", rclcpp::ParameterValue(false)); + node->get_parameter(local_name + "debug_optimizer", debug); + } + + const std::map solver_types = { + {"DENSE_QR", ceres::DENSE_QR}, + {"SPARSE_NORMAL_CHOLESKY", ceres::SPARSE_NORMAL_CHOLESKY}}; + + bool debug; + std::string linear_solver_type; + int max_iterations; // Ceres default: 50 + + double param_tol; // Ceres default: 1e-8 + double fn_tol; // Ceres default: 1e-6 + double gradient_tol; // Ceres default: 1e-10 +}; + +} // namespace nav2_constrained_smoother + +#endif // NAV2_CONSTRAINED_SMOOTHER__OPTIONS_HPP_ diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp new file mode 100644 index 0000000000..b5432bf659 --- /dev/null +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp @@ -0,0 +1,401 @@ +// Copyright (c) 2021 RoboTech Vision +// 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 NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_ +#define NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_constrained_smoother/smoother_cost_function.hpp" +#include "nav2_constrained_smoother/utils.hpp" + +#include "ceres/ceres.h" +#include "Eigen/Core" + +namespace nav2_constrained_smoother +{ + +/** + * @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; + + options_.linear_solver_type = params.solver_types.at(params.linear_solver_type); + + options_.max_num_iterations = params.max_iterations; + + options_.function_tolerance = params.fn_tol; + options_.gradient_tolerance = params.gradient_tol; + options_.parameter_tolerance = params.param_tol; + + if (debug_) { + options_.minimizer_progress_to_stdout = true; + options_.logging_type = ceres::LoggingType::PER_MINIMIZER_ITERATION; + } else { + options_.logging_type = ceres::SILENT; + } + } + + /** + * @brief Smoother method + * @param path Reference to path + * @param start_dir Orientation of the first pose + * @param end_dir Orientation of the last pose + * @param costmap Pointer to minimal costmap + * @param params parameters weights + * @return If smoothing was successful + */ + bool smooth( + std::vector & path, + const Eigen::Vector2d & start_dir, + const Eigen::Vector2d & end_dir, + const nav2_costmap_2d::Costmap2D * costmap, + const SmootherParams & params) + { + // Path has always at least 2 points + if (path.size() < 2) { + throw std::runtime_error("Constrained smoother: Path must have at least 2 points"); + } + + options_.max_solver_time_in_seconds = params.max_time; + + ceres::Problem problem; + std::vector path_optim; + std::vector optimized; + if (buildProblem(path, costmap, params, problem, path_optim, optimized)) { + // solve the problem + ceres::Solver::Summary summary; + ceres::Solve(options_, &problem, &summary); + if (debug_) { + RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "%s", summary.FullReport().c_str()); + } + if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost < 0.0) { + return false; + } + } else { + RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "Path too short to optimize"); + } + + upsampleAndPopulate(path_optim, optimized, start_dir, end_dir, params, path); + + return true; + } + +private: + /** + * @brief Build problem method + * @param path Reference to path + * @param costmap Pointer to costmap + * @param params Smoother parameters + * @param problem Output problem to solve + * @param path_optim Output path on which the problem will be solved + * @param optimized False for points skipped by downsampling + * @return If there is a problem to solve + */ + bool buildProblem( + const std::vector & path, + const nav2_costmap_2d::Costmap2D * costmap, + const SmootherParams & params, + ceres::Problem & problem, + std::vector & path_optim, + std::vector & optimized) + { + // Create costmap grid + costmap_grid_ = std::make_shared>( + costmap->getCharMap(), 0, costmap->getSizeInCellsY(), 0, costmap->getSizeInCellsX()); + auto costmap_interpolator = std::make_shared>>( + *costmap_grid_); + + // Create residual blocks + const double cusp_half_length = params.cusp_zone_length / 2; + ceres::LossFunction * loss_function = NULL; + path_optim = path; + optimized = std::vector(path.size()); + optimized[0] = true; + int prelast_i = -1; + int last_i = 0; + double last_direction = path_optim[0][2]; + bool last_was_cusp = false; + bool last_is_reversing = false; + std::deque> potential_cusp_funcs; + double last_segment_len = EPSILON; + double potential_cusp_funcs_len = 0; + double len_since_cusp = std::numeric_limits::infinity(); + + for (size_t i = 1; i < path_optim.size(); i++) { + auto & pt = path_optim[i]; + bool is_cusp = false; + if (i != path_optim.size() - 1) { + is_cusp = pt[2] * last_direction < 0; + last_direction = pt[2]; + + // skip to downsample if can be skipped (no forward/reverse direction change) + if (!is_cusp && + i > (params.keep_start_orientation ? 1 : 0) && + i < path_optim.size() - (params.keep_goal_orientation ? 2 : 1) && + static_cast(i - last_i) < params.path_downsampling_factor) + { + continue; + } + } + + // keep distance inequalities between poses + // (some might have been downsampled while others might not) + double current_segment_len = (path_optim[i] - path_optim[last_i]).block<2, 1>(0, 0).norm(); + + // forget cost functions which don't have chance to be part of a cusp zone + potential_cusp_funcs_len += current_segment_len; + while (!potential_cusp_funcs.empty() && potential_cusp_funcs_len > cusp_half_length) { + potential_cusp_funcs_len -= potential_cusp_funcs.front().first; + potential_cusp_funcs.pop_front(); + } + + // update cusp zone costmap weights + if (is_cusp) { + double len_to_cusp = current_segment_len; + for (int i = potential_cusp_funcs.size() - 1; i >= 0; i--) { + auto & f = potential_cusp_funcs[i]; + double new_weight = + params.cusp_costmap_weight * (1.0 - len_to_cusp / cusp_half_length) + + params.costmap_weight * len_to_cusp / cusp_half_length; + if (std::abs(new_weight - params.cusp_costmap_weight) < + std::abs(f.second->getCostmapWeight() - params.cusp_costmap_weight)) + { + f.second->setCostmapWeight(new_weight); + } + len_to_cusp += f.first; + } + potential_cusp_funcs_len = 0; + potential_cusp_funcs.clear(); + len_since_cusp = 0; + } + + // add cost function + optimized[i] = true; + if (prelast_i != -1) { + double costmap_weight = params.costmap_weight; + if (len_since_cusp <= cusp_half_length) { + costmap_weight = + params.cusp_costmap_weight * (1.0 - len_since_cusp / cusp_half_length) + + params.costmap_weight * len_since_cusp / cusp_half_length; + } + SmootherCostFunction * cost_function = new SmootherCostFunction( + path[last_i].template block<2, 1>( + 0, + 0), + (last_was_cusp ? -1 : 1) * last_segment_len / current_segment_len, + last_is_reversing, + costmap, + costmap_interpolator, + params, + costmap_weight + ); + problem.AddResidualBlock( + cost_function->AutoDiff(), loss_function, + path_optim[last_i].data(), pt.data(), path_optim[prelast_i].data()); + + potential_cusp_funcs.emplace_back(current_segment_len, cost_function); + } + + // shift current to last and last to pre-last + last_was_cusp = is_cusp; + last_is_reversing = last_direction < 0; + prelast_i = last_i; + last_i = i; + len_since_cusp += current_segment_len; + last_segment_len = std::max(EPSILON, current_segment_len); + } + + int posesToOptimize = problem.NumParameterBlocks() - 2; // minus start and goal + if (params.keep_goal_orientation) { + posesToOptimize -= 1; // minus goal orientation holder + } + if (params.keep_start_orientation) { + posesToOptimize -= 1; // minus start orientation holder + } + if (posesToOptimize <= 0) { + return false; // nothing to optimize + } + // first two and last two points are constant (to keep start and end direction) + problem.SetParameterBlockConstant(path_optim.front().data()); + if (params.keep_start_orientation) { + problem.SetParameterBlockConstant(path_optim[1].data()); + } + if (params.keep_goal_orientation) { + problem.SetParameterBlockConstant(path_optim[path_optim.size() - 2].data()); + } + problem.SetParameterBlockConstant(path_optim.back().data()); + return true; + } + + /** + * @brief Populate optimized points to path, assigning orientations and upsampling poses using cubic bezier + * @param path_optim Path with optimized points + * @param optimized False for points skipped by downsampling + * @param start_dir Orientation of the first pose + * @param end_dir Orientation of the last pose + * @param params Smoother parameters + * @param path Output path with upsampled optimized points + */ + void upsampleAndPopulate( + const std::vector & path_optim, + const std::vector & optimized, + const Eigen::Vector2d & start_dir, + const Eigen::Vector2d & end_dir, + const SmootherParams & params, + std::vector & path) + { + // Populate path, assign orientations, interpolate skipped/upsampled poses + path.clear(); + if (params.path_upsampling_factor > 1) { + path.reserve(params.path_upsampling_factor * (path_optim.size() - 1) + 1); + } + int last_i = 0; + int prelast_i = -1; + Eigen::Vector2d prelast_dir; + for (int i = 1; i <= static_cast(path_optim.size()); i++) { + if (i == static_cast(path_optim.size()) || optimized[i]) { + if (prelast_i != -1) { + Eigen::Vector2d last_dir; + auto & prelast = path_optim[prelast_i]; + auto & last = path_optim[last_i]; + + // Compute orientation of last + if (i < static_cast(path_optim.size())) { + auto & current = path_optim[i]; + Eigen::Vector2d tangent_dir = tangentDir( + prelast.block<2, 1>(0, 0), + last.block<2, 1>(0, 0), + current.block<2, 1>(0, 0), + prelast[2] * last[2] < 0); + + last_dir = + tangent_dir.dot((current - last).block<2, 1>(0, 0) * last[2]) >= 0 ? + tangent_dir : + -tangent_dir; + last_dir.normalize(); + } else if (params.keep_goal_orientation) { + last_dir = end_dir; + } else { + last_dir = (last - prelast).block<2, 1>(0, 0) * last[2]; + last_dir.normalize(); + } + double last_angle = atan2(last_dir[1], last_dir[0]); + + // Interpolate poses between prelast and last + int interp_cnt = (last_i - prelast_i) * params.path_upsampling_factor - 1; + if (interp_cnt > 0) { + Eigen::Vector2d last_pt = last.block<2, 1>(0, 0); + Eigen::Vector2d prelast_pt = prelast.block<2, 1>(0, 0); + double dist = (last_pt - prelast_pt).norm(); + Eigen::Vector2d pt1 = prelast_pt + prelast_dir * dist * 0.4 * prelast[2]; + Eigen::Vector2d pt2 = last_pt - last_dir * dist * 0.4 * prelast[2]; + for (int j = 1; j <= interp_cnt; j++) { + double interp = j / static_cast(interp_cnt + 1); + Eigen::Vector2d pt = cubicBezier(prelast_pt, pt1, pt2, last_pt, interp); + path.emplace_back(pt[0], pt[1], 0.0); + } + } + path.emplace_back(last[0], last[1], last_angle); + + // Assign orientations to interpolated points + for (size_t j = path.size() - 1 - interp_cnt; j < path.size() - 1; j++) { + Eigen::Vector2d tangent_dir = tangentDir( + path[j - 1].block<2, 1>(0, 0), + path[j].block<2, 1>(0, 0), + path[j + 1].block<2, 1>(0, 0), + false); + tangent_dir = + tangent_dir.dot((path[j + 1] - path[j]).block<2, 1>(0, 0) * prelast[2]) >= 0 ? + tangent_dir : + -tangent_dir; + path[j][2] = atan2(tangent_dir[1], tangent_dir[0]); + } + + prelast_dir = last_dir; + } else { // start pose + auto & start = path_optim[0]; + Eigen::Vector2d dir = params.keep_start_orientation ? + start_dir : + ((path_optim[i] - start).block<2, 1>(0, 0) * start[2]).normalized(); + path.emplace_back(start[0], start[1], atan2(dir[1], dir[0])); + prelast_dir = dir; + } + prelast_i = last_i; + last_i = i; + } + } + } + + /* + Piecewise cubic bezier curve as defined by Adobe in Postscript + The two end points are pt0 and pt3 + Their associated control points are pt1 and pt2 + */ + static Eigen::Vector2d cubicBezier( + Eigen::Vector2d & pt0, Eigen::Vector2d & pt1, + Eigen::Vector2d & pt2, Eigen::Vector2d & pt3, double mu) + { + Eigen::Vector2d a, b, c, pt; + + c[0] = 3 * (pt1[0] - pt0[0]); + c[1] = 3 * (pt1[1] - pt0[1]); + b[0] = 3 * (pt2[0] - pt1[0]) - c[0]; + b[1] = 3 * (pt2[1] - pt1[1]) - c[1]; + a[0] = pt3[0] - pt0[0] - c[0] - b[0]; + a[1] = pt3[1] - pt0[1] - c[1] - b[1]; + + pt[0] = a[0] * mu * mu * mu + b[0] * mu * mu + c[0] * mu + pt0[0]; + pt[1] = a[1] * mu * mu * mu + b[1] * mu * mu + c[1] * mu + pt0[1]; + + return pt; + } + + bool debug_; + ceres::Solver::Options options_; + std::shared_ptr> costmap_grid_; +}; + +} // namespace nav2_constrained_smoother + +#endif // NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_ diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp new file mode 100644 index 0000000000..8272b2f2aa --- /dev/null +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp @@ -0,0 +1,252 @@ +// Copyright (c) 2021 RoboTech Vision +// 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 NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_ +#define NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "ceres/cubic_interpolation.h" +#include "Eigen/Core" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_constrained_smoother/options.hpp" +#include "nav2_constrained_smoother/utils.hpp" + +namespace nav2_constrained_smoother +{ + +/** + * @struct nav2_constrained_smoother::SmootherCostFunction + * @brief Cost function for path smoothing with multiple terms + * including curvature, smoothness, distance from original and obstacle avoidance. + */ +class SmootherCostFunction +{ +public: + /** + * @brief A constructor for nav2_constrained_smoother::SmootherCostFunction + * @param original_path Original position of the path node + * @param next_to_last_length_ratio Ratio of next path segment compared to previous. + * Negative if one of them represents reversing motion. + * @param reversing Whether the path segment after this node represents reversing motion. + * @param costmap A costmap to get values for collision and obstacle avoidance + * @param params Optimization weights and parameters + * @param costmap_weight Costmap cost weight. Can be params.costmap_weight or params.cusp_costmap_weight + */ + SmootherCostFunction( + const Eigen::Vector2d & original_pos, + double next_to_last_length_ratio, + bool reversing, + const nav2_costmap_2d::Costmap2D * costmap, + const std::shared_ptr>> & costmap_interpolator, + const SmootherParams & params, + double costmap_weight) + : original_pos_(original_pos), + next_to_last_length_ratio_(next_to_last_length_ratio), + reversing_(reversing), + params_(params), + costmap_weight_(costmap_weight), + costmap_origin_(costmap->getOriginX(), costmap->getOriginY()), + costmap_resolution_(costmap->getResolution()), + costmap_interpolator_(costmap_interpolator) + { + } + + ceres::CostFunction * AutoDiff() + { + return new ceres::AutoDiffCostFunction(this); + } + + void setCostmapWeight(double costmap_weight) + { + costmap_weight_ = costmap_weight; + } + + double getCostmapWeight() + { + return costmap_weight_; + } + + /** + * @brief Smoother cost function evaluation + * @param pt X, Y coords of current point + * @param pt_next X, Y coords of next point + * @param pt_prev X, Y coords of previous point + * @param pt_residual array of output residuals (smoothing, curvature, distance, cost) + * @return if successful in computing values + */ + template + bool operator()( + const T * const pt, const T * const pt_next, const T * const pt_prev, + T * pt_residual) const + { + Eigen::Map> xi(pt); + Eigen::Map> xi_next(pt_next); + Eigen::Map> xi_prev(pt_prev); + Eigen::Map> residual(pt_residual); + residual.setZero(); + + // compute cost + addSmoothingResidual(params_.smooth_weight, xi, xi_next, xi_prev, residual[0]); + addCurvatureResidual(params_.curvature_weight, xi, xi_next, xi_prev, residual[1]); + addDistanceResidual( + params_.distance_weight, xi, + original_pos_.template cast(), residual[2]); + addCostResidual(costmap_weight_, xi, xi_next, xi_prev, residual[3]); + + return true; + } + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt_next Point Xi+1 for calculating Xi's cost + * @param pt_prev Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + template + inline void addSmoothingResidual( + const double & weight, + const Eigen::Matrix & pt, + const Eigen::Matrix & pt_next, + const Eigen::Matrix & pt_prev, + T & r) const + { + Eigen::Matrix d_next = pt_next - pt; + Eigen::Matrix d_prev = pt - pt_prev; + Eigen::Matrix d_diff = next_to_last_length_ratio_ * d_next - d_prev; + r += (T)weight * d_diff.dot(d_diff); // objective function value + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt_next Point Xi+1 for calculating Xi's cost + * @param pt_prev Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + template + inline void addCurvatureResidual( + const double & weight, + const Eigen::Matrix & pt, + const Eigen::Matrix & pt_next, + const Eigen::Matrix & pt_prev, + T & r) const + { + Eigen::Matrix center = arcCenter( + pt_prev, pt, pt_next, + next_to_last_length_ratio_ < 0); + if (ceres::IsInfinite(center[0])) { + return; + } + T turning_rad = (pt - center).norm(); + T ki_minus_kmax = (T)1.0 / turning_rad - params_.max_curvature; + + if (ki_minus_kmax <= (T)EPSILON) { + return; + } + + r += (T)weight * ki_minus_kmax * ki_minus_kmax; // objective function value + } + + /** + * @brief Cost function derivative term for steering away changes in pose + * @param weight Weight to apply to function + * @param xi Point Xi for evaluation + * @param xi_original original point Xi for evaluation + * @param r Residual (cost) of term + */ + template + inline void addDistanceResidual( + const double & weight, + const Eigen::Matrix & xi, + const Eigen::Matrix & xi_original, + T & r) const + { + r += (T)weight * (xi - xi_original).squaredNorm(); // objective function value + } + + /** + * @brief Cost function term for steering away from costs + * @param weight Weight to apply to function + * @param value Point Xi's cost' + * @param params computed values to reduce overhead + * @param r Residual (cost) of term + */ + template + inline void addCostResidual( + const double & weight, + const Eigen::Matrix & pt, + const Eigen::Matrix & pt_next, + const Eigen::Matrix & pt_prev, + T & r) const + { + if (params_.cost_check_points.empty()) { + Eigen::Matrix interp_pos = + (pt - costmap_origin_.template cast()) / (T)costmap_resolution_; + T value; + costmap_interpolator_->Evaluate(interp_pos[1] - (T)0.5, interp_pos[0] - (T)0.5, &value); + r += (T)weight * value * value; // objective function value + } else { + Eigen::Matrix dir = tangentDir( + pt_prev, pt, pt_next, + next_to_last_length_ratio_ < 0); + dir.normalize(); + if (((pt_next - pt).dot(dir) < (T)0) != reversing_) { + dir = -dir; + } + Eigen::Matrix transform; + transform << dir[0], -dir[1], pt[0], + dir[1], dir[0], pt[1], + (T)0, (T)0, (T)1; + for (size_t i = 0; i < params_.cost_check_points.size(); i += 3) { + Eigen::Matrix ccpt((T)params_.cost_check_points[i], + (T)params_.cost_check_points[i + 1], (T)1); + auto ccpt_world = (transform * ccpt).template block<2, 1>(0, 0); + Eigen::Matrix interp_pos = (ccpt_world - costmap_origin_.template cast()) / + (T)costmap_resolution_; + T value; + costmap_interpolator_->Evaluate(interp_pos[1] - (T)0.5, interp_pos[0] - (T)0.5, &value); + + r += (T)weight * (T)params_.cost_check_points[i + 2] * value * value; + } + } + } + + const Eigen::Vector2d original_pos_; + double next_to_last_length_ratio_; + bool reversing_; + SmootherParams params_; + double costmap_weight_; + Eigen::Vector2d costmap_origin_; + double costmap_resolution_; + std::shared_ptr>> costmap_interpolator_; +}; + +} // namespace nav2_constrained_smoother + +#endif // NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_ diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp new file mode 100644 index 0000000000..9be4e88f0b --- /dev/null +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp @@ -0,0 +1,112 @@ +// Copyright (c) 2021 RoboTech Vision +// +// 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_CONSTRAINED_SMOOTHER__UTILS_HPP_ +#define NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_ + +#include +#include "Eigen/Core" + +#define EPSILON 0.0001 + +namespace nav2_constrained_smoother +{ + +/** + * @brief Center of an arc between three points + * @param pt_prev Starting point of the arc + * @param pt Mid point of the arc + * @param pt_next Last point of the arc + * @param is_cusp True if pt is a cusp point + * @result position of the center or Vector2(inf, inf) for straight lines and 180 deg turns + */ +template +inline Eigen::Matrix arcCenter( + Eigen::Matrix pt_prev, + Eigen::Matrix pt, + Eigen::Matrix pt_next, + bool is_cusp) +{ + Eigen::Matrix d1 = pt - pt_prev; + Eigen::Matrix d2 = pt_next - pt; + + if (is_cusp) { + d2 = -d2; + pt_next = pt + d2; + } + + T det = d1[0] * d2[1] - d1[1] * d2[0]; + if (ceres::abs(det) < (T)1e-4) { // straight line + return Eigen::Matrix( + (T)std::numeric_limits::infinity(), (T)std::numeric_limits::infinity()); + } + + // circle center is at the intersection of mirror axes of the segments: + // http://paulbourke.net/geometry/circlesphere/ + // line intersection: + // https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Intersection%20of%20two%20lines + Eigen::Matrix mid1 = (pt_prev + pt) / (T)2; + Eigen::Matrix mid2 = (pt + pt_next) / (T)2; + Eigen::Matrix n1(-d1[1], d1[0]); + Eigen::Matrix n2(-d2[1], d2[0]); + T det1 = (mid1[0] + n1[0]) * mid1[1] - (mid1[1] + n1[1]) * mid1[0]; + T det2 = (mid2[0] + n2[0]) * mid2[1] - (mid2[1] + n2[1]) * mid2[0]; + Eigen::Matrix center((det1 * n2[0] - det2 * n1[0]) / det, + (det1 * n2[1] - det2 * n1[1]) / det); + return center; +} + +/** + * @brief Direction of a line which contains pt and is tangential to arc + * between pt_prev, pt, pt_next + * @param pt_prev Starting point of the arc + * @param pt Mid point of the arc, lying on the tangential line + * @param pt_next Last point of the arc + * @param is_cusp True if pt is a cusp point + * @result Tangential line direction. + * Note: the sign of tangentDir is undefined here, should be assigned in post-process + * depending on movement direction. Also, for speed reasons, direction vector is not normalized. + */ +template +inline Eigen::Matrix tangentDir( + Eigen::Matrix pt_prev, + Eigen::Matrix pt, + Eigen::Matrix pt_next, + bool is_cusp) +{ + Eigen::Matrix center = arcCenter(pt_prev, pt, pt_next, is_cusp); + if (ceres::IsInfinite(center[0])) { // straight line + Eigen::Matrix d1 = pt - pt_prev; + Eigen::Matrix d2 = pt_next - pt; + + if (is_cusp) { + d2 = -d2; + pt_next = pt + d2; + } + + Eigen::Matrix result(pt_next[0] - pt_prev[0], pt_next[1] - pt_prev[1]); + if (result[0] == 0.0 && result[1] == 0.0) { // a very rare edge situation + return Eigen::Matrix(d1[1], -d1[0]); + } + return result; + } + + // tangent is prependicular to (pt - center) + // Note: not determining + or - direction here, this should be handled at the caller side + return Eigen::Matrix(center[1] - pt[1], pt[0] - center[0]); +} + +} // namespace nav2_constrained_smoother + +#endif // NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_ diff --git a/nav2_constrained_smoother/nav2_constrained_smoother.xml b/nav2_constrained_smoother/nav2_constrained_smoother.xml new file mode 100644 index 0000000000..050d69077d --- /dev/null +++ b/nav2_constrained_smoother/nav2_constrained_smoother.xml @@ -0,0 +1,7 @@ + + + + Increases smoothness and distance from obstacles of a path using Ceres solver optimization + + + diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml new file mode 100644 index 0000000000..b253b3b775 --- /dev/null +++ b/nav2_constrained_smoother/package.xml @@ -0,0 +1,31 @@ + + + + nav2_constrained_smoother + 1.1.0 + Ceres constrained smoother + Matej Vargovcik + Steve Macenski + Apache-2.0 + + ament_cmake + nav2_common + angles + rclcpp + nav2_util + nav2_msgs + nav2_costmap_2d + nav2_core + pluginlib + libceres-dev + + ament_lint_common + ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest + + + ament_cmake + + + diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp new file mode 100644 index 0000000000..4ffa4a21c3 --- /dev/null +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -0,0 +1,167 @@ +// Copyright (c) 2021 RoboTech Vision +// 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 + +#include "nav2_constrained_smoother/constrained_smoother.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" + +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" + +#include "tf2/utils.h" + +using nav2_util::declare_parameter_if_not_declared; +using nav2_util::geometry_utils::euclidean_distance; +using namespace nav2_costmap_2d; // NOLINT + +namespace nav2_constrained_smoother +{ + +void ConstrainedSmoother::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_sub, + std::shared_ptr) +{ + auto node = parent.lock(); + if (!node) { + throw std::runtime_error("Unable to lock node!"); + } + + costmap_sub_ = costmap_sub; + tf_ = tf; + plugin_name_ = name; + logger_ = node->get_logger(); + + smoother_ = std::make_unique(); + optimizer_params_.get(node.get(), name); + smoother_params_.get(node.get(), name); + smoother_->initialize(optimizer_params_); +} + +void ConstrainedSmoother::cleanup() +{ + RCLCPP_INFO( + logger_, + "Cleaning up smoother: %s of type" + " nav2_constrained_smoother::ConstrainedSmoother", + plugin_name_.c_str()); +} + +void ConstrainedSmoother::activate() +{ + RCLCPP_INFO( + logger_, + "Activating smoother: %s of type " + "nav2_constrained_smoother::ConstrainedSmoother", + plugin_name_.c_str()); +} + +void ConstrainedSmoother::deactivate() +{ + RCLCPP_INFO( + logger_, + "Deactivating smoother: %s of type " + "nav2_constrained_smoother::ConstrainedSmoother", + plugin_name_.c_str()); +} + +bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Duration & max_time) +{ + if (path.poses.size() < 2) { + return true; + } + + // populate smoother input with (x, y, forward/reverse dir) + std::vector path_world; + path_world.reserve(path.poses.size()); + // smoother keeps record of start/end orientations so that it + // can use them in the final path, preventing degradation of these (often important) values + Eigen::Vector2d start_dir; + Eigen::Vector2d end_dir; + for (size_t i = 0; i < path.poses.size(); i++) { + auto & pose = path.poses[i].pose; + double angle = tf2::getYaw(pose.orientation); + Eigen::Vector2d orientation(cos(angle), sin(angle)); + if (i == path.poses.size() - 1) { + // Note: `reversing` indicates the direction of the segment after the point and + // there is no segment after the last point. Most probably the value is irrelevant, but + // copying it from the last but one point, just to make it defined... + path_world.emplace_back(pose.position.x, pose.position.y, path_world.back()[2]); + end_dir = orientation; + } else { + auto & pos_next = path.poses[i + 1].pose.position; + Eigen::Vector2d mvmt(pos_next.x - pose.position.x, pos_next.y - pose.position.y); + // robot is considered reversing when angle between its orientation and movement direction + // is more than 90 degrees (i.e. dot product is less than 0) + bool reversing = smoother_params_.reversing_enabled && orientation.dot(mvmt) < 0; + // we transform boolean value of "reversing" into sign of movement direction (+1 or -1) + // to simplify further computations + path_world.emplace_back(pose.position.x, pose.position.y, reversing ? -1 : 1); + if (i == 0) { + start_dir = orientation; + } else if (i == 1 && !smoother_params_.keep_start_orientation) { + // overwrite start forward/reverse when orientation was set to be ignored + // note: start_dir is overwritten inside Smoother::upsampleAndPopulate() method + path_world[0][2] = path_world.back()[2]; + } + } + } + + smoother_params_.max_time = max_time.seconds(); + + // Smooth plan + auto costmap = costmap_sub_->getCostmap(); + if (!smoother_->smooth(path_world, start_dir, end_dir, costmap.get(), smoother_params_)) { + RCLCPP_WARN( + logger_, + "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", + plugin_name_.c_str()); + throw new nav2_core::PlannerException( + "Failed to smooth plan, Ceres could not find a usable solution."); + } + + // populate final path + geometry_msgs::msg::PoseStamped pose; + pose.header = path.poses.front().header; + path.poses.clear(); + path.poses.reserve(path_world.size()); + for (auto & pw : path_world) { + pose.pose.position.x = pw[0]; + pose.pose.position.y = pw[1]; + pose.pose.orientation.z = sin(pw[2] / 2); + pose.pose.orientation.w = cos(pw[2] / 2); + + path.poses.push_back(pose); + } + + return true; +} + +} // namespace nav2_constrained_smoother + +// Register this smoother as a nav2_core plugin +PLUGINLIB_EXPORT_CLASS( + nav2_constrained_smoother::ConstrainedSmoother, + nav2_core::Smoother) diff --git a/nav2_constrained_smoother/test/CMakeLists.txt b/nav2_constrained_smoother/test/CMakeLists.txt new file mode 100644 index 0000000000..cf4ecd1967 --- /dev/null +++ b/nav2_constrained_smoother/test/CMakeLists.txt @@ -0,0 +1,20 @@ +ament_add_gtest(test_constrained_smoother + test_constrained_smoother.cpp +) + +target_link_libraries(test_constrained_smoother + ${library_name} +) +ament_target_dependencies(test_constrained_smoother + ${dependencies} +) + +ament_add_gtest(test_smoother_cost_function + test_smoother_cost_function.cpp +) +target_link_libraries(test_smoother_cost_function + ${library_name} +) +ament_target_dependencies(test_smoother_cost_function + ${dependencies} +) \ No newline at end of file diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp new file mode 100644 index 0000000000..f50d2fb780 --- /dev/null +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -0,0 +1,1133 @@ +// Copyright (c) 2021 RoboTech Vision +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/create_timer_ros.h" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/costmap_2d_publisher.hpp" +#include "angles/angles.h" + +#include "nav2_constrained_smoother/constrained_smoother.hpp" + +#include "geometry_msgs/msg/pose_array.hpp" + +class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber +{ +public: + DummyCostmapSubscriber( + nav2_util::LifecycleNode::SharedPtr node, + const std::string & topic_name) + : CostmapSubscriber(node, topic_name) + { + auto costmap = std::make_shared(); + costmap->metadata.size_x = 100; + costmap->metadata.size_y = 100; + costmap->metadata.resolution = 0.1; + costmap->metadata.origin.position.x = -5.0; + costmap->metadata.origin.position.y = -5.0; + + costmap->data.resize(costmap->metadata.size_x * costmap->metadata.size_y, 0); + + // create an obstacle in rect (1.0, -1.0) -> (3.0, -2.0) + // with inflation of radius 2.0 + double cost_scaling_factor = 1.6; + double inscribed_radius = 0.3; + for (int i = 10; i < 60; ++i) { + for (int j = 40; j < 100; ++j) { + int dist_x = std::max(0, std::max(60 - j, j - 80)); + int dist_y = std::max(0, std::max(30 - i, i - 40)); + double dist = sqrt(dist_x * dist_x + dist_y * dist_y); + unsigned char cost; + if (dist == 0) { + cost = nav2_costmap_2d::LETHAL_OBSTACLE; + } else if (dist < inscribed_radius) { + cost = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; + } else { + double factor = + exp( + -1.0 * cost_scaling_factor * (dist * costmap->metadata.resolution - inscribed_radius)); + cost = + static_cast((nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1) * factor); + } + costmap->data[i * costmap->metadata.size_x + j] = cost; + } + } + + setCostmap(costmap); + } + + void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) + { + costmap_msg_ = msg; + costmap_received_ = true; + } +}; + +geometry_msgs::msg::Point pointMsg(double x, double y) +{ + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + return point; +} + +class SmootherTest : public ::testing::Test +{ +protected: + SmootherTest() {SetUp();} + ~SmootherTest() {} + + void SetUp() + { + node_lifecycle_ = + std::make_shared( + "ConstrainedSmootherTestNode", rclcpp::NodeOptions()); + + tf_buffer_ = std::make_shared(node_lifecycle_->get_clock()); + auto timer_interface = std::make_shared( + node_lifecycle_->get_node_base_interface(), + node_lifecycle_->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + + costmap_sub_ = + std::make_shared( + node_lifecycle_, "costmap_topic"); + + path_poses_pub_ = node_lifecycle_->create_publisher( + "/plan_poses_optimized", 100); + path_poses_pub_cmp_ = node_lifecycle_->create_publisher( + "/plan_poses_optimized_cmp", 100); + path_poses_pub_orig_ = node_lifecycle_->create_publisher( + "/plan_poses_original", 100); + costmap_pub_ = std::make_shared( + node_lifecycle_, + costmap_sub_->getCostmap().get(), "map", "/costmap", true); + + node_lifecycle_->configure(); + node_lifecycle_->activate(); + path_poses_pub_->on_activate(); + path_poses_pub_cmp_->on_activate(); + path_poses_pub_orig_->on_activate(); + costmap_pub_->on_activate(); + + + smoother_ = std::make_shared(); + + smoother_->configure( + node_lifecycle_, "SmoothPath", tf_buffer_, costmap_sub_, + std::shared_ptr()); + smoother_->activate(); + + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.minimum_turning_radius", 0.4)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 30.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_dist", 0.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.cusp_zone_length", -1.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost_cusp_multiplier", 1.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_downsampling_factor", 1)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_upsampling_factor", 1)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.reversing_enabled", true)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.keep_start_orientation", true)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.keep_goal_orientation", true)); + node_lifecycle_->set_parameter( + rclcpp::Parameter("SmoothPath.optimizer.linear_solver_type", "SPARSE_NORMAL_CHOLESKY")); + node_lifecycle_->set_parameter( + rclcpp::Parameter( + "SmoothPath.cost_check_points", + std::vector())); + reloadParams(); + } + + void TearDown() override + { + smoother_->deactivate(); + smoother_->cleanup(); + path_poses_pub_->on_deactivate(); + path_poses_pub_cmp_->on_deactivate(); + path_poses_pub_orig_->on_deactivate(); + costmap_pub_->on_deactivate(); + node_lifecycle_->deactivate(); + } + + void reloadParams() + { + smoother_->deactivate(); + smoother_->cleanup(); + smoother_->configure( + node_lifecycle_, "SmoothPath", tf_buffer_, costmap_sub_, + std::shared_ptr()); + smoother_->activate(); + } + + bool smoothPath( + const std::vector & input, std::vector & output, + bool publish = false, bool cmp = false) + { + nav_msgs::msg::Path path; + path.poses.reserve(input.size()); + for (auto & xya : input) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = xya.x(); + pose.pose.position.y = xya.y(); + pose.pose.position.z = 0; + pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(xya.z()); + path.poses.push_back(pose); + } + + if (publish && !path.poses.empty()) { + geometry_msgs::msg::PoseArray poses; + poses.header.frame_id = "map"; + poses.header.stamp = node_lifecycle_->get_clock()->now(); + for (auto & p : path.poses) { + poses.poses.push_back(p.pose); + } + path_poses_pub_orig_->publish(poses); + costmap_pub_->publishCostmap(); + } + + bool result = smoother_->smooth(path, rclcpp::Duration::from_seconds(10.0)); + + if (publish && !path.poses.empty()) { + geometry_msgs::msg::PoseArray poses; + poses.header.frame_id = "map"; + poses.header.stamp = node_lifecycle_->get_clock()->now(); + for (auto & p : path.poses) { + poses.poses.push_back(p.pose); + } + auto & pub = cmp ? path_poses_pub_cmp_ : path_poses_pub_; + pub->publish(poses); + } + + output.clear(); + output.reserve(path.poses.size()); + for (auto & pose : path.poses) { + Eigen::Vector3d xya; + xya.x() = pose.pose.position.x; + xya.y() = pose.pose.position.y; + tf2::Quaternion q; + tf2::fromMsg(pose.pose.orientation, q); + xya.z() = q.getAngle(); + output.push_back(xya); + } + return result; + } + + typedef std::function QualityCriterion3; + typedef std::function QualityCriterion2; + typedef std::function QualityCriterion1; + /** + * @brief Path improvement assessment method + * @param input Smoother input path + * @param output Smoother output path + * @param criterion Criterion of path quality. Total path quality = sqrt(sum(criterion(data[i])^2)/count(data)) + * @return Percentage of improvement (relative to input path quality) + */ + double assessPathImprovement( + const std::vector & input, + const std::vector & output, + const QualityCriterion3 & criterion, + const QualityCriterion3 * criterion_out = nullptr) + { + if (!criterion_out) { + criterion_out = &criterion; + } + double total_input_crit = 0.0; + for (size_t i = 1; i < input.size() - 1; i++) { + double input_crit = criterion(i, input[i - 1], input[i], input[i + 1]); + total_input_crit += input_crit * input_crit; + } + total_input_crit = sqrt(total_input_crit / (input.size() - 2)); + + double total_output_crit = 0.0; + for (size_t i = 1; i < output.size() - 1; i++) { + double output_crit = (*criterion_out)(i, output[i - 1], output[i], output[i + 1]); + total_output_crit += output_crit * output_crit; + } + total_output_crit = sqrt(total_output_crit / (input.size() - 2)); + + return (1.0 - total_output_crit / total_input_crit) * 100; + } + + /** + * @brief Path improvement assessment method + * @param input Smoother input path + * @param output Smoother output path + * @param criterion Criterion of path quality. Total path quality = sqrt(sum(criterion(data[i])^2)/count(data)) + * @return Percentage of improvement (relative to input path quality) + */ + double assessPathImprovement( + const std::vector & input, + const std::vector & output, + const QualityCriterion2 & criterion, + const QualityCriterion2 * criterion_out = nullptr) + { + if (!criterion_out) { + criterion_out = &criterion; + } + double total_input_crit = 0.0; + for (size_t i = 1; i < input.size(); i++) { + double input_crit = criterion(i, input[i - 1], input[i]); + total_input_crit += input_crit * input_crit; + } + total_input_crit = sqrt(total_input_crit / (input.size() - 1)); + + double total_output_crit = 0.0; + for (size_t i = 1; i < output.size(); i++) { + double output_crit = (*criterion_out)(i, output[i - 1], output[i]); + total_output_crit += output_crit * output_crit; + } + total_output_crit = sqrt(total_output_crit / (output.size() - 1)); + + return (1.0 - total_output_crit / total_input_crit) * 100; + } + + /** + * @brief Path improvement assessment method + * @param input Smoother input path + * @param output Smoother output path + * @param criterion Criterion of path quality. Total path quality = sqrt(sum(criterion(data[i])^2)/count(data)) + * @return Percentage of improvement (relative to input path quality) + */ + double assessPathImprovement( + const std::vector & input, + const std::vector & output, + const QualityCriterion1 & criterion, + const QualityCriterion1 * criterion_out = nullptr) + { + if (!criterion_out) { + criterion_out = &criterion; + } + double total_input_crit = 0.0; + for (size_t i = 0; i < input.size(); i++) { + double input_crit = criterion(i, input[i]); + total_input_crit += input_crit * input_crit; + } + total_input_crit = sqrt(total_input_crit / input.size()); + + double total_output_crit = 0.0; + for (size_t i = 0; i < output.size(); i++) { + double output_crit = (*criterion_out)(i, output[i]); + total_output_crit += output_crit * output_crit; + } + total_output_crit = sqrt(total_output_crit / output.size()); + + return (1.0 - total_output_crit / total_input_crit) * 100; + } + + /** + * @brief Worst pose improvement assessment method + * @param input Smoother input path + * @param output Smoother output path + * @param criterion Criterion of path quality. Total path quality = max(criterion(data[i])) + * @return Percentage of improvement (relative to input path quality) + */ + double assessWorstPoseImprovement( + const std::vector & input, + const std::vector & output, + const QualityCriterion1 & criterion) + { + double max_input_crit = 0.0; + for (size_t i = 0; i < input.size(); i++) { + double input_crit = criterion(i, input[i]); + max_input_crit = std::max(max_input_crit, input_crit); + } + + double max_output_crit = 0.0; + for (size_t i = 0; i < output.size(); i++) { + double output_crit = criterion(i, output[i]); + max_output_crit = std::max(max_output_crit, output_crit); + } + + return (1.0 - max_output_crit / max_input_crit) * 100; + } + + std::vector zigZaggedPath( + const std::vector & input, + double offset) + { + auto output = input; + for (size_t i = 1; i < input.size() - 1; i++) { + // add offset prependicular to path + Eigen::Vector2d direction = + (input[i + 1].block<2, 1>(0, 0) - input[i - 1].block<2, 1>(0, 0)).normalized(); + output[i].block<2, 1>( + 0, + 0) += Eigen::Vector2d(direction[1], -direction[0]) * offset * (i % 2 == 0 ? 1.0 : -1.0); + } + return output; + } + + std::shared_ptr node_lifecycle_; + std::shared_ptr smoother_; + std::shared_ptr tf_buffer_; + std::shared_ptr costmap_sub_; + std::shared_ptr footprint_sub_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + path_poses_pub_orig_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr path_poses_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + path_poses_pub_cmp_; + std::shared_ptr costmap_pub_; + + int cusp_i_ = -1; + QualityCriterion3 mvmt_smoothness_criterion_ = + [this](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p, + const Eigen::Vector3d & next_p) { + Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0); + Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0); + if (i == cusp_i_) { + next_mvmt = -next_mvmt; + } + return (next_mvmt - prev_mvmt).norm(); + }; +}; + +TEST_F(SmootherTest, testingSmoothness) +{ + // set w_curve to 0.0 so that the whole job is upon w_smooth + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 0.0)); + reloadParams(); + + std::vector sharp_turn_90 = + {{0, 0, 0}, + {0.1, 0, 0}, + {0.2, 0, 0}, + {0.3, 0, M_PI / 4}, + {0.3, 0.1, M_PI / 2}, + {0.3, 0.2, M_PI / 2}, + {0.3, 0.3, M_PI / 2} + }; + + std::vector smoothed_path; + EXPECT_TRUE(smoothPath(sharp_turn_90, smoothed_path)); + + double mvmt_smoothness_improvement = + assessPathImprovement(sharp_turn_90, smoothed_path, mvmt_smoothness_criterion_); + EXPECT_GT(mvmt_smoothness_improvement, 0.0); + EXPECT_NEAR(mvmt_smoothness_improvement, 55.3, 1.0); + + auto orientation_smoothness_criterion = + [](int, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p) { + return angles::normalize_angle(p.z() - prev_p.z()); + }; + double orientation_smoothness_improvement = + assessPathImprovement(sharp_turn_90, smoothed_path, orientation_smoothness_criterion); + EXPECT_GT(orientation_smoothness_improvement, 0.0); + EXPECT_NEAR(orientation_smoothness_improvement, 38.7, 1.0); + + // path with a cusp + std::vector sharp_turn_90_then_reverse = + {{0, 0, 0}, + {0.1, 0, 0}, + {0.2, 0, 0}, + {0.3, 0, 0}, + {0.4, 0, 0}, + {0.5, 0, 0}, + {0.6, 0, M_PI / 4}, + {0.6, -0.1, M_PI / 2}, + {0.6, -0.2, M_PI / 2}, + {0.6, -0.3, M_PI / 2}, + {0.6, -0.4, M_PI / 2}, + {0.6, -0.5, M_PI / 2}, + {0.6, -0.6, M_PI / 2} + }; + cusp_i_ = 6; + + EXPECT_TRUE(smoothPath(sharp_turn_90_then_reverse, smoothed_path)); + + mvmt_smoothness_improvement = + assessPathImprovement(sharp_turn_90_then_reverse, smoothed_path, mvmt_smoothness_criterion_); + EXPECT_GT(mvmt_smoothness_improvement, 0.0); + EXPECT_NEAR(mvmt_smoothness_improvement, 37.2, 1.0); + + orientation_smoothness_improvement = + assessPathImprovement( + sharp_turn_90_then_reverse, smoothed_path, + orientation_smoothness_criterion); + EXPECT_GT(orientation_smoothness_improvement, 0.0); + EXPECT_NEAR(orientation_smoothness_improvement, 28.5, 1.0); + + SUCCEED(); +} + +TEST_F(SmootherTest, testingAnchoringToOriginalPath) +{ + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 30.0)); + // set w_curve to 0.0, we don't care about turning radius in this test... + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 0.0)); + // first keep w_dist at 0.0 to generate an unanchored smoothed path + reloadParams(); + + std::vector sharp_turn_90 = + {{0, 0, 0}, + {0.1, 0, 0}, + {0.2, 0, 0}, + {0.3, 0, M_PI / 4}, + {0.3, 0.1, M_PI / 2}, + {0.3, 0.2, M_PI / 2}, + {0.3, 0.3, M_PI / 2} + }; + + std::vector smoothed_path; + EXPECT_TRUE(smoothPath(sharp_turn_90, smoothed_path)); + + // then update w_dist and compare the results + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_dist", 30.0)); + reloadParams(); + + std::vector smoothed_path_anchored; + EXPECT_TRUE(smoothPath(sharp_turn_90, smoothed_path_anchored)); + + auto origin_similarity_criterion = + [&sharp_turn_90](int i, const Eigen::Vector3d & p) { + return (p.block<2, 1>(0, 0) - sharp_turn_90[i].block<2, 1>(0, 0)).norm(); + }; + double origin_similarity_improvement = + assessPathImprovement(smoothed_path, smoothed_path_anchored, origin_similarity_criterion); + EXPECT_GT(origin_similarity_improvement, 0.0); + EXPECT_NEAR(origin_similarity_improvement, 45.5, 1.0); + + SUCCEED(); +} + +TEST_F(SmootherTest, testingMaxCurvature) +{ + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 30.0)); + // set w_smooth to a small value so that the whole job is upon w_curve + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 0.3)); + // let's give the smoother more time since w_smooth is so small + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.optimizer.max_iterations", 500)); + reloadParams(); + + // smoother should increase radius from infeasible 0.3 to feasible 0.4 + std::vector radius_0_3_turn_90 = + {{0, 0, 0}, + {0.1, 0, 0}, + {0.2, 0, 0}, + {0.2 + 0.3 * sin(M_PI / 12), 0.3 * (1 - cos(M_PI / 12)), 0}, + {0.2 + 0.3 * sin(M_PI * 2 / 12), 0.3 * (1 - cos(M_PI * 2 / 12)), 0}, + {0.2 + 0.3 * sin(M_PI * 3 / 12), 0.3 * (1 - cos(M_PI * 3 / 12)), 0}, + {0.2 + 0.3 * sin(M_PI * 4 / 12), 0.3 * (1 - cos(M_PI * 4 / 12)), 0}, + {0.2 + 0.3 * sin(M_PI * 5 / 12), 0.3 * (1 - cos(M_PI * 5 / 12)), 0}, + {0.5, 0.3, M_PI / 2}, + {0.5, 0.4, M_PI / 2}, + {0.5, 0.5, M_PI / 2} + }; + + std::vector smoothed_path; + EXPECT_TRUE(smoothPath(radius_0_3_turn_90, smoothed_path)); + + // we don't expect result to be smoother than original as w_smooth is too low + // but let's check for large discontinuities using a well chosen upper bound + auto upper_bound = zigZaggedPath(radius_0_3_turn_90, 0.01); + EXPECT_GT(assessPathImprovement(upper_bound, smoothed_path, mvmt_smoothness_criterion_), 0.0); + + // smoothed path points should form a circle with radius 0.4 + for (size_t i = 1; i < smoothed_path.size() - 1; i++) { + auto & p = smoothed_path[i]; + double r = (p.block<2, 1>(0, 0) - Eigen::Vector2d(0.1, 0.4)).norm(); + EXPECT_NEAR(r, 0.4, 0.01); + } + + // path with a cusp + // smoother should increase radius from infeasible 0.3 to feasible 0.4 + std::vector radius_0_3_turn_90_then_reverse_turn_90 = + {{0, 0, 0}, + {0.1, 0, 0}, + {0.2, 0, 0}, + {0.2 + 0.3 * sin(M_PI / 12), 0.3 * (1 - cos(M_PI / 12)), M_PI / 12}, + {0.2 + 0.3 * sin(M_PI * 2 / 12), 0.3 * (1 - cos(M_PI * 2 / 12)), M_PI *2 / 12}, + {0.2 + 0.3 * sin(M_PI * 3 / 12), 0.3 * (1 - cos(M_PI * 3 / 12)), M_PI *3 / 12}, + {0.2 + 0.3 * sin(M_PI * 4 / 12), 0.3 * (1 - cos(M_PI * 4 / 12)), M_PI *4 / 12}, + {0.2 + 0.3 * sin(M_PI * 5 / 12), 0.3 * (1 - cos(M_PI * 5 / 12)), M_PI *5 / 12}, + {0.5, 0.3, M_PI / 2}, + {0.8 - 0.3 * sin(M_PI * 5 / 12), 0.3 * (1 - cos(M_PI * 5 / 12)), M_PI *7 / 12}, + {0.8 - 0.3 * sin(M_PI * 4 / 12), 0.3 * (1 - cos(M_PI * 4 / 12)), M_PI *8 / 12}, + {0.8 - 0.3 * sin(M_PI * 3 / 12), 0.3 * (1 - cos(M_PI * 3 / 12)), M_PI *9 / 12}, + {0.8 - 0.3 * sin(M_PI * 2 / 12), 0.3 * (1 - cos(M_PI * 2 / 12)), M_PI *10 / 12}, + {0.8 - 0.3 * sin(M_PI / 12), 0.3 * (1 - cos(M_PI / 12)), M_PI *11 / 12}, + {0.8, 0, M_PI}, + {0.9, 0, M_PI}, + {1.0, 0, M_PI} + }; + + EXPECT_TRUE(smoothPath(radius_0_3_turn_90_then_reverse_turn_90, smoothed_path)); + + // we don't expect result to be smoother than original as w_smooth is too low + // but let's check for large discontinuities using a well chosen upper bound + cusp_i_ = 8; + upper_bound = zigZaggedPath(radius_0_3_turn_90_then_reverse_turn_90, 0.01); + EXPECT_GT(assessPathImprovement(upper_bound, smoothed_path, mvmt_smoothness_criterion_), 0.0); + + // smoothed path points should form a circle with radius 0.4 + // for both forward and reverse movements + for (size_t i = 1; i < smoothed_path.size() - 1; i++) { + auto & p = smoothed_path[i]; + double r = (p.block<2, 1>(0, 0) - Eigen::Vector2d(i <= 8 ? 0.1 : 0.9, 0.4)).norm(); + EXPECT_NEAR(r, 0.4, 0.01); + } + + SUCCEED(); +} + +TEST_F(SmootherTest, testingObstacleAvoidance) +{ + auto costmap = costmap_sub_->getCostmap(); + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap); + nav2_costmap_2d::Footprint footprint; + + auto cost_avoidance_criterion = + [&collision_checker, &footprint](int, const Eigen::Vector3d & p) { + return collision_checker.footprintCostAtPose(p[0], p[1], p[2], footprint); + }; + + // a symmetric footprint (diff-drive with 4 actuated wheels) + footprint.push_back(pointMsg(0.4, 0.25)); + footprint.push_back(pointMsg(-0.4, 0.25)); + footprint.push_back(pointMsg(-0.4, -0.25)); + footprint.push_back(pointMsg(0.4, -0.25)); + + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); + reloadParams(); + + std::vector straight_near_obstacle = + {{0.05, 0.05, 0}, + {0.45, 0.05, 0}, + {0.85, 0.05, 0}, + {1.25, 0.05, 0}, + {1.65, 0.05, 0}, + {2.05, 0.05, 0}, + {2.45, 0.05, 0}, + {2.85, 0.05, 0}, + {3.25, 0.05, 0}, + {3.65, 0.05, 0}, + {4.05, 0.05, 0} + }; + + std::vector smoothed_path; + EXPECT_TRUE(smoothPath(straight_near_obstacle, smoothed_path)); + + // we don't expect result to be smoother than original as original straight line was 100% smooth + // but let's check for large discontinuities using a well chosen upper bound + auto upper_bound = zigZaggedPath(straight_near_obstacle, 0.01); + EXPECT_GT(assessPathImprovement(upper_bound, smoothed_path, mvmt_smoothness_criterion_), 0.0); + + double cost_avoidance_improvement = assessPathImprovement( + straight_near_obstacle, smoothed_path, + cost_avoidance_criterion); + EXPECT_GT(cost_avoidance_improvement, 0.0); + EXPECT_NEAR(cost_avoidance_improvement, 12.9, 1.0); +} + +TEST_F(SmootherTest, testingObstacleAvoidanceNearCusps) +{ + auto costmap = costmap_sub_->getCostmap(); + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap); + nav2_costmap_2d::Footprint footprint; + + auto cost_avoidance_criterion = + [&collision_checker, &footprint](int, const Eigen::Vector3d & p) { + return collision_checker.footprintCostAtPose(p[0], p[1], p[2], footprint); + }; + + // path with a cusp + std::vector cusp_near_obstacle = + {{0.05, 0.05, 0}, + {0.15, 0.05, 0}, + {0.25, 0.05, 0}, + {0.35, 0.05, 0}, + {0.45, 0.05, 0}, + {0.55, 0.05, 0}, + {0.65, 0.05, 0}, + {0.75, 0.05, 0}, + {0.85, 0.05, 0}, + {0.95, 0.05, 0}, + {1.05, 0.05, 0}, + {1.15, 0.05, 0}, + {1.25, 0.05, 0}, + {1.25 + 0.4 * sin(M_PI / 12), 0.4 * (1 - cos(M_PI / 12)) + 0.05, M_PI / 12}, + {1.25 + 0.4 * sin(M_PI * 2 / 12), 0.4 * (1 - cos(M_PI * 2 / 12)) + 0.05, M_PI *2 / 12}, + {1.25 + 0.4 * sin(M_PI * 3 / 12), 0.4 * (1 - cos(M_PI * 3 / 12)) + 0.05, M_PI *3 / 12}, + {1.25 + 0.4 * sin(M_PI * 4 / 12), 0.4 * (1 - cos(M_PI * 4 / 12)) + 0.05, M_PI *4 / 12}, + {1.25 + 0.4 * sin(M_PI * 5 / 12), 0.4 * (1 - cos(M_PI * 5 / 12)) + 0.05, M_PI *5 / 12}, + {1.65, 0.45, M_PI / 2}, + {2.05 - 0.4 * sin(M_PI * 5 / 12), 0.4 * (1 - cos(M_PI * 5 / 12)) + 0.05, M_PI *7 / 12}, + {2.05 - 0.4 * sin(M_PI * 4 / 12), 0.4 * (1 - cos(M_PI * 4 / 12)) + 0.05, M_PI *8 / 12}, + {2.05 - 0.4 * sin(M_PI * 3 / 12), 0.4 * (1 - cos(M_PI * 3 / 12)) + 0.05, M_PI *9 / 12}, + {2.05 - 0.4 * sin(M_PI * 2 / 12), 0.4 * (1 - cos(M_PI * 2 / 12)) + 0.05, M_PI *10 / 12}, + {2.05 - 0.4 * sin(M_PI / 12), 0.4 * (1 - cos(M_PI / 12)) + 0.05, M_PI *11 / 12}, + {2.05, 0.05, M_PI}, + {2.15, 0.05, M_PI}, + {2.25, 0.05, M_PI}, + {2.35, 0.05, M_PI}, + {2.45, 0.05, M_PI}, + {2.55, 0.05, M_PI}, + {2.65, 0.05, M_PI}, + {2.75, 0.05, M_PI}, + {2.85, 0.05, M_PI}, + {2.95, 0.05, M_PI}, + {3.05, 0.05, M_PI}, + {3.15, 0.05, M_PI}, + {3.25, 0.05, M_PI}, + {3.35, 0.05, M_PI}, + {3.45, 0.05, M_PI}, + {3.55, 0.05, M_PI}, + {3.65, 0.05, M_PI}, + {3.75, 0.05, M_PI}, + {3.85, 0.05, M_PI}, + {3.95, 0.05, M_PI}, + {4.05, 0.05, M_PI} + }; + cusp_i_ = 18; + + // we don't expect result to be smoother than original + // but let's check for large discontinuities using a well chosen upper bound + auto upper_bound = zigZaggedPath(cusp_near_obstacle, 0.01); + + ///////////////////////////////////////////////////// + // testing option to pay extra attention near cusps + + // extra attention near cusps option is more significant with a long footprint + footprint.clear(); + footprint.push_back(pointMsg(0.4, 0.2)); + footprint.push_back(pointMsg(-0.4, 0.2)); + footprint.push_back(pointMsg(-0.4, -0.2)); + footprint.push_back(pointMsg(0.4, -0.2)); + + // first smooth with homogeneous w_cost to compare + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); + // higher w_curve significantly decreases convergence speed here + // path feasibility can be restored by subsequent resmoothing with higher w_curve + // TODO(afrixs): tune ceres optimizer to "converge" faster, + // see http://ceres-solver.org/nnls_solving.html + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 1.0)); + // let's have more iterations so that the improvement is more significant + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.optimizer.max_iterations", 500)); + reloadParams(); + + std::vector smoothed_path; + EXPECT_TRUE(smoothPath(cusp_near_obstacle, smoothed_path, true, true)); + EXPECT_GT(assessPathImprovement(upper_bound, smoothed_path, mvmt_smoothness_criterion_), 0.0); + double cost_avoidance_improvement_simple = assessPathImprovement( + cusp_near_obstacle, + smoothed_path, + cost_avoidance_criterion); + EXPECT_GT(cost_avoidance_improvement_simple, 0.0); + EXPECT_NEAR(cost_avoidance_improvement_simple, 42.6, 1.0); + double worst_cost_improvement_simple = assessWorstPoseImprovement( + cusp_near_obstacle, + smoothed_path, + cost_avoidance_criterion); + RCLCPP_INFO( + rclcpp::get_logger("ceres_smoother"), "Cost avoidance improvement (cusp, simple): %lf, %lf", + cost_avoidance_improvement_simple, worst_cost_improvement_simple); + EXPECT_GE(worst_cost_improvement_simple, 0.0); + + + // then update parameters so that robot is not so afraid of obstacles + // during simple movement but pays extra attention during rotations near cusps + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.0052)); + node_lifecycle_->set_parameter( + rclcpp::Parameter("SmoothPath.w_cost_cusp_multiplier", 0.027 / 0.0052)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.cusp_zone_length", 2.5)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.optimizer.fn_tol", 1e-15)); + reloadParams(); + + std::vector smoothed_path_ecc; + EXPECT_TRUE(smoothPath(cusp_near_obstacle, smoothed_path_ecc, true, false)); + EXPECT_GT(assessPathImprovement(upper_bound, smoothed_path_ecc, mvmt_smoothness_criterion_), 0.0); + double cost_avoidance_improvement_extra_careful_cusp = assessPathImprovement( + cusp_near_obstacle, + smoothed_path_ecc, + cost_avoidance_criterion); + EXPECT_GT(cost_avoidance_improvement_extra_careful_cusp, 0.0); + EXPECT_NEAR(cost_avoidance_improvement_extra_careful_cusp, 44.2, 1.0); + double worst_cost_improvement_extra_careful_cusp = assessWorstPoseImprovement( + cusp_near_obstacle, + smoothed_path_ecc, + cost_avoidance_criterion); + RCLCPP_INFO( + rclcpp::get_logger("ceres_smoother"), "Cost avoidance improvement (cusp, ecc): %lf, %lf", + cost_avoidance_improvement_extra_careful_cusp, worst_cost_improvement_extra_careful_cusp); + EXPECT_GE(worst_cost_improvement_extra_careful_cusp, 0.0); + EXPECT_GE(worst_cost_improvement_extra_careful_cusp, worst_cost_improvement_simple); + EXPECT_GT(cost_avoidance_improvement_extra_careful_cusp, cost_avoidance_improvement_simple); + + // although extra careful cusp optimization avoids cost better than simple one, + // overall the path doesn't need to deflect so much from original, since w_cost is smaller + // and thus the obstacles are avoided mostly in dangerous zones around cusps + auto origin_similarity_criterion = + [&cusp_near_obstacle](int i, const Eigen::Vector3d & p) { + return (p.block<2, 1>(0, 0) - cusp_near_obstacle[i].block<2, 1>(0, 0)).norm(); + }; + double origin_similarity_improvement = + assessPathImprovement(smoothed_path, smoothed_path_ecc, origin_similarity_criterion); + RCLCPP_INFO( + rclcpp::get_logger( + "ceres_smoother"), "Original similarity improvement (cusp, ecc vs. simple): %lf", + origin_similarity_improvement); + EXPECT_GT(origin_similarity_improvement, 0.0); + EXPECT_NEAR(origin_similarity_improvement, 0.43, 0.02); + + + ///////////////////////////////////////////////////// + // testing asymmetric footprint options + + // (diff-drive with 2 actuated wheels and 2 caster wheels) + footprint.clear(); + footprint.push_back(pointMsg(0.15, 0.2)); + footprint.push_back(pointMsg(-0.65, 0.2)); + footprint.push_back(pointMsg(-0.65, -0.2)); + footprint.push_back(pointMsg(0.15, -0.2)); + + // reset parameters back to homogeneous and shift cost check point to the center of the footprint + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_smooth", 15000)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_curve", 1.0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.015)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.cusp_zone_length", -1.0)); + node_lifecycle_->set_parameter( + rclcpp::Parameter( + "SmoothPath.cost_check_points", + std::vector({-0.05, 0.0, 0.5, -0.45, 0.0, 0.5}) // x1, y1, weight1, x2, y2, weight2 + )); + reloadParams(); + + // cost improvement is different for path smoothed by original optimizer + // since the footprint has changed + cost_avoidance_improvement_simple = assessPathImprovement( + cusp_near_obstacle, smoothed_path, + cost_avoidance_criterion); + worst_cost_improvement_simple = assessWorstPoseImprovement( + cusp_near_obstacle, smoothed_path, + cost_avoidance_criterion); + EXPECT_GT(cost_avoidance_improvement_simple, 0.0); + RCLCPP_INFO( + rclcpp::get_logger( + "ceres_smoother"), "Cost avoidance improvement (cusp_shifted, simple): %lf, %lf", + cost_avoidance_improvement_simple, worst_cost_improvement_simple); + EXPECT_NEAR(cost_avoidance_improvement_simple, 40.2, 1.0); + + // now smooth using the new optimizer with cost check point shifted + std::vector smoothed_path_scc; + EXPECT_TRUE(smoothPath(cusp_near_obstacle, smoothed_path_scc)); + EXPECT_GT(assessPathImprovement(upper_bound, smoothed_path_scc, mvmt_smoothness_criterion_), 0.0); + double cost_avoidance_improvement_shifted_cost_check = assessPathImprovement( + cusp_near_obstacle, + smoothed_path_scc, + cost_avoidance_criterion); + EXPECT_GT(cost_avoidance_improvement_shifted_cost_check, 0.0); + EXPECT_NEAR(cost_avoidance_improvement_shifted_cost_check, 42.0, 1.0); + double worst_cost_improvement_shifted_cost_check = assessWorstPoseImprovement( + cusp_near_obstacle, + smoothed_path_scc, + cost_avoidance_criterion); + RCLCPP_INFO( + rclcpp::get_logger( + "ceres_smoother"), "Cost avoidance improvement (cusp_shifted, scc): %lf, %lf", + cost_avoidance_improvement_shifted_cost_check, worst_cost_improvement_shifted_cost_check); + EXPECT_GE(worst_cost_improvement_shifted_cost_check, 0.0); + EXPECT_GE(worst_cost_improvement_shifted_cost_check, worst_cost_improvement_simple); + EXPECT_GT(cost_avoidance_improvement_shifted_cost_check, cost_avoidance_improvement_simple); + + // same results should be achieved with unnormalized weights + // (testing automatic weights normalization, i.e. using avg instead of sum) + node_lifecycle_->set_parameter( + rclcpp::Parameter( + "SmoothPath.cost_check_points", + std::vector({-0.05, 0.0, 1.0, -0.45, 0.0, 1.0}) // x1, y1, weight1, x2, y2, weight2 + )); + reloadParams(); + std::vector smoothed_path_scc_unnormalized; + EXPECT_TRUE(smoothPath(cusp_near_obstacle, smoothed_path_scc_unnormalized)); + EXPECT_EQ(smoothed_path_scc, smoothed_path_scc_unnormalized); + + //////////////////////////////////////// + // compare also with extra careful cusp + + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.w_cost", 0.0052)); + node_lifecycle_->set_parameter( + rclcpp::Parameter("SmoothPath.w_cost_cusp_multiplier", 0.027 / 0.0052)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.cusp_zone_length", 2.5)); + // we need much more iterations here since it's a more complicated problem + // TODO(afrixs): tune ceres optimizer to "converge" faster + // see http://ceres-solver.org/nnls_solving.html + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.optimizer.max_iterations", 1500)); + reloadParams(); + + std::vector smoothed_path_scce; + EXPECT_TRUE(smoothPath(cusp_near_obstacle, smoothed_path_scce)); + EXPECT_GT( + assessPathImprovement(upper_bound, smoothed_path_scce, mvmt_smoothness_criterion_), + 0.0); + double cost_avoidance_improvement_shifted_extra = assessPathImprovement( + cusp_near_obstacle, + smoothed_path_scce, + cost_avoidance_criterion); + double worst_cost_improvement_shifted_extra = assessWorstPoseImprovement( + cusp_near_obstacle, + smoothed_path_scce, + cost_avoidance_criterion); + RCLCPP_INFO( + rclcpp::get_logger( + "ceres_smoother"), "Cost avoidance improvement (cusp_shifted, scce): %lf, %lf", + cost_avoidance_improvement_shifted_extra, worst_cost_improvement_shifted_extra); + EXPECT_NEAR(cost_avoidance_improvement_shifted_extra, 51.0, 1.0); + EXPECT_GE(worst_cost_improvement_shifted_extra, 0.0); + + // resmooth extra careful cusp with same conditions (higher max_iterations) + node_lifecycle_->set_parameter( + rclcpp::Parameter( + "SmoothPath.cost_check_points", + std::vector())); + reloadParams(); + + EXPECT_TRUE(smoothPath(cusp_near_obstacle, smoothed_path_ecc)); + cost_avoidance_improvement_extra_careful_cusp = assessPathImprovement( + cusp_near_obstacle, + smoothed_path_ecc, + cost_avoidance_criterion); + worst_cost_improvement_extra_careful_cusp = assessWorstPoseImprovement( + cusp_near_obstacle, + smoothed_path_ecc, + cost_avoidance_criterion); + EXPECT_GT(cost_avoidance_improvement_extra_careful_cusp, 0.0); + RCLCPP_INFO( + rclcpp::get_logger( + "ceres_smoother"), "Cost avoidance improvement (cusp_shifted, ecc): %lf, %lf", + cost_avoidance_improvement_extra_careful_cusp, worst_cost_improvement_extra_careful_cusp); + EXPECT_NEAR(cost_avoidance_improvement_extra_careful_cusp, 48.5, 1.0); + EXPECT_GT( + cost_avoidance_improvement_shifted_extra, + cost_avoidance_improvement_extra_careful_cusp); + // worst cost improvement is a bit lower but only by 5% so it's not a big deal + EXPECT_GE(worst_cost_improvement_shifted_extra, worst_cost_improvement_extra_careful_cusp - 6.0); + + SUCCEED(); +} + +TEST_F(SmootherTest, testingDownsamplingUpsampling) +{ + // path with a cusp + std::vector sharp_turn_90_then_reverse = + {{0, 0, 0}, + {0.1, 0, 0}, + {0.2, 0, 0}, + {0.3, 0, 0}, + {0.4, 0, 0}, + {0.5, 0, 0}, + {0.6, 0, M_PI / 4}, + {0.6, -0.1, M_PI / 2}, + {0.6, -0.2, M_PI / 2}, + {0.6, -0.3, M_PI / 2}, + {0.6, -0.4, M_PI / 2}, + {0.6, -0.5, M_PI / 2}, + {0.6, -0.6, M_PI / 2} + }; + cusp_i_ = 6; + + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_downsampling_factor", 2)); + // downsample only + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_upsampling_factor", 0)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.reversing_enabled", false)); + reloadParams(); + std::vector smoothed_path_downsampled; + EXPECT_TRUE(smoothPath(sharp_turn_90_then_reverse, smoothed_path_downsampled)); + // first two, last two and every 2nd pose between them + EXPECT_EQ(smoothed_path_downsampled.size(), 8u); + + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.reversing_enabled", true)); + reloadParams(); + EXPECT_TRUE(smoothPath(sharp_turn_90_then_reverse, smoothed_path_downsampled)); + // same but downsampling is reset on cusp + EXPECT_EQ(smoothed_path_downsampled.size(), 9u); + + // upsample to original size + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_upsampling_factor", 1)); + reloadParams(); + std::vector smoothed_path; + EXPECT_TRUE(smoothPath(sharp_turn_90_then_reverse, smoothed_path)); + EXPECT_EQ(smoothed_path.size(), sharp_turn_90_then_reverse.size()); + + cusp_i_ = 4; // for downsampled path + int cusp_i_out = 6; // for upsampled path + QualityCriterion3 mvmt_smoothness_criterion_out = + [&cusp_i_out](int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p, + const Eigen::Vector3d & next_p) { + Eigen::Vector2d prev_mvmt = p.block<2, 1>(0, 0) - prev_p.block<2, 1>(0, 0); + Eigen::Vector2d next_mvmt = next_p.block<2, 1>(0, 0) - p.block<2, 1>(0, 0); + if (i == cusp_i_out) { + next_mvmt = -next_mvmt; + } + return (next_mvmt - prev_mvmt).norm(); + }; + + double smoothness_improvement = assessPathImprovement( + smoothed_path_downsampled, smoothed_path, + mvmt_smoothness_criterion_, + &mvmt_smoothness_criterion_out); + // more poses -> smoother path + EXPECT_GT(smoothness_improvement, 0.0); + EXPECT_NEAR(smoothness_improvement, 65.7, 1.0); + + // upsample above original size + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.path_upsampling_factor", 2)); + reloadParams(); + EXPECT_TRUE(smoothPath(sharp_turn_90_then_reverse, smoothed_path)); + // every pose except last produces 2 poses + EXPECT_EQ(smoothed_path.size(), sharp_turn_90_then_reverse.size() * 2 - 1); + cusp_i_out = 12; // for upsampled path + smoothness_improvement = assessPathImprovement( + smoothed_path_downsampled, smoothed_path, + mvmt_smoothness_criterion_, + &mvmt_smoothness_criterion_out); + // even more poses -> even smoother path + EXPECT_GT(smoothness_improvement, 0.0); + EXPECT_NEAR(smoothness_improvement, 83.7, 1.0); +} + +TEST_F(SmootherTest, testingStartGoalOrientations) +{ + std::vector sharp_turn_90 = + {{0, 0, 0}, + {0.1, 0, 0}, + {0.2, 0, 0}, + {0.3, 0, M_PI / 4}, + {0.3, 0.1, M_PI / 2}, + {0.3, 0.2, M_PI / 2}, + {0.3, 0.3, M_PI / 2} + }; + + // Keep start and goal orientations (by default) + std::vector smoothed_path; + EXPECT_TRUE(smoothPath(sharp_turn_90, smoothed_path)); + + double mvmt_smoothness_improvement = + assessPathImprovement(sharp_turn_90, smoothed_path, mvmt_smoothness_criterion_); + EXPECT_GT(mvmt_smoothness_improvement, 0.0); + EXPECT_NEAR(mvmt_smoothness_improvement, 53.3, 1.0); + // no change in orientations + EXPECT_NEAR(smoothed_path.front()[2], 0, 0.001); + EXPECT_NEAR(smoothed_path.back()[2], M_PI / 2, 0.001); + + // Overwrite start/goal orientations + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.keep_start_orientation", false)); + node_lifecycle_->set_parameter(rclcpp::Parameter("SmoothPath.keep_goal_orientation", false)); + reloadParams(); + + sharp_turn_90[0][2] = M_PI; // forward/reverse of start pose should not matter + std::vector smoothed_path_sg_overwritten; + EXPECT_TRUE(smoothPath(sharp_turn_90, smoothed_path_sg_overwritten)); + + mvmt_smoothness_improvement = + assessPathImprovement(smoothed_path, smoothed_path_sg_overwritten, mvmt_smoothness_criterion_); + EXPECT_GT(mvmt_smoothness_improvement, 0.0); + EXPECT_NEAR(mvmt_smoothness_improvement, 98.3, 1.0); + // orientations adjusted to follow the path + EXPECT_NEAR(smoothed_path_sg_overwritten.front()[2], M_PI / 4, 0.1); + EXPECT_NEAR(smoothed_path_sg_overwritten.back()[2], M_PI / 4, 0.1); + + // test short paths + std::vector short_screwed_path = + {{0, 0, M_PI * 0.25}, + {0.1, 0, -M_PI * 0.25} + }; + + std::vector adjusted_path; + EXPECT_TRUE(smoothPath(short_screwed_path, adjusted_path)); + EXPECT_NEAR(adjusted_path.front()[2], 0, 0.001); + EXPECT_NEAR(adjusted_path.back()[2], 0, 0.001); + + short_screwed_path[0][2] = M_PI * 0.75; // start is stronger than goal + EXPECT_TRUE(smoothPath(short_screwed_path, adjusted_path)); + EXPECT_NEAR(adjusted_path.front()[2], M_PI, 0.001); + EXPECT_NEAR(adjusted_path.back()[2], M_PI, 0.001); + + std::vector one_pose_path = {{0, 0, M_PI * 0.75}}; + EXPECT_TRUE(smoothPath(one_pose_path, adjusted_path)); + EXPECT_NEAR(adjusted_path.front()[2], M_PI * 0.75, 0.001); +} + +TEST_F(SmootherTest, testingCostCheckPointsParamValidity) +{ + node_lifecycle_->set_parameter( + rclcpp::Parameter( + "SmoothPath.cost_check_points", + std::vector())); + reloadParams(); + + node_lifecycle_->set_parameter( + rclcpp::Parameter( + "SmoothPath.cost_check_points", + std::vector({0, 0, 0, 0, 0, 0, 0, 0, 0}))); // multiple of 3 is ok + reloadParams(); + + node_lifecycle_->set_parameter( + rclcpp::Parameter( + "SmoothPath.cost_check_points", + std::vector({0, 0}))); + EXPECT_THROW(reloadParams(), std::runtime_error); +} + +TEST_F(SmootherTest, testingLinearSolverTypeParamValidity) +{ + node_lifecycle_->set_parameter( + rclcpp::Parameter( + "SmoothPath.optimizer.linear_solver_type", + "SPARSE_NORMAL_CHOLESKY")); + reloadParams(); + + node_lifecycle_->set_parameter( + rclcpp::Parameter( + "SmoothPath.optimizer.linear_solver_type", + "DENSE_QR")); + reloadParams(); + + node_lifecycle_->set_parameter( + rclcpp::Parameter( + "SmoothPath.optimizer.linear_solver_type", + "INVALID_SOLVER")); + EXPECT_THROW(reloadParams(), std::runtime_error); +} + +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_constrained_smoother/test/test_smoother_cost_function.cpp b/nav2_constrained_smoother/test/test_smoother_cost_function.cpp new file mode 100644 index 0000000000..104b949c4b --- /dev/null +++ b/nav2_constrained_smoother/test/test_smoother_cost_function.cpp @@ -0,0 +1,141 @@ +// Copyright (c) 2021 RoboTech Vision +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_constrained_smoother/smoother_cost_function.hpp" + +class TestableSmootherCostFunction : nav2_constrained_smoother::SmootherCostFunction +{ +public: + TestableSmootherCostFunction( + const Eigen::Vector2d & original_pos, + double next_to_last_length_ratio, + bool reversing, + const nav2_costmap_2d::Costmap2D * costmap, + const std::shared_ptr>> & costmap_interpolator, + const nav2_constrained_smoother::SmootherParams & params, + double costmap_weight) + : SmootherCostFunction( + original_pos, next_to_last_length_ratio, reversing, + costmap, costmap_interpolator, + params, costmap_weight) + { + } + + inline double getCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_next, + const Eigen::Vector2d & pt_prev) const + { + double r = 0.0; + addCurvatureResidual(weight, pt, pt_next, pt_prev, r); + return r; + } +}; + +class Test : public ::testing::Test +{ +protected: + void SetUp() + { + } +}; + +TEST_F(Test, testingCurvatureResidual) +{ + nav2_costmap_2d::Costmap2D costmap; + TestableSmootherCostFunction fn( + Eigen::Vector2d(1.0, 0.0), 1.0, false, + &costmap, std::shared_ptr>>(), + nav2_constrained_smoother::SmootherParams(), 0.0 + ); + + // test for edge values + Eigen::Vector2d pt(1.0, 0.0); + Eigen::Vector2d pt_other(0.0, 0.0); + EXPECT_EQ(fn.getCurvatureResidual(0.0, pt, pt_other, pt_other), 0.0); + + nav2_constrained_smoother::SmootherParams params_no_min_turning_radius; + params_no_min_turning_radius.max_curvature = 1.0f / 0.0; + TestableSmootherCostFunction fn_no_min_turning_radius( + Eigen::Vector2d(1.0, 0.0), 1.0, false, + &costmap, std::shared_ptr>>(), + params_no_min_turning_radius, 0.0 + ); + EXPECT_EQ(fn_no_min_turning_radius.getCurvatureResidual(1.0, pt, pt_other, pt_other), 0.0); +} + +TEST_F(Test, testingUtils) +{ + Eigen::Vector2d pt(1.0, 0.0); + Eigen::Vector2d pt_prev(0.0, 0.0); + Eigen::Vector2d pt_next(0.0, 0.0); + + // test for intermediate values + auto center = nav2_constrained_smoother::arcCenter(pt_prev, pt, pt_next, false); + // although in this situation the center would be at (0.5, 0.0), + // cases where pt_prev == pt_next are very rare and thus unhandled + // during the smoothing points will be separated (and thus made valid) by smoothness cost anyways + EXPECT_EQ(center[0], std::numeric_limits::infinity()); + EXPECT_EQ(center[1], std::numeric_limits::infinity()); + + auto tangent = + nav2_constrained_smoother::tangentDir(pt_prev, pt, pt_next, false).normalized(); + EXPECT_NEAR(tangent[0], 0, 1e-10); + EXPECT_NEAR(std::abs(tangent[1]), 1, 1e-10); + + // no rotation when mid point is a cusp + tangent = nav2_constrained_smoother::tangentDir(pt_prev, pt, pt_next, true).normalized(); + EXPECT_NEAR(std::abs(tangent[0]), 1, 1e-10); + EXPECT_NEAR(tangent[1], 0, 1e-10); + + pt_prev[0] = -1.0; + // rotation is mathematically invalid, picking direction of a shorter segment + tangent = nav2_constrained_smoother::tangentDir(pt_prev, pt, pt_next, true).normalized(); + EXPECT_NEAR(std::abs(tangent[0]), 1, 1e-10); + EXPECT_NEAR(tangent[1], 0, 1e-10); + + pt_prev[0] = 0.0; + pt_next[0] = -1.0; + // rotation is mathematically invalid, picking direction of a shorter segment + tangent = nav2_constrained_smoother::tangentDir(pt_prev, pt, pt_next, true).normalized(); + EXPECT_NEAR(std::abs(tangent[0]), 1, 1e-10); + EXPECT_NEAR(tangent[1], 0, 1e-10); +} + +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_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index cf79521aa3..e5ea991c34 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(nav2_common REQUIRED) find_package(angles REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_msgs REQUIRED) @@ -28,16 +29,15 @@ add_executable(${executable_name} set(library_name ${executable_name}_core) -add_library(${library_name} - src/nav2_controller.cpp +add_library(${library_name} SHARED + src/controller_server.cpp ) -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - set(dependencies angles rclcpp rclcpp_action + rclcpp_components std_msgs nav2_msgs nav_2d_utils @@ -49,27 +49,18 @@ set(dependencies add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp) ament_target_dependencies(simple_progress_checker ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(simple_progress_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") add_library(simple_goal_checker SHARED plugins/simple_goal_checker.cpp) ament_target_dependencies(simple_goal_checker ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(simple_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") add_library(stopped_goal_checker SHARED plugins/stopped_goal_checker.cpp) target_link_libraries(stopped_goal_checker simple_goal_checker) ament_target_dependencies(stopped_goal_checker ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(stopped_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_target_dependencies(${library_name} ${dependencies} ) -# prevent pluginlib from using boost -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -86,6 +77,8 @@ ament_target_dependencies(${executable_name} target_link_libraries(${executable_name} ${library_name}) +rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") + install(TARGETS simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -102,7 +95,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) @@ -110,7 +105,7 @@ ament_export_libraries(simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name}) -ament_export_definitions("PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_core plugins.xml) ament_package() diff --git a/nav2_controller/README.md b/nav2_controller/README.md index b0f48490a6..3a77144f16 100644 --- a/nav2_controller/README.md +++ b/nav2_controller/README.md @@ -1,8 +1,9 @@ # Nav2 Controller -The Nav2 Controller is an [Execution Module](../doc/requirements/requirements.md) that implements the `nav2_msgs::action::FollowPath` action server. +The Nav2 Controller is a Task Server in Nav2 that implements the `nav2_msgs::action::FollowPath` action server. -An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. +An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with multiple plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. It also contains progress checkers and goal checker plugins to abstract out that logic from specific controller implementations. +See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available controller plugins. -Currently available controller plugins are: DWB, and [TEB (dashing release)](https://github.com/rst-tu-dortmund/teb_local_planner/tree/dashing-devel). \ No newline at end of file +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-controller-server.html) for additional parameter descriptions and a [tutorial about writing controller plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html). diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp similarity index 79% rename from nav2_controller/include/nav2_controller/nav2_controller.hpp rename to nav2_controller/include/nav2_controller/controller_server.hpp index 3b6479de97..89d9a4822c 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_CONTROLLER__NAV2_CONTROLLER_HPP_ -#define NAV2_CONTROLLER__NAV2_CONTROLLER_HPP_ +#ifndef NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_ +#define NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_ #include #include #include #include #include +#include #include "nav2_core/controller.hpp" #include "nav2_core/progress_checker.hpp" @@ -27,6 +28,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "tf2_ros/transform_listener.h" #include "nav2_msgs/action/follow_path.hpp" +#include "nav2_msgs/msg/speed_limit.hpp" #include "nav_2d_utils/odom_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/simple_action_server.hpp" @@ -47,11 +49,13 @@ class ControllerServer : public nav2_util::LifecycleNode { public: using ControllerMap = std::unordered_map; + using GoalCheckerMap = std::unordered_map; /** * @brief Constructor for nav2_controller::ControllerServer + * @param options Additional options to control creation of the node. */ - ControllerServer(); + explicit ControllerServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Destructor for nav2_controller::ControllerServer */ @@ -129,6 +133,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 @@ -189,6 +202,17 @@ class ControllerServer : public nav2_util::LifecycleNode return twist_thresh; } + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::mutex dynamic_params_lock_; + // The controller needs a costmap node std::shared_ptr costmap_ros_; std::unique_ptr costmap_thread_; @@ -196,6 +220,7 @@ class ControllerServer : public nav2_util::LifecycleNode // Publishers and subscribers std::unique_ptr odom_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_publisher_; + rclcpp::Subscription::SharedPtr speed_limit_sub_; // Progress Checker Plugin pluginlib::ClassLoader progress_checker_loader_; @@ -207,11 +232,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_; @@ -227,10 +253,25 @@ 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_; + geometry_msgs::msg::PoseStamped 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 + * @param msg Shared pointer to nav2_msgs::msg::SpeedLimit + */ + void speedLimitCallback(const nav2_msgs::msg::SpeedLimit::SharedPtr msg); }; } // namespace nav2_controller -#endif // NAV2_CONTROLLER__NAV2_CONTROLLER_HPP_ +#endif // NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_ 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 b7e483c161..e9091db9ae 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -37,10 +37,12 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_core/goal_checker.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" namespace nav2_controller { @@ -58,18 +60,32 @@ class SimpleGoalChecker : public nav2_core::GoalChecker SimpleGoalChecker(); // Standard GoalChecker Interface void initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, - const std::string & plugin_name) override; + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros) override; void reset() override; 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_; + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::string plugin_name_; + + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); }; } // 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 92a5374a5e..656d67fb30 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp @@ -16,6 +16,7 @@ #define NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ #include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_core/progress_checker.hpp" @@ -34,7 +35,7 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker { public: void initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) override; bool check(geometry_msgs::msg::PoseStamped & current_pose) override; void reset() override; @@ -52,7 +53,7 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker */ void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose); - rclcpp_lifecycle::LifecycleNode::SharedPtr nh_; + rclcpp::Clock::SharedPtr clock_; double radius_; rclcpp::Duration time_allowance_{0, 0}; @@ -61,6 +62,16 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker rclcpp::Time baseline_time_; bool baseline_pose_set_{false}; + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::string plugin_name_; + + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); }; } // 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 869edb6330..d7586db6f3 100644 --- a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -37,6 +37,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -55,14 +56,28 @@ class StoppedGoalChecker : public SimpleGoalChecker StoppedGoalChecker(); // Standard GoalChecker Interface void initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, - const std::string & plugin_name) override; + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros) override; 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_; + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::string plugin_name_; + + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); }; } // namespace nav2_controller diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 0e38b5994d..d8ed5aec78 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 0.4.7 + 1.1.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 f1ec7dba6c..597f843eaa 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -34,15 +34,21 @@ #include #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 { @@ -56,24 +62,32 @@ SimpleGoalChecker::SimpleGoalChecker() } void SimpleGoalChecker::initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, - const std::string & plugin_name) + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr/*costmap_ros*/) { + plugin_name_ = plugin_name; + auto node = parent.lock(); + nav2_util::declare_parameter_if_not_declared( - nh, + node, plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25)); nav2_util::declare_parameter_if_not_declared( - nh, + node, plugin_name + ".yaw_goal_tolerance", rclcpp::ParameterValue(0.25)); nav2_util::declare_parameter_if_not_declared( - nh, + node, plugin_name + ".stateful", rclcpp::ParameterValue(true)); - nh->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); - nh->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_); - nh->get_parameter(plugin_name + ".stateful", stateful_); + node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); + node->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_); + node->get_parameter(plugin_name + ".stateful", stateful_); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&SimpleGoalChecker::dynamicParametersCallback, this, _1)); } void SimpleGoalChecker::reset() @@ -103,6 +117,54 @@ 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; +} + +rcl_interfaces::msg::SetParametersResult +SimpleGoalChecker::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto & parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".xy_goal_tolerance") { + xy_goal_tolerance_ = parameter.as_double(); + xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; + } else if (name == plugin_name_ + ".yaw_goal_tolerance") { + yaw_goal_tolerance_ = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".stateful") { + stateful_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + } // 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 498d13256c..1ced16dc5b 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -15,6 +15,8 @@ #include "nav2_controller/plugins/simple_progress_checker.hpp" #include #include +#include +#include #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -22,24 +24,35 @@ #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 &); void SimpleProgressChecker::initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) { - nh_ = node; + plugin_name_ = plugin_name; + auto node = parent.lock(); + + clock_ = node->get_clock(); + nav2_util::declare_parameter_if_not_declared( - nh_, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5)); + node, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5)); nav2_util::declare_parameter_if_not_declared( - nh_, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0)); + node, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0)); // Scale is set to 0 by default, so if it was not set otherwise, set to 0 - nh_->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5); + node->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5); double time_allowance_param = 0.0; - nh_->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0); + node->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0); time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&SimpleProgressChecker::dynamicParametersCallback, this, _1)); } bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) @@ -53,10 +66,7 @@ bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose reset_baseline_pose(current_pose2d); return true; } - if ((nh_->now() - baseline_time_) > time_allowance_) { - return false; - } - return true; + return !((clock_->now() - baseline_time_) > time_allowance_); } void SimpleProgressChecker::reset() @@ -67,17 +77,13 @@ void SimpleProgressChecker::reset() void SimpleProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) { baseline_pose_ = pose; - baseline_time_ = nh_->now(); + baseline_time_ = clock_->now(); baseline_pose_set_ = true; } bool SimpleProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) { - if (pose_distance(pose, baseline_pose_) > radius_) { - return true; - } else { - return false; - } + return pose_distance(pose, baseline_pose_) > radius_; } static double pose_distance( @@ -90,6 +96,26 @@ static double pose_distance( return std::hypot(dx, dy); } +rcl_interfaces::msg::SetParametersResult +SimpleProgressChecker::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".required_movement_radius") { + radius_ = parameter.as_double(); + } else if (name == plugin_name_ + ".movement_time_allowance") { + time_allowance_ = rclcpp::Duration::from_seconds(parameter.as_double()); + } + } + } + result.successful = true; + return result; +} + } // 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 2e24ad181a..179c698bd4 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -35,6 +35,8 @@ #include #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 +44,9 @@ using std::hypot; using std::fabs; +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + namespace nav2_controller { @@ -51,20 +56,28 @@ StoppedGoalChecker::StoppedGoalChecker() } void StoppedGoalChecker::initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, - const std::string & plugin_name) + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros) { - SimpleGoalChecker::initialize(nh, plugin_name); + plugin_name_ = plugin_name; + SimpleGoalChecker::initialize(parent, plugin_name, costmap_ros); + + auto node = parent.lock(); nav2_util::declare_parameter_if_not_declared( - nh, + node, plugin_name + ".rot_stopped_velocity", rclcpp::ParameterValue(0.25)); nav2_util::declare_parameter_if_not_declared( - nh, + node, plugin_name + ".trans_stopped_velocity", rclcpp::ParameterValue(0.25)); - nh->get_parameter(plugin_name + ".rot_stopped_velocity", rot_stopped_velocity_); - nh->get_parameter(plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_); + node->get_parameter(plugin_name + ".rot_stopped_velocity", rot_stopped_velocity_); + node->get_parameter(plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&StoppedGoalChecker::dynamicParametersCallback, this, _1)); } bool StoppedGoalChecker::isGoalReached( @@ -80,6 +93,47 @@ 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; +} + +rcl_interfaces::msg::SetParametersResult +StoppedGoalChecker::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".rot_stopped_velocity") { + rot_stopped_velocity_ = parameter.as_double(); + } else if (name == plugin_name_ + ".trans_stopped_velocity") { + trans_stopped_velocity_ = parameter.as_double(); + } + } + } + result.successful = true; + return result; +} + } // namespace nav2_controller PLUGINLIB_EXPORT_CLASS(nav2_controller::StoppedGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index fe175db445..387bfcc735 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -151,8 +151,10 @@ TEST(VelocityIterator, two_checks) SimpleGoalChecker gc; StoppedGoalChecker sgc; - gc.initialize(x, "nav2_controller"); - sgc.initialize(x, "nav2_controller"); + auto costmap = std::make_shared("test_costmap"); + + gc.initialize(x, "nav2_controller", costmap); + sgc.initialize(x, "nav2_controller", costmap); sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true); sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false); sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false); @@ -164,6 +166,67 @@ TEST(VelocityIterator, two_checks) trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1); } +TEST(StoppedGoalChecker, get_tol_and_dynamic_params) +{ + auto x = std::make_shared("goal_checker"); + + SimpleGoalChecker gc; + StoppedGoalChecker sgc; + auto costmap = std::make_shared("test_costmap"); + + sgc.initialize(x, "test", costmap); + gc.initialize(x, "test2", costmap); + geometry_msgs::msg::Pose pose_tol; + geometry_msgs::msg::Twist vel_tol; + + // Test stopped goal checker's tolerance API + EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(vel_tol.linear.x, 0.25); + EXPECT_EQ(vel_tol.linear.y, 0.25); + EXPECT_EQ(vel_tol.angular.z, 0.25); + + // Test Stopped goal checker's dynamic parameters + auto rec_param = std::make_shared( + x->get_node_base_interface(), x->get_node_topics_interface(), + x->get_node_graph_interface(), + x->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.rot_stopped_velocity", 100.0), + rclcpp::Parameter("test.trans_stopped_velocity", 100.0)}); + + rclcpp::spin_until_future_complete( + x->get_node_base_interface(), + results); + + EXPECT_EQ(x->get_parameter("test.rot_stopped_velocity").as_double(), 100.0); + EXPECT_EQ(x->get_parameter("test.trans_stopped_velocity").as_double(), 100.0); + + // Test normal goal checker's dynamic parameters + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test2.xy_goal_tolerance", 200.0), + rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0), + rclcpp::Parameter("test2.stateful", true)}); + + rclcpp::spin_until_future_complete( + x->get_node_base_interface(), + results); + + EXPECT_EQ(x->get_parameter("test2.xy_goal_tolerance").as_double(), 200.0); + EXPECT_EQ(x->get_parameter("test2.yaw_goal_tolerance").as_double(), 200.0); + EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true); + + // Test the dynamic parameters impacted the tolerances + EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(vel_tol.linear.x, 100.0); + EXPECT_EQ(vel_tol.linear.y, 100.0); + EXPECT_EQ(vel_tol.angular.z, 100.0); + + EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol)); + EXPECT_EQ(pose_tol.position.x, 200.0); + EXPECT_EQ(pose_tol.position.y, 200.0); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/controller_server.cpp similarity index 57% rename from nav2_controller/src/nav2_controller.cpp rename to nav2_controller/src/controller_server.cpp index 26738b8e39..a44caeda3d 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -17,27 +17,30 @@ #include #include #include +#include #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav_2d_utils/tf_help.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav2_controller/nav2_controller.hpp" +#include "nav2_controller/controller_server.hpp" using namespace std::chrono_literals; +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; namespace nav2_controller { -ControllerServer::ControllerServer() -: LifecycleNode("controller_server", "", true), +ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("controller_server", "", options), progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"), 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,12 +50,16 @@ 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)); declare_parameter("min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001)); + 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"); @@ -63,7 +70,10 @@ ControllerServer::ControllerServer() ControllerServer::~ControllerServer() { - RCLCPP_INFO(get_logger(), "Destroying"); + progress_checker_.reset(); + goal_checkers_.clear(); + controllers_.clear(); + costmap_thread_.reset(); } nav2_util::CallbackReturn @@ -79,11 +89,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_); @@ -94,7 +108,9 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) rclcpp::ParameterValue(default_types_[i])); } } + 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_); @@ -102,6 +118,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_); RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); + std::string speed_limit_topic; + get_parameter("speed_limit_topic", speed_limit_topic); + get_parameter("failure_tolerance", failure_tolerance_); + costmap_ros_->on_configure(state); try { @@ -117,20 +137,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], costmap_ros_); + 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]); @@ -164,8 +197,17 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) // Create the action server that we implement with our followPath method action_server_ = std::make_unique( - rclcpp_node_, "follow_path", - std::bind(&ControllerServer::computeControl, this)); + shared_from_this(), + "follow_path", + std::bind(&ControllerServer::computeControl, this), + nullptr, + std::chrono::milliseconds(500), + true); + + // Set subscribtion to the speed limiting topic + speed_limit_sub_ = create_subscription( + speed_limit_topic, rclcpp::QoS(10), + std::bind(&ControllerServer::speedLimitCallback, this, std::placeholders::_1)); return nav2_util::CallbackReturn::SUCCESS; } @@ -183,6 +225,14 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & state) vel_publisher_->on_activate(); action_server_->activate(); + auto node = shared_from_this(); + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&ControllerServer::dynamicParametersCallback, this, _1)); + + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -200,6 +250,10 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) publishZeroVelocity(); vel_publisher_->on_deactivate(); + dyn_params_handler_.reset(); + + // destroy bond connection + destroyBond(); return nav2_util::CallbackReturn::SUCCESS; } @@ -215,14 +269,15 @@ 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 action_server_.reset(); odom_sub_.reset(); vel_publisher_.reset(); - action_server_.reset(); - goal_checker_->reset(); + speed_limit_sub_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -248,7 +303,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; } @@ -260,8 +315,36 @@ 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() { + std::lock_guard lock(dynamic_params_lock_); + RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { @@ -274,9 +357,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()) { @@ -291,6 +384,12 @@ void ControllerServer::computeControl() return; } + // Don't compute a trajectory until costmap is valid (after clear costmap) + rclcpp::Rate r(100); + while (!costmap_ros_->isCurrent()) { + r.sleep(); + } + updateGlobalPath(); computeAndPublishVelocity(); @@ -331,18 +430,15 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) } controllers_[current_controller_]->setPlan(path); - auto end_pose = path.poses.back(); - end_pose.header.frame_id = path.header.frame_id; - rclcpp::Duration tolerance(costmap_ros_->getTransformTolerance() * 1e9); - nav_2d_utils::transformPose( - costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), - end_pose, end_pose, tolerance); - goal_checker_->reset(); + end_pose_ = path.poses.back(); + end_pose_.header.frame_id = path.header.frame_id; + 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; + end_pose_.pose.position.x, end_pose_.pose.position.y); + + current_path_ = path; } void ControllerServer::computeAndPublishVelocity() @@ -359,14 +455,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()); @@ -424,7 +564,16 @@ 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); + + geometry_msgs::msg::PoseStamped transformed_end_pose; + rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance())); + nav_2d_utils::transformPose( + costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), + end_pose_, transformed_end_pose, tolerance); + + return goal_checkers_[current_goal_checker_]->isGoalReached( + pose.pose, transformed_end_pose.pose, + velocity); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) @@ -437,4 +586,65 @@ bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) return true; } +void ControllerServer::speedLimitCallback(const nav2_msgs::msg::SpeedLimit::SharedPtr msg) +{ + ControllerMap::iterator it; + for (it = controllers_.begin(); it != controllers_.end(); ++it) { + it->second->setSpeedLimit(msg->speed_limit, msg->percentage); + } +} + +rcl_interfaces::msg::SetParametersResult +ControllerServer::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + // If we are trying to change the parameter of a plugin we can just skip it at this point + // as they handle parameter changes themselves and don't need to lock the mutex + if (name.find('.') != std::string::npos) { + continue; + } + + if (!dynamic_params_lock_.try_lock()) { + RCLCPP_WARN( + get_logger(), + "Unable to dynamically change Parameters while the controller is currently running"); + result.successful = false; + result.reason = + "Unable to dynamically change Parameters while the controller is currently running"; + return result; + } + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == "controller_frequency") { + controller_frequency_ = parameter.as_double(); + } else if (name == "min_x_velocity_threshold") { + min_x_velocity_threshold_ = parameter.as_double(); + } else if (name == "min_y_velocity_threshold") { + min_y_velocity_threshold_ = parameter.as_double(); + } else if (name == "min_theta_velocity_threshold") { + min_theta_velocity_threshold_ = parameter.as_double(); + } else if (name == "failure_tolerance") { + failure_tolerance_ = parameter.as_double(); + } + } + + dynamic_params_lock_.unlock(); + } + + result.successful = true; + return result; +} + } // namespace nav2_controller + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_controller::ControllerServer) diff --git a/nav2_controller/src/main.cpp b/nav2_controller/src/main.cpp index 0a79e614e2..a69adf583b 100644 --- a/nav2_controller/src/main.cpp +++ b/nav2_controller/src/main.cpp @@ -14,7 +14,7 @@ #include -#include "nav2_controller/nav2_controller.hpp" +#include "nav2_controller/controller_server.hpp" #include "rclcpp/rclcpp.hpp" int main(int argc, char ** argv) diff --git a/nav2_controller/test/CMakeLists.txt b/nav2_controller/test/CMakeLists.txt new file mode 100644 index 0000000000..d415d906ef --- /dev/null +++ b/nav2_controller/test/CMakeLists.txt @@ -0,0 +1,10 @@ +# Test dynamic parameters +ament_add_gtest(test_dynamic_parameters + test_dynamic_parameters.cpp +) +ament_target_dependencies(test_dynamic_parameters + ${dependencies} +) +target_link_libraries(test_dynamic_parameters + ${library_name} +) diff --git a/nav2_controller/test/test_dynamic_parameters.cpp b/nav2_controller/test/test_dynamic_parameters.cpp new file mode 100644 index 0000000000..e1c1be5d88 --- /dev/null +++ b/nav2_controller/test/test_dynamic_parameters.cpp @@ -0,0 +1,88 @@ +// 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 "gtest/gtest.h" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_controller/controller_server.hpp" +#include "rclcpp/rclcpp.hpp" + +class ControllerShim : public nav2_controller::ControllerServer +{ +public: + ControllerShim() + : nav2_controller::ControllerServer(rclcpp::NodeOptions()) + { + } + + // Since we cannot call configure/activate due to costmaps + // requiring TF + void setDynamicCallback() + { + auto node = shared_from_this(); + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&ControllerShim::dynamicParamsShim, this, std::placeholders::_1)); + } + + rcl_interfaces::msg::SetParametersResult + dynamicParamsShim(std::vector parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + dynamicParametersCallback(parameters); + return result; + } +}; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(WPTest, test_dynamic_parameters) +{ + auto controller = std::make_shared(); + controller->setDynamicCallback(); + + auto rec_param = std::make_shared( + controller->get_node_base_interface(), controller->get_node_topics_interface(), + controller->get_node_graph_interface(), + controller->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("controller_frequency", 100.0), + rclcpp::Parameter("min_x_velocity_threshold", 100.0), + rclcpp::Parameter("min_y_velocity_threshold", 100.0), + rclcpp::Parameter("min_theta_velocity_threshold", 100.0), + rclcpp::Parameter("failure_tolerance", 5.0)}); + + rclcpp::spin_until_future_complete( + controller->get_node_base_interface(), + results); + + EXPECT_EQ(controller->get_parameter("controller_frequency").as_double(), 100.0); + EXPECT_EQ(controller->get_parameter("min_x_velocity_threshold").as_double(), 100.0); + EXPECT_EQ(controller->get_parameter("min_y_velocity_threshold").as_double(), 100.0); + EXPECT_EQ(controller->get_parameter("min_theta_velocity_threshold").as_double(), 100.0); + EXPECT_EQ(controller->get_parameter("failure_tolerance").as_double(), 5.0); +} diff --git a/nav2_core/README.md b/nav2_core/README.md index 062fda5a4d..1d7e9e0bc2 100644 --- a/nav2_core/README.md +++ b/nav2_core/README.md @@ -2,6 +2,12 @@ This package hosts the abstract interface (virtual base classes) for plugins to be used with the following: - global planner (e.g., `nav2_navfn_planner`) -- controller (i.e, path execution controller, e.g `nav2_dwb_controller`) -- goal checker -- recovery behaviors +- controller (e.g., path execution controller, e.g `nav2_dwb_controller`) +- smoother (e.g., `nav2_ceres_costaware_smoother`) +- goal checker (e.g. `simple_goal_checker`) +- behaviors (e.g. `drive_on_heading`) +- progress checker (e.g. `simple_progress_checker`) +- waypoint task executor (e.g. `take_pictures`) +- exceptions in planning and control + +The purposes of these plugin interfaces are to create a separation of concern from the system software engineers and the researcher / algorithm designers. Each plugin type is hosted in a "task server" (e.g. planner, recovery, control servers) which handles requests and multiple algorithm plugin instances. The plugins are used to compute a value back to the server without having to worry about ROS 2 actions, topics, or other software utilities. A plugin designer can simply use the tools provided in the API to do their work, or create new ones if they like internally to gain additional information or capabilities. diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/behavior.hpp similarity index 65% rename from nav2_core/include/nav2_core/recovery.hpp rename to nav2_core/include/nav2_core/behavior.hpp index 5d3fa6de52..f3510e8684 100644 --- a/nav2_core/include/nav2_core/recovery.hpp +++ b/nav2_core/include/nav2_core/behavior.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_CORE__RECOVERY_HPP_ -#define NAV2_CORE__RECOVERY_HPP_ +#ifndef NAV2_CORE__BEHAVIOR_HPP_ +#define NAV2_CORE__BEHAVIOR_HPP_ #include #include @@ -27,18 +27,18 @@ namespace nav2_core { /** - * @class Recovery - * @brief Abstract interface for recoveries to adhere to with pluginlib + * @class Behavior + * @brief Abstract interface for behaviors to adhere to with pluginlib */ -class Recovery +class Behavior { public: - using Ptr = std::shared_ptr; + using Ptr = std::shared_ptr; /** * @brief Virtual destructor */ - virtual ~Recovery() {} + virtual ~Behavior() {} /** * @param parent pointer to user's node @@ -47,7 +47,7 @@ class Recovery * @param costmap_ros A pointer to the costmap */ virtual void configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & name, std::shared_ptr tf, std::shared_ptr collision_checker) = 0; @@ -57,25 +57,16 @@ class Recovery virtual void cleanup() = 0; /** - * @brief Method to active recovery and any threads involved in execution. + * @brief Method to active Behavior and any threads involved in execution. */ virtual void activate() = 0; /** - * @brief Method to deactive recovery and any threads involved in execution. + * @brief Method to deactive Behavior and any threads involved in execution. */ virtual void deactivate() = 0; - - /** - * @brief Method Execute recovery behavior - * @param name The name of this planner - * @return true if successful, false is failed to execute fully - */ - // TODO(stevemacenski) evaluate design for recoveries to not host - // their own servers and utilize a recovery server exposed action. - // virtual bool executeRecovery() = 0; }; } // namespace nav2_core -#endif // NAV2_CORE__RECOVERY_HPP_ +#endif // NAV2_CORE__BEHAVIOR_HPP_ diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 69aa10f25b..4e89e0b117 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 @@ -72,9 +73,9 @@ class Controller * @param costmap_ros A pointer to the costmap */ virtual void configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr &, - std::string name, const std::shared_ptr &, - const std::shared_ptr &) = 0; + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string name, std::shared_ptr, + std::shared_ptr) = 0; /** * @brief Method to cleanup resources. @@ -107,11 +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 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, const bool & percentage) = 0; }; } // namespace nav2_core diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index e04b0d86f0..bdb53696b7 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/nav2_core/include/nav2_core/global_planner.hpp @@ -50,10 +50,10 @@ namespace nav2_core * @param tf A pointer to a TF buffer * @param costmap_ros A pointer to the costmap */ - virtual void configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, std::shared_ptr tf, - std::shared_ptr costmap_ros) = 0; + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) = 0; /** * @brief Method to cleanup resources used on shutdown. diff --git a/nav2_core/include/nav2_core/goal_checker.hpp b/nav2_core/include/nav2_core/goal_checker.hpp index a8006e137a..03e11c9a3e 100644 --- a/nav2_core/include/nav2_core/goal_checker.hpp +++ b/nav2_core/include/nav2_core/goal_checker.hpp @@ -43,6 +43,9 @@ #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + + namespace nav2_core { @@ -64,12 +67,15 @@ class GoalChecker /** * @brief Initialize any parameters from the NodeHandle - * @param nh NodeHandle for grabbing parameters + * @param parent Node pointer for grabbing parameters */ virtual void initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, - const std::string & plugin_name) = 0; + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros) = 0; + virtual void reset() = 0; + /** * @brief Check whether the goal should be considered reached * @param query_pose The pose to check @@ -80,6 +86,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/include/nav2_core/progress_checker.hpp b/nav2_core/include/nav2_core/progress_checker.hpp index 2dd9c5bef8..0ab0402bf1 100644 --- a/nav2_core/include/nav2_core/progress_checker.hpp +++ b/nav2_core/include/nav2_core/progress_checker.hpp @@ -40,10 +40,10 @@ class ProgressChecker /** * @brief Initialize parameters for ProgressChecker - * @param node Node pointer + * @param parent Node pointer */ virtual void initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) = 0; /** * @brief Checks if the robot has moved compare to previous diff --git a/nav2_core/include/nav2_core/smoother.hpp b/nav2_core/include/nav2_core/smoother.hpp new file mode 100644 index 0000000000..e58b4a5ec6 --- /dev/null +++ b/nav2_core/include/nav2_core/smoother.hpp @@ -0,0 +1,83 @@ +// Copyright (c) 2021 RoboTech Vision +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_CORE__SMOOTHER_HPP_ +#define NAV2_CORE__SMOOTHER_HPP_ + +#include +#include + +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_costmap_2d/footprint_subscriber.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "pluginlib/class_loader.hpp" +#include "nav_msgs/msg/path.hpp" + + +namespace nav2_core +{ + +/** + * @class Smoother + * @brief smoother interface that acts as a virtual base class for all smoother plugins + */ +class Smoother +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Virtual destructor + */ + virtual ~Smoother() {} + + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string name, std::shared_ptr, + std::shared_ptr, + std::shared_ptr) = 0; + + /** + * @brief Method to cleanup resources. + */ + virtual void cleanup() = 0; + + /** + * @brief Method to activate smoother and any threads involved in execution. + */ + virtual void activate() = 0; + + /** + * @brief Method to deactivate smoother and any threads involved in execution. + */ + virtual void deactivate() = 0; + + /** + * @brief Method to smooth given path + * + * @param path In-out path to be smoothed + * @param max_time Maximum duration smoothing should take + * @return If smoothing was completed (true) or interrupted by time limit (false) + */ + virtual bool smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) = 0; +}; + +} // namespace nav2_core + +#endif // NAV2_CORE__SMOOTHER_HPP_ diff --git a/nav2_core/include/nav2_core/waypoint_task_executor.hpp b/nav2_core/include/nav2_core/waypoint_task_executor.hpp new file mode 100644 index 0000000000..ffdbfd95e1 --- /dev/null +++ b/nav2_core/include/nav2_core/waypoint_task_executor.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2020 Fetullah Atas +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_ +#define NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_ +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace nav2_core +{ +/** + * @brief Base class for creating a plugin in order to perform a specific task at waypoint arrivals. + * + */ +class WaypointTaskExecutor +{ +public: + /** + * @brief Construct a new Simple Task Execution At Waypoint Base object + * + */ + WaypointTaskExecutor() {} + + /** + * @brief Destroy the Simple Task Execution At Waypoint Base object + * + */ + virtual ~WaypointTaskExecutor() {} + + /** + * @brief Override this to setup your pub, sub or any ros services that you will use in the plugin. + * + * @param parent parent node that plugin will be created within(for an example see nav_waypoint_follower) + * @param plugin_name plugin name comes from parameters in yaml file + */ + virtual void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) = 0; + + /** + * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint + * + * @param curr_pose current pose of the robot + * @param curr_waypoint_index current waypoint, that robot just arrived + * @return true if task execution was successful + * @return false if task execution failed + */ + virtual bool processAtWaypoint( + const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index) = 0; +}; +} // namespace nav2_core +#endif // NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_ diff --git a/nav2_core/package.xml b/nav2_core/package.xml index e1cc7e6e0d..9bf5dcf1e5 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,8 +2,8 @@ nav2_core - 0.4.7 - A set of headers for plugins core to the navigation2 stack + 1.1.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/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 0c3c58481d..35fc721f70 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -49,11 +49,9 @@ add_library(nav2_costmap_2d_core SHARED src/observation_buffer.cpp src/clear_costmap_service.cpp src/footprint_collision_checker.cpp + plugins/costmap_filters/costmap_filter.cpp ) -# prevent pluginlib from using boost -target_compile_definitions(nav2_costmap_2d_core PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - set(dependencies geometry_msgs laser_geometry @@ -95,6 +93,17 @@ target_link_libraries(layers nav2_costmap_2d_core ) +add_library(filters SHARED + plugins/costmap_filters/keepout_filter.cpp + plugins/costmap_filters/speed_filter.cpp +) +ament_target_dependencies(filters + ${dependencies} +) +target_link_libraries(filters + nav2_costmap_2d_core +) + add_library(nav2_costmap_2d_client SHARED src/footprint_subscriber.cpp src/costmap_subscriber.cpp @@ -131,11 +140,13 @@ ament_target_dependencies(nav2_costmap_2d target_link_libraries(nav2_costmap_2d nav2_costmap_2d_core layers + filters ) install(TARGETS nav2_costmap_2d_core layers + filters nav2_costmap_2d_client ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -169,7 +180,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(layers nav2_costmap_2d_core nav2_costmap_2d_client) +ament_export_libraries(layers filters nav2_costmap_2d_core nav2_costmap_2d_client) ament_export_dependencies(${dependencies}) ament_export_definitions("PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md index 9dd12247e1..b2eb9a2b94 100644 --- a/nav2_costmap_2d/README.md +++ b/nav2_costmap_2d/README.md @@ -1,59 +1,12 @@ # Nav2 Costmap_2d -The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors A plugin interface allows for the layers to be combined into the -costmap and finally inflated via a user specified inflation radius. The nav2 version of the costmap_2d package is mostly a direct -ROS2 port of the ROS1 navigation stack version, with minimal noteable changes necessary due to support in ROS2. +The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors. A plugin interface allows for the layers to be combined into the costmap and finally inflated via an inflation radius based on the robot footprint. The nav2 version of the costmap_2d package is mostly a direct ROS2 port of the ROS1 navigation stack version, with minimal noteable changes necessary due to support in ROS2. -## Overview of Changes from ROS1 Navigation Costmap_2d -- Removal of legacy parameter style ("Loading from pre-hydro parameter style") -- Intermediate replacement of dynamic reconfigure (not ported to ROS2). This discussion started here with costmap_2d but is a more -widespread discussion throughout the navigation stack (see issue https://github.com/ros-planning/navigation2/issues/177) and -general ROS2 community. A proposal temporary replacement has been submitted as a PR here: https://github.com/ros-planning/navigation2/pull/196 +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-costmaps.html) for additional parameter descriptions for the costmap and its included plugins. The [tutorials](https://navigation.ros.org/tutorials/index.html) and [first-time setup guides](https://navigation.ros.org/setup_guides/index.html) also provide helpful context for working with the costmap 2D package and its layers. A [tutorial](https://navigation.ros.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html) is also provided to explain how to create costmap plugins. -## How to configure using Voxel Layer plugin: -By default, the navigation stack uses the _Obstacle Layer_ to avoid obstacles in 2D. The _Voxel Layer_ allows for detecting obstacles in 3D using Pointcloud2 data. It requires Pointcloud2 data being published on some topic. For simulation, a Gazebo model of the robot with depth camera enabled will publish a pointcloud topic. +See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. -The Voxel Layer plugin can be used to update the local costmap, glocal costmap or both, depending on how you define it in the `nav2_params.yaml` file in the nav2_bringup directory. The voxel layer plugin has to be added to the list of ```plugin_names``` and ```plugin_types``` in the global/local costmap scopes in the param file. If these are not defined in the param file, the default plugins set in nav2_costmap_2d will be used. - -Inside each costmap layer (voxel, obstacle, etc) define `observation_sources` param. Here you can define multiple sources to be used with the layer. The param configuration example below shows the way you can configure costmaps to use voxel layer. - -The `voxel_layer` param has `observation_source: pointcloud` in it's scope, and the param `pointcloud` has `topic`, and `data_type` inside it's scope. By default, the data_type is `LaserScan`, thus you need to specify `PointCloud2` if you are using PointCould2 data from a depth sensor. - -Example param configuration snippet for enabling voxel layer in local costmap is shown below (not all params are shown): -``` -local_costmap: - local_costmap: - ros__parameters: - plugin_names: ["obstacle_layer", "voxel_layer", "inflation_layer"] - plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"] - obstacle_layer: - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - voxel_layer: - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.2 - z_voxels: 10 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: pointcloud - pointcloud: - topic: /intel_realsense_r200_depth/points - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "PointCloud2" -``` -Please note that not all params needed to run the navigation stack are shown here. This example only shows how you can add different costmap layers, with multiple input sources of different types. - -### To visualize the voxels in RVIZ: +## To visualize the voxels in RVIZ: - Make sure `publish_voxel_map` in `voxel_layer` param's scope is set to `True`. - Open a new terminal and run: ```ros2 run nav2_costmap_2d nav2_costmap_2d_markers voxel_grid:=/local_costmap/voxel_grid visualization_marker:=/my_marker``` @@ -62,34 +15,13 @@ Please note that not all params needed to run the navigation stack are shown her - Then add `my_marker` to RVIZ using the GUI. -####Errata: +### Errata: - To see the markers in 3D, you will need to change the _view_ in RVIZ to a 3 dimensional view (e.g. orbit) from the RVIZ GUI. - Currently due to some bug in rviz, you need to set the `fixed_frame` in the rviz display, to `odom` frame. - Using pointcloud data from a saved bag file while using gazebo simulation can be troublesome due to the clock time skipping to an earlier time. -## How to use multiple sensor sources: -Multiple sources can be added into a costmap layer (e.g., obstacle layer), by listing them under the 'observation_sources' for that layer. -For example, to add laser scan and pointcloud as two different sources of inputs to the obstacle layer, you can define them in the params file as shown below for the local costmap: -``` -local_costmap: - local_costmap: - ros__parameters: - plugin_names: ["obstacle_layer", "inflation_layer"] - plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"] - obstacle_layer: - enabled: True - observation_sources: scan pointcloud - scan: - topic: /scan - data_type: "LaserScan" - pointcloud: - topic: /intel_realsense_r200_depth/points - data_type: "PointCloud2" -``` -In order to add multiple sources to the global costmap, follow the same procedure shown in the example above, but now adding the sources and their specific params under the `global_costmap` scope. +## Costmap Filters -## 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 -to broaden this world model concept and use costmap's layer concept as motivation for providing a service-style interface to -potential clients needing information about the world (see issue https://github.com/ros-planning/navigation2/issues/18) +### 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 Nav2 website: https://navigation.ros.org. diff --git a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg index 0337e4123a..a94e98fbbe 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/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index a072539233..d7cb985493 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -16,5 +16,14 @@ A range-sensor (sonar, IR) based obstacle layer for costmap_2d + + + + Prevents the robot from appearing in keepout zones marked on map. + + + Restricts maximum speed of robot in speed-limit zones marked on map. + + 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 f1378848df..46d79bd9df 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,25 +31,36 @@ namespace nav2_costmap_2d class Costmap2DROS; +/** + * @class ClearCostmapService + * @brief Exposes services to clear costmap objects in inclusive/exclusive regions or completely + */ class ClearCostmapService { public: - ClearCostmapService(nav2_util::LifecycleNode::SharedPtr node, Costmap2DROS & costmap); - + /** + * @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 - void clearExceptRegion(double reset_distance = 3.0); - - // Clears within a window around the robot - void clearAroundRobot(double window_size_x, double window_size_y); + /** + * @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: - // The ROS node to use for getting parameters, creating the service and logging - nav2_util::LifecycleNode::SharedPtr node_; + // The Logger object for logging + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; // The costmap to clear Costmap2DROS & costmap_; @@ -60,31 +71,43 @@ 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); - void clearLayerExceptRegion( - std::shared_ptr & costmap, double pose_x, double pose_y, double reset_distance); - - bool isClearable(const std::string & layer_name) const; + /** + * @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; - - std::string getLayerName(const Layer & layer) const; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp index 45725757f2..dca22c9f7f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp @@ -42,6 +42,7 @@ namespace nav2_costmap_2d static constexpr unsigned char NO_INFORMATION = 255; static constexpr unsigned char LETHAL_OBSTACLE = 254; static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; +static constexpr unsigned char MAX_NON_OBSTACLE = 252; static constexpr unsigned char FREE_SPACE = 0; } #endif // NAV2_COSTMAP_2D__COST_VALUES_HPP_ 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 d74895b40a..04b9f4daa4 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -48,6 +48,7 @@ #include #include #include "geometry_msgs/msg/point.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" namespace nav2_costmap_2d { @@ -87,6 +88,12 @@ class Costmap2D */ Costmap2D(const Costmap2D & map); + /** + * @brief Constructor for a costmap from an OccupancyGrid map + * @param map The OccupancyGrid map to create costmap from + */ + explicit Costmap2D(const nav_msgs::msg::OccupancyGrid & map); + /** * @brief Overloaded assignment operator * @param map The costmap to copy @@ -107,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 */ @@ -251,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_; @@ -302,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); @@ -383,16 +423,41 @@ 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; + int dx_full = x1 - x0; + int dy_full = y1 - y0; + + // we need to chose how much to scale our dominant dimension, + // based on the maximum length of the line + double dist = std::hypot(dx_full, dy_full); + if (dist < min_length) { + return; + } + + unsigned int min_x0, min_y0; + if (dist > 0.0) { + // Adjust starting point and offset to start from min_length distance + min_x0 = (unsigned int)(x0 + dx_full / dist * min_length); + min_y0 = (unsigned int)(y0 + dy_full / dist * min_length); + } else { + // dist can be 0 if [x0, y0]==[x1, y1]. + // In this case only this cell should be processed. + min_x0 = x0; + min_y0 = y0; + } + unsigned int offset = min_y0 * size_x_ + min_x0; + + int dx = x1 - min_x0; + int dy = y1 - min_y0; unsigned int abs_dx = abs(dx); unsigned int abs_dy = abs(dy); @@ -400,32 +465,27 @@ 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 x is dominant if (abs_dx >= abs_dy) { int error_y = abs_dx / 2; + 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, (unsigned int)(scale * abs_dx)); return; } // otherwise y is dominant int error_x = abs_dy / 2; + 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, (unsigned int)(scale * abs_dy)); } 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( @@ -447,6 +507,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 83349d4470..2074ad5ea6 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 { @@ -67,7 +67,7 @@ class Costmap2DPublisher * @brief Constructor for the Costmap2DPublisher */ Costmap2DPublisher( - nav2_util::LifecycleNode::SharedPtr ros_node, + const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2D * costmap, std::string global_frame, std::string topic_name, @@ -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. */ @@ -130,7 +145,9 @@ class Costmap2DPublisher const std::shared_ptr request, const std::shared_ptr response); - nav2_util::LifecycleNode::SharedPtr node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; + Costmap2D * costmap_; std::string global_frame_; std::string topic_name_; 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 124d1666a5..a5dfc6182a 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,9 +215,12 @@ 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_; + return layered_costmap_.get(); } /** @brief Returns the current padded footprint as a geometry_msgs::msg::Polygon. */ @@ -256,35 +294,52 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ bool getUseRadius() {return use_radius_;} -protected: - rclcpp::Node::SharedPtr client_node_; + /** + * @brief Get the costmap's robot_radius_ parameter, corresponding to + * raidus of the robot footprint when it is defined as as circle + * (i.e. when use_radius_ == true). + * @return robot_radius_ + */ + double getRobotRadius() {return robot_radius_;} +protected: // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; - Costmap2DPublisher * costmap_publisher_{nullptr}; + std::unique_ptr costmap_publisher_{nullptr}; rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; + // Dedicated callback group and executor for tf timer_interface and message fillter + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr executor_thread_; + // Transform listener std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - LayeredCostmap * layered_costmap_{nullptr}; + 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}; bool initialized_{false}; bool stopped_{true}; - std::thread * map_update_thread_{nullptr}; ///< @brief A thread for updating the map + std::unique_ptr map_update_thread_; ///< @brief A thread for updating the map rclcpp::Time last_publish_{0, 0, RCL_ROS_TIME}; 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 +355,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_; @@ -313,6 +370,16 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector padded_footprint_; std::unique_ptr clear_costmap_service_; + + // Dynamic parameters handler + OnSetParametersCallbackHandle::SharedPtr dyn_params_handler; + + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); }; } // namespace nav2_costmap_2d 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 new file mode 100644 index 0000000000..c71d3bfea1 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -0,0 +1,185 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Samsung Research Russia + * 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 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. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_ + +#include +#include + +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_costmap_2d/layer.hpp" + +namespace nav2_costmap_2d +{ + +/** + * @brief: CostmapFilter basic class. It is inherited from Layer in order to avoid + * hidden problems when the shared handling of costmap_ resource (PR #1936) + */ +class CostmapFilter : public Layer +{ +public: + /** + * @brief A constructor + */ + CostmapFilter(); + /** + * @brief A destructor + */ + ~CostmapFilter(); + + /** + * @brief: Provide a typedef to ease future code maintenance + */ + typedef std::recursive_mutex mutex_t; + /** + * @brief: returns pointer to a mutex + */ + mutex_t * getMutex() + { + return access_; + } + + /** + * @brief Initialization process of layer on startup + */ + 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 + */ + 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 + */ + 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 + */ + void activate() final; + /** + * @brief Deactivate the layer + */ + void deactivate() final; + /** + * @brief Reset the layer + */ + void reset() final; + + /** + * @brief If clearing operations should be processed on this layer or not + */ + bool isClearable() {return false;} + + /** CostmapFilter API **/ + /** + * @brief: Initializes costmap filter. Creates subscriptions to filter-related topics + * @param: Name of costmap filter info topic + */ + virtual void initializeFilter( + const std::string & filter_info_topic) = 0; + + /** + * @brief: An algorithm for how to use that map's information. Fills the Costmap2D with + * calculated data and makes an action based on processed data + * @param: Reference to a master costmap2d + * @param: Low window map boundary OX + * @param: Low window map boundary OY + * @param: High window map boundary OX + * @param: High window map boundary OY + * @param: Robot 2D-pose + */ + virtual 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) = 0; + + /** + * @brief: Resets costmap filter. Stops all subscriptions + */ + virtual void resetFilter() = 0; + +protected: + /** + * @brief: Name of costmap filter info topic + */ + std::string filter_info_topic_; + + /** + * @brief: Name of filter mask topic + */ + std::string mask_topic_; + + /** + * @brief: mask_frame_->global_frame_ transform tolerance + */ + tf2::Duration transform_tolerance_; + +private: + /** + * @brief: Latest robot position + */ + geometry_msgs::msg::Pose2D latest_pose_; + + /** + * @brief: Mutex for locking filter's resources + */ + mutex_t * access_; +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp new file mode 100644 index 0000000000..03a157a3e4 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp @@ -0,0 +1,62 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Samsung Research Russia + * 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 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. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_ + +/** Provides constants used in costmap filters */ + +namespace nav2_costmap_2d +{ + +/** Types of costmap filter */ +static constexpr uint8_t KEEPOUT_FILTER = 0; +static constexpr uint8_t SPEED_FILTER_PERCENT = 1; +static constexpr uint8_t SPEED_FILTER_ABSOLUTE = 2; + +/** Default values for base and multiplier */ +static constexpr double BASE_DEFAULT = 0.0; +static constexpr double MULTIPLIER_DEFAULT = 1.0; + +/** Speed filter constants */ +static constexpr int8_t SPEED_MASK_UNKNOWN = -1; +static constexpr int8_t SPEED_MASK_NO_LIMIT = 0; +static constexpr double NO_SPEED_LIMIT = 0.0; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_ 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 new file mode 100644 index 0000000000..e5434571d4 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -0,0 +1,111 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Samsung Research Russia + * 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 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. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_ + +#include +#include + +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" + +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_; + rclcpp::Subscription::SharedPtr mask_sub_; + + std::unique_ptr mask_costmap_; + + std::string mask_frame_; // Frame where mask located in + std::string global_frame_; // Frame of currnet layer (master_grid) +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_ 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 new file mode 100644 index 0000000000..c4655607aa --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp @@ -0,0 +1,148 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Samsung Research Russia + * 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 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. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_ + +#include +#include + +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" + +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" +#include "nav2_msgs/msg/speed_limit.hpp" + +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: + /** + * @brief: Transforms robot pose from current layer frame to mask frame + * @param: pose Robot pose in costmap frame + * @param: mask_pose Robot pose in mask frame + * @return: True if the transformation was successful, false otherwise + */ + bool transformPose( + const geometry_msgs::msg::Pose2D & pose, + geometry_msgs::msg::Pose2D & mask_pose) const; + + /** + * @brief: Convert from world coordinates to mask coordinates. + Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated mask x coordinate + * @param my Will be set to the associated mask y coordinate + * @return True if the conversion was successful (legal bounds) false otherwise + */ + bool worldToMask(double wx, double wy, unsigned int & mx, unsigned int & my) const; + + /** + * @brief Get the data of a cell in the filter mask + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @return The data of the selected cell + */ + inline int8_t getMaskData( + 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_; + rclcpp::Subscription::SharedPtr mask_sub_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr speed_limit_pub_; + + nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; + + std::string mask_frame_; // Frame where mask located in + std::string global_frame_; // Frame of currnet layer (master_grid) + + double base_, multiplier_; + bool percentage_; + double speed_limit_, speed_limit_prev_; +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_ 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 86b293cbfc..15285df5fe 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp @@ -45,22 +45,41 @@ 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(); - virtual void clearArea(int start_x, int start_y, int end_x, int end_y); + /** + * @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); /** * If an external source changes values in the costmap, 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 83d656842a..2c21d859b2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp @@ -56,17 +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); -bool intersects(std::vector & polygon, float testx, float testy); - -bool intersects( - std::vector & polygon1, - std::vector & polygon2); - #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 d124eaf04c..e4ea745015 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -25,37 +25,47 @@ namespace nav2_costmap_2d { - +/** + * @class CostmapSubscriber + * @brief Subscribes to the costmap via a ros topic + */ class CostmapSubscriber { public: + /** + * @brief A constructor + */ CostmapSubscriber( - nav2_util::LifecycleNode::SharedPtr node, - const std::string & topic_name); - - CostmapSubscriber( - rclcpp::Node::SharedPtr node, + const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name); + /** + * @brief A constructor + */ CostmapSubscriber( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + 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: - // Interfaces used for logging and creating publishers and subscribers - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - + /** + * @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); +protected: std::shared_ptr costmap_; nav2_msgs::msg::Costmap::SharedPtr costmap_msg_; std::string topic_name_; 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 9f54728e63..4f645422fd 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 @@ -29,46 +29,71 @@ #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_costmap_2d/footprint_subscriber.hpp" -#include "nav2_util/robot_utils.hpp" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#include "tf2/utils.h" -#pragma GCC diagnostic pop 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, - tf2_ros::Buffer & tf, - std::string name = "collision_checker", - std::string global_frame = "map", - std::string robot_base_frame = "base_link", - double transform_tolerance = 0.1); + std::string name = "collision_checker"); + /** + * @brief A destructor + */ ~CostmapTopicCollisionChecker() = default; - // Returns the obstacle footprint score for a particular pose - double scorePose(const geometry_msgs::msg::Pose2D & pose); - bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose); + /** + * @brief Returns the obstacle footprint score for a particular pose + * + * @param pose Pose to get score at + * @param fetch_costmap_and_footprint Defaults to true. When checking with multiple poses at once, + * data should be fetched in the first check but fetching can be skipped in consequent checks for speedup + */ + double scorePose( + const geometry_msgs::msg::Pose2D & pose, + bool fetch_costmap_and_footprint = true); + + /** + * @brief Returns if a pose is collision free + * + * @param pose Pose to check collision at + * @param fetch_costmap_and_footprint Defaults to true. When checking with multiple poses at once, + * data should be fetched in the first check but fetching can be skipped in consequent checks for speedup + */ + bool isCollisionFree( + const geometry_msgs::msg::Pose2D & pose, + bool fetch_costmap_and_footprint = true); protected: - void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint); - Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose); + /** + * @brief Get a footprint at a set pose + * + * @param pose Pose to get footprint at + * @param fetch_latest_footprint Defaults to true. When checking with multiple poses at once, + * footprint should be fetched in the first check but fetching can be skipped in consequent checks for speedup + */ + Footprint getFootprint( + const geometry_msgs::msg::Pose2D & pose, + bool fetch_latest_footprint = true); // Name used for logging std::string name_; - std::string global_frame_; - std::string robot_base_frame_; - tf2_ros::Buffer & tf_; CostmapSubscriber & costmap_sub_; FootprintSubscriber & footprint_sub_; - double transform_tolerance_; FootprintCollisionChecker> collision_checker_; + rclcpp::Clock::SharedPtr clock_; + Footprint footprint_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp index 0646555dad..0db73867cb 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 39c51328b8..c90bad5ddd 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 acc3e7c234..9993d9190a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp @@ -21,57 +21,76 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/footprint.hpp" #include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/robot_utils.hpp" 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( - nav2_util::LifecycleNode::SharedPtr node, + const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name, - const double & footprint_timeout); + tf2_ros::Buffer & tf, + std::string robot_base_frame = "base_link", + double transform_tolerance = 0.1); + /** + * @brief A constructor + */ FootprintSubscriber( - rclcpp::Node::SharedPtr node, + const rclcpp::Node::WeakPtr & parent, const std::string & topic_name, - const double & footprint_timeout); - - FootprintSubscriber( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, - const std::string & topic_name, - const double & footprint_timeout); + tf2_ros::Buffer & tf, + std::string robot_base_frame = "base_link", + double transform_tolerance = 0.1); + /** + * @brief A destructor + */ ~FootprintSubscriber() {} - // Returns an oriented robot footprint at current time. - bool getFootprint( + /** + * @brief Returns the latest robot footprint, in the form as received from topic (oriented). + * + * @param footprint Output param. Latest received footprint + * @param footprint_header Output param. Header associated with the footprint + * @return False if no footprint has been received + */ + bool getFootprintRaw( std::vector & footprint, - rclcpp::Duration & valid_footprint_timeout); - bool getFootprint(std::vector & footprint); + std_msgs::msg::Header & footprint_header); - // Returns an oriented robot footprint. - // The second parameter is the time the fooptrint was at this orientation. - bool getFootprint( + /** + * @brief Returns the latest robot footprint, transformed into robot base frame (unoriented). + * + * @param footprint Output param. Latest received footprint, unoriented + * @param footprint_header Output param. Header associated with the footprint + * @return False if no footprint has been received or if transformation failed + */ + bool getFootprintInRobotFrame( std::vector & footprint, - rclcpp::Time & stamp, rclcpp::Duration valid_footprint_timeout); + std_msgs::msg::Header & footprint_header); protected: - // Interfaces used for logging and creating publishers and subscribers - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; - + /** + * @brief Callback to process new footprint updates. + */ void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg); std::string topic_name_; + tf2_ros::Buffer & tf_; + std::string robot_base_frame_; + double transform_tolerance_; bool footprint_received_{false}; - rclcpp::Duration footprint_timeout_; geometry_msgs::msg::PolygonStamped::SharedPtr footprint_; rclcpp::Subscription::SharedPtr footprint_sub_; }; 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 4acf05bd55..5f68dcaed1 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -73,28 +73,73 @@ 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(); + current_ = false; } /** @brief Given a distance, compute a cost. @@ -118,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: @@ -161,19 +213,38 @@ 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); + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + double inflation_radius_, inscribed_radius_, cost_scaling_factor_; bool inflate_unknown_, inflate_around_unknown_; unsigned int cell_inflation_radius_; @@ -193,6 +264,8 @@ class InflationLayer : public Layer // Indicates that the entire costmap should be reinflated next time around. bool need_reinflation_; mutex_t * access_; + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index b0eca7251a..72125143ba 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -51,23 +51,43 @@ 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, tf2_ros::Buffer * tf, - nav2_util::LifecycleNode::SharedPtr 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. */ + const nav2_util::LifecycleNode::WeakPtr & node, + rclcpp::CallbackGroup::SharedPtr callback_group); + /** @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; /** * @brief This is called by the LayeredCostmap to poll this plugin as to how @@ -98,7 +118,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_; @@ -123,18 +143,26 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface const std::vector & getFootprint() const; /** @brief Convenience functions for declaring ROS parameters */ - void declareParameter(const std::string & param_name, const rclcpp::ParameterValue & value); + 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 rclcpp::ParameterType & param_type); + /** @brief Convenience functions for declaring ROS parameters */ bool hasParameter(const std::string & param_name); - void undeclareAllParameters(); + /** @brief Convenience functions for declaring ROS parameters */ std::string getFullName(const std::string & param_name); protected: LayeredCostmap * layered_costmap_; std::string name_; tf2_ros::Buffer * tf_; - nav2_util::LifecycleNode::SharedPtr node_; - rclcpp::Node::SharedPtr client_node_; - rclcpp::Node::SharedPtr rclcpp_node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; /** @brief This is called at the end of initialize(). Override to * implement subclass-specific initialization. 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 4964b03ef8..ddf64ffbdc 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,77 @@ 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_; } - void addPlugin(std::shared_ptr plugin) + /** + * @brief Get the vector of pointers to the costmap filters + */ + std::vector> * getFilters() { - plugins_.push_back(plugin); + return &filters_; } + /** + * @brief Add a new plugin to the plugins vector to process + */ + void addPlugin(std::shared_ptr 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 +176,9 @@ class LayeredCostmap *yn = byn_; } + /** + * @brief if the costmap is initialized + */ bool isInitialized() { return initialized_; @@ -163,7 +211,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 +226,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 5095a84590..9e1356cb6c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp @@ -50,10 +50,14 @@ 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_; @@ -67,8 +71,10 @@ class Observation { 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_; return *this; } @@ -77,14 +83,19 @@ class Observation * @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) { } @@ -94,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 4c816dad28..2ff834a41e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp @@ -41,14 +41,15 @@ #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" + namespace nav2_costmap_2d { /** @@ -65,37 +66,32 @@ 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 * @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame */ ObservationBuffer( - nav2_util::LifecycleNode::SharedPtr nh, + const nav2_util::LifecycleNode::WeakPtr & parent, 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, - double tf_tolerance); + tf2::Duration tf_tolerance); /** * @brief Destructor... cleans up */ ~ObservationBuffer(); - /** - * @brief Sets the global frame of an observation buffer. This will - * transform all the currently cached observations to the new global - * frame - * @param new_global_frame The name of the new global frame. - * @return True if the operation succeeds, false otherwise - */ - bool setGlobalFrame(const std::string new_global_frame); - /** * @brief Transforms a PointCloud to the global frame and buffers it * Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier @@ -142,10 +138,11 @@ class ObservationBuffer */ void purgeStaleObservations(); + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; tf2_ros::Buffer & tf2_buffer_; const rclcpp::Duration observation_keep_time_; const rclcpp::Duration expected_update_rate_; - nav2_util::LifecycleNode::SharedPtr nh_; rclcpp::Time last_updated_; std::string global_frame_; std::string sensor_frame_; @@ -153,8 +150,8 @@ 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 tf_tolerance_; + double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_; + tf2::Duration tf_tolerance_; }; } // namespace nav2_costmap_2d #endif // NAV2_COSTMAP_2D__OBSERVATION_BUFFER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 8e56cb3836..fbced80c80 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,83 @@ 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;} + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + /** * @brief triggers the update of observations buffer */ @@ -150,14 +205,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, @@ -170,7 +231,8 @@ class ObstacleLayer : public CostmapLayer /// @brief Used to project laser scans into point clouds laser_geometry::LaserProjection projector_; /// @brief Used for the observation message filters - std::vector> observation_subscribers_; + std::vector>> + observation_subscribers_; /// @brief Used to make sure that transforms are available for each sensor std::vector> observation_notifiers_; /// @brief Used to store observations from various sensors @@ -180,11 +242,15 @@ class ObstacleLayer : public CostmapLayer /// @brief Used to store observation buffers used for clearing obstacles std::vector> clearing_buffers_; + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + // Used only for testing purposes std::vector static_clearing_observations_; std::vector static_marking_observations_; bool rolling_window_; + bool was_reset_; int combination_method_; }; 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 ce2861599d..71cd4654a9 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,44 +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); @@ -116,6 +208,7 @@ class RangeSensorLayer : public CostmapLayer double clear_threshold_, mark_threshold_; bool clear_on_max_reading_; + bool was_reset_; tf2::Duration transform_tolerance_; double no_readings_timeout_; @@ -124,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 bbd5daa757..def79e349f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -40,6 +40,7 @@ #include #include +#include #include "map_msgs/msg/occupancy_grid_update.hpp" #include "message_filters/subscriber.h" @@ -51,29 +52,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); /** @@ -83,10 +141,25 @@ 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); + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + std::string global_frame_; ///< @brief The global frame for the costmap std::string map_frame_; /// @brief frame that map is located in @@ -113,6 +186,8 @@ class StaticLayer : public CostmapLayer tf2::Duration transform_tolerance_; std::atomic update_in_progress_; nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 92445cd71f..14baefcbdd 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -38,6 +38,9 @@ #ifndef NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_ #define NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_ +#include +#include "message_filters/subscriber.h" + #include #include #include @@ -48,45 +51,92 @@ #include #include #include -#include #include #include 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(); - 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, @@ -98,8 +148,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) @@ -117,6 +171,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) @@ -136,6 +193,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, @@ -147,15 +207,31 @@ 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_; } + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/launch/example_params.yaml b/nav2_costmap_2d/launch/example_params.yaml index 783d348663..72bf695fad 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 5688118923..e466dd9847 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.7 + 1.1.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 @@ -19,7 +19,6 @@ ament_cmake nav2_common - angles geometry_msgs laser_geometry map_msgs @@ -38,6 +37,7 @@ tf2_ros tf2_sensor_msgs visualization_msgs + angles ament_lint_common ament_lint_auto diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp new file mode 100644 index 0000000000..cdec0a4b2e --- /dev/null +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -0,0 +1,123 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Samsung Research Russia + * 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 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. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" + +#include + +namespace nav2_costmap_2d +{ + +CostmapFilter::CostmapFilter() +: filter_info_topic_(""), mask_topic_("") +{ + access_ = new mutex_t(); +} + +CostmapFilter::~CostmapFilter() +{ + delete access_; +} + +void CostmapFilter::onInitialize() +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + 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(); + 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; + } +} + +void CostmapFilter::activate() +{ + initializeFilter(filter_info_topic_); +} + +void CostmapFilter::deactivate() +{ + resetFilter(); +} + +void CostmapFilter::reset() +{ + resetFilter(); + initializeFilter(filter_info_topic_); + current_ = false; +} + +void CostmapFilter::updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * /*min_x*/, double * /*min_y*/, double * /*max_x*/, double * /*max_y*/) +{ + if (!enabled_) { + return; + } + + latest_pose_.x = robot_x; + latest_pose_.y = robot_y; + latest_pose_.theta = robot_yaw; +} + +void CostmapFilter::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) { + return; + } + + process(master_grid, min_i, min_j, max_i, max_j, latest_pose_); + current_ = true; +} + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp new file mode 100644 index 0000000000..4a5571e42e --- /dev/null +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -0,0 +1,315 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Samsung Research Russia + * 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 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. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#include +#include +#include +#include "tf2/convert.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" + +namespace nav2_costmap_2d +{ + +KeepoutFilter::KeepoutFilter() +: filter_info_sub_(nullptr), mask_sub_(nullptr), mask_costmap_(nullptr), + mask_frame_(""), global_frame_("") +{ +} + +void KeepoutFilter::initializeFilter( + const std::string & filter_info_topic) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + filter_info_topic_ = filter_info_topic; + // Setting new costmap filter info subscriber + RCLCPP_INFO( + logger_, + "KeepoutFilter: Subscribing to \"%s\" topic for filter info...", + filter_info_topic_.c_str()); + filter_info_sub_ = node->create_subscription( + filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1)); + + global_frame_ = layered_costmap_->getGlobalFrameID(); +} + +void KeepoutFilter::filterInfoCallback( + const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!mask_sub_) { + RCLCPP_INFO( + logger_, + "KeepoutFilter: Received filter info from %s topic.", filter_info_topic_.c_str()); + } else { + RCLCPP_WARN( + logger_, + "KeepoutFilter: New costmap filter info arrived from %s topic. Updating old filter info.", + filter_info_topic_.c_str()); + // Resetting previous subscriber each time when new costmap filter information arrives + mask_sub_.reset(); + } + + // Checking that base and multiplier are set to their default values + if (msg->base != BASE_DEFAULT || msg->multiplier != MULTIPLIER_DEFAULT) { + RCLCPP_ERROR( + logger_, + "KeepoutFilter: For proper use of keepout filter base and multiplier" + " in CostmapFilterInfo message should be set to their default values (%f and %f)", + BASE_DEFAULT, MULTIPLIER_DEFAULT); + } + + mask_topic_ = msg->filter_mask_topic; + + // Setting new filter mask subscriber + RCLCPP_INFO( + logger_, + "KeepoutFilter: Subscribing to \"%s\" topic for filter mask...", + mask_topic_.c_str()); + mask_sub_ = node->create_subscription( + mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1)); +} + +void KeepoutFilter::maskCallback( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!mask_costmap_) { + RCLCPP_INFO( + logger_, + "KeepoutFilter: Received filter mask from %s topic.", mask_topic_.c_str()); + } else { + RCLCPP_WARN( + logger_, + "KeepoutFilter: New filter mask arrived from %s topic. Updating old filter mask.", + mask_topic_.c_str()); + mask_costmap_.reset(); + } + + // Making a new mask_costmap_ + mask_costmap_ = std::make_unique(*msg); + mask_frame_ = msg->header.frame_id; +} + +void KeepoutFilter::process( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j, + const geometry_msgs::msg::Pose2D & /*pose*/) +{ + std::lock_guard guard(*getMutex()); + + if (!mask_costmap_) { + // Show warning message every 2 seconds to not litter an output + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 2000, + "KeepoutFilter: Filter mask was not received"); + return; + } + + tf2::Transform tf2_transform; + tf2_transform.setIdentity(); // initialize by identical transform + int mg_min_x, mg_min_y; // masger_grid indexes of bottom-left window corner + int mg_max_x, mg_max_y; // masger_grid indexes of top-right window corner + + if (mask_frame_ != global_frame_) { + // Filter mask and current layer are in different frames: + // prepare frame transformation if mask_frame_ != global_frame_ + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_->lookupTransform( + mask_frame_, global_frame_, tf2::TimePointZero, + transform_tolerance_); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, + "KeepoutFilter: Failed to get costmap frame (%s) " + "transformation to mask frame (%s) with error: %s", + global_frame_.c_str(), mask_frame_.c_str(), ex.what()); + return; + } + tf2::fromMsg(transform.transform, tf2_transform); + + mg_min_x = min_i; + mg_min_y = min_j; + mg_max_x = max_i; + mg_max_y = max_j; + } else { + // Filter mask and current layer are in the same frame: + // apply the following optimization - iterate only in overlapped + // (min_i, min_j)..(max_i, max_j) & mask_costmap_ area. + // + // mask_costmap_ + // *----------------------------* + // | | + // | | + // | (2) | + // *-----+-------* | + // | |///////|<- overlapped area | + // | |///////| to iterate in | + // | *-------+--------------------* + // | (1) | + // | | + // *-------------* + // master_grid (min_i, min_j)..(max_i, max_j) window + // + // ToDo: after costmap rotation will be added, this should be re-worked. + + double wx, wy; // world coordinates + + // Calculating bounds corresponding to bottom-left overlapping (1) corner + // mask_costmap_ -> master_grid intexes conversion + const double half_cell_size = 0.5 * mask_costmap_->getResolution(); + wx = mask_costmap_->getOriginX() + half_cell_size; + wy = mask_costmap_->getOriginY() + half_cell_size; + master_grid.worldToMapNoBounds(wx, wy, mg_min_x, mg_min_y); + // Calculation of (1) corner bounds + if (mg_min_x >= max_i || mg_min_y >= max_j) { + // There is no overlapping. Do nothing. + return; + } + mg_min_x = std::max(min_i, mg_min_x); + mg_min_y = std::max(min_j, mg_min_y); + + // Calculating bounds corresponding to top-right window (2) corner + // mask_costmap_ -> master_grid intexes conversion + wx = mask_costmap_->getOriginX() + + mask_costmap_->getSizeInCellsX() * mask_costmap_->getResolution() + half_cell_size; + wy = mask_costmap_->getOriginY() + + mask_costmap_->getSizeInCellsY() * mask_costmap_->getResolution() + half_cell_size; + master_grid.worldToMapNoBounds(wx, wy, mg_max_x, mg_max_y); + // Calculation of (2) corner bounds + if (mg_max_x <= min_i || mg_max_y <= min_j) { + // There is no overlapping. Do nothing. + return; + } + mg_max_x = std::min(max_i, mg_max_x); + mg_max_y = std::min(max_j, mg_max_y); + } + + // unsigned<-signed conversions. + unsigned const int mg_min_x_u = static_cast(mg_min_x); + unsigned const int mg_min_y_u = static_cast(mg_min_y); + unsigned const int mg_max_x_u = static_cast(mg_max_x); + unsigned const int mg_max_y_u = static_cast(mg_max_y); + + unsigned int i, j; // master_grid iterators + unsigned int index; // corresponding index of master_grid + double gl_wx, gl_wy; // world coordinates in a global_frame_ + double msk_wx, msk_wy; // world coordinates in a mask_frame_ + unsigned int mx, my; // mask_costmap_ coordinates + unsigned char data, old_data; // master_grid element data + + // Main master_grid updating loop + // Iterate in costmap window by master_grid indexes + unsigned char * master_array = master_grid.getCharMap(); + for (i = mg_min_x_u; i < mg_max_x_u; i++) { + for (j = mg_min_y_u; j < mg_max_y_u; j++) { + index = master_grid.getIndex(i, j); + old_data = master_array[index]; + // Calculating corresponding to (i, j) point at mask_costmap_: + // Get world coordinates in global_frame_ + master_grid.mapToWorld(i, j, gl_wx, gl_wy); + if (mask_frame_ != global_frame_) { + // Transform (i, j) point from global_frame_ to mask_frame_ + tf2::Vector3 point(gl_wx, gl_wy, 0); + point = tf2_transform * point; + msk_wx = point.x(); + msk_wy = point.y(); + } else { + // In this case master_grid and filter-mask are in the same frame + msk_wx = gl_wx; + msk_wy = gl_wy; + } + // Get mask coordinates corresponding to (i, j) point at mask_costmap_ + if (mask_costmap_->worldToMap(msk_wx, msk_wy, mx, my)) { + data = mask_costmap_->getCost(mx, my); + // Update if mask_ data is valid and greater than existing master_grid's one + if (data == NO_INFORMATION) { + continue; + } + if (data > old_data || old_data == NO_INFORMATION) { + master_array[index] = data; + } + } + } + } +} + +void KeepoutFilter::resetFilter() +{ + std::lock_guard guard(*getMutex()); + + filter_info_sub_.reset(); + mask_sub_.reset(); +} + +bool KeepoutFilter::isActive() +{ + std::lock_guard guard(*getMutex()); + + if (mask_costmap_) { + return true; + } + return false; +} + +} // namespace nav2_costmap_2d + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::KeepoutFilter, nav2_costmap_2d::Layer) diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp new file mode 100644 index 0000000000..cc368b4683 --- /dev/null +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -0,0 +1,354 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Samsung Research Russia + * 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 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. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#include "nav2_costmap_2d/costmap_filters/speed_filter.hpp" + +#include +#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" + +namespace nav2_costmap_2d +{ + +SpeedFilter::SpeedFilter() +: filter_info_sub_(nullptr), mask_sub_(nullptr), + speed_limit_pub_(nullptr), filter_mask_(nullptr), mask_frame_(""), global_frame_(""), + speed_limit_(NO_SPEED_LIMIT), speed_limit_prev_(NO_SPEED_LIMIT) +{ +} + +void SpeedFilter::initializeFilter( + const std::string & filter_info_topic) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Declare "speed_limit_topic" parameter specific to SpeedFilter only + std::string speed_limit_topic; + declareParameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit")); + node->get_parameter(name_ + "." + "speed_limit_topic", speed_limit_topic); + + filter_info_topic_ = filter_info_topic; + // Setting new costmap filter info subscriber + RCLCPP_INFO( + logger_, + "SpeedFilter: Subscribing to \"%s\" topic for filter info...", + filter_info_topic_.c_str()); + filter_info_sub_ = node->create_subscription( + filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&SpeedFilter::filterInfoCallback, this, std::placeholders::_1)); + + // Get global frame required for speed limit publisher + global_frame_ = layered_costmap_->getGlobalFrameID(); + + // Create new speed limit publisher + speed_limit_pub_ = node->create_publisher( + speed_limit_topic, rclcpp::QoS(10)); + speed_limit_pub_->on_activate(); + + // Reset speed conversion states + base_ = BASE_DEFAULT; + multiplier_ = MULTIPLIER_DEFAULT; + percentage_ = false; +} + +void SpeedFilter::filterInfoCallback( + const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!mask_sub_) { + RCLCPP_INFO( + logger_, + "SpeedFilter: Received filter info from %s topic.", filter_info_topic_.c_str()); + } else { + RCLCPP_WARN( + logger_, + "SpeedFilter: New costmap filter info arrived from %s topic. Updating old filter info.", + filter_info_topic_.c_str()); + // Resetting previous subscriber each time when new costmap filter information arrives + mask_sub_.reset(); + } + + // Set base_/multiplier_ or use speed limit in % of maximum speed + base_ = msg->base; + 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) { + // 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; + } + + mask_topic_ = msg->filter_mask_topic; + + // Setting new filter mask subscriber + RCLCPP_INFO( + logger_, + "SpeedFilter: Subscribing to \"%s\" topic for filter mask...", + mask_topic_.c_str()); + mask_sub_ = node->create_subscription( + mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&SpeedFilter::maskCallback, this, std::placeholders::_1)); +} + +void SpeedFilter::maskCallback( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + std::lock_guard guard(*getMutex()); + + if (!filter_mask_) { + RCLCPP_INFO( + logger_, + "SpeedFilter: Received filter mask from %s topic.", mask_topic_.c_str()); + } else { + RCLCPP_WARN( + logger_, + "SpeedFilter: New filter mask arrived from %s topic. Updating old filter mask.", + mask_topic_.c_str()); + filter_mask_.reset(); + } + + filter_mask_ = msg; + mask_frame_ = msg->header.frame_id; +} + +bool SpeedFilter::transformPose( + const geometry_msgs::msg::Pose2D & pose, + geometry_msgs::msg::Pose2D & mask_pose) const +{ + if (mask_frame_ != global_frame_) { + // Filter mask and current layer are in different frames: + // Transform (pose.x, pose.y) point from current layer frame (global_frame_) + // to mask_pose point in mask_frame_ + geometry_msgs::msg::TransformStamped transform; + geometry_msgs::msg::PointStamped in, out; + in.header.stamp = clock_->now(); + in.header.frame_id = global_frame_; + in.point.x = pose.x; + in.point.y = pose.y; + in.point.z = 0; + + try { + tf_->transform(in, out, mask_frame_, transform_tolerance_); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, + "SpeedFilter: failed to get costmap frame (%s) " + "transformation to mask frame (%s) with error: %s", + global_frame_.c_str(), mask_frame_.c_str(), ex.what()); + return false; + } + mask_pose.x = out.point.x; + mask_pose.y = out.point.y; + } else { + // Filter mask and current layer are in the same frame: + // Just use pose coordinates + mask_pose = pose; + } + + return true; +} + +bool SpeedFilter::worldToMask(double wx, double wy, unsigned int & mx, unsigned int & my) const +{ + double origin_x = filter_mask_->info.origin.position.x; + double origin_y = filter_mask_->info.origin.position.y; + double resolution = filter_mask_->info.resolution; + unsigned int size_x = filter_mask_->info.width; + unsigned int size_y = filter_mask_->info.height; + + if (wx < origin_x || wy < origin_y) { + return false; + } + + mx = std::round((wx - origin_x) / resolution); + my = std::round((wy - origin_y) / resolution); + if (mx >= size_x || my >= size_y) { + return false; + } + + return true; +} + +inline int8_t SpeedFilter::getMaskData( + const unsigned int mx, const unsigned int my) const +{ + return filter_mask_->data[my * filter_mask_->info.width + mx]; +} + +void SpeedFilter::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) +{ + std::lock_guard guard(*getMutex()); + + if (!filter_mask_) { + // Show warning message every 2 seconds to not litter an output + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 2000, + "SpeedFilter: Filter mask was not received"); + return; + } + + geometry_msgs::msg::Pose2D mask_pose; // robot coordinates in mask frame + + // Transforming robot pose from current layer frame to mask frame + if (!transformPose(pose, mask_pose)) { + return; + } + + // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) + unsigned int mask_robot_i, mask_robot_j; + if (!worldToMask(mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { + return; + } + + // Getting filter_mask data from cell where the robot placed and + // calculating speed limit value + int8_t speed_mask_data = getMaskData(mask_robot_i, mask_robot_j); + if (speed_mask_data == SPEED_MASK_NO_LIMIT) { + // Corresponding filter mask cell is free. + // Setting no speed limit there. + speed_limit_ = NO_SPEED_LIMIT; + } else if (speed_mask_data == SPEED_MASK_UNKNOWN) { + // Corresponding filter mask cell is unknown. + // Do nothing. + RCLCPP_ERROR( + logger_, + "SpeedFilter: Found unknown cell in filter_mask[%i, %i], " + "which is invalid for this kind of filter", + mask_robot_i, mask_robot_j); + return; + } else { + // Normal case: speed_mask_data in range of [1..100] + speed_limit_ = speed_mask_data * multiplier_ + base_; + 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", speed_limit_); + } else { + RCLCPP_DEBUG(logger_, "SpeedFilter: Speed limit is set to its default value"); + } + + // Forming and publishing new SpeedLimit message + std::unique_ptr msg = + std::make_unique(); + msg->header.frame_id = global_frame_; + msg->header.stamp = clock_->now(); + msg->percentage = percentage_; + msg->speed_limit = speed_limit_; + speed_limit_pub_->publish(std::move(msg)); + + speed_limit_prev_ = speed_limit_; + } +} + +void SpeedFilter::resetFilter() +{ + std::lock_guard guard(*getMutex()); + + filter_info_sub_.reset(); + mask_sub_.reset(); + if (speed_limit_pub_) { + speed_limit_pub_->on_deactivate(); + speed_limit_pub_.reset(); + } +} + +bool SpeedFilter::isActive() +{ + std::lock_guard guard(*getMutex()); + + if (filter_mask_) { + return true; + } + return false; +} + +} // namespace nav2_costmap_2d + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::SpeedFilter, nav2_costmap_2d::Layer) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 13feac3cbc..ac9245e8ad 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -53,6 +53,7 @@ PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::InflationLayer, nav2_costmap_2d::Layer) using nav2_costmap_2d::LETHAL_OBSTACLE; using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; using nav2_costmap_2d::NO_INFORMATION; +using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { @@ -77,6 +78,7 @@ InflationLayer::InflationLayer() InflationLayer::~InflationLayer() { + dyn_params_handler_.reset(); delete access_; } @@ -89,11 +91,22 @@ InflationLayer::onInitialize() declareParameter("inflate_unknown", rclcpp::ParameterValue(false)); declareParameter("inflate_around_unknown", rclcpp::ParameterValue(false)); - node_->get_parameter(name_ + "." + "enabled", enabled_); - node_->get_parameter(name_ + "." + "inflation_radius", inflation_radius_); - node_->get_parameter(name_ + "." + "cost_scaling_factor", cost_scaling_factor_); - node_->get_parameter(name_ + "." + "inflate_unknown", inflate_unknown_); - node_->get_parameter(name_ + "." + "inflate_around_unknown", inflate_around_unknown_); + { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "inflation_radius", inflation_radius_); + node->get_parameter(name_ + "." + "cost_scaling_factor", cost_scaling_factor_); + node->get_parameter(name_ + "." + "inflate_unknown", inflate_unknown_); + node->get_parameter(name_ + "." + "inflate_around_unknown", inflate_around_unknown_); + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &InflationLayer::dynamicParametersCallback, + this, std::placeholders::_1)); + } current_ = true; seen_.clear(); @@ -107,6 +120,7 @@ InflationLayer::onInitialize() void InflationLayer::matchSize() { + std::lock_guard guard(*getMutex()); nav2_costmap_2d::Costmap2D * costmap = layered_costmap_->getCostmap(); resolution_ = costmap->getResolution(); cell_inflation_radius_ = cellDistance(inflation_radius_); @@ -119,6 +133,7 @@ InflationLayer::updateBounds( double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x, double * min_y, double * max_x, double * max_y) { + std::lock_guard guard(*getMutex()); if (need_reinflation_) { last_min_x_ = *min_x; last_min_y_ = *min_y; @@ -149,14 +164,14 @@ InflationLayer::updateBounds( void InflationLayer::onFootprintChanged() { + std::lock_guard guard(*getMutex()); inscribed_radius_ = layered_costmap_->getInscribedRadius(); cell_inflation_radius_ = cellDistance(inflation_radius_); computeCaches(); need_reinflation_ = true; RCLCPP_DEBUG( - rclcpp::get_logger( - "nav2_costmap_2d"), "InflationLayer::onFootprintChanged(): num footprint points: %lu," + logger_, "InflationLayer::onFootprintChanged(): num footprint points: %lu," " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_); } @@ -175,7 +190,7 @@ InflationLayer::updateCosts( // make sure the inflation list is empty at the beginning of the cycle (should always be true) for (auto & dist : inflation_cells_) { RCLCPP_FATAL_EXPRESSION( - rclcpp::get_logger("nav2_costmap_2d"), + logger_, !dist.empty(), "The inflation list must be empty at the beginning of inflation"); } @@ -184,8 +199,7 @@ InflationLayer::updateCosts( if (seen_.size() != size_x * size_y) { RCLCPP_WARN( - rclcpp::get_logger( - "nav2_costmap_2d"), "InflationLayer::updateCosts(): seen_ vector size is wrong"); + logger_, "InflationLayer::updateCosts(): seen_ vector size is wrong"); seen_ = std::vector(size_x * size_y, false); } @@ -195,6 +209,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_); @@ -247,12 +265,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 @@ -275,6 +302,8 @@ InflationLayer::updateCosts( dist.clear(); dist.reserve(200); } + + current_ = true; } /** @@ -388,4 +417,62 @@ InflationLayer::generateIntegerDistances() return level; } +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +InflationLayer::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard guard(*getMutex()); + rcl_interfaces::msg::SetParametersResult result; + + bool need_cache_recompute = false; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + "." + "inflation_radius" && + inflation_radius_ != parameter.as_double()) + { + inflation_radius_ = parameter.as_double(); + need_reinflation_ = true; + need_cache_recompute = true; + } else if (param_name == name_ + "." + "cost_scaling_factor" && // NOLINT + cost_scaling_factor_ != parameter.as_double()) + { + cost_scaling_factor_ = parameter.as_double(); + need_reinflation_ = true; + need_cache_recompute = true; + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { + enabled_ = parameter.as_bool(); + need_reinflation_ = true; + current_ = false; + } else if (param_name == name_ + "." + "inflate_unknown" && // NOLINT + inflate_unknown_ != parameter.as_bool()) + { + inflate_unknown_ = parameter.as_bool(); + need_reinflation_ = true; + } else if (param_name == name_ + "." + "inflate_around_unknown" && // NOLINT + inflate_around_unknown_ != parameter.as_bool()) + { + inflate_around_unknown_ = parameter.as_bool(); + need_reinflation_ = true; + } + } + } + + if (need_cache_recompute) { + matchSize(); + } + + result.successful = true; + return result; +} + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index a881f14f49..e7836f9639 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -55,12 +55,14 @@ using nav2_costmap_2d::FREE_SPACE; using nav2_costmap_2d::ObservationBuffer; using nav2_costmap_2d::Observation; +using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { ObstacleLayer::~ObstacleLayer() { + dyn_params_handler_.reset(); for (auto & notifier : observation_notifiers_) { notifier.reset(); } @@ -74,24 +76,34 @@ void ObstacleLayer::onInitialize() // The topics that we'll subscribe to from the parameter server std::string topics_string; - // TODO(mjeronimo): these four are candidates for dynamic update declareParameter("enabled", rclcpp::ParameterValue(true)); - declareParameter( - "footprint_clearing_enabled", - rclcpp::ParameterValue(true)); + declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); declareParameter("combination_method", rclcpp::ParameterValue(1)); declareParameter("observation_sources", rclcpp::ParameterValue(std::string(""))); - node_->get_parameter(name_ + "." + "enabled", enabled_); - node_->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); - node_->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); - node_->get_parameter(name_ + "." + "combination_method", combination_method_); - node_->get_parameter("track_unknown_space", track_unknown_space); - node_->get_parameter("transform_tolerance", transform_tolerance); - node_->get_parameter(name_ + "." + "observation_sources", topics_string); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); + node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); + node->get_parameter(name_ + "." + "combination_method", combination_method_); + node->get_parameter("track_unknown_space", track_unknown_space); + node->get_parameter("transform_tolerance", transform_tolerance); + node->get_parameter(name_ + "." + "observation_sources", topics_string); - RCLCPP_INFO(node_->get_logger(), "Subscribed to Topics: %s", topics_string.c_str()); + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &ObstacleLayer::dynamicParametersCallback, + this, + std::placeholders::_1)); + + RCLCPP_INFO( + logger_, + "Subscribed to Topics: %s", topics_string.c_str()); rolling_window_ = layered_costmap_->isRolling(); @@ -103,9 +115,13 @@ void ObstacleLayer::onInitialize() ObstacleLayer::matchSize(); current_ = true; + was_reset_ = false; global_frame_ = layered_costmap_->getGlobalFrameID(); + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; + // now we need to split the topics based on whitespace which we can use a stringstream for std::stringstream ss(topics_string); @@ -126,42 +142,47 @@ 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)); - - node_->get_parameter(name_ + "." + source + "." + "topic", topic); - node_->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame); - node_->get_parameter( + 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); + node->get_parameter( name_ + "." + source + "." + "observation_persistence", observation_keep_time); - node_->get_parameter( + node->get_parameter( name_ + "." + source + "." + "expected_update_rate", expected_update_rate); - node_->get_parameter(name_ + "." + source + "." + "data_type", data_type); - node_->get_parameter(name_ + "." + source + "." + "min_obstacle_height", min_obstacle_height); - node_->get_parameter(name_ + "." + source + "." + "max_obstacle_height", max_obstacle_height); - node_->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid); - node_->get_parameter(name_ + "." + source + "." + "marking", marking); - node_->get_parameter(name_ + "." + source + "." + "clearing", clearing); + node->get_parameter(name_ + "." + source + "." + "data_type", data_type); + node->get_parameter(name_ + "." + source + "." + "min_obstacle_height", min_obstacle_height); + node->get_parameter(name_ + "." + source + "." + "max_obstacle_height", max_obstacle_height); + node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid); + node->get_parameter(name_ + "." + source + "." + "marking", marking); + node->get_parameter(name_ + "." + source + "." + "clearing", clearing); if (!(data_type == "PointCloud2" || data_type == "LaserScan")) { RCLCPP_FATAL( - node_->get_logger(), + logger_, "Only topics that use point cloud2s or laser scans are currently supported"); throw std::runtime_error( "Only topics that use point cloud2s or laser scans are currently supported"); } // 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( - node_->get_logger(), + logger_, "Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(), sensor_frame.c_str()); @@ -171,10 +192,12 @@ void ObstacleLayer::onInitialize() std::shared_ptr( new ObservationBuffer( - node_, topic, observation_keep_time, expected_update_rate, + node, topic, observation_keep_time, expected_update_rate, min_obstacle_height, - max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, - sensor_frame, transform_tolerance))); + 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 if (marking) { @@ -187,7 +210,7 @@ void ObstacleLayer::onInitialize() } RCLCPP_DEBUG( - node_->get_logger(), + logger_, "Created an observation buffer for source %s, topic %s, global frame: %s, " "expected update rate: %.2f, observation persistence: %.2f", source.c_str(), topic.c_str(), @@ -198,14 +221,15 @@ void ObstacleLayer::onInitialize() // create a callback for the topic if (data_type == "LaserScan") { - std::shared_ptr> sub( - new message_filters::Subscriber( - rclcpp_node_, topic, custom_qos_profile)); + auto sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); sub->unsubscribe(); - std::shared_ptr> filter( - new tf2_ros::MessageFilter( - *sub, *tf_, global_frame_, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance))); + auto filter = std::make_shared>( + *sub, *tf_, global_frame_, 50, + node->get_node_logging_interface(), + node->get_node_clock_interface(), + tf2::durationFromSec(transform_tolerance)); if (inf_is_valid) { filter->registerCallback( @@ -226,20 +250,21 @@ void ObstacleLayer::onInitialize() observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05)); } else { - std::shared_ptr> sub( - new message_filters::Subscriber( - rclcpp_node_, topic, custom_qos_profile)); + auto sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); sub->unsubscribe(); if (inf_is_valid) { RCLCPP_WARN( - node_->get_logger(), + logger_, "obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); } - std::shared_ptr> filter( - new tf2_ros::MessageFilter( - *sub, *tf_, global_frame_, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance))); + auto filter = std::make_shared>( + *sub, *tf_, global_frame_, 50, + node->get_node_logging_interface(), + node->get_node_clock_interface(), + tf2::durationFromSec(transform_tolerance)); filter->registerCallback( std::bind( @@ -259,6 +284,38 @@ void ObstacleLayer::onInitialize() } } +rcl_interfaces::msg::SetParametersResult +ObstacleLayer::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard guard(*getMutex()); + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + "." + "max_obstacle_height") { + max_obstacle_height_ = parameter.as_double(); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } else if (param_name == name_ + "." + "footprint_clearing_enabled") { + footprint_clearing_enabled_ = parameter.as_bool(); + } + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == name_ + "." + "combination_method") { + combination_method_ = parameter.as_int(); + } + } + } + + result.successful = true; + return result; +} + void ObstacleLayer::laserScanCallback( sensor_msgs::msg::LaserScan::ConstSharedPtr message, @@ -273,11 +330,18 @@ ObstacleLayer::laserScanCallback( projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_); } catch (tf2::TransformException & ex) { RCLCPP_WARN( - node_->get_logger(), + logger_, "High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(), ex.what()); projector_.projectLaser(*message, cloud); + } catch (std::runtime_error & ex) { + RCLCPP_WARN( + logger_, + "transformLaserScanToPointCloud error, it seems the message from laser is malformed." + " Ignore this message. what(): %s", + ex.what()); + return; } // buffer the point cloud @@ -310,10 +374,17 @@ ObstacleLayer::laserScanValidInfCallback( projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); } catch (tf2::TransformException & ex) { RCLCPP_WARN( - node_->get_logger(), + logger_, "High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(), ex.what()); projector_.projectLaser(message, cloud); + } catch (std::runtime_error & ex) { + RCLCPP_WARN( + logger_, + "transformLaserScanToPointCloud error, it seems the message from laser is malformed." + " Ignore this message. what(): %s", + ex.what()); + return; } // buffer the point cloud @@ -338,6 +409,7 @@ ObstacleLayer::updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y) { + std::lock_guard guard(*getMutex()); if (rolling_window_) { updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); } @@ -371,7 +443,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"); @@ -382,7 +455,7 @@ ObstacleLayer::updateBounds( // if the obstacle is too high or too far away from the robot we won't add it if (pz > max_obstacle_height_) { - RCLCPP_DEBUG(node_->get_logger(), "The point is too high"); + RCLCPP_DEBUG(logger_, "The point is too high"); continue; } @@ -393,15 +466,21 @@ 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) { - RCLCPP_DEBUG(node_->get_logger(), "The point is too far away"); + 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)) { - RCLCPP_DEBUG(node_->get_logger(), "Computing map coords failed"); + RCLCPP_DEBUG(logger_, "Computing map coords failed"); continue; } @@ -435,10 +514,17 @@ ObstacleLayer::updateCosts( int max_i, int max_j) { + std::lock_guard guard(*getMutex()); if (!enabled_) { return; } + // if not current due to reset, set current now after clearing + if (!current_ && was_reset_) { + was_reset_ = false; + current_ = true; + } + if (footprint_clearing_enabled_) { setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE); } @@ -528,9 +614,12 @@ ObstacleLayer::raytraceFreespace( unsigned int x0, y0; if (!worldToMap(ox, oy, x0, y0)) { RCLCPP_WARN( - node_->get_logger(), - "Sensor origin at (%.2f, %.2f) is out of map bounds. The costmap cannot raytrace for it.", - ox, oy); + logger_, + "Sensor origin at (%.2f, %.2f) is out of map bounds (%.2f, %.2f) to (%.2f, %.2f). " + "The costmap cannot raytrace for it.", + ox, oy, + origin_x_, origin_y_, + origin_x_ + getSizeInMetersX(), origin_y_ + getSizeInMetersY()); return; } @@ -588,13 +677,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); } } @@ -627,12 +718,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); } @@ -642,7 +736,8 @@ ObstacleLayer::reset() { resetMaps(); resetBuffersLastUpdated(); - current_ = true; + current_ = false; + was_reset_ = true; } void diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp index c0cb111155..ffbe8a3ef8 100644 --- a/nav2_costmap_2d/plugins/range_sensor_layer.cpp +++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp @@ -60,46 +60,52 @@ RangeSensorLayer::RangeSensorLayer() {} void RangeSensorLayer::onInitialize() { current_ = true; + was_reset_ = false; buffered_readings_ = 0; - last_reading_time_ = node_->now(); + last_reading_time_ = clock_->now(); default_value_ = to_cost(0.5); matchSize(); resetRange(); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + declareParameter("enabled", rclcpp::ParameterValue(true)); - node_->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "enabled", enabled_); declareParameter("phi", rclcpp::ParameterValue(1.2)); - node_->get_parameter(name_ + "." + "phi", phi_v_); + node->get_parameter(name_ + "." + "phi", phi_v_); declareParameter("inflate_cone", rclcpp::ParameterValue(1.0)); - node_->get_parameter(name_ + "." + "inflate_cone", inflate_cone_); + node->get_parameter(name_ + "." + "inflate_cone", inflate_cone_); declareParameter("no_readings_timeout", rclcpp::ParameterValue(0.0)); - node_->get_parameter(name_ + "." + "no_readings_timeout", no_readings_timeout_); + node->get_parameter(name_ + "." + "no_readings_timeout", no_readings_timeout_); declareParameter("clear_threshold", rclcpp::ParameterValue(0.2)); - node_->get_parameter(name_ + "." + "clear_threshold", clear_threshold_); + node->get_parameter(name_ + "." + "clear_threshold", clear_threshold_); declareParameter("mark_threshold", rclcpp::ParameterValue(0.8)); - node_->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); + node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); declareParameter("clear_on_max_reading", rclcpp::ParameterValue(false)); - node_->get_parameter(name_ + "." + "clear_on_max_reading", clear_on_max_reading_); + node->get_parameter(name_ + "." + "clear_on_max_reading", clear_on_max_reading_); double temp_tf_tol = 0.0; - node_->get_parameter("transform_tolerance", temp_tf_tol); + node->get_parameter("transform_tolerance", temp_tf_tol); transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); std::vector topic_names{}; declareParameter("topics", rclcpp::ParameterValue(topic_names)); - node_->get_parameter(name_ + "." + "topics", topic_names); + node->get_parameter(name_ + "." + "topics", topic_names); InputSensorType input_sensor_type = InputSensorType::ALL; std::string sensor_type_name; declareParameter("input_sensor_type", rclcpp::ParameterValue("ALL")); - node_->get_parameter(name_ + "." + "input_sensor_type", sensor_type_name); + node->get_parameter(name_ + "." + "input_sensor_type", sensor_type_name); std::transform( sensor_type_name.begin(), sensor_type_name.end(), sensor_type_name.begin(), ::toupper); RCLCPP_INFO( - node_->get_logger(), "%s: %s as input_sensor_type given", + logger_, "%s: %s as input_sensor_type given", name_.c_str(), sensor_type_name.c_str()); if (sensor_type_name == "VARIABLE") { @@ -110,14 +116,14 @@ void RangeSensorLayer::onInitialize() input_sensor_type = InputSensorType::ALL; } else { RCLCPP_ERROR( - node_->get_logger(), "%s: Invalid input sensor type: %s. Defaulting to ALL.", + logger_, "%s: Invalid input sensor type: %s. Defaulting to ALL.", name_.c_str(), sensor_type_name.c_str()); } // Validate topic names list: it must be a (normally non-empty) list of strings if (topic_names.empty()) { RCLCPP_FATAL( - node_->get_logger(), "Invalid topic names list: it must" + logger_, "Invalid topic names list: it must" "be a non-empty list of strings"); return; } @@ -138,19 +144,19 @@ void RangeSensorLayer::onInitialize() std::placeholders::_1); } else { RCLCPP_ERROR( - node_->get_logger(), + logger_, "%s: Invalid input sensor type: %s. Did you make a new type" "and forgot to choose the subscriber for it?", name_.c_str(), sensor_type_name.c_str()); } range_subs_.push_back( - node_->create_subscription( + node->create_subscription( topic_name, rclcpp::SensorDataQoS(), std::bind( &RangeSensorLayer::bufferIncomingRangeMsg, this, std::placeholders::_1))); RCLCPP_INFO( - node_->get_logger(), "RangeSensorLayer: subscribed to " + logger_, "RangeSensorLayer: subscribed to " "topic %s", range_subs_.back()->get_topic_name()); } global_frame_ = layered_costmap_->getGlobalFrameID(); @@ -238,7 +244,7 @@ void RangeSensorLayer::processFixedRangeMsg(sensor_msgs::msg::Range & range_mess { if (!std::isinf(range_message.range)) { RCLCPP_ERROR( - node_->get_logger(), + logger_, "Fixed distance ranger (min_range == max_range) in frame %s sent invalid value. " "Only -Inf (== object detected) and Inf (== no object detected) are valid.", range_message.header.frame_id.c_str()); @@ -287,10 +293,12 @@ void RangeSensorLayer::updateCostmap( in.header.frame_id = range_message.header.frame_id; if (!tf_->canTransform( - in.header.frame_id, global_frame_, tf2_ros::fromMsg(in.header.stamp))) + in.header.frame_id, global_frame_, + tf2_ros::fromMsg(in.header.stamp), + tf2_ros::fromRclcpp(transform_tolerance_))) { RCLCPP_INFO( - node_->get_logger(), "Range sensor layer can't transform from %s to %s", + logger_, "Range sensor layer can't transform from %s to %s", global_frame_.c_str(), in.header.frame_id.c_str()); return; } @@ -387,7 +395,7 @@ void RangeSensorLayer::updateCostmap( } buffered_readings_++; - last_reading_time_ = node_->now(); + last_reading_time_ = clock_->now(); } void RangeSensorLayer::update_cell( @@ -409,8 +417,12 @@ void RangeSensorLayer::update_cell( double prob_not = (1 - sensor) * (1 - prior); double new_prob = prob_occ / (prob_occ + prob_not); - RCLCPP_DEBUG(node_->get_logger(), "%f %f | %f %f = %f", dx, dy, theta, phi, sensor); - RCLCPP_DEBUG(node_->get_logger(), "%f | %f %f | %f", prior, prob_occ, prob_not, new_prob); + RCLCPP_DEBUG( + logger_, + "%f %f | %f %f = %f", dx, dy, theta, phi, sensor); + RCLCPP_DEBUG( + logger_, + "%f | %f %f | %f", prior, prob_occ, prob_not, new_prob); unsigned char c = to_cost(new_prob); setCost(x, y, c); } @@ -448,12 +460,13 @@ void RangeSensorLayer::updateBounds( if (buffered_readings_ == 0) { if (no_readings_timeout_ > 0.0 && - (node_->now() - last_reading_time_).seconds() > no_readings_timeout_) + (clock_->now() - last_reading_time_).seconds() > + no_readings_timeout_) { RCLCPP_WARN( - node_->get_logger(), "No range readings received for %.2f seconds, " - "while expected at least every %.2f seconds.", - (node_->now() - last_reading_time_).seconds(), + logger_, + "No range readings received for %.2f seconds, while expected at least every %.2f seconds.", + (clock_->now() - last_reading_time_).seconds(), no_readings_timeout_); current_ = false; } @@ -499,15 +512,20 @@ void RangeSensorLayer::updateCosts( } buffered_readings_ = 0; - current_ = true; + + // if not current due to reset, set current now after clearing + if (!current_ && was_reset_) { + was_reset_ = false; + current_ = true; + } } void RangeSensorLayer::reset() { - RCLCPP_DEBUG(node_->get_logger(), "Reseting range sensor layer..."); + RCLCPP_DEBUG(logger_, "Reseting range sensor layer..."); deactivate(); resetMaps(); - current_ = true; + was_reset_ = true; activate(); } diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index cb3a01f1be..073109c235 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -40,19 +40,18 @@ #include "nav2_costmap_2d/static_layer.hpp" #include -#include #include -#include "nav2_costmap_2d/costmap_math.hpp" #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) using nav2_costmap_2d::NO_INFORMATION; using nav2_costmap_2d::LETHAL_OBSTACLE; using nav2_costmap_2d::FREE_SPACE; +using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { @@ -81,17 +80,23 @@ StaticLayer::onInitialize() } RCLCPP_INFO( - node_->get_logger(), + logger_, "Subscribing to the map topic (%s) with %s durability", map_topic_.c_str(), map_subscribe_transient_local_ ? "transient local" : "volatile"); - map_sub_ = node_->create_subscription( + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + map_sub_ = node->create_subscription( map_topic_, map_qos, std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1)); if (subscribe_to_updates_) { - RCLCPP_INFO(node_->get_logger(), "Subscribing to updates"); - map_update_sub_ = node_->create_subscription( + RCLCPP_INFO(logger_, "Subscribing to updates"); + map_update_sub_ = node->create_subscription( map_topic_ + "_updates", rclcpp::SystemDefaultsQoS(), std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1)); @@ -106,12 +111,14 @@ StaticLayer::activate() void StaticLayer::deactivate() { + dyn_params_handler_.reset(); } void StaticLayer::reset() { has_updated_data_ = true; + current_ = false; } void @@ -126,25 +133,30 @@ StaticLayer::getParameters() declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); declareParameter("map_topic", rclcpp::ParameterValue("")); - node_->get_parameter(name_ + "." + "enabled", enabled_); - node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); std::string private_map_topic, global_map_topic; - node_->get_parameter(name_ + "." + "map_topic", private_map_topic); - node_->get_parameter("map_topic", global_map_topic); + node->get_parameter(name_ + "." + "map_topic", private_map_topic); + node->get_parameter("map_topic", global_map_topic); if (!private_map_topic.empty()) { map_topic_ = private_map_topic; } else { map_topic_ = global_map_topic; } - node_->get_parameter( + node->get_parameter( name_ + "." + "map_subscribe_transient_local", map_subscribe_transient_local_); - node_->get_parameter("track_unknown_space", track_unknown_space_); - node_->get_parameter("use_maximum", use_maximum_); - node_->get_parameter("lethal_cost_threshold", temp_lethal_threshold); - node_->get_parameter("unknown_cost_value", unknown_cost_value_); - node_->get_parameter("trinary_costmap", trinary_costmap_); - node_->get_parameter("transform_tolerance", temp_tf_tol); + node->get_parameter("track_unknown_space", track_unknown_space_); + node->get_parameter("use_maximum", use_maximum_); + node->get_parameter("lethal_cost_threshold", temp_lethal_threshold); + node->get_parameter("unknown_cost_value", unknown_cost_value_); + node->get_parameter("trinary_costmap", trinary_costmap_); + node->get_parameter("transform_tolerance", temp_tf_tol); // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); @@ -152,18 +164,24 @@ StaticLayer::getParameters() update_in_progress_.store(false); transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &StaticLayer::dynamicParametersCallback, + this, std::placeholders::_1)); } void StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) { - RCLCPP_DEBUG(node_->get_logger(), "StaticLayer: Process map"); + RCLCPP_DEBUG(logger_, "StaticLayer: Process map"); unsigned int size_x = new_map.info.width; unsigned int size_y = new_map.info.height; RCLCPP_DEBUG( - node_->get_logger(), + logger_, "StaticLayer: Received a %d X %d map at %f m/pix", size_x, size_y, new_map.info.resolution); @@ -178,7 +196,7 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) { // Update the size of the layered costmap (and all layers, including this one) RCLCPP_INFO( - node_->get_logger(), + logger_, "StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution); layered_costmap_->resizeMap( @@ -193,7 +211,7 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) { // only update the size of the costmap stored locally in this layer RCLCPP_INFO( - node_->get_logger(), + logger_, "StaticLayer: Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution); resizeMap( @@ -282,7 +300,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u x_ + width_ < update->x + update->width) { RCLCPP_WARN( - node_->get_logger(), + logger_, "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n" "Static layer origin: %d, %d bounds: %d X %d\n" "Update origin: %d, %d bounds: %d X %d", @@ -293,7 +311,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u if (update->header.frame_id != map_frame_) { RCLCPP_WARN( - node_->get_logger(), + logger_, "StaticLayer: Map update ignored. Current map is in frame %s " "but update was in frame %s", map_frame_.c_str(), update->header.frame_id.c_str()); @@ -308,10 +326,6 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u } } - x_ = update->x; - y_ = update->y; - width_ = update->width; - height_ = update->height; has_updated_data_ = true; } @@ -362,6 +376,7 @@ StaticLayer::updateCosts( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) { + std::lock_guard guard(*getMutex()); if (!enabled_) { update_in_progress_.store(false); return; @@ -370,7 +385,7 @@ StaticLayer::updateCosts( static int count = 0; // throttle warning down to only 1/10 message rate if (++count == 10) { - RCLCPP_WARN(node_->get_logger(), "Can't update static costmap layer, no map received"); + RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received"); count = 0; } update_in_progress_.store(false); @@ -395,7 +410,7 @@ StaticLayer::updateCosts( map_frame_, global_frame_, tf2::TimePointZero, transform_tolerance_); } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what()); + RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what()); update_in_progress_.store(false); return; } @@ -422,6 +437,49 @@ StaticLayer::updateCosts( } } update_in_progress_.store(false); + current_ = true; +} + +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +StaticLayer::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard guard(*getMutex()); + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_name == name_ + "." + "map_subscribe_transient_local" || + param_name == name_ + "." + "map_topic" || + param_name == name_ + "." + "subscribe_to_updates") + { + RCLCPP_WARN( + logger_, "%s is not a dynamic parameter " + "cannot be changed while running. Rejecting parameter update.", param_name.c_str()); + } else if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + "." + "transform_tolerance") { + transform_tolerance_ = tf2::durationFromSec(parameter.as_double()); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { + enabled_ = parameter.as_bool(); + + x_ = y_ = 0; + width_ = size_x_; + height_ = size_y_; + has_updated_data_ = true; + current_ = false; + } + } + } + result.successful = true; + return result; } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 9a960dc689..149dfd6ede 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -53,6 +53,7 @@ PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::VoxelLayer, nav2_costmap_2d::Layer) using nav2_costmap_2d::NO_INFORMATION; using nav2_costmap_2d::LETHAL_OBSTACLE; using nav2_costmap_2d::FREE_SPACE; +using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { @@ -72,38 +73,52 @@ void VoxelLayer::onInitialize() declareParameter("combination_method", rclcpp::ParameterValue(1)); declareParameter("publish_voxel_map", rclcpp::ParameterValue(false)); - node_->get_parameter(name_ + "." + "enabled", enabled_); - node_->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); - node_->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); - node_->get_parameter(name_ + "." + "z_voxels", size_z_); - node_->get_parameter(name_ + "." + "origin_z", origin_z_); - node_->get_parameter(name_ + "." + "z_resolution", z_resolution_); - node_->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_); - node_->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); - node_->get_parameter(name_ + "." + "combination_method", combination_method_); - node_->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); + node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); + node->get_parameter(name_ + "." + "z_voxels", size_z_); + node->get_parameter(name_ + "." + "origin_z", origin_z_); + node->get_parameter(name_ + "." + "z_resolution", z_resolution_); + node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_); + node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); + node->get_parameter(name_ + "." + "combination_method", combination_method_); + node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_); auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); if (publish_voxel_) { - voxel_pub_ = node_->create_publisher( + voxel_pub_ = node->create_publisher( "voxel_grid", custom_qos); 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(); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &VoxelLayer::dynamicParametersCallback, + this, std::placeholders::_1)); } VoxelLayer::~VoxelLayer() { + dyn_params_handler_.reset(); } void VoxelLayer::matchSize() { + std::lock_guard guard(*getMutex()); ObstacleLayer::matchSize(); voxel_grid_.resize(size_x_, size_y_, size_z_); assert(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_); @@ -130,6 +145,8 @@ void VoxelLayer::updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y) { + std::lock_guard guard(*getMutex()); + if (rolling_window_) { updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); } @@ -163,7 +180,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"); @@ -181,7 +199,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; } @@ -224,7 +247,7 @@ void VoxelLayer::updateBounds( grid_msg->resolutions.y = resolution_; grid_msg->resolutions.z = z_resolution_; grid_msg->header.frame_id = global_frame_; - grid_msg->header.stamp = node_->now(); + grid_msg->header.stamp = clock_->now(); voxel_pub_->publish(std::move(grid_msg)); } @@ -232,70 +255,15 @@ void VoxelLayer::updateBounds( updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); } -void VoxelLayer::clearNonLethal( - double wx, double wy, double w_size_x, double w_size_y, - bool clear_no_info) -{ - // get the cell coordinates of the center point of the window - unsigned int mx, my; - if (!worldToMap(wx, wy, mx, my)) { - return; - } - - // compute the bounds of the window - double start_x = wx - w_size_x / 2; - double start_y = wy - w_size_y / 2; - double end_x = start_x + w_size_x; - double end_y = start_y + w_size_y; - - // scale the window based on the bounds of the costmap - start_x = std::max(origin_x_, start_x); - start_y = std::max(origin_y_, start_y); - - end_x = std::min(origin_x_ + getSizeInMetersX(), end_x); - end_y = std::min(origin_y_ + getSizeInMetersY(), end_y); - - // get the map coordinates of the bounds of the window - unsigned int map_sx, map_sy, map_ex, map_ey; - - // check for legality just in case - if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey)) { - return; - } - - // we know that we want to clear all non-lethal obstacles in this - // window to get it ready for inflation - unsigned int index = getIndex(map_sx, map_sy); - unsigned char * current = &costmap_[index]; - for (unsigned int j = map_sy; j <= map_ey; ++j) { - for (unsigned int i = map_sx; i <= map_ex; ++i) { - // if the cell is a lethal obstacle... we'll keep it and queue it, - // otherwise... we'll clear it - if (*current != LETHAL_OBSTACLE) { - if (clear_no_info || *current != NO_INFORMATION) { - *current = FREE_SPACE; - voxel_grid_.clearVoxelColumn(index); - } - } - current++; - index++; - } - current += size_x_ - (map_ex - map_sx) - 1; - index += size_x_ - (map_ex - map_sx) - 1; - } -} - void VoxelLayer::raytraceFreespace( const Observation & clearing_observation, double * min_x, double * min_y, 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; } @@ -306,18 +274,42 @@ void VoxelLayer::raytraceFreespace( if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) { RCLCPP_WARN( - node_->get_logger(), - "Sensor origin: (%.2f, %.2f, %.2f), out of map bounds. The costmap can't raytrace for it.", - ox, oy, oz); + logger_, + "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds (%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f). " + "The costmap cannot raytrace for it.", + ox, oy, oz, + ox + getSizeInMetersX(), oy + getSizeInMetersY(), oz + getSizeInMetersZ(), + origin_x_, origin_y_, origin_z_); + return; } - bool publish_clearing_points = (node_->count_subscribers("clearing_endpoints") > 0); - if (publish_clearing_points) { - clearing_endpoints_->points.clear(); - clearing_endpoints_->points.reserve(clearing_observation_cloud_size); + bool publish_clearing_points; + + { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + publish_clearing_points = (node->count_subscribers("clearing_endpoints") > 0); } + 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(); @@ -376,26 +368,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; } } } @@ -475,4 +472,65 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) delete[] local_voxel_map; } +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +VoxelLayer::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard guard(*getMutex()); + rcl_interfaces::msg::SetParametersResult result; + bool resize_map_needed = false; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + "." + "max_obstacle_height") { + max_obstacle_height_ = parameter.as_double(); + } else if (param_name == name_ + "." + "origin_z") { + origin_z_ = parameter.as_double(); + resize_map_needed = true; + } else if (param_name == name_ + "." + "z_resolution") { + z_resolution_ = parameter.as_double(); + resize_map_needed = true; + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + current_ = false; + } else if (param_name == name_ + "." + "footprint_clearing_enabled") { + footprint_clearing_enabled_ = parameter.as_bool(); + } else if (param_name == name_ + "." + "publish_voxel_map") { + RCLCPP_WARN( + logger_, "publish voxel map is not a dynamic parameter " + "cannot be changed while running. Rejecting parameter update."); + continue; + } + + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == name_ + "." + "z_voxels") { + size_z_ = parameter.as_int(); + resize_map_needed = true; + } else if (param_name == name_ + "." + "unknown_threshold") { + unknown_threshold_ = parameter.as_int() + (VOXEL_BITS - size_z_); + } else if (param_name == name_ + "." + "mark_threshold") { + mark_threshold_ = parameter.as_int(); + } else if (param_name == name_ + "." + "combination_method") { + combination_method_ = parameter.as_int(); + } + } + } + + if (resize_map_needed) { + matchSize(); + } + + result.successful = true; + return result; +} + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index bdadecdaf2..18039f4298 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -32,27 +32,29 @@ using ClearAroundRobot = nav2_msgs::srv::ClearCostmapAroundRobot; using ClearEntirely = nav2_msgs::srv::ClearEntireCostmap; ClearCostmapService::ClearCostmapService( - nav2_util::LifecycleNode::SharedPtr node, + const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap) -: node_(node), costmap_(costmap) +: costmap_(costmap) { + auto node = parent.lock(); + logger_ = node->get_logger(); reset_value_ = costmap_.getCostmap()->getDefaultValue(); - node_->get_parameter("clearable_layers", clearable_layers_); + node->get_parameter("clearable_layers", clearable_layers_); - clear_except_service_ = node_->create_service( + clear_except_service_ = node->create_service( "clear_except_" + costmap_.getName(), std::bind( &ClearCostmapService::clearExceptRegionCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - clear_around_service_ = node_->create_service( + clear_around_service_ = node->create_service( "clear_around_" + costmap.getName(), std::bind( &ClearCostmapService::clearAroundRobotCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - clear_entire_service_ = node_->create_service( + clear_entire_service_ = node->create_service( "clear_entirely_" + costmap_.getName(), std::bind( &ClearCostmapService::clearEntireCallback, this, @@ -65,10 +67,10 @@ void ClearCostmapService::clearExceptRegionCallback( const shared_ptr/*response*/) { RCLCPP_INFO( - node_->get_logger(), - "Received request to clear except a region the " + costmap_.getName()); + logger_, + ("Received request to clear except a region the " + costmap_.getName()).c_str()); - clearExceptRegion(request->reset_distance); + clearRegion(request->reset_distance, true); } void ClearCostmapService::clearAroundRobotCallback( @@ -76,16 +78,7 @@ void ClearCostmapService::clearAroundRobotCallback( const shared_ptr request, const shared_ptr/*response*/) { - RCLCPP_INFO( - node_->get_logger(), - "Received request to clear around robot the " + costmap_.getName()); - - if ((request->window_size_x == 0) || (request->window_size_y == 0)) { - clearEntirely(); - return; - } - - clearAroundRobot(request->window_size_x, request->window_size_y); + clearRegion(request->reset_distance, false); } void ClearCostmapService::clearEntireCallback( @@ -93,74 +86,40 @@ void ClearCostmapService::clearEntireCallback( const std::shared_ptr/*request*/, const std::shared_ptr/*response*/) { - RCLCPP_INFO(node_->get_logger(), "Received request to clear entirely the " + costmap_.getName()); + RCLCPP_INFO( + logger_, + ("Received request to clear entirely the " + costmap_.getName()).c_str()); clearEntirely(); } -void ClearCostmapService::clearExceptRegion(const double reset_distance) +void ClearCostmapService::clearRegion(const double reset_distance, bool invert) { double x, y; if (!getPosition(x, y)) { - RCLCPP_ERROR(node_->get_logger(), "Cannot clear map because robot pose cannot be retrieved."); + RCLCPP_ERROR( + logger_, + "Cannot clear map because robot pose cannot be retrieved."); return; } auto layers = costmap_.getLayeredCostmap()->getPlugins(); for (auto & layer : *layers) { - if (isClearable(getLayerName(*layer))) { + if (layer->isClearable()) { auto costmap_layer = std::static_pointer_cast(layer); - clearLayerExceptRegion(costmap_layer, x, y, reset_distance); + clearLayerRegion(costmap_layer, x, y, reset_distance, invert); } } -} - -void ClearCostmapService::clearAroundRobot(double window_size_x, double window_size_y) -{ - double pose_x, pose_y; - - if (!getPosition(pose_x, pose_y)) { - RCLCPP_ERROR(node_->get_logger(), "Cannot clear map because robot pose cannot be retrieved."); - return; - } - - std::vector clear_poly; - geometry_msgs::msg::Point pt; - - pt.x = pose_x - window_size_x / 2; - pt.y = pose_y - window_size_y / 2; - clear_poly.push_back(pt); - - pt.x = pose_x + window_size_x / 2; - pt.y = pose_y - window_size_y / 2; - clear_poly.push_back(pt); - - pt.x = pose_x + window_size_x / 2; - pt.y = pose_y + window_size_y / 2; - clear_poly.push_back(pt); - - pt.x = pose_x - window_size_x / 2; - pt.y = pose_y + window_size_y / 2; - clear_poly.push_back(pt); - - costmap_.getCostmap()->setConvexPolygonCost(clear_poly, reset_value_); -} - -void ClearCostmapService::clearEntirely() -{ - std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); - costmap_.resetLayers(); -} -bool ClearCostmapService::isClearable(const string & layer_name) const -{ - return count(begin(clearable_layers_), end(clearable_layers_), layer_name) != 0; + // AlexeyMerzlyakov: No need to clear layer region for costmap filters + // as they are always supposed to be not clearable. } -void ClearCostmapService::clearLayerExceptRegion( - shared_ptr & costmap, double pose_x, double pose_y, double reset_distance) +void ClearCostmapService::clearLayerRegion( + shared_ptr & costmap, double pose_x, double pose_y, double reset_distance, + bool invert) { std::unique_lock lock(*(costmap->getMutex())); @@ -170,27 +129,22 @@ void ClearCostmapService::clearLayerExceptRegion( double end_point_y = start_point_y + reset_distance; int start_x, start_y, end_x, end_y; - costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y); - costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y); - - unsigned int size_x = costmap->getSizeInCellsX(); - unsigned int size_y = costmap->getSizeInCellsY(); - - // Clearing the four rectangular regions around the one we want to keep - // top region - costmap->resetMapToValue(0, 0, size_x, start_y, reset_value_); - // left region - costmap->resetMapToValue(0, start_y, start_x, end_y, reset_value_); - // right region - costmap->resetMapToValue(end_x, start_y, size_x, end_y, reset_value_); - // bottom region - costmap->resetMapToValue(0, end_y, size_x, size_y, reset_value_); + costmap->worldToMapEnforceBounds(start_point_x, start_point_y, start_x, start_y); + costmap->worldToMapEnforceBounds(end_point_x, end_point_y, end_x, end_y); + + costmap->clearArea(start_x, start_y, end_x, end_y, invert); double ox = costmap->getOriginX(), oy = costmap->getOriginY(); double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY(); costmap->addExtraBounds(ox, oy, ox + width, oy + height); } +void ClearCostmapService::clearEntirely() +{ + std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); + costmap_.resetLayers(); +} + bool ClearCostmapService::getPosition(double & x, double & y) const { geometry_msgs::msg::PoseStamped pose; @@ -204,17 +158,4 @@ bool ClearCostmapService::getPosition(double & x, double & y) const return true; } -string ClearCostmapService::getLayerName(const Layer & layer) const -{ - string name = layer.getName(); - - size_t slash = name.rfind('/'); - - if (slash != std::string::npos) { - name = name.substr(slash + 1); - } - - return name; -} - } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 21a96bcf8b..e72807da59 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -41,6 +41,8 @@ #include #include #include +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/occ_grid_values.hpp" namespace nav2_costmap_2d { @@ -57,6 +59,37 @@ Costmap2D::Costmap2D( resetMaps(); } +Costmap2D::Costmap2D(const nav_msgs::msg::OccupancyGrid & map) +: default_value_(FREE_SPACE) +{ + access_ = new mutex_t(); + + // fill local variables + size_x_ = map.info.width; + size_y_ = map.info.height; + resolution_ = map.info.resolution; + origin_x_ = map.info.origin.position.x; + origin_y_ = map.info.origin.position.y; + + // create the costmap + costmap_ = new unsigned char[size_x_ * size_y_]; + + // fill the costmap with a data + int8_t data; + for (unsigned int it = 0; it < size_x_ * size_y_; it++) { + data = map.data[it]; + if (data == nav2_util::OCC_GRID_UNKNOWN) { + costmap_[it] = NO_INFORMATION; + } else { + // Linear conversion from OccupancyGrid data range [OCC_GRID_FREE..OCC_GRID_OCCUPIED] + // to costmap data range [FREE_SPACE..LETHAL_OBSTACLE] + costmap_[it] = std::round( + static_cast(data) * (LETHAL_OBSTACLE - FREE_SPACE) / + (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE)); + } + } +} + void Costmap2D::deleteMaps() { // clean up data @@ -151,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 @@ -234,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; } diff --git a/nav2_costmap_2d/src/costmap_2d_cloud.cpp b/nav2_costmap_2d/src/costmap_2d_cloud.cpp index 52157b3b7a..8c40709be6 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_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 56797e566b..906a9f0324 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -50,27 +50,35 @@ namespace nav2_costmap_2d char * Costmap2DPublisher::cost_translation_table_ = NULL; Costmap2DPublisher::Costmap2DPublisher( - nav2_util::LifecycleNode::SharedPtr ros_node, Costmap2D * costmap, + const nav2_util::LifecycleNode::WeakPtr & parent, + Costmap2D * costmap, std::string global_frame, std::string topic_name, bool always_send_full_costmap) -: node_(ros_node), costmap_(costmap), global_frame_(global_frame), topic_name_(topic_name), - active_(false), always_send_full_costmap_(always_send_full_costmap) +: costmap_(costmap), + global_frame_(global_frame), + topic_name_(topic_name), + active_(false), + always_send_full_costmap_(always_send_full_costmap) { + auto node = parent.lock(); + clock_ = node->get_clock(); + logger_ = node->get_logger(); + auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); // TODO(bpwilcox): port onNewSubscription functionality for publisher - costmap_pub_ = node_->create_publisher( + costmap_pub_ = node->create_publisher( topic_name, custom_qos); - costmap_raw_pub_ = node_->create_publisher( + costmap_raw_pub_ = node->create_publisher( topic_name + "_raw", custom_qos); - costmap_update_pub_ = node_->create_publisher( + costmap_update_pub_ = node->create_publisher( topic_name + "_updates", custom_qos); // Create a service that will use the callback function to handle requests. - costmap_service_ = node_->create_service( + costmap_service_ = node->create_service( "get_costmap", std::bind( &Costmap2DPublisher::costmap_service_callback, this, std::placeholders::_1, std::placeholders::_2, @@ -118,7 +126,7 @@ void Costmap2DPublisher::prepareGrid() grid_ = std::make_unique(); grid_->header.frame_id = global_frame_; - grid_->header.stamp = rclcpp::Time(); + grid_->header.stamp = clock_->now(); grid_->info.resolution = grid_resolution; @@ -150,7 +158,7 @@ void Costmap2DPublisher::prepareCostmap() costmap_raw_ = std::make_unique(); costmap_raw_->header.frame_id = global_frame_; - costmap_raw_->header.stamp = node_->now(); + costmap_raw_->header.stamp = clock_->now(); costmap_raw_->metadata.layer = "master"; costmap_raw_->metadata.resolution = resolution; @@ -175,7 +183,7 @@ void Costmap2DPublisher::prepareCostmap() void Costmap2DPublisher::publishCostmap() { - if (node_->count_subscribers(costmap_raw_pub_->get_topic_name()) > 0) { + if (costmap_raw_pub_->get_subscription_count() > 0) { prepareCostmap(); costmap_raw_pub_->publish(std::move(costmap_raw_)); } @@ -187,12 +195,12 @@ void Costmap2DPublisher::publishCostmap() saved_origin_x_ != costmap_->getOriginX() || saved_origin_y_ != costmap_->getOriginY()) { - if (node_->count_subscribers(costmap_pub_->get_topic_name()) > 0) { + if (costmap_pub_->get_subscription_count() > 0) { prepareGrid(); costmap_pub_->publish(std::move(grid_)); } } else if (x0_ < xn_) { - if (node_->count_subscribers(costmap_update_pub_->get_topic_name()) > 0) { + if (costmap_update_pub_->get_subscription_count() > 0) { std::unique_lock lock(*(costmap_->getMutex())); // Publish Just an Update auto update = std::make_unique(); @@ -225,7 +233,7 @@ Costmap2DPublisher::costmap_service_callback( const std::shared_ptr/*request*/, const std::shared_ptr response) { - RCLCPP_DEBUG(node_->get_logger(), "Received costmap service request"); + RCLCPP_DEBUG(logger_, "Received costmap service request"); // TODO(bpwilcox): Grab correct orientation information tf2::Quaternion quaternion; @@ -235,7 +243,7 @@ Costmap2DPublisher::costmap_service_callback( auto size_y = costmap_->getSizeInCellsY(); auto data_length = size_x * size_y; unsigned char * data = costmap_->getCharMap(); - auto current_time = node_->now(); + auto current_time = clock_->now(); response->map.header.stamp = current_time; response->map.header.frame_id = global_frame_; diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 89c3d28aca..12d6b2ee68 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -39,6 +39,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include +#include #include #include #include @@ -46,11 +47,14 @@ #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" +#include "rcl_interfaces/msg/set_parameters_result.hpp" using namespace std::chrono_literals; +using std::placeholders::_1; +using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { @@ -61,7 +65,7 @@ Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, const std::string & local_namespace) -: nav2_util::LifecycleNode(name, "", true, +: nav2_util::LifecycleNode(name, "", // NodeOption arguments take precedence over the ones provided on the command line // use this to make sure the node is placed on the provided namespace // TODO(orduno) Pass a sub-node instead of creating a new node for better handling @@ -80,11 +84,8 @@ Costmap2DROS::Costmap2DROS( "nav2_costmap_2d::InflationLayer"} { RCLCPP_INFO(get_logger(), "Creating Costmap"); - auto options = rclcpp::NodeOptions().arguments( - {"--ros-args", "-r", std::string("__node:=") + get_name() + "_client", "--"}); - client_node_ = std::make_shared("_", options); - std::vector clearable_layers{"obstacle_layer"}; + std::vector clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"}; declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false)); declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f)); @@ -100,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"))); @@ -116,7 +118,6 @@ Costmap2DROS::Costmap2DROS( Costmap2DROS::~Costmap2DROS() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn @@ -125,8 +126,12 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Configuring"); getParameters(); + callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + // Create the costmap itself - layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window_, track_unknown_space_); + layered_costmap_ = std::make_unique( + global_frame_, rolling_window_, track_unknown_space_); if (!layered_costmap_->isSizeLocked()) { layered_costmap_->resizeMap( @@ -135,10 +140,11 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) } // Create the transform-related objects - tf_buffer_ = std::make_shared(rclcpp_node_->get_clock()); + tf_buffer_ = std::make_shared(get_clock()); auto timer_interface = std::make_shared( - rclcpp_node_->get_node_base_interface(), - rclcpp_node_->get_node_timers_interface()); + get_node_base_interface(), + get_node_timers_interface(), + callback_group_); tf_buffer_->setCreateTimerInterface(timer_interface); tf_listener_ = std::make_shared(*tf_buffer_); @@ -151,11 +157,24 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) // TODO(mjeronimo): instead of get(), use a shared ptr plugin->initialize( - layered_costmap_, plugin_names_[i], tf_buffer_.get(), - shared_from_this(), client_node_, rclcpp_node_); + layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(), + shared_from_this(), callback_group_); 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(), callback_group_); + + RCLCPP_INFO(get_logger(), "Initialized costmap filter \"%s\"", filter_names_[i].c_str()); + } // Create the publishers and subscribers footprint_sub_ = create_subscription( @@ -166,7 +185,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) footprint_pub_ = create_publisher( "published_footprint", rclcpp::SystemDefaultsQoS()); - costmap_publisher_ = new Costmap2DPublisher( + costmap_publisher_ = std::make_unique( shared_from_this(), layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap_); @@ -183,6 +202,9 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) // Add cleaning service clear_costmap_service_ = std::make_unique(shared_from_this(), *this); + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, get_node_base_interface()); + executor_thread_ = std::make_unique(executor_); return nav2_util::CallbackReturn::SUCCESS; } @@ -220,13 +242,15 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) stopped_ = true; // to active plugins stop_updates_ = false; map_update_thread_shutdown_ = false; - - map_update_thread_ = new std::thread( - std::bind( - &Costmap2DROS::mapUpdateLoop, this, map_update_frequency_)); + map_update_thread_ = std::make_unique( + std::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency_)); start(); + // Add callback for dynamic parameters + dyn_params_handler = this->add_on_set_parameters_callback( + std::bind(&Costmap2DROS::dynamicParametersCallback, this, _1)); + return nav2_util::CallbackReturn::SUCCESS; } @@ -235,17 +259,15 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); + dyn_params_handler.reset(); costmap_publisher_->on_deactivate(); footprint_pub_->on_deactivate(); stop(); // Map thread stuff - // TODO(mjeronimo): unique_ptr map_update_thread_shutdown_ = true; map_update_thread_->join(); - delete map_update_thread_; - map_update_thread_ = nullptr; return nav2_util::CallbackReturn::SUCCESS; } @@ -255,8 +277,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - delete layered_costmap_; - layered_costmap_ = nullptr; + layered_costmap_.reset(); tf_listener_.reset(); tf_buffer_.reset(); @@ -264,13 +285,10 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); - if (costmap_publisher_ != nullptr) { - delete costmap_publisher_; - costmap_publisher_ = nullptr; - } - + costmap_publisher_.reset(); clear_costmap_service_.reset(); + executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -304,6 +322,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(); @@ -314,17 +333,21 @@ 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) { publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_); } else { - publish_cycle_ = rclcpp::Duration(-1); + publish_cycle_ = rclcpp::Duration(-1s); } // 3. If the footprint has been specified, it must be in the correct format @@ -398,7 +421,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) timer.end(); RCLCPP_DEBUG(get_logger(), "Map update time: %.9f", timer.elapsed_time_in_seconds()); - if (publish_cycle_ > rclcpp::Duration(0) && layered_costmap_->isInitialized()) { + if (publish_cycle_ > rclcpp::Duration(0s) && layered_costmap_->isInitialized()) { unsigned int x0, y0, xn, yn; layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); @@ -443,8 +466,7 @@ Costmap2DROS::updateMap() layered_costmap_->updateMap(x, y, yaw); auto footprint = std::make_unique(); - footprint->header.frame_id = global_frame_; - footprint->header.stamp = now(); + footprint->header = pose.header; transformFootprint(x, y, yaw, padded_footprint_, *footprint); RCLCPP_DEBUG(get_logger(), "Publishing footprint"); @@ -459,6 +481,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_) { @@ -469,6 +492,12 @@ Costmap2DROS::start() { (*plugin)->activate(); } + for (std::vector>::iterator filter = filters->begin(); + filter != filters->end(); + ++filter) + { + (*filter)->activate(); + } stopped_ = false; } stop_updates_ = false; @@ -486,12 +515,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; } @@ -523,11 +558,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 @@ -538,4 +579,109 @@ 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_); + } +} + +rcl_interfaces::msg::SetParametersResult +Costmap2DROS::dynamicParametersCallback(std::vector parameters) +{ + auto result = rcl_interfaces::msg::SetParametersResult(); + bool resize_map = false; + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == "robot_radius") { + robot_radius_ = parameter.as_double(); + // Set the footprint + if (use_radius_) { + setRobotFootprint(makeFootprintFromRadius(robot_radius_)); + } + } else if (name == "footprint_padding") { + footprint_padding_ = parameter.as_double(); + padded_footprint_ = unpadded_footprint_; + padFootprint(padded_footprint_, footprint_padding_); + layered_costmap_->setFootprint(padded_footprint_); + } else if (name == "transform_tolerance") { + transform_tolerance_ = parameter.as_double(); + } else if (name == "publish_frequency") { + map_publish_frequency_ = parameter.as_double(); + if (map_publish_frequency_ > 0) { + publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_); + } else { + publish_cycle_ = rclcpp::Duration(-1s); + } + } else if (name == "resolution") { + resize_map = true; + resolution_ = parameter.as_double(); + } else if (name == "origin_x") { + resize_map = true; + origin_x_ = parameter.as_double(); + } else if (name == "origin_y") { + resize_map = true; + origin_y_ = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_INTEGER) { + if (name == "width") { + resize_map = true; + map_width_meters_ = parameter.as_int(); + } else if (name == "height") { + resize_map = true; + map_height_meters_ = parameter.as_int(); + } + } else if (type == ParameterType::PARAMETER_STRING) { + if (name == "footprint") { + footprint_ = parameter.as_string(); + std::vector new_footprint; + if (makeFootprintFromString(footprint_, new_footprint)) { + setRobotFootprint(new_footprint); + } + } else if (name == "robot_base_frame") { + // First, make sure that the transform between the robot base frame + // and the global frame is available + std::string tf_error; + RCLCPP_INFO(get_logger(), "Checking transform"); + if (!tf_buffer_->canTransform( + global_frame_, parameter.as_string(), tf2::TimePointZero, + tf2::durationFromSec(1.0), &tf_error)) + { + RCLCPP_WARN( + get_logger(), "Timed out waiting for transform from %s to %s" + " to become available, tf error: %s", + parameter.as_string().c_str(), global_frame_.c_str(), tf_error.c_str()); + RCLCPP_WARN( + get_logger(), "Rejecting robot_base_frame change to %s , leaving it to its original" + " value of %s", parameter.as_string().c_str(), robot_base_frame_.c_str()); + result.successful = false; + return result; + } + robot_base_frame_ = parameter.as_string(); + } + } + } + + if (resize_map && !layered_costmap_->isSizeLocked()) { + layered_costmap_->resizeMap( + (unsigned int)(map_width_meters_ / resolution_), + (unsigned int)(map_height_meters_ / resolution_), resolution_, origin_x_, origin_y_); + } + + result.successful = true; + return result; +} + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp index b3aecb7534..3f9da4013d 100644 --- a/nav2_costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -61,14 +61,15 @@ void CostmapLayer::matchSize() master->getOriginX(), master->getOriginY()); } -void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y) +void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert) { + current_ = false; unsigned char * grid = getCharMap(); for (int x = 0; x < static_cast(getSizeInCellsX()); x++) { bool xrange = x > start_x && x < end_x; for (int y = 0; y < static_cast(getSizeInCellsY()); y++) { - if (xrange && y > start_y && y < end_y) { + if ((xrange && y > start_y && y < end_y) == invert) { continue; } int index = getIndex(x, y); diff --git a/nav2_costmap_2d/src/costmap_math.cpp b/nav2_costmap_2d/src/costmap_math.cpp index 7de87fa6f2..9d376e6e6d 100644 --- a/nav2_costmap_2d/src/costmap_math.cpp +++ b/nav2_costmap_2d/src/costmap_math.cpp @@ -57,36 +57,3 @@ double distanceToLine(double pX, double pY, double x0, double y0, double x1, dou return distance(pX, pY, xx, yy); } - -bool intersects(std::vector & polygon, float testx, float testy) -{ - bool c = false; - int i, j, nvert = polygon.size(); - for (i = 0, j = nvert - 1; i < nvert; j = i++) { - float yi = polygon[i].y, yj = polygon[j].y, xi = polygon[i].x, xj = polygon[j].x; - - if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi)) { - c = !c; - } - } - return c; -} - -bool intersects_helper( - std::vector & polygon1, - std::vector & polygon2) -{ - for (unsigned int i = 0; i < polygon1.size(); i++) { - if (intersects(polygon2, polygon1[i].x, polygon1[i].y)) { - return true; - } - } - return false; -} - -bool intersects( - std::vector & polygon1, - std::vector & polygon2) -{ - return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1); -} diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index 18d9efc3fd..d56ac942e9 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -21,35 +21,25 @@ namespace nav2_costmap_2d { CostmapSubscriber::CostmapSubscriber( - nav2_util::LifecycleNode::SharedPtr node, + const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name) -: CostmapSubscriber(node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_logging_interface(), - topic_name) -{} - -CostmapSubscriber::CostmapSubscriber( - rclcpp::Node::SharedPtr node, - const std::string & topic_name) -: CostmapSubscriber(node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_logging_interface(), - topic_name) -{} +: topic_name_(topic_name) +{ + auto node = parent.lock(); + costmap_sub_ = node->create_subscription( + topic_name_, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); +} CostmapSubscriber::CostmapSubscriber( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const rclcpp::Node::WeakPtr & parent, const std::string & topic_name) -: node_base_(node_base), - node_topics_(node_topics), - node_logging_(node_logging), - topic_name_(topic_name) +: topic_name_(topic_name) { - costmap_sub_ = rclcpp::create_subscription( - node_topics_, topic_name_, + auto node = parent.lock(); + costmap_sub_ = node->create_subscription( + topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); } @@ -65,30 +55,32 @@ std::shared_ptr CostmapSubscriber::getCostmap() void CostmapSubscriber::toCostmap2D() { + auto current_costmap_msg = std::atomic_load(&costmap_msg_); + if (costmap_ == nullptr) { costmap_ = std::make_shared( - costmap_msg_->metadata.size_x, costmap_msg_->metadata.size_y, - costmap_msg_->metadata.resolution, costmap_msg_->metadata.origin.position.x, - costmap_msg_->metadata.origin.position.y); - } else if (costmap_->getSizeInCellsX() != costmap_msg_->metadata.size_x || // NOLINT - costmap_->getSizeInCellsY() != costmap_msg_->metadata.size_y || - costmap_->getResolution() != costmap_msg_->metadata.resolution || - costmap_->getOriginX() != costmap_msg_->metadata.origin.position.x || - costmap_->getOriginY() != costmap_msg_->metadata.origin.position.y) + current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, + current_costmap_msg->metadata.resolution, current_costmap_msg->metadata.origin.position.x, + current_costmap_msg->metadata.origin.position.y); + } else if (costmap_->getSizeInCellsX() != current_costmap_msg->metadata.size_x || // NOLINT + costmap_->getSizeInCellsY() != current_costmap_msg->metadata.size_y || + costmap_->getResolution() != current_costmap_msg->metadata.resolution || + costmap_->getOriginX() != current_costmap_msg->metadata.origin.position.x || + costmap_->getOriginY() != current_costmap_msg->metadata.origin.position.y) { // Update the size of the costmap costmap_->resizeMap( - costmap_msg_->metadata.size_x, costmap_msg_->metadata.size_y, - costmap_msg_->metadata.resolution, - costmap_msg_->metadata.origin.position.x, - costmap_msg_->metadata.origin.position.y); + current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y, + current_costmap_msg->metadata.resolution, + current_costmap_msg->metadata.origin.position.x, + current_costmap_msg->metadata.origin.position.y); } unsigned char * master_array = costmap_->getCharMap(); unsigned int index = 0; - for (unsigned int i = 0; i < costmap_msg_->metadata.size_x; ++i) { - for (unsigned int j = 0; j < costmap_msg_->metadata.size_y; ++j) { - master_array[index] = costmap_msg_->data[index]; + for (unsigned int i = 0; i < current_costmap_msg->metadata.size_x; ++i) { + for (unsigned int j = 0; j < current_costmap_msg->metadata.size_y; ++j) { + master_array[index] = current_costmap_msg->data[index]; ++index; } } @@ -96,7 +88,7 @@ void CostmapSubscriber::toCostmap2D() void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) { - costmap_msg_ = msg; + std::atomic_store(&costmap_msg_, msg); if (!costmap_received_) { costmap_received_ = true; } diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp index 225e6ac3fb..11168f6373 100644 --- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp @@ -35,27 +35,19 @@ namespace nav2_costmap_2d CostmapTopicCollisionChecker::CostmapTopicCollisionChecker( CostmapSubscriber & costmap_sub, FootprintSubscriber & footprint_sub, - tf2_ros::Buffer & tf, - std::string name, - std::string global_frame, - std::string robot_base_frame, - double transform_tolerance) + std::string name) : name_(name), - global_frame_(global_frame), - robot_base_frame_(robot_base_frame), - tf_(tf), costmap_sub_(costmap_sub), footprint_sub_(footprint_sub), - transform_tolerance_(transform_tolerance), collision_checker_(nullptr) -{ -} +{} bool CostmapTopicCollisionChecker::isCollisionFree( - const geometry_msgs::msg::Pose2D & pose) + const geometry_msgs::msg::Pose2D & pose, + bool fetch_costmap_and_footprint) { try { - if (scorePose(pose) >= LETHAL_OBSTACLE) { + if (scorePose(pose, fetch_costmap_and_footprint) >= LETHAL_OBSTACLE) { return false; } return true; @@ -72,12 +64,15 @@ bool CostmapTopicCollisionChecker::isCollisionFree( } double CostmapTopicCollisionChecker::scorePose( - const geometry_msgs::msg::Pose2D & pose) + const geometry_msgs::msg::Pose2D & pose, + bool fetch_costmap_and_footprint) { - try { - collision_checker_.setCostmap(costmap_sub_.getCostmap()); - } catch (const std::runtime_error & e) { - throw CollisionCheckerException(e.what()); + if (fetch_costmap_and_footprint) { + try { + collision_checker_.setCostmap(costmap_sub_.getCostmap()); + } catch (const std::runtime_error & e) { + throw CollisionCheckerException(e.what()); + } } unsigned int cell_x, cell_y; @@ -86,43 +81,23 @@ double CostmapTopicCollisionChecker::scorePose( throw IllegalPoseException(name_, "Pose Goes Off Grid."); } - return collision_checker_.footprintCost(getFootprint(pose)); + return collision_checker_.footprintCost(getFootprint(pose, fetch_costmap_and_footprint)); } -Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose) +Footprint CostmapTopicCollisionChecker::getFootprint( + const geometry_msgs::msg::Pose2D & pose, + bool fetch_latest_footprint) { - Footprint footprint; - if (!footprint_sub_.getFootprint(footprint)) { - throw CollisionCheckerException("Current footprint not available."); + if (fetch_latest_footprint) { + std_msgs::msg::Header header; + if (!footprint_sub_.getFootprintInRobotFrame(footprint_, header)) { + throw CollisionCheckerException("Current footprint not available."); + } } - - Footprint footprint_spec; - unorientFootprint(footprint, footprint_spec); - transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint); + Footprint footprint; + transformFootprint(pose.x, pose.y, pose.theta, footprint_, footprint); return footprint; } -void CostmapTopicCollisionChecker::unorientFootprint( - const std::vector & oriented_footprint, - std::vector & reset_footprint) -{ - geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose( - current_pose, tf_, global_frame_, robot_base_frame_, - transform_tolerance_)) - { - throw CollisionCheckerException("Robot pose unavailable."); - } - - double x = current_pose.pose.position.x; - double y = current_pose.pose.position.y; - double theta = tf2::getYaw(current_pose.pose.orientation); - - Footprint temp; - transformFootprint(-x, -y, 0, oriented_footprint, temp); - transformFootprint(0, 0, -theta, temp, reset_footprint); -} - - } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/footprint.cpp b/nav2_costmap_2d/src/footprint.cpp index 1efe6c5396..608a494a9d 100644 --- a/nav2_costmap_2d/src/footprint.cpp +++ b/nav2_costmap_2d/src/footprint.cpp @@ -112,14 +112,15 @@ void transformFootprint( std::vector & oriented_footprint) { // build the oriented footprint at a given location - oriented_footprint.clear(); + oriented_footprint.resize(footprint_spec.size()); double cos_th = cos(theta); double sin_th = sin(theta); for (unsigned int i = 0; i < footprint_spec.size(); ++i) { - geometry_msgs::msg::Point new_pt; - new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th); - new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th); - oriented_footprint.push_back(new_pt); + double new_x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th); + double new_y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th); + geometry_msgs::msg::Point & new_pt = oriented_footprint[i]; + new_pt.x = new_x; + new_pt.y = new_y; } } diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 20d60b3442..5898879e8c 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -51,36 +51,37 @@ 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)) { return static_cast(LETHAL_OBSTACLE); } 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(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 @@ -92,6 +93,11 @@ double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) { point_cost = pointCost(line.getX(), line.getY()); // Score the current point + // if in collision, no need to continue + if (point_cost == static_cast(LETHAL_OBSTACLE)) { + return point_cost; + } + if (line_cost < point_cost) { line_cost = point_cost; } diff --git a/nav2_costmap_2d/src/footprint_subscriber.cpp b/nav2_costmap_2d/src/footprint_subscriber.cpp index 242c7aec37..45e60db65e 100644 --- a/nav2_costmap_2d/src/footprint_subscriber.cpp +++ b/nav2_costmap_2d/src/footprint_subscriber.cpp @@ -17,93 +17,100 @@ #include #include "nav2_costmap_2d/footprint_subscriber.hpp" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" +#include "tf2/utils.h" +#pragma GCC diagnostic pop namespace nav2_costmap_2d { FootprintSubscriber::FootprintSubscriber( - nav2_util::LifecycleNode::SharedPtr node, + const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name, - const double & footprint_timeout) -: FootprintSubscriber(node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_logging_interface(), - node->get_node_clock_interface(), - topic_name, footprint_timeout) -{} - -FootprintSubscriber::FootprintSubscriber( - rclcpp::Node::SharedPtr node, - const std::string & topic_name, - const double & footprint_timeout) -: FootprintSubscriber(node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_logging_interface(), - node->get_node_clock_interface(), - topic_name, footprint_timeout) -{} + tf2_ros::Buffer & tf, + std::string robot_base_frame, + double transform_tolerance) +: topic_name_(topic_name), + tf_(tf), + robot_base_frame_(robot_base_frame), + transform_tolerance_(transform_tolerance) +{ + auto node = parent.lock(); + footprint_sub_ = node->create_subscription( + topic_name, rclcpp::SystemDefaultsQoS(), + std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1)); +} FootprintSubscriber::FootprintSubscriber( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + const rclcpp::Node::WeakPtr & parent, const std::string & topic_name, - const double & footprint_timeout) -: node_base_(node_base), - node_topics_(node_topics), - node_logging_(node_logging), - node_clock_(node_clock), - topic_name_(topic_name), - footprint_timeout_(rclcpp::Duration::from_seconds(footprint_timeout)) + tf2_ros::Buffer & tf, + std::string robot_base_frame, + double transform_tolerance) +: topic_name_(topic_name), + tf_(tf), + robot_base_frame_(robot_base_frame), + transform_tolerance_(transform_tolerance) { - footprint_sub_ = rclcpp::create_subscription( - node_topics_, + auto node = parent.lock(); + footprint_sub_ = node->create_subscription( topic_name, rclcpp::SystemDefaultsQoS(), std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1)); } bool -FootprintSubscriber::getFootprint( +FootprintSubscriber::getFootprintRaw( std::vector & footprint, - rclcpp::Time & stamp, - rclcpp::Duration valid_footprint_timeout) + std_msgs::msg::Header & footprint_header) { if (!footprint_received_) { return false; } + auto current_footprint = std::atomic_load(&footprint_); footprint = toPointVector( - std::make_shared(footprint_->polygon)); - auto & footprint_stamp = footprint_->header.stamp; - - if (stamp - footprint_stamp > valid_footprint_timeout) { - return false; - } + std::make_shared(current_footprint->polygon)); + footprint_header = current_footprint->header; return true; } bool -FootprintSubscriber::getFootprint( +FootprintSubscriber::getFootprintInRobotFrame( std::vector & footprint, - rclcpp::Duration & valid_footprint_timeout) + std_msgs::msg::Header & footprint_header) { - rclcpp::Time t = node_clock_->get_clock()->now(); - return getFootprint(footprint, t, valid_footprint_timeout); -} + if (!getFootprintRaw(footprint, footprint_header)) { + return false; + } -bool -FootprintSubscriber::getFootprint( - std::vector & footprint) -{ - return getFootprint(footprint, footprint_timeout_); + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, tf_, footprint_header.frame_id, robot_base_frame_, + transform_tolerance_, footprint_header.stamp)) + { + return false; + } + + double x = current_pose.pose.position.x; + double y = current_pose.pose.position.y; + double theta = tf2::getYaw(current_pose.pose.orientation); + + std::vector temp; + transformFootprint(-x, -y, 0, footprint, temp); + transformFootprint(0, 0, -theta, temp, footprint); + + footprint_header.frame_id = robot_base_frame_; + footprint_header.stamp = current_pose.header.stamp; + + return true; } void FootprintSubscriber::footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg) { - footprint_ = msg; + std::atomic_store(&footprint_, msg); if (!footprint_received_) { footprint_received_ = true; } diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index 228c060f70..852b4b29b5 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -46,17 +46,22 @@ Layer::Layer() void Layer::initialize( - LayeredCostmap * parent, std::string name, tf2_ros::Buffer * tf, - nav2_util::LifecycleNode::SharedPtr node, - rclcpp::Node::SharedPtr client_node, - rclcpp::Node::SharedPtr rclcpp_node) + LayeredCostmap * parent, + std::string name, + tf2_ros::Buffer * tf, + const nav2_util::LifecycleNode::WeakPtr & node, + rclcpp::CallbackGroup::SharedPtr callback_group) { layered_costmap_ = parent; name_ = name; tf_ = tf; node_ = node; - client_node_ = client_node; - rclcpp_node_ = rclcpp_node; + callback_group_ = callback_group; + { + auto node_shared_ptr = node_.lock(); + logger_ = node_shared_ptr->get_logger(); + clock_ = node_shared_ptr->get_clock(); + } onInitialize(); } @@ -69,25 +74,40 @@ Layer::getFootprint() const void Layer::declareParameter( - const std::string & param_name, const rclcpp::ParameterValue & value) + const std::string & param_name, + const rclcpp::ParameterValue & value) { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } local_params_.insert(param_name); - nav2_util::declare_parameter_if_not_declared(node_, getFullName(param_name), value); + nav2_util::declare_parameter_if_not_declared( + node, getFullName(param_name), value); } -bool -Layer::hasParameter(const std::string & param_name) +void +Layer::declareParameter( + const std::string & param_name, + const rclcpp::ParameterType & param_type) { - return node_->has_parameter(getFullName(param_name)); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + local_params_.insert(param_name); + nav2_util::declare_parameter_if_not_declared( + node, getFullName(param_name), param_type); } -void -Layer::undeclareAllParameters() +bool +Layer::hasParameter(const std::string & param_name) { - for (auto & param_name : local_params_) { - node_->undeclare_parameter(getFullName(param_name)); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; } - local_params_.clear(); + return node->has_parameter(getFullName(param_name)); } std::string diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index 4d0b9673c3..42c14ef7b4 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,15 @@ LayeredCostmap::~LayeredCostmap() while (plugins_.size() > 0) { plugins_.pop_back(); } + while (filters_.size() > 0) { + filters_.pop_back(); + } +} + +void LayeredCostmap::addPlugin(std::shared_ptr plugin) +{ + std::unique_lock lock(*(combined_costmap_.getMutex())); + plugins_.push_back(plugin); } void LayeredCostmap::resizeMap( @@ -90,34 +101,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 +144,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 +169,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 +205,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 +258,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 +279,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 f87d59bc25..8a06823df3 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -40,82 +40,46 @@ #include #include #include +#include #include "tf2/convert.h" #include "sensor_msgs/point_cloud2_iterator.hpp" +using namespace std::chrono_literals; namespace nav2_costmap_2d { ObservationBuffer::ObservationBuffer( - nav2_util::LifecycleNode::SharedPtr nh, std::string topic_name, double observation_keep_time, + const nav2_util::LifecycleNode::WeakPtr & parent, + 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, double 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)), nh_(nh), - last_updated_(nh->now()), global_frame_(global_frame), sensor_frame_(sensor_frame), + expected_update_rate_(rclcpp::Duration::from_seconds(expected_update_rate)), + global_frame_(global_frame), + 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(); + logger_ = node->get_logger(); + last_updated_ = node->now(); } ObservationBuffer::~ObservationBuffer() { } -bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) -{ - rclcpp::Time transform_time = nh_->now(); - std::string tf_error; - - geometry_msgs::msg::TransformStamped transformStamped; - if (!tf2_buffer_.canTransform( - new_global_frame, global_frame_, tf2_ros::fromMsg(transform_time), - tf2::durationFromSec(tf_tolerance_), &tf_error)) - { - RCLCPP_ERROR( - rclcpp::get_logger( - "nav2_costmap_2d"), "Transform between %s and %s with tolerance %.2f failed: %s.", - new_global_frame.c_str(), - global_frame_.c_str(), tf_tolerance_, tf_error.c_str()); - return false; - } - - std::list::iterator obs_it; - for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) { - try { - Observation & obs = *obs_it; - - geometry_msgs::msg::PointStamped origin; - origin.header.frame_id = global_frame_; - origin.header.stamp = transform_time; - origin.point = obs.origin_; - - // we need to transform the origin of the observation to the new global frame - tf2_buffer_.transform(origin, origin, new_global_frame, tf2::durationFromSec(tf_tolerance_)); - obs.origin_ = origin.point; - - // we also need to transform the cloud of the observation to the new global frame - tf2_buffer_.transform( - *(obs.cloud_), *(obs.cloud_), new_global_frame, tf2::durationFromSec(tf_tolerance_)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - rclcpp::get_logger( - "nav2_costmap_2d"), "TF Error attempting to transform an observation from %s to %s: %s", - global_frame_.c_str(), - new_global_frame.c_str(), ex.what()); - return false; - } - } - - // now we need to update our global_frame member - global_frame_ = new_global_frame; - return true; -} - void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud) { geometry_msgs::msg::PointStamped global_origin; @@ -136,18 +100,20 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud) local_origin.point.x = 0; local_origin.point.y = 0; local_origin.point.z = 0; - tf2_buffer_.transform(local_origin, global_origin, global_frame_); + tf2_buffer_.transform(local_origin, global_origin, global_frame_, tf_tolerance_); tf2::convert(global_origin.point, observation_list_.front().origin_); // 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; // transform the point cloud - tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_); + tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_, tf_tolerance_); global_frame_cloud.header.stamp = cloud.header.stamp; // now we need to remove observations from the cloud that are below @@ -191,8 +157,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud) // if an exception occurs, we need to remove the empty observation from the list observation_list_.pop_front(); RCLCPP_ERROR( - rclcpp::get_logger( - "nav2_costmap_2d"), + logger_, "TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what()); @@ -200,7 +165,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud) } // if the update was successful, we want to update the last updated time - last_updated_ = nh_->now(); + last_updated_ = clock_->now(); // we'll also remove any stale observations from the list purgeStaleObservations(); @@ -224,7 +189,7 @@ void ObservationBuffer::purgeStaleObservations() if (!observation_list_.empty()) { std::list::iterator obs_it = observation_list_.begin(); // if we're keeping observations for no time... then we'll only keep one observation - if (observation_keep_time_ == rclcpp::Duration(0.0)) { + if (observation_keep_time_ == rclcpp::Duration(0.0s)) { observation_list_.erase(++obs_it, observation_list_.end()); return; } @@ -234,7 +199,9 @@ void ObservationBuffer::purgeStaleObservations() Observation & obs = *obs_it; // check if the observation is out of date... and if it is, // remove it and those that follow from the list - if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_) { + if ((clock_->now() - obs.cloud_->header.stamp) > + observation_keep_time_) + { observation_list_.erase(obs_it, observation_list_.end()); return; } @@ -244,24 +211,26 @@ void ObservationBuffer::purgeStaleObservations() bool ObservationBuffer::isCurrent() const { - if (expected_update_rate_ == rclcpp::Duration(0.0)) { + if (expected_update_rate_ == rclcpp::Duration(0.0s)) { return true; } - bool current = (nh_->now() - last_updated_) <= expected_update_rate_; + bool current = (clock_->now() - last_updated_) <= + expected_update_rate_; if (!current) { RCLCPP_WARN( - rclcpp::get_logger( - "nav2_costmap_2d"), - "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.", //NOLINT + logger_, + "The %s observation buffer has not been updated for %.2f seconds, " + "and it should be updated every %.2f seconds.", topic_name_.c_str(), - (nh_->now() - last_updated_).seconds(), expected_update_rate_.seconds()); + (clock_->now() - last_updated_).seconds(), + expected_update_rate_.seconds()); } return current; } void ObservationBuffer::resetLastUpdated() { - last_updated_ = nh_->now(); + last_updated_ = clock_->now(); } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/CMakeLists.txt b/nav2_costmap_2d/test/CMakeLists.txt index 44b89f8693..ad7905ad1f 100644 --- a/nav2_costmap_2d/test/CMakeLists.txt +++ b/nav2_costmap_2d/test/CMakeLists.txt @@ -3,3 +3,4 @@ set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files) add_subdirectory(unit) add_subdirectory(integration) +add_subdirectory(regression) diff --git a/nav2_costmap_2d/test/costmap_params.yaml b/nav2_costmap_2d/test/costmap_params.yaml index b402413b2e..d3d9403b86 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/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 5ecbeba923..ef0b148440 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -54,6 +54,16 @@ target_link_libraries(range_tests_exec layers ) +ament_add_gtest(dyn_params_tests + dyn_params_tests.cpp +) +ament_target_dependencies(dyn_params_tests + ${dependencies} +) +target_link_libraries(dyn_params_tests + nav2_costmap_2d_core +) + ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" diff --git a/nav2_costmap_2d/test/integration/dyn_params_tests.cpp b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp new file mode 100644 index 0000000000..4d0a3bef54 --- /dev/null +++ b/nav2_costmap_2d/test/integration/dyn_params_tests.cpp @@ -0,0 +1,105 @@ +// Copyright (c) 2021 Wyca Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "tf2_ros/transform_broadcaster.h" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class DynParamTestNode +{ +public: + DynParamTestNode() {} + ~DynParamTestNode() {} +}; + +TEST(DynParamTestNode, testDynParamsSet) +{ + auto node = std::make_shared("dyn_param_tester"); + auto costmap = std::make_shared("test_costmap"); + costmap->on_configure(rclcpp_lifecycle::State()); + + // Set tf between default global_frame and robot_base_frame in order not to block in on_activate + std::unique_ptr tf_broadcaster_ = + std::make_unique(node); + geometry_msgs::msg::TransformStamped t; + t.header.stamp = node->get_clock()->now(); + t.header.frame_id = "map"; + t.child_frame_id = "base_link"; + tf_broadcaster_->sendTransform(t); + t.header.frame_id = "map"; + t.child_frame_id = "test_frame"; + tf_broadcaster_->sendTransform(t); + + costmap->on_activate(rclcpp_lifecycle::State()); + + auto parameter_client = std::make_shared( + node->shared_from_this(), + "/test_costmap/test_costmap", + rmw_qos_profile_parameters); + auto results1 = parameter_client->set_parameters_atomically( + { + rclcpp::Parameter("robot_radius", 1.234), + rclcpp::Parameter("footprint_padding", 2.345), + rclcpp::Parameter("transform_tolerance", 3.456), + rclcpp::Parameter("publish_frequency", 4.567), + rclcpp::Parameter("resolution", 5.678), + rclcpp::Parameter("origin_x", 6.789), + rclcpp::Parameter("origin_y", 7.891), + rclcpp::Parameter("width", 2), + rclcpp::Parameter("height", 3), + rclcpp::Parameter( + "footprint", + "[[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]"), + rclcpp::Parameter("robot_base_frame", "test_frame"), + }); + + // Try setting robot_base_frame to an invalid frame, should be rejected + auto results2 = parameter_client->set_parameters_atomically( + { + rclcpp::Parameter("robot_base_frame", "wrong_test_frame"), + }); + + rclcpp::spin_some(costmap->get_node_base_interface()); + + EXPECT_EQ(costmap->get_parameter("robot_radius").as_double(), 1.234); + EXPECT_EQ(costmap->get_parameter("footprint_padding").as_double(), 2.345); + EXPECT_EQ(costmap->get_parameter("transform_tolerance").as_double(), 3.456); + EXPECT_EQ(costmap->get_parameter("publish_frequency").as_double(), 4.567); + EXPECT_EQ(costmap->get_parameter("resolution").as_double(), 5.678); + EXPECT_EQ(costmap->get_parameter("origin_x").as_double(), 6.789); + EXPECT_EQ(costmap->get_parameter("origin_y").as_double(), 7.891); + EXPECT_EQ(costmap->get_parameter("width").as_int(), 2); + EXPECT_EQ(costmap->get_parameter("height").as_int(), 3); + EXPECT_EQ( + costmap->get_parameter("footprint").as_string(), + "[[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]"); + EXPECT_EQ(costmap->get_parameter("robot_base_frame").as_string(), "test_frame"); + + costmap->on_deactivate(rclcpp_lifecycle::State()); + costmap->on_cleanup(rclcpp_lifecycle::State()); + costmap->on_shutdown(rclcpp_lifecycle::State()); +} diff --git a/nav2_costmap_2d/test/integration/footprint_tests.cpp b/nav2_costmap_2d/test/integration/footprint_tests.cpp index 8f668e9b5c..0cab6fb818 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 2e3aa19c53..ad867128b1 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -46,6 +46,7 @@ #include "nav2_costmap_2d/observation_buffer.hpp" #include "../testing_helper.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" using geometry_msgs::msg::Point; using nav2_costmap_2d::CellData; @@ -144,6 +145,12 @@ void TestNode::validatePointInflation( continue; } + if (dist == bin->first) { + // Adding to our current bin could cause a reallocation + // Which appears to cause the iterator to get messed up + dist += 0.001; + } + if (cell.x_ > 0) { CellData data(costmap->getIndex(cell.x_ - 1, cell.y_), cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_); @@ -176,7 +183,7 @@ void TestNode::initNode(std::vector parameters) options.parameter_overrides(parameters); node_ = std::make_shared( - "inflation_test_node", "", false, options); + "inflation_test_node", "", options); // Declare non-plugin specific costmap parameters node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); @@ -322,7 +329,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); } @@ -594,3 +601,44 @@ TEST_F(TestNode, testInflation3) ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1u); ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 4u); } + +/** + * Test dynamic parameter setting of inflation layer + */ +TEST_F(TestNode, testDynParamsSet) +{ + auto costmap = std::make_shared("test_costmap"); + + costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link"))); + costmap->on_configure(rclcpp_lifecycle::State()); + + costmap->on_activate(rclcpp_lifecycle::State()); + + auto parameter_client = std::make_shared( + costmap->get_node_base_interface(), costmap->get_node_topics_interface(), + costmap->get_node_graph_interface(), + costmap->get_node_services_interface()); + + auto results = parameter_client->set_parameters_atomically( + { + rclcpp::Parameter("inflation_layer.inflation_radius", 0.0), + rclcpp::Parameter("inflation_layer.cost_scaling_factor", 0.0), + rclcpp::Parameter("inflation_layer.inflate_unknown", true), + rclcpp::Parameter("inflation_layer.inflate_around_unknown", true), + rclcpp::Parameter("inflation_layer.enabled", false) + }); + + rclcpp::spin_until_future_complete( + costmap->get_node_base_interface(), + results); + + EXPECT_EQ(costmap->get_parameter("inflation_layer.inflation_radius").as_double(), 0.0); + EXPECT_EQ(costmap->get_parameter("inflation_layer.cost_scaling_factor").as_double(), 0.0); + EXPECT_EQ(costmap->get_parameter("inflation_layer.inflate_unknown").as_bool(), true); + EXPECT_EQ(costmap->get_parameter("inflation_layer.inflate_around_unknown").as_bool(), true); + EXPECT_EQ(costmap->get_parameter("inflation_layer.enabled").as_bool(), false); + + costmap->on_deactivate(rclcpp_lifecycle::State()); + costmap->on_cleanup(rclcpp_lifecycle::State()); + costmap->on_shutdown(rclcpp_lifecycle::State()); +} diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index a635cd8e6a..707d795a34 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -42,6 +42,7 @@ #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_costmap_2d/observation_buffer.hpp" #include "../testing_helper.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" using std::begin; using std::end; @@ -369,3 +370,187 @@ 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); +} + +/** + * Test dynamic parameter setting of obstacle layer + */ +TEST_F(TestNode, testDynParamsSetObstacle) +{ + auto costmap = std::make_shared("test_costmap"); + + // Add obstacle layer + std::vector plugins_str; + plugins_str.push_back("obstacle_layer"); + costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str)); + costmap->declare_parameter( + "obstacle_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::ObstacleLayer"))); + + costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link"))); + costmap->on_configure(rclcpp_lifecycle::State()); + + costmap->on_activate(rclcpp_lifecycle::State()); + + auto parameter_client = std::make_shared( + costmap->get_node_base_interface(), costmap->get_node_topics_interface(), + costmap->get_node_graph_interface(), + costmap->get_node_services_interface()); + + auto results = parameter_client->set_parameters_atomically( + { + rclcpp::Parameter("obstacle_layer.combination_method", 5), + rclcpp::Parameter("obstacle_layer.max_obstacle_height", 4.0), + rclcpp::Parameter("obstacle_layer.enabled", false), + rclcpp::Parameter("obstacle_layer.footprint_clearing_enabled", false) + }); + + rclcpp::spin_until_future_complete( + costmap->get_node_base_interface(), + results); + + EXPECT_EQ(costmap->get_parameter("obstacle_layer.combination_method").as_int(), 5); + EXPECT_EQ(costmap->get_parameter("obstacle_layer.max_obstacle_height").as_double(), 4.0); + EXPECT_EQ(costmap->get_parameter("obstacle_layer.enabled").as_bool(), false); + EXPECT_EQ(costmap->get_parameter("obstacle_layer.footprint_clearing_enabled").as_bool(), false); + + costmap->on_deactivate(rclcpp_lifecycle::State()); + costmap->on_cleanup(rclcpp_lifecycle::State()); + costmap->on_shutdown(rclcpp_lifecycle::State()); +} + +/** + * Test dynamic parameter setting of voxel layer + */ +TEST_F(TestNode, testDynParamsSetVoxel) +{ + auto costmap = std::make_shared("test_costmap"); + + // Add voxel layer + std::vector plugins_str; + plugins_str.push_back("voxel_layer"); + costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str)); + costmap->declare_parameter( + "voxel_layer.plugin", + rclcpp::ParameterValue(std::string("nav2_costmap_2d::VoxelLayer"))); + + costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link"))); + costmap->on_configure(rclcpp_lifecycle::State()); + + costmap->on_activate(rclcpp_lifecycle::State()); + + auto parameter_client = std::make_shared( + costmap->get_node_base_interface(), costmap->get_node_topics_interface(), + costmap->get_node_graph_interface(), + costmap->get_node_services_interface()); + + auto results = parameter_client->set_parameters_atomically( + { + rclcpp::Parameter("voxel_layer.combination_method", 0), + rclcpp::Parameter("voxel_layer.mark_threshold", 1), + rclcpp::Parameter("voxel_layer.unknown_threshold", 10), + rclcpp::Parameter("voxel_layer.z_resolution", 0.4), + rclcpp::Parameter("voxel_layer.origin_z", 1.0), + rclcpp::Parameter("voxel_layer.z_voxels", 14), + rclcpp::Parameter("voxel_layer.max_obstacle_height", 4.0), + rclcpp::Parameter("voxel_layer.footprint_clearing_enabled", false), + rclcpp::Parameter("voxel_layer.enabled", false), + rclcpp::Parameter("voxel_layer.publish_voxel_map", true) + }); + + rclcpp::spin_until_future_complete( + costmap->get_node_base_interface(), + results); + + EXPECT_EQ(costmap->get_parameter("voxel_layer.combination_method").as_int(), 0); + EXPECT_EQ(costmap->get_parameter("voxel_layer.mark_threshold").as_int(), 1); + EXPECT_EQ(costmap->get_parameter("voxel_layer.unknown_threshold").as_int(), 10); + EXPECT_EQ(costmap->get_parameter("voxel_layer.z_resolution").as_double(), 0.4); + EXPECT_EQ(costmap->get_parameter("voxel_layer.origin_z").as_double(), 1.0); + EXPECT_EQ(costmap->get_parameter("voxel_layer.z_voxels").as_int(), 14); + EXPECT_EQ(costmap->get_parameter("voxel_layer.max_obstacle_height").as_double(), 4.0); + EXPECT_EQ(costmap->get_parameter("voxel_layer.footprint_clearing_enabled").as_bool(), false); + EXPECT_EQ(costmap->get_parameter("voxel_layer.enabled").as_bool(), false); + EXPECT_EQ(costmap->get_parameter("voxel_layer.publish_voxel_map").as_bool(), true); + + costmap->on_deactivate(rclcpp_lifecycle::State()); + costmap->on_cleanup(rclcpp_lifecycle::State()); + costmap->on_shutdown(rclcpp_lifecycle::State()); +} + +/** + * Test dynamic parameter setting of static layer + */ +TEST_F(TestNode, testDynParamsSetStatic) +{ + auto costmap = std::make_shared("test_costmap"); + + costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link"))); + costmap->on_configure(rclcpp_lifecycle::State()); + + costmap->on_activate(rclcpp_lifecycle::State()); + + auto parameter_client = std::make_shared( + costmap->get_node_base_interface(), costmap->get_node_topics_interface(), + costmap->get_node_graph_interface(), + costmap->get_node_services_interface()); + + auto results = parameter_client->set_parameters_atomically( + { + rclcpp::Parameter("static_layer.transform_tolerance", 1.0), + rclcpp::Parameter("static_layer.enabled", false), + rclcpp::Parameter("static_layer.map_subscribe_transient_local", false), + rclcpp::Parameter("static_layer.map_topic", "dynamic_topic"), + rclcpp::Parameter("static_layer.subscribe_to_updates", true) + }); + + rclcpp::spin_until_future_complete( + costmap->get_node_base_interface(), + results); + + EXPECT_EQ(costmap->get_parameter("static_layer.transform_tolerance").as_double(), 1.0); + EXPECT_EQ(costmap->get_parameter("static_layer.enabled").as_bool(), false); + EXPECT_EQ(costmap->get_parameter("static_layer.map_subscribe_transient_local").as_bool(), false); + EXPECT_EQ(costmap->get_parameter("static_layer.map_topic").as_string(), "dynamic_topic"); + EXPECT_EQ(costmap->get_parameter("static_layer.subscribe_to_updates").as_bool(), true); + + costmap->on_deactivate(rclcpp_lifecycle::State()); + costmap->on_cleanup(rclcpp_lifecycle::State()); + costmap->on_shutdown(rclcpp_lifecycle::State()); +} diff --git a/nav2_costmap_2d/test/integration/range_tests.cpp b/nav2_costmap_2d/test/integration/range_tests.cpp index 196406e7e0..1ee5380b7a 100644 --- a/nav2_costmap_2d/test/integration/range_tests.cpp +++ b/nav2_costmap_2d/test/integration/range_tests.cpp @@ -115,6 +115,7 @@ class TestNode : public ::testing::Test : node_(std::make_shared("range_test_node")), tf_(node_->get_clock()) { + tf_.setUsingDedicatedThread(true); // Standard non-plugin specific parameters node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 619613c678..cfafb6eb4e 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" @@ -71,8 +72,9 @@ class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber public: DummyFootprintSubscriber( nav2_util::LifecycleNode::SharedPtr node, - std::string & topic_name) - : FootprintSubscriber(node, topic_name, 10.0) + std::string & topic_name, + tf2_ros::Buffer & tf_) + : FootprintSubscriber(node, topic_name, tf_) {} void setFootprint(geometry_msgs::msg::PolygonStamped::SharedPtr msg) @@ -86,7 +88,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode { public: explicit TestCollisionChecker(std::string name) - : LifecycleNode(name, "", true), + : LifecycleNode(name), global_frame_("map") { // Declare non-plugin specific costmap parameters @@ -104,10 +106,13 @@ class TestCollisionChecker : public nav2_util::LifecycleNode on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); + callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); tf_buffer_ = std::make_shared(get_clock()); auto timer_interface = std::make_shared( - rclcpp_node_->get_node_base_interface(), - rclcpp_node_->get_node_timers_interface()); + get_node_base_interface(), + get_node_timers_interface(), + callback_group_); tf_buffer_->setCreateTimerInterface(timer_interface); tf_listener_ = std::make_shared(*tf_buffer_); tf_broadcaster_ = std::make_shared(shared_from_this()); @@ -121,23 +126,27 @@ class TestCollisionChecker : public nav2_util::LifecycleNode footprint_sub_ = std::make_shared( shared_from_this(), - footprint_topic); + footprint_topic, + *tf_buffer_); collision_checker_ = std::make_unique( - *costmap_sub_, *footprint_sub_, *tf_buffer_, get_name(), "map"); + *costmap_sub_, *footprint_sub_, get_name()); layers_ = new nav2_costmap_2d::LayeredCostmap("map", false, false); // Add Static Layer std::shared_ptr slayer = nullptr; - addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer); + addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer, callback_group_); while (!slayer->isCurrent()) { rclcpp::spin_some(this->get_node_base_interface()); } // Add Inflation Layer std::shared_ptr ilayer = nullptr; - addInflationLayer(*layers_, *tf_buffer_, shared_from_this(), ilayer); + addInflationLayer(*layers_, *tf_buffer_, shared_from_this(), ilayer, callback_group_); + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, get_node_base_interface()); + executor_thread_ = std::make_unique(executor_); return nav2_util::CallbackReturn::SUCCESS; } @@ -162,6 +171,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode delete layers_; layers_ = nullptr; + executor_thread_.reset(); tf_buffer_.reset(); footprint_sub_.reset(); @@ -174,13 +184,14 @@ class TestCollisionChecker : public nav2_util::LifecycleNode bool testPose(double x, double y, double theta) { - publishPose(x, y, theta); + rclcpp::Time stamp = now(); + publishPose(x, y, theta, stamp); geometry_msgs::msg::Pose2D pose; pose.x = x; pose.y = y; pose.theta = theta; - setPose(x, y, theta); + setPose(x, y, theta, stamp); publishFootprint(); publishCostmap(); rclcpp::sleep_for(std::chrono::milliseconds(1000)); @@ -197,23 +208,25 @@ class TestCollisionChecker : public nav2_util::LifecycleNode } protected: - void setPose(double x, double y, double theta) + void setPose(double x, double y, double theta, const rclcpp::Time & stamp) { x_ = x; y_ = y; yaw_ = theta; + stamp_ = stamp; current_pose_.pose.position.x = x_; current_pose_.pose.position.y = y_; current_pose_.pose.position.z = 0; current_pose_.pose.orientation = orientationAroundZAxis(yaw_); + current_pose_.header.stamp = stamp; } void publishFootprint() { geometry_msgs::msg::PolygonStamped oriented_footprint; oriented_footprint.header.frame_id = global_frame_; - oriented_footprint.header.stamp = now(); + oriented_footprint.header.stamp = stamp_; nav2_costmap_2d::transformFootprint(x_, y_, yaw_, footprint_, oriented_footprint); footprint_sub_->setFootprint( std::make_shared(oriented_footprint)); @@ -226,11 +239,11 @@ class TestCollisionChecker : public nav2_util::LifecycleNode std::make_shared(toCostmapMsg(layers_->getCostmap()))); } - void publishPose(double x, double y, double /*theta*/) + void publishPose(double x, double y, double /*theta*/, const rclcpp::Time & stamp) { geometry_msgs::msg::TransformStamped tf_stamped; tf_stamped.header.frame_id = "map"; - tf_stamped.header.stamp = now() + rclcpp::Duration(1.0); + tf_stamped.header.stamp = stamp; tf_stamped.child_frame_id = "base_link"; tf_stamped.transform.translation.x = x; tf_stamped.transform.translation.y = y; @@ -250,7 +263,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode nav2_msgs::msg::Costmap costmap_msg; costmap_msg.header.frame_id = global_frame_; - costmap_msg.header.stamp = now(); + costmap_msg.header.stamp = stamp_; costmap_msg.metadata.layer = "master"; costmap_msg.metadata.resolution = resolution; costmap_msg.metadata.size_x = costmap->getSizeInCellsX(); @@ -272,6 +285,10 @@ class TestCollisionChecker : public nav2_util::LifecycleNode std::shared_ptr tf_listener_; std::shared_ptr tf_broadcaster_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr executor_thread_; + std::shared_ptr costmap_sub_; std::shared_ptr footprint_sub_; std::unique_ptr collision_checker_; @@ -279,6 +296,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode nav2_costmap_2d::LayeredCostmap * layers_{nullptr}; std::string global_frame_; double x_, y_, yaw_; + rclcpp::Time stamp_; geometry_msgs::msg::PoseStamped current_pose_; std::vector footprint_; }; @@ -304,7 +322,7 @@ class TestNode : public ::testing::Test std::shared_ptr collision_checker_; }; -TEST_F(TestNode, uknownSpace) +TEST_F(TestNode, unknownSpace) { collision_checker_->setFootprint(0, 1); diff --git a/nav2_costmap_2d/test/module_tests.cpp b/nav2_costmap_2d/test/module_tests.cpp index 8df74ac724..989d81fe72 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/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt new file mode 100644 index 0000000000..c492459d2f --- /dev/null +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -0,0 +1,4 @@ +ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) +target_link_libraries(costmap_bresenham_2d + nav2_costmap_2d_core +) diff --git a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp new file mode 100644 index 0000000000..a45cb38a7f --- /dev/null +++ b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp @@ -0,0 +1,159 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2022 Samsung Research Russia +* 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 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. +* +* Author: Alexey Merzlyakov +*********************************************************************/ +#include +#include + +class CostmapAction +{ +public: + explicit CostmapAction( + unsigned char * costmap, unsigned int size, unsigned char mark_val = 128) + : costmap_(costmap), size_(size), mark_val_(mark_val) + { + } + + inline void operator()(unsigned int off) + { + ASSERT_TRUE(off < size_); + costmap_[off] = mark_val_; + } + + inline unsigned int get(unsigned int off) + { + return costmap_[off]; + } + +private: + unsigned char * costmap_; + unsigned int size_; + unsigned char mark_val_; +}; + +class CostmapTest : public nav2_costmap_2d::Costmap2D +{ +public: + CostmapTest( + unsigned int size_x, unsigned int size_y, double resolution, + double origin_x, double origin_y, unsigned char default_val = 0) + : nav2_costmap_2d::Costmap2D(size_x, size_y, resolution, origin_x, origin_y, default_val) + { + } + + unsigned char * getCostmap() + { + return costmap_; + } + + unsigned int getSize() + { + return size_x_ * size_y_; + } + + void raytraceLine( + CostmapAction ca, unsigned int x0, unsigned int y0, unsigned int x1, + unsigned int y1, + unsigned int max_length = UINT_MAX, unsigned int min_length = 0) + { + nav2_costmap_2d::Costmap2D::raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); + } +}; + +TEST(costmap_2d, bresenham2DBoundariesCheck) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 6; + CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); + CostmapAction ca(ct.getCostmap(), ct.getSize()); + + // Initial point - some assymetrically standing point in order to cover most corner cases + const unsigned int x0 = 2; + const unsigned int y0 = 4; + // (x1, y1) point will move + unsigned int x1, y1; + + // Running on (x, 0) edge + y1 = 0; + for (x1 = 0; x1 < sz_x; x1++) { + ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (x, sz_y) edge + y1 = sz_y - 1; + for (x1 = 0; x1 < sz_x; x1++) { + ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (0, y) edge + x1 = 0; + for (y1 = 0; y1 < sz_y; y1++) { + ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); + } + + // Running on (sz_x, y) edge + x1 = sz_x - 1; + for (y1 = 0; y1 < sz_y; y1++) { + ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length); + } +} + +TEST(costmap_2d, bresenham2DSamePoint) +{ + const unsigned int sz_x = 60; + const unsigned int sz_y = 60; + const unsigned int max_length = 60; + const unsigned int min_length = 0; + CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); + CostmapAction ca(ct.getCostmap(), ct.getSize()); + + // Initial point + const double x0 = 2; + const double y0 = 4; + + unsigned int offset = y0 * sz_x + x0; + unsigned char val_before = ca.get(offset); + // Same point to check + ct.raytraceLine(ca, x0, y0, x0, y0, max_length, min_length); + unsigned char val_after = ca.get(offset); + ASSERT_FALSE(val_before == val_after); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nav2_costmap_2d/test/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp index 82c7d53ec2..8ab95e6d44 100644 --- a/nav2_costmap_2d/test/testing_helper.hpp +++ b/nav2_costmap_2d/test/testing_helper.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_COSTMAP_2D__TESTING_HELPER_HPP_ -#define NAV2_COSTMAP_2D__TESTING_HELPER_HPP_ +#ifndef TESTING_HELPER_HPP_ +#define TESTING_HELPER_HPP_ #include @@ -30,16 +30,6 @@ const double MAX_Z(1.0); -void setValues(nav2_costmap_2d::Costmap2D & costmap, const unsigned char * map) -{ - int index = 0; - for (unsigned int i = 0; i < costmap.getSizeInCellsY(); i++) { - for (unsigned int j = 0; j < costmap.getSizeInCellsX(); j++) { - costmap.setCost(j, i, map[index]); - } - } -} - char printableCost(unsigned char cost) { switch (cost) { @@ -81,36 +71,43 @@ unsigned int countValues( void addStaticLayer( nav2_costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, - std::shared_ptr & slayer) + std::shared_ptr & slayer, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) { slayer = std::make_shared(); layers.addPlugin(std::shared_ptr(slayer)); - slayer->initialize(&layers, "static", &tf, node, nullptr, nullptr /*TODO*/); + slayer->initialize(&layers, "static", &tf, node, callback_group); } void addObstacleLayer( nav2_costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, - std::shared_ptr & olayer) + std::shared_ptr & olayer, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) { olayer = std::make_shared(); - olayer->initialize(&layers, "obstacles", &tf, node, nullptr, nullptr /*TODO*/); + olayer->initialize(&layers, "obstacles", &tf, node, callback_group); layers.addPlugin(std::shared_ptr(olayer)); } void addRangeLayer( nav2_costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, - std::shared_ptr & rlayer) + std::shared_ptr & rlayer, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) { rlayer = std::make_shared(); - rlayer->initialize(&layers, "range", &tf, node, nullptr, nullptr /*TODO*/); + rlayer->initialize(&layers, "range", &tf, node, callback_group); layers.addPlugin(std::shared_ptr(rlayer)); } 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); @@ -128,21 +125,22 @@ 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); } void addInflationLayer( nav2_costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, - std::shared_ptr & ilayer) + std::shared_ptr & ilayer, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr) { ilayer = std::make_shared(); - ilayer->initialize(&layers, "inflation", &tf, node, nullptr, nullptr /*TODO*/); + ilayer->initialize(&layers, "inflation", &tf, node, callback_group); std::shared_ptr ipointer(ilayer); layers.addPlugin(ipointer); } -#endif // NAV2_COSTMAP_2D__TESTING_HELPER_HPP_ +#endif // TESTING_HELPER_HPP_ diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 78085141b3..7a1ec6a909 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -7,3 +7,30 @@ ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test nav2_costmap_2d_core ) + +ament_add_gtest(costmap_convesion_test costmap_conversion_test.cpp) +target_link_libraries(costmap_convesion_test + nav2_costmap_2d_core +) + +ament_add_gtest(declare_parameter_test declare_parameter_test.cpp) +target_link_libraries(declare_parameter_test + nav2_costmap_2d_core +) + +ament_add_gtest(keepout_filter_test keepout_filter_test.cpp) +target_link_libraries(keepout_filter_test + nav2_costmap_2d_core + filters +) + +ament_add_gtest(speed_filter_test speed_filter_test.cpp) +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 0000000000..ce943e1cc3 --- /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_EQ(dst.getCost(0, 0), 100); + ASSERT_EQ(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/costmap_conversion_test.cpp b/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp new file mode 100644 index 0000000000..f0cf789a29 --- /dev/null +++ b/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp @@ -0,0 +1,122 @@ +// Copyright (c) 2020 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 +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/occ_grid_values.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); +static constexpr double RESOLUTION = 0.05; +static constexpr double ORIGIN_X = 0.1; +static constexpr double ORIGIN_Y = 0.2; + +class TestNode : public ::testing::Test +{ +public: + TestNode() {} + + ~TestNode() + { + occ_grid_.reset(); + costmap_.reset(); + } + +protected: + void createMaps(); + void verifyCostmap(); + +private: + std::shared_ptr occ_grid_; + std::shared_ptr costmap_; +}; + +void TestNode::createMaps() +{ + // Create occ_grid_ map + occ_grid_ = std::make_shared(); + + const unsigned int width = 4; + const unsigned int height = 3; + + occ_grid_->info.resolution = RESOLUTION; + occ_grid_->info.width = width; + occ_grid_->info.height = height; + occ_grid_->info.origin.position.x = ORIGIN_X; + occ_grid_->info.origin.position.y = ORIGIN_Y; + occ_grid_->info.origin.position.z = 0.0; + occ_grid_->info.origin.orientation.x = 0.0; + occ_grid_->info.origin.orientation.y = 0.0; + occ_grid_->info.origin.orientation.z = 0.0; + occ_grid_->info.origin.orientation.w = 1.0; + occ_grid_->data.resize(width * height); + + int8_t data; + for (unsigned int i = 0; i < width * height; i++) { + data = i * 10; + if (data <= nav2_util::OCC_GRID_OCCUPIED) { + occ_grid_->data[i] = data; + } else { + occ_grid_->data[i] = nav2_util::OCC_GRID_UNKNOWN; + } + } + + // Create costmap_ (convert OccupancyGrid -> to Costmap2D) + costmap_ = std::make_shared(*occ_grid_); +} + +void TestNode::verifyCostmap() +{ + // Verify Costmap2D info + EXPECT_NEAR(costmap_->getResolution(), RESOLUTION, EPSILON); + EXPECT_NEAR(costmap_->getOriginX(), ORIGIN_X, EPSILON); + EXPECT_NEAR(costmap_->getOriginY(), ORIGIN_Y, EPSILON); + + // Verify Costmap2D data + unsigned int it; + unsigned char data, data_ref; + for (it = 0; it < (costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY() - 1); it++) { + data = costmap_->getCharMap()[it]; + if (it != costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY() - 1) { + data_ref = std::round( + static_cast(nav2_costmap_2d::LETHAL_OBSTACLE - nav2_costmap_2d::FREE_SPACE) * it / + 10); + } else { + data_ref = nav2_costmap_2d::NO_INFORMATION; + } + EXPECT_EQ(data, data_ref); + } +} + +TEST_F(TestNode, convertOccGridToCostmap) +{ + createMaps(); + verifyCostmap(); +} diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp new file mode 100644 index 0000000000..a8a717c8f9 --- /dev/null +++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp @@ -0,0 +1,75 @@ +// Copyright (c) 2020 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/layer.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class LayerWrapper : public nav2_costmap_2d::Layer +{ + void reset() {} + void updateBounds(double, double, double, double *, double *, double *, double *) {} + void updateCosts(nav2_costmap_2d::Costmap2D &, int, int, int, int) {} + bool isClearable() {return false;} +}; + +TEST(DeclareParameter, useValidParameter) +{ + 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); + + layer.declareParameter("test1", rclcpp::ParameterValue("test_val1")); + try { + std::string val = node->get_parameter("test_layer.test1").as_string(); + EXPECT_EQ(val, "test_val1"); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + FAIL() << "test_layer.test1 parameter is not set"; + } +} + +TEST(DeclareParameter, useInvalidParameter) +{ + 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); + + layer.declareParameter("test2", rclcpp::PARAMETER_STRING); + try { + std::string val = node->get_parameter("test_layer.test2").as_string(); + 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/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index 1be0d1282d..28c20c6696 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -18,6 +18,7 @@ #include "gtest/gtest.h" #include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/footprint.hpp" TEST(collision_footprint, test_basic) { @@ -156,3 +157,96 @@ TEST(collision_footprint, test_point_and_line_cost) auto right_value = collision_checker.footprintCostAtPose(5.2, 5.0, 0.0, footprint); EXPECT_NEAR(right_value, 254.0, 0.001); } + +TEST(collision_footprint, not_enough_points) +{ + geometry_msgs::msg::Point p1; + p1.x = 2.0; + p1.y = 2.0; + + geometry_msgs::msg::Point p2; + p2.x = -2.0; + p2.y = -2.0; + + std::vector footprint = {p1, p2}; + double min_dist = 0.0; + double max_dist = 0.0; + + nav2_costmap_2d::calculateMinAndMaxDistances(footprint, min_dist, max_dist); + EXPECT_EQ(min_dist, std::numeric_limits::max()); + EXPECT_EQ(max_dist, 0.0f); +} + +TEST(collision_footprint, to_point_32) { + geometry_msgs::msg::Point p; + p.x = 123.0; + p.y = 456.0; + p.z = 789.0; + + geometry_msgs::msg::Point32 p32; + p32 = nav2_costmap_2d::toPoint32(p); + EXPECT_NEAR(p.x, p32.x, 1e-5); + EXPECT_NEAR(p.y, p32.y, 1e-5); + EXPECT_NEAR(p.z, p32.z, 1e-5); +} + +TEST(collision_footprint, to_polygon) { + geometry_msgs::msg::Point p1; + p1.x = 1.2; + p1.y = 3.4; + p1.z = 5.1; + + geometry_msgs::msg::Point p2; + p2.x = -5.6; + p2.y = -7.8; + p2.z = -9.1; + std::vector pts = {p1, p2}; + + geometry_msgs::msg::Polygon poly; + poly = nav2_costmap_2d::toPolygon(pts); + + EXPECT_EQ(2u, sizeof(poly.points) / sizeof(poly.points[0])); + EXPECT_NEAR(poly.points[0].x, p1.x, 1e-5); + EXPECT_NEAR(poly.points[0].y, p1.y, 1e-5); + EXPECT_NEAR(poly.points[0].z, p1.z, 1e-5); + EXPECT_NEAR(poly.points[1].x, p2.x, 1e-5); + EXPECT_NEAR(poly.points[1].y, p2.y, 1e-5); + EXPECT_NEAR(poly.points[1].z, p2.z, 1e-5); +} + +TEST(collision_footprint, make_footprint_from_string) { + std::vector footprint; + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]", footprint); + EXPECT_EQ(result, true); + EXPECT_EQ(4u, footprint.size()); + EXPECT_NEAR(footprint[0].x, 1.0, 1e-5); + EXPECT_NEAR(footprint[0].y, 2.2, 1e-5); + EXPECT_NEAR(footprint[1].x, 0.3, 1e-5); + EXPECT_NEAR(footprint[1].y, -4e4, 1e-5); + EXPECT_NEAR(footprint[2].x, -0.3, 1e-5); + EXPECT_NEAR(footprint[2].y, -4e4, 1e-5); + EXPECT_NEAR(footprint[3].x, -1.0, 1e-5); + EXPECT_NEAR(footprint[3].y, 2.2, 1e-5); +} + +TEST(collision_footprint, make_footprint_from_string_parse_error) { + std::vector footprint; + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[bad_string", footprint); + EXPECT_EQ(result, false); +} + +TEST(collision_footprint, make_footprint_from_string_two_points_error) { + std::vector footprint; + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[1, 2.2], [.3, -4e4]", footprint); + EXPECT_EQ(result, false); +} + +TEST(collision_footprint, make_footprint_from_string_not_pairs) { + std::vector footprint; + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2, 5.6]]", footprint); + EXPECT_EQ(result, false); +} diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp new file mode 100644 index 0000000000..304b423f5c --- /dev/null +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -0,0 +1,481 @@ +// Copyright (c) 2020 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 +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" +#include "nav2_util/occ_grid_values.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" + +using namespace std::chrono_literals; + +static const char FILTER_NAME[]{"keepout_filter"}; +static const char INFO_TOPIC[]{"costmap_filter_info"}; +static const char MASK_TOPIC[]{"mask"}; + +class InfoPublisher : public rclcpp::Node +{ +public: + InfoPublisher(double base, double multiplier) + : Node("costmap_filter_info_pub") + { + publisher_ = this->create_publisher( + INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + msg->type = 0; + msg->filter_mask_topic = MASK_TOPIC; + msg->base = static_cast(base); + msg->multiplier = static_cast(multiplier); + + publisher_->publish(std::move(msg)); + } + + ~InfoPublisher() + { + publisher_.reset(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; // InfoPublisher + +class MaskPublisher : public rclcpp::Node +{ +public: + explicit MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) + : Node("mask_pub") + { + publisher_ = this->create_publisher( + MASK_TOPIC, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + publisher_->publish(mask); + } + + ~MaskPublisher() + { + publisher_.reset(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; // MaskPublisher + +struct Point +{ + unsigned int x, y; +}; + +class TestNode : public ::testing::Test +{ +public: + TestNode() {} + + ~TestNode() {} + +protected: + void createMaps(unsigned char master_value, int8_t mask_value, const std::string & mask_frame); + void publishMaps(); + void rePublishInfo(double base, double multiplier); + void rePublishMask(); + void waitSome(const std::chrono::nanoseconds & duration); + void createKeepoutFilter(const std::string & global_frame); + void createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame); + void verifyMasterGrid(unsigned char free_value, unsigned char keepout_value); + void testStandardScenario(unsigned char free_value, unsigned char keepout_value); + void testFramesScenario(unsigned char free_value, unsigned char keepout_value); + void reset(); + + std::shared_ptr keepout_filter_; + std::shared_ptr master_grid_; + + std::vector keepout_points_; + +private: + nav2_util::LifecycleNode::SharedPtr node_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::shared_ptr tf_broadcaster_; + std::unique_ptr transform_; + + std::shared_ptr mask_; + + std::shared_ptr info_publisher_; + std::shared_ptr mask_publisher_; +}; + +void TestNode::createMaps( + unsigned char master_value, int8_t mask_value, const std::string & mask_frame) +{ + // Make map and mask put as follows: + // + // map (10,10) + // *----------------* + // | mask (6,6) | + // | *-----* | + // | |/////| | + // | |/////| | + // | *-----* | + // | (3,3) | + // *----------------* + // (0,0) + + const double resolution = 1.0; + + // Create master_grid_ + unsigned int width = 10; + unsigned int height = 10; + master_grid_ = std::make_shared( + width, height, resolution, 0.0, 0.0, master_value); + + // Create mask_ + width = 3; + height = 3; + mask_ = std::make_shared(); + mask_->info.resolution = resolution; + mask_->header.frame_id = mask_frame; + mask_->info.width = width; + mask_->info.height = height; + mask_->info.origin.position.x = 3.0; + mask_->info.origin.position.y = 3.0; + mask_->info.origin.position.z = 0.0; + mask_->info.origin.orientation.x = 0.0; + mask_->info.origin.orientation.y = 0.0; + mask_->info.origin.orientation.z = 0.0; + mask_->info.origin.orientation.w = 1.0; + mask_->data.resize(width * height, mask_value); +} + +void TestNode::publishMaps() +{ + info_publisher_ = std::make_shared(0.0, 1.0); + mask_publisher_ = std::make_shared(*mask_); +} + +void TestNode::rePublishInfo(double base, double multiplier) +{ + info_publisher_.reset(); + info_publisher_ = std::make_shared(base, multiplier); + // Allow both CostmapFilterInfo and filter mask subscribers + // to receive a new message + waitSome(100ms); +} + +void TestNode::rePublishMask() +{ + mask_publisher_.reset(); + mask_publisher_ = std::make_shared(*mask_); + // Allow filter mask subscriber to receive a new message + waitSome(100ms); +} + +void TestNode::waitSome(const std::chrono::nanoseconds & duration) +{ + rclcpp::Time start_time = node_->now(); + while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) { + rclcpp::spin_some(node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } +} + +void TestNode::createKeepoutFilter(const std::string & global_frame) +{ + node_ = std::make_shared("test_node"); + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); + + nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false); + + node_->declare_parameter( + std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5)); + node_->declare_parameter( + std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC)); + + keepout_filter_ = std::make_shared(); + keepout_filter_->initialize(&layers, std::string(FILTER_NAME), tf_buffer_.get(), node_, nullptr); + keepout_filter_->initializeFilter(INFO_TOPIC); + + // Wait until mask will be received by KeepoutFilter + while (!keepout_filter_->isActive()) { + rclcpp::spin_some(node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } +} + +void TestNode::createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame) +{ + tf_broadcaster_ = std::make_shared(node_); + + transform_ = std::make_unique(); + transform_->header.frame_id = mask_frame; + transform_->child_frame_id = global_frame; + + transform_->header.stamp = node_->now(); + transform_->transform.translation.x = 1.0; + transform_->transform.translation.y = 1.0; + transform_->transform.translation.z = 0.0; + transform_->transform.rotation.x = 0.0; + transform_->transform.rotation.y = 0.0; + transform_->transform.rotation.z = 0.0; + transform_->transform.rotation.w = 1.0; + + tf_broadcaster_->sendTransform(*transform_); + + // Allow tf_buffer_ to be filled by listener + waitSome(100ms); +} + +void TestNode::verifyMasterGrid(unsigned char free_value, unsigned char keepout_value) +{ + unsigned int x, y; + bool is_checked; + + for (y = 0; y < master_grid_->getSizeInCellsY(); y++) { + for (x = 0; x < master_grid_->getSizeInCellsX(); x++) { + is_checked = false; + for (std::vector::iterator it = keepout_points_.begin(); + it != keepout_points_.end(); it++) + { + if (x == it->x && y == it->y) { + EXPECT_EQ(master_grid_->getCost(x, y), keepout_value); + is_checked = true; + break; + } + } + if (!is_checked) { + EXPECT_EQ(master_grid_->getCost(x, y), free_value); + } + } + } +} + +void TestNode::testStandardScenario(unsigned char free_value, unsigned char keepout_value) +{ + geometry_msgs::msg::Pose2D pose; + // Intersection window: added 4 points + keepout_filter_->process(*master_grid_, 2, 2, 5, 5, pose); + keepout_points_.push_back(Point{3, 3}); + keepout_points_.push_back(Point{3, 4}); + keepout_points_.push_back(Point{4, 3}); + keepout_points_.push_back(Point{4, 4}); + verifyMasterGrid(free_value, keepout_value); + // Two windows outside on the horisontal/vertical edge: no new points added + keepout_filter_->process(*master_grid_, 3, 6, 5, 7, pose); + keepout_filter_->process(*master_grid_, 6, 3, 7, 5, pose); + verifyMasterGrid(free_value, keepout_value); + // Corner window: added 1 point + keepout_filter_->process(*master_grid_, 5, 5, 6, 6, pose); + keepout_points_.push_back(Point{5, 5}); + verifyMasterGrid(free_value, keepout_value); + // Outside windows: no new points added + keepout_filter_->process(*master_grid_, 0, 0, 2, 2, pose); + keepout_filter_->process(*master_grid_, 0, 7, 2, 9, pose); + keepout_filter_->process(*master_grid_, 7, 0, 9, 2, pose); + keepout_filter_->process(*master_grid_, 7, 7, 9, 9, pose); + verifyMasterGrid(free_value, keepout_value); +} + +void TestNode::testFramesScenario(unsigned char free_value, unsigned char keepout_value) +{ + geometry_msgs::msg::Pose2D pose; + // Intersection window: added all 9 points because of map->odom frame shift + keepout_filter_->process(*master_grid_, 2, 2, 5, 5, pose); + keepout_points_.push_back(Point{2, 2}); + keepout_points_.push_back(Point{2, 3}); + keepout_points_.push_back(Point{2, 4}); + keepout_points_.push_back(Point{3, 2}); + keepout_points_.push_back(Point{3, 3}); + keepout_points_.push_back(Point{3, 4}); + keepout_points_.push_back(Point{4, 2}); + keepout_points_.push_back(Point{4, 3}); + keepout_points_.push_back(Point{4, 4}); + verifyMasterGrid(free_value, keepout_value); +} + +void TestNode::reset() +{ + mask_.reset(); + master_grid_.reset(); + info_publisher_.reset(); + mask_publisher_.reset(); + keepout_filter_.reset(); + node_.reset(); + tf_listener_.reset(); + tf_broadcaster_.reset(); + tf_buffer_.reset(); + keepout_points_.clear(); +} + +TEST_F(TestNode, testFreeMasterLethalKeepout) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); + publishMaps(); + createKeepoutFilter("map"); + + // Test KeepoutFilter + testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testUnknownMasterNonLethalKeepout) +{ + // Initilize test system + createMaps( + nav2_costmap_2d::NO_INFORMATION, + (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE) / 2, + "map"); + publishMaps(); + createKeepoutFilter("map"); + + // Test KeepoutFilter + testStandardScenario( + nav2_costmap_2d::NO_INFORMATION, + (nav2_costmap_2d::LETHAL_OBSTACLE - nav2_costmap_2d::FREE_SPACE) / 2); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testFreeKeepout) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_FREE, "map"); + publishMaps(); + createKeepoutFilter("map"); + + // Test KeepoutFilter + geometry_msgs::msg::Pose2D pose; + // Check whole area window + keepout_filter_->process(*master_grid_, 0, 0, 10, 10, pose); + // There should be no one point appeared on master_grid_ after process() + verifyMasterGrid(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testUnknownKeepout) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_UNKNOWN, "map"); + publishMaps(); + createKeepoutFilter("map"); + + // Test KeepoutFilter + geometry_msgs::msg::Pose2D pose; + // Check whole area window + keepout_filter_->process(*master_grid_, 0, 0, 10, 10, pose); + // There should be no one point appeared on master_grid_ after process() + verifyMasterGrid(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testInfoRePublish) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); + publishMaps(); + createKeepoutFilter("map"); + + // Re-publish filter info (with incorrect base and multiplier) + // and test that everything is working after + rePublishInfo(0.1, 0.2); + + // Test KeepoutFilter + testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testMaskRePublish) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); + publishMaps(); + createKeepoutFilter("map"); + + // Re-publish filter mask and test that everything is working after + rePublishMask(); + + // Test KeepoutFilter + testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testDifferentFrames) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map"); + publishMaps(); + createKeepoutFilter("odom"); + createTFBroadcaster("map", "odom"); + + // Test KeepoutFilter + testFramesScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.jpg b/nav2_costmap_2d/test/unit/keepout_filter_test.jpg new file mode 100644 index 0000000000..18d21a118e Binary files /dev/null and b/nav2_costmap_2d/test/unit/keepout_filter_test.jpg differ diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp new file mode 100644 index 0000000000..d419cade36 --- /dev/null +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -0,0 +1,789 @@ +// Copyright (c) 2020 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 +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" +#include "nav2_util/occ_grid_values.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" +#include "nav2_msgs/msg/speed_limit.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_costmap_2d/costmap_filters/speed_filter.hpp" + +using namespace std::chrono_literals; + +static const char FILTER_NAME[]{"speed_filter"}; +static const char INFO_TOPIC[]{"costmap_filter_info"}; +static const char MASK_TOPIC[]{"mask"}; +static const char SPEED_LIMIT_TOPIC[]{"speed_limit"}; + +static const double NO_TRANSLATION = 0.0; +static const double TRANSLATION_X = 1.0; +static const double TRANSLATION_Y = 1.0; + +static const uint8_t INCORRECT_TYPE = 200; + +static constexpr double EPSILON = 1e-5; + +class InfoPublisher : public rclcpp::Node +{ +public: + InfoPublisher(uint8_t type, double base, double multiplier) + : Node("costmap_filter_info_pub") + { + publisher_ = this->create_publisher( + INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + msg->type = type; + msg->filter_mask_topic = MASK_TOPIC; + msg->base = static_cast(base); + msg->multiplier = static_cast(multiplier); + + publisher_->publish(std::move(msg)); + } + + ~InfoPublisher() + { + publisher_.reset(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; // InfoPublisher + +class MaskPublisher : public rclcpp::Node +{ +public: + explicit MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) + : Node("mask_pub") + { + publisher_ = this->create_publisher( + MASK_TOPIC, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + publisher_->publish(mask); + } + + ~MaskPublisher() + { + publisher_.reset(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; // MaskPublisher + +class SpeedLimitSubscriber : public rclcpp::Node +{ +public: + explicit SpeedLimitSubscriber(const std::string & speed_limit_topic) + : Node("speed_limit_sub"), speed_limit_updated_(false) + { + subscriber_ = this->create_subscription( + speed_limit_topic, rclcpp::QoS(10), + std::bind(&SpeedLimitSubscriber::speedLimitCallback, this, std::placeholders::_1)); + } + + void speedLimitCallback( + const nav2_msgs::msg::SpeedLimit::SharedPtr msg) + { + msg_ = msg; + speed_limit_updated_ = true; + } + + nav2_msgs::msg::SpeedLimit::SharedPtr getSpeedLimit() + { + return msg_; + } + + inline bool speedLimitUpdated() + { + return speed_limit_updated_; + } + + inline void resetSpeedLimitIndicator() + { + speed_limit_updated_ = false; + } + +private: + rclcpp::Subscription::SharedPtr subscriber_; + nav2_msgs::msg::SpeedLimit::SharedPtr msg_; + bool speed_limit_updated_; +}; // SpeedLimitSubscriber + +class TestMask : public nav_msgs::msg::OccupancyGrid +{ +public: + TestMask( + unsigned int width, unsigned int height, double resolution, + const std::string & mask_frame) + : width_(width), height_(height) + { + // Fill filter mask info + header.frame_id = mask_frame; + info.resolution = resolution; + info.width = width_; + info.height = height_; + info.origin.position.x = 0.0; + info.origin.position.y = 0.0; + info.origin.position.z = 0.0; + info.origin.orientation.x = 0.0; + info.origin.orientation.y = 0.0; + info.origin.orientation.z = 0.0; + info.origin.orientation.w = 1.0; + + // Fill test mask as follows: + // + // mask (10,11) + // *----------------* + // |91|92|...|99|100| + // |... | + // |... | + // |11|12|13|...| 20| + // | 1| 2| 3|...| 10| + // |-1| 0| 0|...| 0| + // *----------------* + // (0,0) + data.resize(width_ * height_, nav2_util::OCC_GRID_UNKNOWN); + + unsigned int mx, my; + data[0] = -1; + for (mx = 1; mx < width_; mx++) { + data[mx] = 0; + } + unsigned int it; + for (my = 1; my < height_; my++) { + for (mx = 0; mx < width_; mx++) { + it = mx + my * width_; + data[it] = makeData(mx, my); + } + } + } + + inline int8_t makeData(unsigned int mx, unsigned int my) + { + return mx + (my - 1) * width_ + 1; + } + +private: + const unsigned int width_; + const unsigned int height_; +}; // TestMask + +class TestNode : public ::testing::Test +{ +public: + TestNode() {} + + ~TestNode() {} + +protected: + void createMaps(const std::string & mask_frame); + void publishMaps(uint8_t type, double base, double multiplier); + void rePublishInfo(uint8_t type, double base, double multiplier); + void rePublishMask(); + bool createSpeedFilter(const std::string & global_frame); + void createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame); + void publishTransform(); + + // Test methods + void testFullMask( + uint8_t type, double base, double multiplier, + double tr_x, double tr_y); + void testSimpleMask( + uint8_t type, double base, double multiplier, + double tr_x, double tr_y); + void testOutOfMask(uint8_t type, double base, double multiplier); + void testIncorrectLimits(uint8_t type, double base, double multiplier); + + void reset(); + + std::shared_ptr speed_filter_; + std::shared_ptr master_grid_; + +private: + void waitSome(const std::chrono::nanoseconds & duration); + void verifySpeedLimit( + uint8_t type, double base, double multiplier, + unsigned int x, unsigned int y, + nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit); + nav2_msgs::msg::SpeedLimit::SharedPtr getSpeedLimit(); + nav2_msgs::msg::SpeedLimit::SharedPtr waitSpeedLimit(); + + const unsigned int width_ = 10; + const unsigned int height_ = 11; + const double resolution_ = 1.0; + + nav2_util::LifecycleNode::SharedPtr node_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::shared_ptr tf_broadcaster_; + std::unique_ptr transform_; + + std::shared_ptr mask_; + + std::shared_ptr info_publisher_; + std::shared_ptr mask_publisher_; + std::shared_ptr speed_limit_subscriber_; +}; + +void TestNode::createMaps(const std::string & mask_frame) +{ + // Make map and mask put as follows: + // master_grid (12,13) + // *----------------* + // | | + // | mask (10,11) | + // | *-------* | + // | |///////| | + // | |///////| | + // | |///////| | + // | *-------* | + // | (0,0) | + // | | + // *----------------* + // (-2,-2) + + // Create master_grid_ + master_grid_ = std::make_shared( + width_ + 4, height_ + 4, resolution_, -2.0, -2.0, nav2_costmap_2d::FREE_SPACE); + + // Create mask_ + mask_ = std::make_shared(width_, height_, resolution_, mask_frame); +} + +void TestNode::publishMaps(uint8_t type, double base, double multiplier) +{ + info_publisher_ = std::make_shared(type, base, multiplier); + mask_publisher_ = std::make_shared(*mask_); +} + +void TestNode::rePublishInfo(uint8_t type, double base, double multiplier) +{ + info_publisher_.reset(); + info_publisher_ = std::make_shared(type, base, multiplier); + // Allow both CostmapFilterInfo and filter mask subscribers + // to receive a new message + waitSome(100ms); +} + +void TestNode::rePublishMask() +{ + mask_publisher_.reset(); + mask_publisher_ = std::make_shared(*mask_); + // Allow filter mask subscriber to receive a new message + waitSome(100ms); +} + +nav2_msgs::msg::SpeedLimit::SharedPtr TestNode::getSpeedLimit() +{ + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(speed_limit_subscriber_); + return speed_limit_subscriber_->getSpeedLimit(); +} + +nav2_msgs::msg::SpeedLimit::SharedPtr TestNode::waitSpeedLimit() +{ + const std::chrono::nanoseconds timeout = 500ms; + + rclcpp::Time start_time = node_->now(); + speed_limit_subscriber_->resetSpeedLimitIndicator(); + while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (speed_limit_subscriber_->speedLimitUpdated()) { + speed_limit_subscriber_->resetSpeedLimitIndicator(); + return speed_limit_subscriber_->getSpeedLimit(); + } + rclcpp::spin_some(speed_limit_subscriber_); + std::this_thread::sleep_for(10ms); + } + return nullptr; +} + +void TestNode::waitSome(const std::chrono::nanoseconds & duration) +{ + rclcpp::Time start_time = node_->now(); + while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) { + rclcpp::spin_some(node_->get_node_base_interface()); + rclcpp::spin_some(speed_limit_subscriber_); + std::this_thread::sleep_for(10ms); + } +} + +bool TestNode::createSpeedFilter(const std::string & global_frame) +{ + node_ = std::make_shared("test_node"); + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); + + nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false); + + node_->declare_parameter( + std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5)); + node_->declare_parameter( + std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC)); + node_->declare_parameter( + std::string(FILTER_NAME) + ".speed_limit_topic", rclcpp::ParameterValue(SPEED_LIMIT_TOPIC)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".speed_limit_topic", SPEED_LIMIT_TOPIC)); + + speed_filter_ = std::make_shared(); + speed_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr); + speed_filter_->initializeFilter(INFO_TOPIC); + + speed_limit_subscriber_ = std::make_shared(SPEED_LIMIT_TOPIC); + + // Wait until mask will be received by SpeedFilter + const std::chrono::nanoseconds timeout = 500ms; + rclcpp::Time start_time = node_->now(); + while (!speed_filter_->isActive()) { + if (node_->now() - start_time > rclcpp::Duration(timeout)) { + return false; + } + rclcpp::spin_some(node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return true; +} + +void TestNode::createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame) +{ + tf_broadcaster_ = std::make_shared(node_); + + transform_ = std::make_unique(); + transform_->header.frame_id = mask_frame; + transform_->child_frame_id = global_frame; + + transform_->header.stamp = node_->now() + rclcpp::Duration(100ms); + transform_->transform.translation.x = TRANSLATION_X; + transform_->transform.translation.y = TRANSLATION_Y; + transform_->transform.translation.z = 0.0; + transform_->transform.rotation.x = 0.0; + transform_->transform.rotation.y = 0.0; + transform_->transform.rotation.z = 0.0; + transform_->transform.rotation.w = 1.0; + + tf_broadcaster_->sendTransform(*transform_); + + // Allow tf_buffer_ to be filled by listener + waitSome(100ms); +} + +void TestNode::publishTransform() +{ + if (tf_broadcaster_) { + transform_->header.stamp = node_->now() + rclcpp::Duration(100ms); + tf_broadcaster_->sendTransform(*transform_); + } +} + +void TestNode::verifySpeedLimit( + uint8_t type, double base, double multiplier, + unsigned int x, unsigned int y, + nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit) +{ + int8_t cost = mask_->makeData(x, y); + // 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 || expected_limit > 100.0) { + expected_limit = nav2_costmap_2d::NO_SPEED_LIMIT; + } + 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"; + } +} + +void TestNode::testFullMask( + uint8_t type, double base, double multiplier, + double tr_x, double tr_y) +{ + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit; + + // data = 0 + pose.x = 1 - tr_x; + pose.y = -tr_y; + publishTransform(); + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = getSpeedLimit(); + ASSERT_TRUE(speed_limit == nullptr); + + // data in range [1..100] + unsigned int x, y; + for (y = 1; y < height_; y++) { + for (x = 0; x < width_; x++) { + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = waitSpeedLimit(); + ASSERT_TRUE(speed_limit != nullptr); + verifySpeedLimit(type, base, multiplier, x, y, speed_limit); + } + } + + // data = 0 + pose.x = 1 - tr_x; + pose.y = -tr_y; + publishTransform(); + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = waitSpeedLimit(); + ASSERT_TRUE(speed_limit != nullptr); + EXPECT_EQ(speed_limit->speed_limit, nav2_costmap_2d::NO_SPEED_LIMIT); + + // data = -1 + pose.x = -tr_x; + pose.y = -tr_y; + publishTransform(); + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = getSpeedLimit(); + ASSERT_TRUE(speed_limit != nullptr); + EXPECT_EQ(speed_limit->speed_limit, nav2_costmap_2d::NO_SPEED_LIMIT); +} + +void TestNode::testSimpleMask( + uint8_t type, double base, double multiplier, + double tr_x, double tr_y) +{ + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit; + + // data = 0 + pose.x = 1 - tr_x; + pose.y = -tr_y; + publishTransform(); + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = getSpeedLimit(); + ASSERT_TRUE(speed_limit == nullptr); + + // data = + unsigned int x = width_ / 2 - 1; + unsigned int y = height_ / 2 - 1; + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = waitSpeedLimit(); + ASSERT_TRUE(speed_limit != nullptr); + verifySpeedLimit(type, base, multiplier, x, y, speed_limit); + + // data = 100 + x = width_ - 1; + y = height_ - 1; + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = waitSpeedLimit(); + ASSERT_TRUE(speed_limit != nullptr); + verifySpeedLimit(type, base, multiplier, x, y, speed_limit); + + // data = 0 + pose.x = 1 - tr_x; + pose.y = -tr_y; + publishTransform(); + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = waitSpeedLimit(); + ASSERT_TRUE(speed_limit != nullptr); + EXPECT_EQ(speed_limit->speed_limit, nav2_costmap_2d::NO_SPEED_LIMIT); + + // data = -1 + pose.x = -tr_x; + pose.y = -tr_y; + publishTransform(); + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = getSpeedLimit(); + ASSERT_TRUE(speed_limit != nullptr); + EXPECT_EQ(speed_limit->speed_limit, nav2_costmap_2d::NO_SPEED_LIMIT); +} + +void TestNode::testOutOfMask(uint8_t type, double base, double multiplier) +{ + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + nav2_msgs::msg::SpeedLimit::SharedPtr old_speed_limit, speed_limit; + + // data = + pose.x = width_ / 2 - 1; + pose.y = height_ / 2 - 1; + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + old_speed_limit = waitSpeedLimit(); + ASSERT_TRUE(old_speed_limit != nullptr); + verifySpeedLimit(type, base, multiplier, pose.x, pose.y, old_speed_limit); + + // Then go to out of mask bounds and ensure that speed limit was not updated + pose.x = -2.0; + pose.y = -2.0; + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = getSpeedLimit(); + ASSERT_TRUE(speed_limit == old_speed_limit); + + pose.x = width_ + 1.0; + pose.y = height_ + 1.0; + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = getSpeedLimit(); + ASSERT_TRUE(speed_limit == old_speed_limit); +} + +void TestNode::testIncorrectLimits(uint8_t type, double base, double multiplier) +{ + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + nav2_msgs::msg::SpeedLimit::SharedPtr speed_limit; + + std::vector> points; + + // 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 limit value: data = 1, value < 0 + points.push_back(std::make_tuple(0, 1)); + // 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 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) { + pose.x = static_cast(std::get<0>(*it)); + pose.y = static_cast(std::get<1>(*it)); + speed_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + speed_limit = waitSpeedLimit(); + ASSERT_TRUE(speed_limit != nullptr); + verifySpeedLimit(type, base, multiplier, pose.x, pose.y, speed_limit); + } +} + +void TestNode::reset() +{ + mask_.reset(); + master_grid_.reset(); + info_publisher_.reset(); + mask_publisher_.reset(); + speed_limit_subscriber_.reset(); + speed_filter_.reset(); + node_.reset(); + tf_listener_.reset(); + tf_broadcaster_.reset(); + tf_buffer_.reset(); +} + +TEST_F(TestNode, testPercentSpeedLimit) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0); + EXPECT_TRUE(createSpeedFilter("map")); + + // Test SpeedFilter + testFullMask(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + speed_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testIncorrectPercentSpeedLimit) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0); + EXPECT_TRUE(createSpeedFilter("map")); + + // Test SpeedFilter + testIncorrectLimits(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0); + + // Clean-up + speed_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testAbsoluteSpeedLimit) +{ + // Initilize test system + createMaps("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(); + reset(); +} + +TEST_F(TestNode, testOutOfBounds) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0); + EXPECT_TRUE(createSpeedFilter("map")); + + // Test SpeedFilter + testOutOfMask(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0); + + // Clean-up + speed_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testInfoRePublish) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5); + EXPECT_TRUE(createSpeedFilter("map")); + + // Re-publish filter info (with incorrect base and multiplier) + // and test that everything is working after + rePublishInfo(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.1, 0.2); + + // Test SpeedFilter + testSimpleMask( + nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.1, 0.2, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + speed_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testMaskRePublish) +{ + // Initilize test system + createMaps("map"); + 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 + rePublishMask(); + + // Test SpeedFilter + testSimpleMask( + nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + speed_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testIncorrectFilterType) +{ + // Initilize test system + createMaps("map"); + publishMaps(INCORRECT_TYPE, 1.23, 4.5); + EXPECT_FALSE(createSpeedFilter("map")); + + // Clean-up + speed_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testDifferentFrame) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0); + EXPECT_TRUE(createSpeedFilter("odom")); + createTFBroadcaster("map", "odom"); + + // Test SpeedFilter + testFullMask(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.0, 1.0, TRANSLATION_X, TRANSLATION_Y); + + // Clean-up + speed_filter_->resetFilter(); + reset(); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_dwb_controller/README.md b/nav2_dwb_controller/README.md index 976cdc646f..b440f29a82 100644 --- a/nav2_dwb_controller/README.md +++ b/nav2_dwb_controller/README.md @@ -1,37 +1,16 @@ -# Local Planner +# DWB Controller -## Local Planner under ROS 1 +The DWB controller is the successor to the base local planner and DWA controllers in ROS 1. It was created in ROS 1 by David Lu!! at Locus Robotics as part of the `robot_navigation` project. It was then ported to ROS 2 for use in Nav2 as its critic-based controller algorithm. -Under ROS 1, the navigation stack provides a `BaseLocalPlanner` interface -used by `MoveBase`. The `BaseLocalPlanner` component is expected to take a path and current position and produce a command velocity. The most commonly used implementation of the local planner uses the DWA algorithm, however, the Trajectory Rollout algorithm is also available in the ROS 1 navigation stack. +DWB improves on DWA in a few major ways: -The navigation repository provides two implementations of the DWA algorithm: `base_local_planner` and `dwa_local_planner`. In addition, there is another DWA based planner in the [Robot Navigation repository](https://github.com/locusrobotics/robot_navigation) called DWB. The `dwa_local_planner` was meant to replace `base_local_planner` and provides a better DWA algorithm, but failed to -provide the Trajectory Rollout algorithm, so was unable to completely replace -`base_local_planner`. +- It implements plugin-based critics to allow users to specify new critic functions to use in the system. They can be dynamically reconfigured, reweighted, and tuned to gain very particular behavior in your robot system. +- It implements plugin-based trajectory generation techniques, so that users can generate trajectories any number of ways and for any number of types of vehicles +- Includes a number of plugin implementations for common use -DWB was meant to replace both the planners in the navigation repository and provides -an implementation of both DWA and Trajectory Rollout. +It is possible to tune DWB to gain both DWA and base local planner behaviors, as well as expansions using new plugins for totally use-case specific behaviors. The current trajectory generator plugins work for omnidirectional and differential drive robots, though an ackermann generator would be trivial to add. The current critic plugins work for both circular and non-circular robots and include many of the cost functions needed to build a path tracking system with various attributes. -![Local Planner Structure](./images/LocalPlanner.svg "Local planner structure under ROS 1") - -## Migrating to ROS 2 - -Rather than continue with 3 overlapping implementations of DWA, the DWB planner -was chosen to use as the default controller plugin. - -The advantages of DWB were: -* it was designed with backwards compatibility with the other controllers -* it was designed with clear extension points and modularity - -## Changes to DWB - -For the most part, the DWB codebase is unchanged, other than what was required for ROS2 and the `nav2_core` interface for Controllers. - -![ROS 1 DWB Structure](./images/DWB_Structure_Simplified.svg "ROS 1 DWB Structure") - -## New local planner interface - -See `nav2_core` `Controller` interface, which defines an interface for the controller plugins. These plugins are loaded into the `nav2_controller_server` to compute control commands from requests to the ROS2 action server. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-dwb-controller.html) for additional parameter descriptions. ## DWB Plugins @@ -52,15 +31,6 @@ loaded at a time. algorithm used in base_local_planner in ROS 1. * **LimitedAccelGenerator** - This is similar to DWA used in ROS 1. -### Goal Checker Plugins - -These plugins check whether we have reached the goal or not. Again, only one can -be loaded at a time. - -* **SimpleGoalChecker** - This checks whether the robot has reached the goal pose -* **StoppedGoalChecker** - This checks whether the robot has reached the goal pose - and come to a stop. - ### Critic Plugins These plugins score the trajectories generated by the trajectory generator. diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 23a2029c47..c4d1f04f28 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.7 + 1.1.0 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt index e3ee96b5b8..76ab9eea84 100644 --- a/nav2_dwb_controller/dwb_core/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_core/CMakeLists.txt @@ -48,9 +48,6 @@ add_library(dwb_core SHARED src/trajectory_utils.cpp ) -# prevent pluginlib from using boost -target_compile_definitions(dwb_core PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - ament_target_dependencies(dwb_core ${dependencies} ) 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 f4a588611a..ba2c87d67b 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 @@ -67,14 +67,25 @@ class DWBLocalPlanner : public nav2_core::Controller DWBLocalPlanner(); void configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) override; + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; virtual ~DWBLocalPlanner() {} + /** + * @brief Activate lifecycle node + */ void activate() override; + + /** + * @brief Deactivate lifecycle node + */ void deactivate() override; + + /** + * @brief Cleanup lifecycle node + */ void cleanup() override; /** @@ -93,11 +104,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. @@ -131,6 +144,20 @@ class DWBLocalPlanner : public nav2_core::Controller const nav_2d_msgs::msg::Twist2D & velocity, std::shared_ptr & results); + /** + * @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 + { + if (traj_generator_) { + traj_generator_->setSpeedLimit(speed_limit, percentage); + } + } + protected: /** * @brief Helper method for two common operations for the operating on the global_plan @@ -189,9 +216,10 @@ class DWBLocalPlanner : public nav2_core::Controller */ virtual void loadCritics(); - void loadBackwardsCompatibleParameters(); + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("DWBLocalPlanner")}; - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; std::shared_ptr tf_; std::shared_ptr costmap_ros_; 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 c8bfe27d4f..bd2b210e03 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp @@ -69,7 +69,9 @@ namespace dwb_core class DWBPublisher { public: - explicit DWBPublisher(nav2_util::LifecycleNode::SharedPtr node, const std::string & plugin_name); + explicit DWBPublisher( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name); nav2_util::CallbackReturn on_configure(); nav2_util::CallbackReturn on_activate(); @@ -122,9 +124,10 @@ 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_; - nav2_util::LifecycleNode::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + rclcpp::Clock::SharedPtr clock_; std::string plugin_name_; }; 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 98594af1f4..17899a2994 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" @@ -93,20 +94,20 @@ class TrajectoryCritic */ void initialize( const nav2_util::LifecycleNode::SharedPtr & nh, - std::string & name, - std::string & ns, + const std::string & name, + const std::string & ns, std::shared_ptr costmap_ros) { + node_ = nh; name_ = name; costmap_ros_ = costmap_ros; - nh_ = nh; dwb_plugin_name_ = ns; - if (!nh_->has_parameter(dwb_plugin_name_ + "." + name_ + ".scale")) { - nh_->declare_parameter( + if (!nh->has_parameter(dwb_plugin_name_ + "." + name_ + ".scale")) { + nh->declare_parameter( dwb_plugin_name_ + "." + name_ + ".scale", rclcpp::ParameterValue(1.0)); } - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_); + nh->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_); onInit(); } virtual void onInit() {} @@ -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() { @@ -181,7 +182,7 @@ class TrajectoryCritic std::string dwb_plugin_name_; std::shared_ptr costmap_ros_; double scale_; - nav2_util::LifecycleNode::SharedPtr nh_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; }; } // namespace dwb_core 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 6ce9d1bda5..d918d61ddd 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 @@ -123,6 +123,15 @@ class TrajectoryGenerator const geometry_msgs::msg::Pose2D & start_pose, const nav_2d_msgs::msg::Twist2D & start_vel, const nav_2d_msgs::msg::Twist2D & cmd_vel) = 0; + + /** + * @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. + */ + 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 fbb064d070..a3f64e07a1 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.7 + 1.1.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 793e9c90f7..0be00388ca 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -46,6 +46,7 @@ #include "nav_2d_utils/conversions.hpp" #include "nav_2d_utils/parameters.hpp" #include "nav_2d_utils/tf_help.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" #include "pluginlib/class_list_macros.hpp" @@ -53,6 +54,7 @@ #include "geometry_msgs/msg/twist_stamped.hpp" using nav2_util::declare_parameter_if_not_declared; +using nav2_util::geometry_utils::euclidean_distance; namespace dwb_core { @@ -64,65 +66,73 @@ DWBLocalPlanner::DWBLocalPlanner() } void DWBLocalPlanner::configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) { - node_ = node; + node_ = parent; + auto node = node_.lock(); + + logger_ = node->get_logger(); + clock_ = node->get_clock(); 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_ + ".prune_plan", + 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)); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".prune_distance", - rclcpp::ParameterValue(1.0)); + node, dwb_plugin_name_ + ".prune_distance", + rclcpp::ParameterValue(2.0)); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".debug_trajectory_details", + node, dwb_plugin_name_ + ".debug_trajectory_details", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".trajectory_generator_name", + node, dwb_plugin_name_ + ".trajectory_generator_name", rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator"))); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".transform_tolerance", + node, dwb_plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".shorten_transformed_plan", + node, dwb_plugin_name_ + ".shorten_transformed_plan", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", + node, dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", rclcpp::ParameterValue(true)); std::string traj_generator_name; double transform_tolerance; - node_->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance); + node->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance); transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance); - RCLCPP_INFO(node_->get_logger(), "Setting transform_tolerance to %f", transform_tolerance); + RCLCPP_INFO(logger_, "Setting transform_tolerance to %f", transform_tolerance); - node_->get_parameter(dwb_plugin_name_ + ".prune_plan", prune_plan_); - node_->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_); - node_->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_); - node_->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name); - node_->get_parameter( + node->get_parameter(dwb_plugin_name_ + ".prune_plan", prune_plan_); + node->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_); + node->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_); + node->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name); + 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_ = std::make_unique(node, dwb_plugin_name_); pub_->on_configure(); traj_generator_ = traj_gen_loader_.createUniqueInstance(traj_generator_name); - traj_generator_->initialize(node_, dwb_plugin_name_); + traj_generator_->initialize(node, dwb_plugin_name_); try { loadCritics(); } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "Couldn't load critics! Caught exception: %s", e.what()); + RCLCPP_ERROR(logger_, "Couldn't load critics! Caught exception: %s", e.what()); throw; } } @@ -168,101 +178,53 @@ DWBLocalPlanner::resolveCriticClassName(std::string base_name) void DWBLocalPlanner::loadCritics() { - node_->get_parameter(dwb_plugin_name_ + ".default_critic_namespaces", default_critic_namespaces_); - if (default_critic_namespaces_.size() == 0) { - default_critic_namespaces_.push_back("dwb_critics"); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->get_parameter(dwb_plugin_name_ + ".default_critic_namespaces", default_critic_namespaces_); + if (default_critic_namespaces_.empty()) { + default_critic_namespaces_.emplace_back("dwb_critics"); } std::vector critic_names; - if (!node_->get_parameter(dwb_plugin_name_ + ".critics", critic_names)) { - loadBackwardsCompatibleParameters(); + if (!node->get_parameter(dwb_plugin_name_ + ".critics", critic_names)) { + throw std::runtime_error("No critics defined for " + dwb_plugin_name_); } - node_->get_parameter(dwb_plugin_name_ + ".critics", critic_names); + node->get_parameter(dwb_plugin_name_ + ".critics", critic_names); for (unsigned int i = 0; i < critic_names.size(); i++) { std::string critic_plugin_name = critic_names[i]; std::string plugin_class; declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + "." + critic_plugin_name + ".class", + node, dwb_plugin_name_ + "." + critic_plugin_name + ".class", rclcpp::ParameterValue(critic_plugin_name)); - node_->get_parameter(dwb_plugin_name_ + "." + critic_plugin_name + ".class", plugin_class); + node->get_parameter(dwb_plugin_name_ + "." + critic_plugin_name + ".class", plugin_class); plugin_class = resolveCriticClassName(plugin_class); TrajectoryCritic::Ptr plugin = critic_loader_.createUniqueInstance(plugin_class); RCLCPP_INFO( - node_->get_logger(), + logger_, "Using critic \"%s\" (%s)", critic_plugin_name.c_str(), plugin_class.c_str()); critics_.push_back(plugin); try { - plugin->initialize(node_, critic_plugin_name, dwb_plugin_name_, costmap_ros_); + plugin->initialize(node, critic_plugin_name, dwb_plugin_name_, costmap_ros_); } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "Couldn't initialize critic plugin!"); + RCLCPP_ERROR(logger_, "Couldn't initialize critic plugin!"); throw; } - RCLCPP_INFO(node_->get_logger(), "Critic plugin initialized"); + RCLCPP_INFO(logger_, "Critic plugin initialized"); } } -void -DWBLocalPlanner::loadBackwardsCompatibleParameters() -{ - std::vector critic_names; - RCLCPP_INFO( - node_->get_logger(), - "DWBLocalPlanner", "No critics configured! Using the default set."); - critic_names.push_back("RotateToGoal"); // discards trajectories that move forward when - // already at goal - critic_names.push_back("Oscillation"); // discards oscillating motions (assisgns cost -1) - critic_names.push_back("ObstacleFootprint"); // discards trajectories that move into obstacles - critic_names.push_back("GoalAlign"); // prefers trajectories that make the - // nose go towards (local) nose goal - critic_names.push_back("PathAlign"); // prefers trajectories that keep the - // robot nose on nose path - critic_names.push_back("PathDist"); // prefers trajectories on global path - critic_names.push_back("GoalDist"); // prefers trajectories that go towards - // (local) goal, based on wave propagation - node_->set_parameters({rclcpp::Parameter(dwb_plugin_name_ + ".critics", critic_names)}); - - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".path_distance_bias"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".goal_distance_bias"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".occdist_scale"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".max_scaling_factor"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".scaling_speed"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".PathAlign.scale"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".GoalAlign.scale"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".PathDist.scale"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".GoalDist.scale"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".ObstacleFootprint.scale"); - declare_parameter_if_not_declared( - node_, - dwb_plugin_name_ + ".ObstacleFootprint.max_scaling_factor"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".ObstacleFootprint.scaling_speed"); - - /* *INDENT-OFF* */ - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".path_distance_bias", - dwb_plugin_name_ + ".PathAlign.scale", 32.0, false); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".goal_distance_bias", - dwb_plugin_name_ + ".GoalAlign.scale", 24.0, false); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".path_distance_bias", - dwb_plugin_name_ + ".PathDist.scale", 32.0); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".goal_distance_bias", - dwb_plugin_name_ + ".GoalDist.scale", 24.0); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".occdist_scale", - dwb_plugin_name_ + ".ObstacleFootprint.scale", 0.01); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".max_scaling_factor", - dwb_plugin_name_ + ".ObstacleFootprint.max_scaling_factor", 0.2); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".scaling_speed", - dwb_plugin_name_ + ".ObstacleFootprint.scaling_speed", 0.25); - /* *INDENT-ON* */ -} - void DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) { auto path2d = nav_2d_utils::pathToPath2D(path); - for (TrajectoryCritic::Ptr critic : critics_) { + for (TrajectoryCritic::Ptr & critic : critics_) { critic->reset(); } @@ -275,7 +237,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()) { @@ -321,7 +284,7 @@ DWBLocalPlanner::computeVelocityCommands( { if (results) { results->header.frame_id = pose.header.frame_id; - results->header.stamp = node_->now(); + results->header.stamp = clock_->now(); } nav_2d_msgs::msg::Path2D transformed_plan; @@ -332,8 +295,8 @@ DWBLocalPlanner::computeVelocityCommands( nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock lock(*(costmap->getMutex())); - for (TrajectoryCritic::Ptr critic : critics_) { - if (critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan) == false) { + for (TrajectoryCritic::Ptr & critic : critics_) { + if (!critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan)) { RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare"); } } @@ -343,11 +306,11 @@ DWBLocalPlanner::computeVelocityCommands( // Return Value nav_2d_msgs::msg::Twist2DStamped cmd_vel; - cmd_vel.header.stamp = node_->now(); + cmd_vel.header.stamp = clock_->now(); cmd_vel.velocity = best.traj.velocity; // debrief stateful scoring functions - for (TrajectoryCritic::Ptr critic : critics_) { + for (TrajectoryCritic::Ptr & critic : critics_) { critic->debrief(cmd_vel.velocity); } @@ -361,7 +324,7 @@ DWBLocalPlanner::computeVelocityCommands( nav_2d_msgs::msg::Twist2D empty_cmd; dwb_msgs::msg::Trajectory2D empty_traj; // debrief stateful scoring functions - for (TrajectoryCritic::Ptr critic : critics_) { + for (TrajectoryCritic::Ptr & critic : critics_) { critic->debrief(empty_cmd); } @@ -450,7 +413,7 @@ DWBLocalPlanner::scoreTrajectory( dwb_msgs::msg::TrajectoryScore score; score.traj = traj; - for (TrajectoryCritic::Ptr critic : critics_) { + for (TrajectoryCritic::Ptr & critic : critics_) { dwb_msgs::msg::CriticScore cs; cs.name = critic->getName(); cs.scale = critic->getScale(); @@ -473,22 +436,11 @@ DWBLocalPlanner::scoreTrajectory( return score; } -double -getSquareDistance( - const geometry_msgs::msg::Pose2D & pose_a, - const geometry_msgs::msg::Pose2D & pose_b) -{ - double x_diff = pose_a.x - pose_b.x; - double y_diff = pose_a.y - pose_b.y; - - return x_diff * x_diff + y_diff * y_diff; -} - nav_2d_msgs::msg::Path2D DWBLocalPlanner::transformGlobalPlan( const nav_2d_msgs::msg::Pose2DStamped & pose) { - if (global_plan_.poses.size() == 0) { + if (global_plan_.poses.empty()) { throw nav2_core::PlannerException("Received plan with zero length"); } @@ -506,46 +458,51 @@ DWBLocalPlanner::transformGlobalPlan( nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * costmap->getResolution() / 2.0; - double sq_dist_threshold = dist_threshold * dist_threshold; + // If prune_plan is enabled (it is by default) then we want to restrict the // plan to distances within that range as well. - double sq_prune_dist = prune_distance_ * prune_distance_; + double prune_dist = prune_distance_; // Set the maximum distance we'll include points before getting to the part // of the path where the robot is located (the start of the plan). Basically, // these are the points the robot has already passed. - double sq_transform_start_threshold; + double transform_start_threshold; if (prune_plan_) { - sq_transform_start_threshold = std::min(sq_dist_threshold, sq_prune_dist); + transform_start_threshold = std::min(dist_threshold, prune_dist); } else { - sq_transform_start_threshold = sq_dist_threshold; + transform_start_threshold = dist_threshold; } // 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; + double transform_end_threshold; if (shorten_transformed_plan_) { - sq_transform_end_threshold = std::min(sq_dist_threshold, sq_prune_dist); + transform_end_threshold = std::min(dist_threshold, prune_dist); } else { - sq_transform_end_threshold = sq_dist_threshold; + transform_end_threshold = dist_threshold; } - // Find the first pose in the plan that's less than sq_transform_start_threshold + // Find the first pose in the global plan that's further than prune distance + // from the robot using integrated distance + auto prune_point = nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_.poses.begin(), global_plan_.poses.end(), prune_dist); + + // Find the first pose in the plan (upto prune_point) that's less than transform_start_threshold // from the robot. auto transformation_begin = std::find_if( - begin(global_plan_.poses), end(global_plan_.poses), + begin(global_plan_.poses), prune_point, [&](const auto & global_plan_pose) { - return getSquareDistance(robot_pose.pose, global_plan_pose) < sq_transform_start_threshold; + return euclidean_distance(robot_pose.pose, global_plan_pose) < transform_start_threshold; }); - // Find the first pose in the end of the plan that's further than sq_transform_end_threshold - // from the robot + // Find the first pose in the end of the plan that's further than transform_end_threshold + // from the robot using integrated distance auto transformation_end = std::find_if( - transformation_begin, end(global_plan_.poses), - [&](const auto & global_plan_pose) { - return getSquareDistance(robot_pose.pose, global_plan_pose) > sq_transform_end_threshold; + transformation_begin, global_plan_.poses.end(), + [&](const auto & pose) { + return euclidean_distance(pose, robot_pose.pose) > transform_end_threshold; }); // Transform the near part of the global plan into the robot's frame of reference. @@ -577,7 +534,7 @@ DWBLocalPlanner::transformGlobalPlan( pub_->publishGlobalPlan(global_plan_); } - if (transformed_plan.poses.size() == 0) { + if (transformed_plan.poses.empty()) { throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); } return transformed_plan; diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp index 7f7ceb62c3..7b860fef65 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" @@ -54,52 +55,61 @@ namespace dwb_core { DWBPublisher::DWBPublisher( - nav2_util::LifecycleNode::SharedPtr node, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) -: node_(node), plugin_name_(plugin_name) +: node_(parent), + plugin_name_(plugin_name) { + auto node = node_.lock(); + clock_ = node->get_clock(); +} + +nav2_util::CallbackReturn +DWBPublisher::on_configure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + declare_parameter_if_not_declared( - node_, plugin_name + ".publish_evaluation", + node, plugin_name_ + ".publish_evaluation", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, plugin_name + ".publish_global_plan", + node, plugin_name_ + ".publish_global_plan", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, plugin_name + ".publish_transformed_plan", + node, plugin_name_ + ".publish_transformed_plan", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, plugin_name + ".publish_local_plan", + node, plugin_name_ + ".publish_local_plan", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, plugin_name + ".publish_trajectories", + node, plugin_name_ + ".publish_trajectories", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, plugin_name + ".publish_cost_grid_pc", + node, plugin_name_ + ".publish_cost_grid_pc", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( - node_, plugin_name + ".marker_lifetime", + node, plugin_name_ + ".marker_lifetime", rclcpp::ParameterValue(0.1)); -} -nav2_util::CallbackReturn -DWBPublisher::on_configure() -{ - node_->get_parameter(plugin_name_ + ".publish_evaluation", publish_evaluation_); - node_->get_parameter(plugin_name_ + ".publish_global_plan", publish_global_plan_); - node_->get_parameter(plugin_name_ + ".publish_transformed_plan", publish_transformed_); - node_->get_parameter(plugin_name_ + ".publish_local_plan", publish_local_plan_); - node_->get_parameter(plugin_name_ + ".publish_trajectories", publish_trajectories_); - node_->get_parameter(plugin_name_ + ".publish_cost_grid_pc", publish_cost_grid_pc_); - - eval_pub_ = node_->create_publisher("evaluation", 1); - global_pub_ = node_->create_publisher("received_global_plan", 1); - 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); + node->get_parameter(plugin_name_ + ".publish_evaluation", publish_evaluation_); + node->get_parameter(plugin_name_ + ".publish_global_plan", publish_global_plan_); + node->get_parameter(plugin_name_ + ".publish_transformed_plan", publish_transformed_); + node->get_parameter(plugin_name_ + ".publish_local_plan", publish_local_plan_); + node->get_parameter(plugin_name_ + ".publish_trajectories", publish_trajectories_); + node->get_parameter(plugin_name_ + ".publish_cost_grid_pc", publish_cost_grid_pc_); + + eval_pub_ = node->create_publisher("evaluation", 1); + global_pub_ = node->create_publisher("received_global_plan", 1); + 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); double marker_lifetime = 0.0; - node_->get_parameter(plugin_name_ + ".marker_lifetime", marker_lifetime); + node->get_parameter(plugin_name_ + ".marker_lifetime", marker_lifetime); marker_lifetime_ = rclcpp::Duration::from_seconds(marker_lifetime); return nav2_util::CallbackReturn::SUCCESS; @@ -147,22 +157,19 @@ DWBPublisher::on_cleanup() void DWBPublisher::publishEvaluation(std::shared_ptr results) { - if (node_->count_subscribers(eval_pub_->get_topic_name()) < 1) {return;} - - if (results == nullptr) {return;} - - if (publish_evaluation_) { - auto msg = std::make_unique(*results); - eval_pub_->publish(std::move(msg)); + if (results) { + if (publish_evaluation_ && eval_pub_->get_subscription_count() > 0) { + auto msg = std::make_unique(*results); + eval_pub_->publish(std::move(msg)); + } + publishTrajectories(*results); } - - publishTrajectories(*results); } void DWBPublisher::publishTrajectories(const dwb_msgs::msg::LocalPlanEvaluation & results) { - if (node_->count_subscribers(marker_pub_->get_topic_name()) < 1) {return;} + if (marker_pub_->get_subscription_count() < 1) {return;} if (!publish_trajectories_) {return;} auto ma = std::make_unique(); @@ -235,7 +242,8 @@ DWBPublisher::publishLocalPlan( nav_2d_utils::poses2DToPath( traj.poses, header.frame_id, header.stamp)); - if (node_->count_subscribers(local_pub_->get_topic_name()) > 0) { + + if (local_pub_->get_subscription_count() > 0) { local_pub_->publish(std::move(path)); } } @@ -245,50 +253,87 @@ DWBPublisher::publishCostGrid( const std::shared_ptr costmap_ros, const std::vector critics) { - if (node_->count_subscribers(cost_grid_pc_pub_->get_topic_name()) < 1) {return;} + if (cost_grid_pc_pub_->get_subscription_count() < 1) {return;} 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 = node_->now(); + cost_grid_pc->header.stamp = clock_->now(); nav2_costmap_2d::Costmap2D * costmap = costmap_ros->getCostmap(); 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)); } @@ -315,7 +360,7 @@ DWBPublisher::publishGenericPlan( const nav_2d_msgs::msg::Path2D plan, rclcpp::Publisher & pub, bool flag) { - if (node_->count_subscribers(pub.get_topic_name()) < 1) {return;} + if (pub.get_subscription_count() < 1) {return;} if (!flag) {return;} auto path = std::make_unique(nav_2d_utils::pathToPath(plan)); pub.publish(std::move(path)); diff --git a/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp b/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp index 23dad70fdf..a7764d4b7a 100644 --- a/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp +++ b/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp @@ -33,10 +33,13 @@ */ #include -#include -#include + #include +#include "rclcpp/duration.hpp" + +#include "dwb_core/exceptions.hpp" + namespace dwb_core { const geometry_msgs::msg::Pose2D & getClosestPose( diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt index beff593c4d..abe8f8e408 100644 --- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt @@ -36,9 +36,6 @@ add_library(${PROJECT_NAME} SHARED src/twirling.cpp ) -# prevent pluginlib from using boost -target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - set(dependencies angles nav2_costmap_2d @@ -74,6 +71,9 @@ if(BUILD_TESTING) # the following line skips the linter which checks for copyrights set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + + add_subdirectory(test) endif() ament_export_include_directories(include) 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 5f33445995..9bd4675ccb 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 4d77e5019c..611aad4644 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/include/dwb_critics/oscillation.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp index 538ddc7d29..bb2280801e 100644 --- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp @@ -37,8 +37,11 @@ #include #include +#include #include "dwb_core/trajectory_critic.hpp" +using namespace std::chrono_literals; // NOLINT + namespace dwb_critics { @@ -80,7 +83,7 @@ class OscillationCritic : public dwb_core::TrajectoryCritic { public: OscillationCritic() - : oscillation_reset_time_(0) {} + : oscillation_reset_time_(0s) {} void onInit() override; bool prepare( const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel, @@ -152,6 +155,7 @@ class OscillationCritic : public dwb_core::TrajectoryCritic geometry_msgs::msg::Pose2D pose_, prev_stationary_pose_; // Saved timestamp rclcpp::Time prev_reset_time_; + rclcpp::Clock::SharedPtr clock_; }; } // namespace dwb_critics diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 6b121ecd50..a213c6e200 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.7 + 1.1.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 b3870343cd..2dfa3d415a 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" @@ -47,10 +50,15 @@ void BaseObstacleCritic::onInit() { costmap_ = costmap_ros_->getCostmap(); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + nav2_util::declare_parameter_if_not_declared( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".sum_scores", rclcpp::ParameterValue(false)); - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".sum_scores", sum_scores_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".sum_scores", sum_scores_); } double BaseObstacleCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) @@ -87,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/goal_align.cpp b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp index d234d27762..6ff25bfa93 100644 --- a/nav2_dwb_controller/dwb_critics/src/goal_align.cpp +++ b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp @@ -46,8 +46,14 @@ void GoalAlignCritic::onInit() { GoalDistCritic::onInit(); stop_on_failure_ = false; + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + forward_point_distance_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325); } diff --git a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp index bcfc5c44e6..030e77cd63 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" @@ -61,13 +63,18 @@ void MapGridCritic::onInit() // Always set to true, but can be overriden by subclasses stop_on_failure_ = true; + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + nav2_util::declare_parameter_if_not_declared( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".aggregation_type", rclcpp::ParameterValue(std::string("last"))); std::string aggro_str; - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".aggregation_type", aggro_str); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".aggregation_type", aggro_str); std::transform(aggro_str.begin(), aggro_str.end(), aggro_str.begin(), ::tolower); if (aggro_str == "last") { aggregationType_ = ScoreAggregationType::Last; @@ -159,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/src/oscillation.cpp b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp index e7ff9c6fb8..450f5b9751 100644 --- a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp +++ b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp @@ -91,20 +91,27 @@ bool OscillationCritic::CommandTrend::hasSignFlipped() void OscillationCritic::onInit() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + clock_ = node->get_clock(); + oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".oscillation_reset_dist", 0.05); oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_; oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".oscillation_reset_angle", 0.2); oscillation_reset_time_ = rclcpp::Duration::from_seconds( nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".oscillation_reset_time", -1.0)); nav2_util::declare_parameter_if_not_declared( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".x_only_threshold", rclcpp::ParameterValue(0.05)); /** @@ -114,23 +121,23 @@ void OscillationCritic::onInit() * If min_trans_vel is set in the namespace, as it used to be used for trajectory generation, complain then use that. * Otherwise, set x_only_threshold_ to 0.05 */ - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".x_only_threshold", x_only_threshold_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".x_only_threshold", x_only_threshold_); // TODO(crdelsey): How to handle searchParam? // std::string resolved_name; - // if (nh_->hasParam("x_only_threshold")) + // if (node->hasParam("x_only_threshold")) // { - // nh_->param("x_only_threshold", x_only_threshold_); + // node->param("x_only_threshold", x_only_threshold_); // } - // else if (nh_->searchParam("min_speed_xy", resolved_name)) + // else if (node->searchParam("min_speed_xy", resolved_name)) // { - // nh_->param(resolved_name, x_only_threshold_); + // node->param(resolved_name, x_only_threshold_); // } - // else if (nh_->searchParam("min_trans_vel", resolved_name)) + // else if (node->searchParam("min_trans_vel", resolved_name)) // { // ROS_WARN_NAMED("OscillationCritic", // "Parameter min_trans_vel is deprecated. " // "Please use the name min_speed_xy or x_only_threshold instead."); - // nh_->param(resolved_name, x_only_threshold_); + // node->param(resolved_name, x_only_threshold_); // } // else // { @@ -154,7 +161,7 @@ void OscillationCritic::debrief(const nav_2d_msgs::msg::Twist2D & cmd_vel) { if (setOscillationFlags(cmd_vel)) { prev_stationary_pose_ = pose_; - prev_reset_time_ = nh_->now(); + prev_reset_time_ = clock_->now(); } // if we've got restrictions... check if we can reset any oscillation flags @@ -183,7 +190,7 @@ bool OscillationCritic::resetAvailable() } } if (oscillation_reset_time_ >= rclcpp::Duration::from_seconds(0.0)) { - auto t_diff = (nh_->now() - prev_reset_time_); + auto t_diff = (clock_->now() - prev_reset_time_); if (t_diff > oscillation_reset_time_) { return true; } diff --git a/nav2_dwb_controller/dwb_critics/src/path_align.cpp b/nav2_dwb_controller/dwb_critics/src/path_align.cpp index 952d4561c0..23c9dfb235 100644 --- a/nav2_dwb_controller/dwb_critics/src/path_align.cpp +++ b/nav2_dwb_controller/dwb_critics/src/path_align.cpp @@ -46,8 +46,14 @@ void PathAlignCritic::onInit() { PathDistCritic::onInit(); stop_on_failure_ = false; + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + forward_point_distance_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325); } diff --git a/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp b/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp index 584e339c8e..53fdee4fd6 100644 --- a/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp +++ b/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp @@ -46,23 +46,28 @@ namespace dwb_critics void PreferForwardCritic::onInit() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + declare_parameter_if_not_declared( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".penalty", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".strafe_x", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( - nh_, dwb_plugin_name_ + "." + name_ + ".strafe_theta", + node, dwb_plugin_name_ + "." + name_ + ".strafe_theta", rclcpp::ParameterValue(0.2)); declare_parameter_if_not_declared( - nh_, dwb_plugin_name_ + "." + name_ + ".theta_scale", + node, dwb_plugin_name_ + "." + name_ + ".theta_scale", rclcpp::ParameterValue(10.0)); - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".penalty", penalty_); - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_x", strafe_x_); - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_theta", strafe_theta_); - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".theta_scale", theta_scale_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".penalty", penalty_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_x", strafe_x_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_theta", strafe_theta_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".theta_scale", theta_scale_); } double PreferForwardCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) diff --git a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp index 71683ebc8d..7a46a0340e 100644 --- a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp +++ b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp @@ -53,19 +53,24 @@ inline double hypot_sq(double dx, double dy) void RotateToGoalCritic::onInit() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + ".xy_goal_tolerance", 0.25); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; double stopped_xy_velocity = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + ".trans_stopped_velocity", 0.25); stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity; slowing_factor_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".slowing_factor", 5.0); lookahead_time_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".lookahead_time", -1.0); reset(); } diff --git a/nav2_dwb_controller/dwb_critics/src/twirling.cpp b/nav2_dwb_controller/dwb_critics/src/twirling.cpp index 3615e6f036..7973cc0979 100644 --- a/nav2_dwb_controller/dwb_critics/src/twirling.cpp +++ b/nav2_dwb_controller/dwb_critics/src/twirling.cpp @@ -39,8 +39,12 @@ namespace dwb_critics { void TwirlingCritic::onInit() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } // Scale is set to 0 by default, so if it was not set otherwise, set to 0 - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_); } double TwirlingCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) diff --git a/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt new file mode 100644 index 0000000000..07672ca698 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt @@ -0,0 +1,14 @@ +ament_add_gtest(prefer_forward_tests prefer_forward_test.cpp) +target_link_libraries(prefer_forward_tests dwb_critics) + +ament_add_gtest(base_obstacle_tests base_obstacle_test.cpp) +target_link_libraries(base_obstacle_tests dwb_critics) + +ament_add_gtest(obstacle_footprint_tests obstacle_footprint_test.cpp) +target_link_libraries(obstacle_footprint_tests dwb_critics) + +ament_add_gtest(alignment_util_tests alignment_util_test.cpp) +target_link_libraries(alignment_util_tests dwb_critics) + +ament_add_gtest(twirling_tests twirling_test.cpp) +target_link_libraries(twirling_tests dwb_critics) diff --git a/nav2_dwb_controller/dwb_critics/test/alignment_util_test.cpp b/nav2_dwb_controller/dwb_critics/test/alignment_util_test.cpp new file mode 100644 index 0000000000..3f96647471 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/test/alignment_util_test.cpp @@ -0,0 +1,77 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Samsung Research America + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "dwb_critics/alignment_util.hpp" +#include "dwb_core/exceptions.hpp" + +TEST(AlignmentUtil, TestProjection) +{ + geometry_msgs::msg::Pose2D pose, pose_out; + pose.x = 1.0; + pose.y = -1.0; + double distance = 1.0; + pose_out = dwb_critics::getForwardPose(pose, distance); + EXPECT_EQ(pose_out.x, 2.0); + EXPECT_EQ(pose_out.y, -1.0); + EXPECT_EQ(pose_out.theta, pose.theta); + + pose.x = 2.0; + pose.y = -10.0; + pose.theta = 0.54; + pose_out = dwb_critics::getForwardPose(pose, distance); + EXPECT_NEAR(pose_out.x, 2.8577, 0.01); + EXPECT_NEAR(pose_out.y, -9.4858, 0.01); + EXPECT_EQ(pose_out.theta, pose.theta); +} + +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_dwb_controller/dwb_critics/test/base_obstacle_test.cpp b/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp new file mode 100644 index 0000000000..15f5dd70a6 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp @@ -0,0 +1,173 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Wilco Bonestroo + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "dwb_critics/obstacle_footprint.hpp" +#include "dwb_core/exceptions.hpp" + +TEST(BaseObstacle, IsValidCost) +{ + std::shared_ptr critic = + std::make_shared(); + + for (int i = 0; i < 256; i++) { + // for these 3 values the cost is not "valid" + if (i == nav2_costmap_2d::LETHAL_OBSTACLE || + i == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE || + i == nav2_costmap_2d::NO_INFORMATION) + { + ASSERT_FALSE(critic->isValidCost(i)); + } else { + ASSERT_TRUE(critic->isValidCost(i)); + } + } +} + +TEST(BaseObstacle, ScorePose) +{ + std::shared_ptr critic = + std::make_shared(); + + auto node = nav2_util::LifecycleNode::make_shared("base_obstacle_critic_tester"); + + auto costmap_ros = std::make_shared("test_global_costmap"); + costmap_ros->configure(); + + std::string name = "name"; + std::string ns = "ns"; + + critic->initialize(node, name, ns, costmap_ros); + + costmap_ros->getCostmap()->setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE); + costmap_ros->getCostmap()->setCost(0, 1, nav2_costmap_2d::NO_INFORMATION); + const int some_other_cost = 128; + costmap_ros->getCostmap()->setCost(0, 2, some_other_cost); + + // The pose is in "world" coordinates. The (default) resolution is 0.1 m. + geometry_msgs::msg::Pose2D pose; + pose.x = 0; + pose.y = 0; + + ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException); + + pose.x = 0; + pose.y = 0.15; + ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException); + + pose.y = 0.25; + pose.x = 0.05; + ASSERT_EQ(critic->scorePose(pose), some_other_cost); + + // The theta should not influence the cost + for (int i = -50; i < 150; i++) { + pose.theta = (1.0 / 50) * i * M_PI; + ASSERT_EQ(critic->scorePose(pose), some_other_cost); + } + + // Poses outside the map should throw an exception. + pose.x = 1.0; + pose.y = -0.1; + ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException); + + pose.x = costmap_ros->getCostmap()->getSizeInMetersX() + 0.1; + pose.y = 1.0; + ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException); + + pose.x = 1.0; + pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + 0.1; + ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException); + + pose.x = -0.1; + pose.y = 1.0; + ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException); +} + +TEST(BaseObstacle, CriticVisualization) +{ + std::shared_ptr critic = + std::make_shared(); + + auto node = nav2_util::LifecycleNode::make_shared("base_obstacle_critic_tester"); + + auto costmap_ros = std::make_shared("test_global_costmap"); + costmap_ros->configure(); + + std::string name = "name"; + std::string ns = "ns"; + + critic->initialize(node, name, ns, costmap_ros); + + costmap_ros->getCostmap()->setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE); + costmap_ros->getCostmap()->setCost(0, 1, nav2_costmap_2d::NO_INFORMATION); + // Some random values + costmap_ros->getCostmap()->setCost(3, 2, 64); + costmap_ros->getCostmap()->setCost(30, 12, 85); + costmap_ros->getCostmap()->setCost(10, 49, 24); + costmap_ros->getCostmap()->setCost(45, 2, 12); + + std::vector>> cost_channels; + critic->addCriticVisualization(cost_channels); + + unsigned int size_x = costmap_ros->getCostmap()->getSizeInCellsX(); + unsigned int size_y = costmap_ros->getCostmap()->getSizeInCellsY(); + + // 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 = cost_channels[0].second[y * size_y + x]; + ASSERT_EQ(static_cast(pointValue), costmap_ros->getCostmap()->getCost(x, y)); + } + } +} + +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_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp new file mode 100644 index 0000000000..30d8f47c6a --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp @@ -0,0 +1,264 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Wilco Bonestroo + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "dwb_critics/obstacle_footprint.hpp" +#include "dwb_core/exceptions.hpp" + +class OpenObstacleFootprintCritic : public dwb_critics::ObstacleFootprintCritic +{ +public: + double pointCost(int x, int y) + { + return dwb_critics::ObstacleFootprintCritic::pointCost(x, y); + } + + double lineCost(int x0, int x1, int y0, int y1) + { + return dwb_critics::ObstacleFootprintCritic::lineCost(x0, x1, y0, y1); + } +}; + +// Rotate the given point for angle radians around the origin. +geometry_msgs::msg::Point rotate_origin(geometry_msgs::msg::Point p, double angle) +{ + double s = sin(angle); + double c = cos(angle); + + // rotate point + double xnew = p.x * c - p.y * s; + double ynew = p.x * s + p.y * c; + + p.x = xnew; + p.y = ynew; + + return p; +} + +// Auxilary function to create a Point with given x and y values. +geometry_msgs::msg::Point getPoint(double x, double y) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + return p; +} + +// Variables +double footprint_size_x_half = 1.8; +double footprint_size_y_half = 1.6; + +std::vector getFootprint() +{ + std::vector footprint; + footprint.push_back(getPoint(footprint_size_x_half, footprint_size_y_half)); + footprint.push_back(getPoint(footprint_size_x_half, -footprint_size_y_half)); + footprint.push_back(getPoint(-footprint_size_x_half, -footprint_size_y_half)); + footprint.push_back(getPoint(-footprint_size_x_half, footprint_size_y_half)); + return footprint; +} + +TEST(ObstacleFootprint, GetOrientedFootprint) +{ + double theta = 0.1234; + + std::vector footprint_before = getFootprint(); + + std::vector footprint_after; + geometry_msgs::msg::Pose2D pose; + pose.theta = theta; + footprint_after = dwb_critics::getOrientedFootprint(pose, footprint_before); + + uint i; + for (i = 0; i < footprint_before.size(); i++) { + ASSERT_EQ(rotate_origin(footprint_before[i], theta), footprint_after[i]); + } + + theta = 5.123; + pose.theta = theta; + footprint_after = dwb_critics::getOrientedFootprint(pose, footprint_before); + + for (unsigned int i = 0; i < footprint_before.size(); i++) { + ASSERT_EQ(rotate_origin(footprint_before[i], theta), footprint_after[i]); + } +} + +TEST(ObstacleFootprint, Prepare) +{ + std::shared_ptr critic = + std::make_shared(); + + auto node = nav2_util::LifecycleNode::make_shared("costmap_tester"); + + auto costmap_ros = std::make_shared("test_global_costmap"); + costmap_ros->configure(); + + std::string name = "name"; + std::string ns = "ns"; + critic->initialize(node, name, ns, costmap_ros); + + geometry_msgs::msg::Pose2D pose; + nav_2d_msgs::msg::Twist2D vel; + geometry_msgs::msg::Pose2D goal; + nav_2d_msgs::msg::Path2D global_plan; + + // no footprint set in the costmap. Prepare should return false; + std::vector footprint; + costmap_ros->setRobotFootprint(footprint); + ASSERT_FALSE(critic->prepare(pose, vel, goal, global_plan)); + + costmap_ros->setRobotFootprint(getFootprint()); + ASSERT_TRUE(critic->prepare(pose, vel, goal, global_plan)); + + double epsilon = 0.01; + // If the robot footprint goes of the map, it should throw an exception + // The following cases put the robot over the edge of the map on the left, bottom, right and top + + pose.x = footprint_size_x_half; // This gives an error + pose.y = footprint_size_y_half + epsilon; + ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException); + + pose.x = footprint_size_x_half + epsilon; + pose.y = footprint_size_y_half; // error + ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException); + + pose.x = costmap_ros->getCostmap()->getSizeInMetersX() - footprint_size_x_half; // error + pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + footprint_size_y_half - epsilon; + ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException); + + pose.x = costmap_ros->getCostmap()->getSizeInMetersX() - footprint_size_x_half - epsilon; + pose.y = costmap_ros->getCostmap()->getSizeInMetersY() + footprint_size_y_half; // error + ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException); + + pose.x = footprint_size_x_half + epsilon; + pose.y = footprint_size_y_half + epsilon; + ASSERT_EQ(critic->scorePose(pose), 0.0); + + for (unsigned int i = 1; i < costmap_ros->getCostmap()->getSizeInCellsX(); i++) { + costmap_ros->getCostmap()->setCost(i, 10, nav2_costmap_2d::LETHAL_OBSTACLE); + } + // It should now hit an obstacle (throw an expection) + ASSERT_THROW(critic->scorePose(pose), dwb_core::IllegalTrajectoryException); +} + +// todo: wilcobonestroo Add tests for other footprint shapes and costmaps. + +TEST(ObstacleFootprint, PointCost) +{ + std::shared_ptr critic = + std::make_shared(); + + auto node = nav2_util::LifecycleNode::make_shared("costmap_tester"); + + auto costmap_ros = std::make_shared("test_global_costmap"); + costmap_ros->configure(); + + std::string name = "name"; + std::string ns = "ns"; + critic->initialize(node, name, ns, costmap_ros); + + costmap_ros->getCostmap()->setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE); + costmap_ros->getCostmap()->setCost(0, 1, nav2_costmap_2d::NO_INFORMATION); + costmap_ros->getCostmap()->setCost(0, 2, 128); + + ASSERT_THROW(critic->pointCost(0, 0), dwb_core::IllegalTrajectoryException); + ASSERT_THROW(critic->pointCost(0, 1), dwb_core::IllegalTrajectoryException); + ASSERT_EQ(critic->pointCost(0, 2), 128); +} + +TEST(ObstacleFootprint, LineCost) +{ + std::shared_ptr critic = + std::make_shared(); + + auto node = nav2_util::LifecycleNode::make_shared("costmap_tester"); + + auto costmap_ros = std::make_shared("test_global_costmap"); + costmap_ros->configure(); + + std::string name = "name"; + std::string ns = "ns"; + critic->initialize(node, name, ns, costmap_ros); + + costmap_ros->getCostmap()->setCost(3, 3, nav2_costmap_2d::LETHAL_OBSTACLE); + costmap_ros->getCostmap()->setCost(3, 4, nav2_costmap_2d::LETHAL_OBSTACLE); + costmap_ros->getCostmap()->setCost(4, 3, nav2_costmap_2d::LETHAL_OBSTACLE); + costmap_ros->getCostmap()->setCost(4, 4, nav2_costmap_2d::LETHAL_OBSTACLE); + + ASSERT_THROW(critic->lineCost(0, 5, 2, 6), dwb_core::IllegalTrajectoryException); + ASSERT_THROW(critic->lineCost(5, 0, 6, 2), dwb_core::IllegalTrajectoryException); + + ASSERT_THROW(critic->lineCost(2, 4, 0, 10), dwb_core::IllegalTrajectoryException); + ASSERT_THROW(critic->lineCost(4, 2, 10, 0), dwb_core::IllegalTrajectoryException); + + // These all miss the obstacle + ASSERT_EQ(critic->lineCost(2, 2, 0, 10), 0.0); + ASSERT_EQ(critic->lineCost(2, 2, 10, 0), 0.0); + ASSERT_EQ(critic->lineCost(5, 5, 0, 10), 0.0); + ASSERT_EQ(critic->lineCost(5, 5, 10, 0), 0.0); + ASSERT_EQ(critic->lineCost(0, 50, 2, 2), 0.0); + ASSERT_EQ(critic->lineCost(50, 0, 2, 2), 0.0); + ASSERT_EQ(critic->lineCost(0, 50, 5, 5), 0.0); + ASSERT_EQ(critic->lineCost(50, 0, 5, 5), 0.0); + + // Use valid costs + costmap_ros->getCostmap()->setCost(3, 3, 50); + costmap_ros->getCostmap()->setCost(3, 4, 50); + costmap_ros->getCostmap()->setCost(4, 3, 100); + costmap_ros->getCostmap()->setCost(4, 4, 100); + + ASSERT_EQ(critic->lineCost(3, 3, 0, 50), 50); // all 50 + ASSERT_EQ(critic->lineCost(4, 4, 0, 10), 100); // all 100 + ASSERT_EQ(critic->lineCost(0, 50, 3, 3), 100); // pass 50 and 100 +} + +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_dwb_controller/dwb_critics/test/prefer_forward_test.cpp b/nav2_dwb_controller/dwb_critics/test/prefer_forward_test.cpp new file mode 100644 index 0000000000..460b5d0405 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/test/prefer_forward_test.cpp @@ -0,0 +1,229 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Wilco Bonestroo + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include + +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "dwb_critics/prefer_forward.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" + +static constexpr double default_penalty = 1.0; +static constexpr double default_strafe_x = 0.1; +static constexpr double default_strafe_theta = 0.2; +static constexpr double default_theta_scale = 10.0; + +TEST(PreferForward, StartNode) +{ + auto critic = std::make_shared(); + auto costmap_ros = std::make_shared("test_global_costmap"); + auto node = nav2_util::LifecycleNode::make_shared("costmap_tester"); + node->configure(); + node->activate(); + + std::string name = "test"; + std::string ns = "ns"; + critic->initialize(node, name, ns, costmap_ros); + + EXPECT_EQ(node->get_parameter(ns + "." + name + ".penalty").as_double(), default_penalty); + EXPECT_EQ(node->get_parameter(ns + "." + name + ".strafe_x").as_double(), default_strafe_x); + EXPECT_EQ( + node->get_parameter(ns + "." + name + ".strafe_theta").as_double(), default_strafe_theta); + EXPECT_EQ(node->get_parameter(ns + "." + name + ".theta_scale").as_double(), default_theta_scale); +} + +TEST(PreferForward, NegativeVelocityX) +{ + auto critic = std::make_shared(); + dwb_msgs::msg::Trajectory2D trajectory; + + // score must be equal to the penalty (1.0) for any negative x velocity + trajectory.velocity.x = -1.0; + EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty); + + trajectory.velocity.x = -0.00001; + EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty); + + trajectory.velocity.x = -0.1; + EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty); + + trajectory.velocity.x = std::numeric_limits::lowest(); + EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty); + + trajectory.velocity.x = -std::numeric_limits::min(); + EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty); +} + +TEST(PreferForward, Strafe) +{ + auto critic = std::make_shared(); + dwb_msgs::msg::Trajectory2D trajectory; + + // score must be equal to the penalty (1.0) when x vel is lower than 0.1 + // and theta is between -0.2 and 0.2 + trajectory.velocity.x = 0.05; + trajectory.velocity.theta = -0.1; + EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty); + + trajectory.velocity.x = 0.0999999; + EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty); + + trajectory.velocity.x = 0.000001; + EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty); + + trajectory.velocity.theta = -0.19; + EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty); + + trajectory.velocity.theta = 0.19; + EXPECT_EQ(critic->scoreTrajectory(trajectory), default_penalty); +} + +TEST(PreferForward, Normal) +{ + auto critic = std::make_shared(); + dwb_msgs::msg::Trajectory2D trajectory; + + // score must be equal to the theta * scaling factor (10.0) + trajectory.velocity.x = 0.2; + trajectory.velocity.theta = -0.1; + EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * default_theta_scale); + + trajectory.velocity.theta = 0.1; + EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * default_theta_scale); + + trajectory.velocity.theta = -0.2; + EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * default_theta_scale); + + trajectory.velocity.theta = 0.2; + EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * default_theta_scale); + + trajectory.velocity.theta = 1.5; + EXPECT_EQ(critic->scoreTrajectory(trajectory), 1.5 * default_theta_scale); +} + +TEST(PreferForward, NoneDefaultValues) +{ + auto critic = std::make_shared(); + auto costmap_ros = std::make_shared("test_global_costmap"); + auto node = nav2_util::LifecycleNode::make_shared("costmap_tester"); + node->configure(); + node->activate(); + + double penalty = 18.3; + double strafe_x = 0.5; + double strafe_theta = 0.4; + double theta_scale = 15.0; + + std::string name = "test"; + std::string ns = "ns"; + + nav2_util::declare_parameter_if_not_declared( + node, ns + "." + name + ".penalty", + rclcpp::ParameterValue(penalty)); + nav2_util::declare_parameter_if_not_declared( + node, ns + "." + name + ".strafe_x", + rclcpp::ParameterValue(strafe_x)); + nav2_util::declare_parameter_if_not_declared( + node, ns + "." + name + ".strafe_theta", + rclcpp::ParameterValue(strafe_theta)); + nav2_util::declare_parameter_if_not_declared( + node, ns + "." + name + ".theta_scale", + rclcpp::ParameterValue(theta_scale)); + + critic->initialize(node, name, ns, costmap_ros); + critic->onInit(); + + dwb_msgs::msg::Trajectory2D trajectory; + trajectory.velocity.x = 0.05; + trajectory.velocity.theta = -0.1; + + EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty); + + // score must be equal to the penalty when x vel is lower than strafe_x + // and theta is between -strafe_theta and strafe_theta + trajectory.velocity.x = 0.4; + trajectory.velocity.theta = -0.39; + EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty); + + trajectory.velocity.x = 0.0999999; + EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty); + + trajectory.velocity.x = 0.000001; + EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty); + + trajectory.velocity.theta = -0.09999; + EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty); + + trajectory.velocity.theta = 0.09999; + EXPECT_EQ(critic->scoreTrajectory(trajectory), penalty); + + // score must be equal to the theta * scaling factor (10.0) + trajectory.velocity.x = 0.5; + trajectory.velocity.theta = -0.1; + EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * theta_scale); + + trajectory.velocity.theta = 0.1; + EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.1 * theta_scale); + + trajectory.velocity.theta = -0.2; + EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * theta_scale); + + trajectory.velocity.theta = 0.2; + EXPECT_EQ(critic->scoreTrajectory(trajectory), 0.2 * theta_scale); + + trajectory.velocity.theta = 1.5; + EXPECT_EQ(critic->scoreTrajectory(trajectory), 1.5 * theta_scale); +} + +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_dwb_controller/dwb_critics/test/twirling_test.cpp b/nav2_dwb_controller/dwb_critics/test/twirling_test.cpp new file mode 100644 index 0000000000..949b1edc28 --- /dev/null +++ b/nav2_dwb_controller/dwb_critics/test/twirling_test.cpp @@ -0,0 +1,78 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Samsung Research America + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "dwb_critics/twirling.hpp" +#include "dwb_core/exceptions.hpp" + +TEST(TwirlingTests, Scoring) +{ + std::shared_ptr critic = + std::make_shared(); + + auto node = nav2_util::LifecycleNode::make_shared("costmap_tester"); + + auto costmap_ros = std::make_shared("test_global_costmap"); + costmap_ros->configure(); + + std::string name = "name"; + std::string ns = "ns"; + critic->initialize(node, name, ns, costmap_ros); + + dwb_msgs::msg::Trajectory2D traj; + traj.velocity.theta = 1.0; + EXPECT_EQ(critic->scoreTrajectory(traj), 1.0); + traj.velocity.theta = -1.0; + EXPECT_EQ(critic->scoreTrajectory(traj), 1.0); +} + +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_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 057b293335..9c84c226fe 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.7 + 1.1.0 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt index 36f1dcccce..e5d8ec1f67 100644 --- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt @@ -33,8 +33,6 @@ add_library(standard_traj_generator SHARED src/kinematic_parameters.cpp src/xy_theta_iterator.cpp) ament_target_dependencies(standard_traj_generator ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(standard_traj_generator PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") if(BUILD_TESTING) 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 b39e36b627..ed3cabc3d0 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 @@ -37,6 +37,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -80,9 +81,13 @@ struct KinematicParameters double min_vel_y_{0}; double max_vel_x_{0}; double max_vel_y_{0}; + double base_max_vel_x_{0}; + double base_max_vel_y_{0}; double max_vel_theta_{0}; + double base_max_vel_theta_{0}; double min_speed_xy_{0}; double max_speed_xy_{0}; + double base_max_speed_xy_{0}; double min_speed_theta_{0}; double acc_lim_x_{0}; double acc_lim_y_{0}; @@ -109,15 +114,21 @@ class KinematicsHandler inline KinematicParameters getKinematics() {return *kinematics_.load();} + void setSpeedLimit(const double & speed_limit, const bool & percentage); + using Ptr = std::shared_ptr; protected: std::atomic kinematics_; - // Subscription for parameter change - rclcpp::AsyncParametersClient::SharedPtr parameters_client_; - rclcpp::Subscription::SharedPtr parameter_event_sub_; - void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); void update_kinematics(KinematicParameters kinematics); std::string plugin_name_; }; 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 ec380674d4..87ba77baf0 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 @@ -68,6 +68,20 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator const nav_2d_msgs::msg::Twist2D & start_vel, const nav_2d_msgs::msg::Twist2D & cmd_vel) 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 + { + if (kinematics_handler_) { + kinematics_handler_->setSpeedLimit(speed_limit, percentage); + } + } + protected: /** * @brief Initialize the VelocityIterator pointer. Put in its own function for easy overriding diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index f425fc26aa..2236fc8274 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.7 + 1.1.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 27af24f24e..2ca2b71f58 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -36,12 +36,13 @@ #include #include +#include #include "nav_2d_utils/parameters.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" using nav2_util::declare_parameter_if_not_declared; -using nav_2d_utils::moveDeprecatedParameter; using rcl_interfaces::msg::ParameterType; using std::placeholders::_1; @@ -63,11 +64,6 @@ void KinematicsHandler::initialize( const std::string & plugin_name) { plugin_name_ = plugin_name; - // Special handling for renamed parameters - moveDeprecatedParameter(nh, plugin_name + ".max_vel_theta", "max_rot_vel"); - moveDeprecatedParameter(nh, plugin_name + ".min_speed_xy", "min_trans_vel"); - moveDeprecatedParameter(nh, plugin_name + ".max_speed_xy", "max_trans_vel"); - moveDeprecatedParameter(nh, plugin_name + ".min_speed_theta", "min_rot_vel"); declare_parameter_if_not_declared(nh, plugin_name + ".min_vel_x", rclcpp::ParameterValue(0.0)); declare_parameter_if_not_declared(nh, plugin_name + ".min_vel_y", rclcpp::ParameterValue(0.0)); @@ -113,15 +109,14 @@ void KinematicsHandler::initialize( nh->get_parameter(plugin_name + ".decel_lim_y", kinematics.decel_lim_y_); nh->get_parameter(plugin_name + ".decel_lim_theta", kinematics.decel_lim_theta_); - // Setup callback for changes to parameters. - parameters_client_ = std::make_shared( - nh->get_node_base_interface(), - nh->get_node_topics_interface(), - nh->get_node_graph_interface(), - nh->get_node_services_interface()); + kinematics.base_max_vel_x_ = kinematics.max_vel_x_; + kinematics.base_max_vel_y_ = kinematics.max_vel_y_; + kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_; + kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_; - parameter_event_sub_ = parameters_client_->on_parameter_event( - std::bind(&KinematicsHandler::on_parameter_event_callback, this, _1)); + // Add callback for dynamic parameters + dyn_params_handler_ = nh->add_on_set_parameters_callback( + std::bind(&KinematicsHandler::dynamicParametersCallback, this, _1)); kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_; kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_; @@ -129,53 +124,97 @@ void KinematicsHandler::initialize( update_kinematics(kinematics); } +void KinematicsHandler::setSpeedLimit( + const double & speed_limit, const bool & percentage) +{ + KinematicParameters kinematics(*kinematics_.load()); + + if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { + // Restore default value + kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_; + kinematics.max_vel_x_ = kinematics.base_max_vel_x_; + kinematics.max_vel_y_ = kinematics.base_max_vel_y_; + kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_; + } else { + 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 + kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_; + + update_kinematics(kinematics); +} -void -KinematicsHandler::on_parameter_event_callback( - const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +rcl_interfaces::msg::SetParametersResult +KinematicsHandler::dynamicParametersCallback(std::vector parameters) { + rcl_interfaces::msg::SetParametersResult result; KinematicParameters kinematics(*kinematics_.load()); - 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; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); if (type == ParameterType::PARAMETER_DOUBLE) { if (name == plugin_name_ + ".min_vel_x") { - kinematics.min_vel_x_ = value.double_value; + kinematics.min_vel_x_ = parameter.as_double(); } else if (name == plugin_name_ + ".min_vel_y") { - kinematics.min_vel_y_ = value.double_value; + kinematics.min_vel_y_ = parameter.as_double(); } else if (name == plugin_name_ + ".max_vel_x") { - kinematics.max_vel_x_ = value.double_value; + kinematics.max_vel_x_ = parameter.as_double(); + kinematics.base_max_vel_x_ = kinematics.max_vel_x_; } else if (name == plugin_name_ + ".max_vel_y") { - kinematics.max_vel_y_ = value.double_value; + kinematics.max_vel_y_ = parameter.as_double(); + kinematics.base_max_vel_y_ = kinematics.max_vel_y_; } else if (name == plugin_name_ + ".max_vel_theta") { - kinematics.max_vel_theta_ = value.double_value; + kinematics.max_vel_theta_ = parameter.as_double(); + kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_; } else if (name == plugin_name_ + ".min_speed_xy") { - kinematics.min_speed_xy_ = value.double_value; + kinematics.min_speed_xy_ = parameter.as_double(); kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_; } else if (name == plugin_name_ + ".max_speed_xy") { - kinematics.max_speed_xy_ = value.double_value; + kinematics.max_speed_xy_ = parameter.as_double(); + kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_; } else if (name == plugin_name_ + ".min_speed_theta") { - kinematics.min_speed_theta_ = value.double_value; + kinematics.min_speed_theta_ = parameter.as_double(); kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_; } else if (name == plugin_name_ + ".acc_lim_x") { - kinematics.acc_lim_x_ = value.double_value; + kinematics.acc_lim_x_ = parameter.as_double(); } else if (name == plugin_name_ + ".acc_lim_y") { - kinematics.acc_lim_y_ = value.double_value; + kinematics.acc_lim_y_ = parameter.as_double(); } else if (name == plugin_name_ + ".acc_lim_theta") { - kinematics.acc_lim_theta_ = value.double_value; + kinematics.acc_lim_theta_ = parameter.as_double(); } else if (name == plugin_name_ + ".decel_lim_x") { - kinematics.decel_lim_x_ = value.double_value; + kinematics.decel_lim_x_ = parameter.as_double(); } else if (name == plugin_name_ + ".decel_lim_y") { - kinematics.decel_lim_y_ = value.double_value; + kinematics.decel_lim_y_ = parameter.as_double(); } else if (name == plugin_name_ + ".decel_lim_theta") { - kinematics.decel_lim_theta_ = value.double_value; + kinematics.decel_lim_theta_ = parameter.as_double(); } } } update_kinematics(kinematics); + result.successful = true; + return result; } void KinematicsHandler::update_kinematics(KinematicParameters kinematics) 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 cc56b11f6c..7591563370 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/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp index 3b7e168579..49a62de302 100644 --- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp @@ -43,8 +43,6 @@ #include "dwb_core/exceptions.hpp" #include "nav2_util/node_utils.hpp" -using nav_2d_utils::loadParameterWithDeprecation; - namespace dwb_plugins { diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt index 29845a8499..c5b9308e8d 100644 --- a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt @@ -2,3 +2,9 @@ ament_add_gtest(vtest velocity_iterator_test.cpp) ament_add_gtest(twist_gen_test twist_gen.cpp) target_link_libraries(twist_gen_test standard_traj_generator) + +ament_add_gtest(kinematic_parameters_test kinematic_parameters_test.cpp) +target_link_libraries(kinematic_parameters_test standard_traj_generator) + +ament_add_gtest(speed_limit_test speed_limit_test.cpp) +target_link_libraries(speed_limit_test standard_traj_generator) diff --git a/nav2_dwb_controller/dwb_plugins/test/kinematic_parameters_test.cpp b/nav2_dwb_controller/dwb_plugins/test/kinematic_parameters_test.cpp new file mode 100644 index 0000000000..a68fe691ba --- /dev/null +++ b/nav2_dwb_controller/dwb_plugins/test/kinematic_parameters_test.cpp @@ -0,0 +1,129 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Wilco Bonestroo + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include +#include + +#include "gtest/gtest.h" +#include "dwb_plugins/kinematic_parameters.hpp" + +using rcl_interfaces::msg::Parameter; +using rcl_interfaces::msg::ParameterType; +using rcl_interfaces::msg::ParameterEvent; + +class KinematicsHandlerTest : public dwb_plugins::KinematicsHandler +{ +public: + void simulate_event( + std::vector parameters) + { + dynamicParametersCallback(parameters); + } +}; + +TEST(KinematicParameters, SetAllParameters) { + std::string nodeName = "test_node"; + auto node = nav2_util::LifecycleNode::make_shared(nodeName); + KinematicsHandlerTest kh; + kh.initialize(node, nodeName); + + std::vector parameters; + rclcpp::Parameter + p_minX(nodeName + ".min_vel_x", 12.34), + p_maxX(nodeName + ".max_vel_x", 23.45), + p_minY(nodeName + ".min_vel_y", 34.56), + p_maxY(nodeName + ".max_vel_y", 45.67), + p_accX(nodeName + ".acc_lim_x", 56.78), + p_decelX(nodeName + ".acc_lim_y", 67.89), + p_accY(nodeName + ".decel_lim_x", 78.90), + p_decelY(nodeName + ".decel_lim_y", 89.01), + p_minSpeedXY(nodeName + ".min_speed_xy", 90.12), + p_maxSpeedXY(nodeName + ".max_speed_xy", 123.456), + p_maxTheta(nodeName + ".max_vel_theta", 345.678), + p_accTheta(nodeName + ".acc_lim_theta", 234.567), + p_decelTheta(nodeName + ".decel_lim_theta", 456.789), + p_minSpeedTheta(nodeName + ".min_speed_theta", 567.890); + + parameters.push_back(p_minX); + parameters.push_back(p_minX); + parameters.push_back(p_maxX); + parameters.push_back(p_minY); + parameters.push_back(p_maxY); + parameters.push_back(p_accX); + parameters.push_back(p_accY); + parameters.push_back(p_decelX); + parameters.push_back(p_decelY); + parameters.push_back(p_minSpeedXY); + parameters.push_back(p_maxSpeedXY); + parameters.push_back(p_maxTheta); + parameters.push_back(p_accTheta); + parameters.push_back(p_decelTheta); + parameters.push_back(p_minSpeedTheta); + + kh.simulate_event(parameters); + + dwb_plugins::KinematicParameters kp = kh.getKinematics(); + + EXPECT_EQ(kp.getMinX(), 12.34); + EXPECT_EQ(kp.getMaxX(), 23.45); + EXPECT_EQ(kp.getMinY(), 34.56); + EXPECT_EQ(kp.getMaxY(), 45.67); + EXPECT_EQ(kp.getAccX(), 56.78); + EXPECT_EQ(kp.getAccY(), 67.89); + EXPECT_EQ(kp.getDecelX(), 78.90); + EXPECT_EQ(kp.getDecelY(), 89.01); + EXPECT_EQ(kp.getMinSpeedXY(), 90.12); + EXPECT_EQ(kp.getMaxSpeedXY(), 123.456); + EXPECT_EQ(kp.getAccTheta(), 234.567); + EXPECT_EQ(kp.getMaxTheta(), 345.678); + EXPECT_EQ(kp.getDecelTheta(), 456.789); + EXPECT_EQ(kp.getMinSpeedTheta(), 567.890); +} + + +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_dwb_controller/dwb_plugins/test/speed_limit_test.cpp b/nav2_dwb_controller/dwb_plugins/test/speed_limit_test.cpp new file mode 100644 index 0000000000..ca102dae46 --- /dev/null +++ b/nav2_dwb_controller/dwb_plugins/test/speed_limit_test.cpp @@ -0,0 +1,171 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Samsung Research Russia + * 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 copyright holder 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 HOLDER 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. + * + * Author: Alexey Merzlyakov + */ + +#include + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" + +#include "dwb_plugins/kinematic_parameters.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = 1e-5; + +static const char NODE_NAME[] = "test_node"; +static const double MAX_VEL_X = 40.0; +static const double MAX_VEL_Y = 30.0; +static const double MAX_VEL_THETA = 15.0; +static const double MAX_VEL_LINEAR = 50.0; + +class TestNode : public ::testing::Test +{ +public: + TestNode() + { + const std::string node_name = NODE_NAME; + node_ = nav2_util::LifecycleNode::make_shared(node_name); + + node_->declare_parameter( + node_name + ".max_vel_x", rclcpp::ParameterValue(MAX_VEL_X)); + node_->set_parameter( + rclcpp::Parameter(node_name + ".max_vel_x", MAX_VEL_X)); + + node_->declare_parameter( + node_name + ".max_vel_y", rclcpp::ParameterValue(MAX_VEL_Y)); + node_->set_parameter( + rclcpp::Parameter(node_name + ".max_vel_y", MAX_VEL_Y)); + + node_->declare_parameter( + node_name + ".max_vel_theta", rclcpp::ParameterValue(MAX_VEL_THETA)); + node_->set_parameter( + rclcpp::Parameter(node_name + ".max_vel_theta", MAX_VEL_THETA)); + + node_->declare_parameter( + node_name + ".max_speed_xy", rclcpp::ParameterValue(MAX_VEL_LINEAR)); + node_->set_parameter( + rclcpp::Parameter(node_name + ".max_speed_xy", MAX_VEL_LINEAR)); + } + + ~TestNode() {} + +protected: + nav2_util::LifecycleNode::SharedPtr node_; +}; + +TEST_F(TestNode, TestPercentLimit) +{ + 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 30% from maximum robot speed + kh.setSpeedLimit(30, true); + + // Update KinematicParameters values from KinematicsHandler + kp = kh.getKinematics(); + EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X * 0.3, EPSILON); + EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y * 0.3, EPSILON); + EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA * 0.3, EPSILON); + EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR * 0.3, EPSILON); + + // Restore maximum speed to its default + 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(); + 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); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp index 1e4ca4cc6c..6b925c2ca3 100644 --- a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp +++ b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp @@ -51,6 +51,15 @@ geometry_msgs::msg::Pose2D origin; nav_2d_msgs::msg::Twist2D zero; nav_2d_msgs::msg::Twist2D forward; +class LimitedAccelGeneratorTest : public dwb_plugins::LimitedAccelGenerator +{ +public: + double getAccelerationTime() + { + return acceleration_time_; + } +}; + std::vector getDefaultKinematicParameters() { std::vector parameters; @@ -222,6 +231,52 @@ TEST(VelocityIterator, dwa_gen) checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1); } +TEST(VelocityIterator, dwa_gen_zero_frequency) +{ + auto nh = makeTestNode("dwa_gen"); + nh->declare_parameter("controller_frequency", 0.0); + LimitedAccelGeneratorTest gen; + gen.initialize(nh, "dwb"); + // Default value should be 0.05 + EXPECT_EQ(gen.getAccelerationTime(), 0.05); +} + +TEST(VelocityIterator, dwa_gen_one_frequency) +{ + auto nh = makeTestNode("dwa_gen"); + nh->declare_parameter("controller_frequency", 1.0); + LimitedAccelGeneratorTest gen; + gen.initialize(nh, "dwb"); + EXPECT_EQ(gen.getAccelerationTime(), 1.0); +} + +TEST(VelocityIterator, dwa_gen_ten_frequency) +{ + auto nh = makeTestNode("dwa_gen"); + nh->declare_parameter("controller_frequency", 10.0); + LimitedAccelGeneratorTest gen; + gen.initialize(nh, "dwb"); + EXPECT_EQ(gen.getAccelerationTime(), 0.1); +} + +TEST(VelocityIterator, dwa_gen_fifty_frequency) +{ + auto nh = makeTestNode("dwa_gen"); + nh->declare_parameter("controller_frequency", 50.0); + LimitedAccelGeneratorTest gen; + gen.initialize(nh, "dwb"); + EXPECT_EQ(gen.getAccelerationTime(), 0.02); +} + +TEST(VelocityIterator, dwa_gen_hundred_frequency) +{ + auto nh = makeTestNode("dwa_gen"); + nh->declare_parameter("controller_frequency", 100.0); + LimitedAccelGeneratorTest gen; + gen.initialize(nh, "dwb"); + EXPECT_EQ(gen.getAccelerationTime(), 0.01); +} + TEST(VelocityIterator, nonzero) { auto nh = makeTestNode("nonzero", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 1ae96ea402..cab00606dc 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.7 + 1.1.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 3579746eaf..f9ca6fba94 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.7 + 1.1.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/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt index adb0123d7f..9d45a06ab8 100644 --- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt @@ -4,6 +4,7 @@ project(nav_2d_utils) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(nav_2d_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) @@ -18,11 +19,13 @@ set(dependencies geometry_msgs nav_2d_msgs nav_msgs + std_msgs rclcpp tf2 tf2_geometry_msgs nav2_msgs nav2_util + nav_2d_msgs ) include_directories( @@ -47,14 +50,12 @@ add_library(tf_help SHARED src/tf_help.cpp ) -target_link_libraries(tf_help - conversions -) - ament_target_dependencies(tf_help ${dependencies} ) +target_link_libraries(tf_help conversions) + install(TARGETS conversions path_ops tf_help ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -69,6 +70,9 @@ if(BUILD_TESTING) # the following line skips the linter which checks for copyrights set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) endif() ament_export_include_directories(include) diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp index 15dd3b9e4d..c3c30428fe 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp @@ -64,15 +64,7 @@ param_t searchAndGetParam( const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & param_name, const param_t & default_value) { - // TODO(crdelsey): Handle searchParam - // std::string resolved_name; - // if (nh->searchParam(param_name, resolved_name)) - // { - // param_t value = 0; - // nh->param(resolved_name, value, default_value); - // return value; - // } - param_t value = 0; + param_t value; nav2_util::declare_parameter_if_not_declared( nh, param_name, rclcpp::ParameterValue(default_value)); @@ -80,95 +72,6 @@ param_t searchAndGetParam( return value; } -/** - * @brief Load a parameter from one of two namespaces. Complain if it uses the old name. - * @param nh NodeHandle to look for the parameter in - * @param current_name Parameter name that is current, i.e. not deprecated - * @param old_name Deprecated parameter name - * @param default_value If neither parameter is present, return this value - * @return The value of the parameter or the default value - */ -template -param_t loadParameterWithDeprecation( - const nav2_util::LifecycleNode::SharedPtr & nh, const std::string current_name, - const std::string old_name, const param_t & default_value) -{ - nav2_util::declare_parameter_if_not_declared( - nh, current_name, rclcpp::ParameterValue(default_value)); - nav2_util::declare_parameter_if_not_declared( - nh, old_name, rclcpp::ParameterValue(default_value)); - - param_t current_name_value; - nh->get_parameter(current_name, current_name_value); - param_t old_name_value; - nh->get_parameter(old_name, old_name_value); - - if (old_name_value != current_name_value && old_name_value != default_value) { - RCLCPP_WARN( - nh->get_logger(), - "Parameter %s is deprecated. Please use the name %s instead.", - old_name.c_str(), current_name.c_str()); - // set both parameters to the same value - nh->set_parameters({rclcpp::Parameter(current_name, old_name_value)}); - current_name_value = old_name_value; - } - return current_name_value; -} - -/** - * @brief If a deprecated parameter exists, complain and move to its new location - * @param nh NodeHandle to look for the parameter in - * @param current_name Parameter name that is current, i.e. not deprecated - * @param old_name Deprecated parameter name - */ -template -void moveDeprecatedParameter( - const nav2_util::LifecycleNode::SharedPtr & nh, const std::string current_name, - const std::string old_name) -{ - param_t value; - if (!nh->get_parameter(old_name, value)) {return;} - - RCLCPP_WARN( - nh->get_logger(), - "Parameter %s is deprecated. Please use the name %s instead.", - old_name.c_str(), current_name.c_str()); - nh->get_parameter(old_name, value); - nh->set_parameters({rclcpp::Parameter(current_name, value)}); -} - -/** - * @brief Move a parameter from one place to another - * - * For use loading old parameter structures into new places. - * - * If the new name already has a value, don't move the old value there. - * - * @param nh NodeHandle for loading/saving params - * @param old_name Parameter name to move/remove - * @param current_name Destination parameter name - * @param default_value Value to save in the new name if old parameter is not found. - * @param should_delete If true, whether to delete the parameter from the old name - */ -template -void moveParameter( - const nav2_util::LifecycleNode::SharedPtr & nh, std::string old_name, - std::string current_name, param_t default_value, bool should_delete = true) -{ - param_t value = 0; - if (nh->get_parameter(current_name, value)) { - if (should_delete) {nh->undeclare_parameter(old_name);} - return; - } - if (nh->get_parameter(old_name, value)) { - if (should_delete) {nh->undeclare_parameter(old_name);} - } else { - value = default_value; - } - nh->set_parameter(rclcpp::Parameter(current_name, value)); -} - - } // namespace nav_2d_utils #pragma GCC diagnostic pop 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 3b4687bd10..442eb36797 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 75d5cda8bb..90da3fc628 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.7 + 1.1.0 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause @@ -13,6 +13,7 @@ geometry_msgs nav_2d_msgs nav_msgs + std_msgs tf2 tf2_geometry_msgs nav2_msgs @@ -20,6 +21,7 @@ ament_lint_common ament_lint_auto + ament_cmake_gtest ament_cmake diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp index 78056fa1de..077707b394 100644 --- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp +++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp @@ -33,12 +33,20 @@ */ #include "nav_2d_utils/conversions.hpp" + #include #include + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #include "tf2/utils.h" #pragma GCC diagnostic pop + #include "nav2_util/geometry_utils.hpp" namespace nav_2d_utils diff --git a/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp new file mode 100644 index 0000000000..8f2bd171b2 --- /dev/null +++ b/nav2_dwb_controller/nav_2d_utils/test/2d_utils_test.cpp @@ -0,0 +1,162 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Wilco Bonestroo + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav_2d_utils/conversions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" + +using nav_2d_utils::posesToPath; +using nav_2d_utils::pathToPath; + +TEST(nav_2d_utils, PosesToPathEmpty) +{ + std::vector poses; + nav_msgs::msg::Path path = posesToPath(poses); + + EXPECT_EQ(path.poses.size(), 0ul); +} + +TEST(nav_2d_utils, PosesToPathNonEmpty) +{ + std::vector poses; + geometry_msgs::msg::PoseStamped pose1; + rclcpp::Time time1, time2; + auto node = rclcpp::Node::make_shared("twod_utils_test_node"); + time1 = node->now(); + + tf2::Quaternion quat1, quat2; + quat1.setRPY(0, 0, 0.123); + pose1.pose.position.x = 1.0; + pose1.pose.position.y = 2.0; + pose1.pose.orientation.w = quat1.w(); + pose1.pose.orientation.x = quat1.x(); + pose1.pose.orientation.y = quat1.y(); + pose1.pose.orientation.z = quat1.z(); + pose1.header.stamp = time1; + pose1.header.frame_id = "frame1_id"; + + geometry_msgs::msg::PoseStamped pose2; + pose2.pose.position.x = 4.0; + pose2.pose.position.y = 5.0; + quat2.setRPY(0, 0, 0.987); + pose2.pose.orientation.w = quat2.w(); + pose2.pose.orientation.x = quat2.x(); + pose2.pose.orientation.y = quat2.y(); + pose2.pose.orientation.z = quat2.z(); + + time2 = node->now(); + pose2.header.stamp = time2; + pose2.header.frame_id = "frame2_id"; + + poses.push_back(pose1); + poses.push_back(pose2); + + nav_msgs::msg::Path path = posesToPath(poses); + + EXPECT_EQ(path.poses.size(), 2ul); + EXPECT_EQ(path.poses[0].pose.position.x, 1.0); + EXPECT_EQ(path.poses[0].pose.position.y, 2.0); + EXPECT_EQ(path.poses[0].header.stamp, time1); + EXPECT_EQ(path.poses[0].header.frame_id, "frame1_id"); + EXPECT_EQ(path.poses[1].pose.position.x, 4.0); + EXPECT_EQ(path.poses[1].pose.position.y, 5.0); + EXPECT_EQ(path.poses[1].header.frame_id, "frame2_id"); + + EXPECT_EQ(path.header.stamp, time1); +} + +TEST(nav_2d_utils, PathToPathEmpty) +{ + nav_2d_msgs::msg::Path2D path2d; + nav_msgs::msg::Path path = pathToPath(path2d); + EXPECT_EQ(path.poses.size(), 0ul); +} + +TEST(nav_2d_utils, PathToPathNoNEmpty) +{ + nav_2d_msgs::msg::Path2D path2d; + + geometry_msgs::msg::Pose2D pose1; + pose1.x = 1.0; + pose1.y = 2.0; + pose1.theta = M_PI / 2.0; + + geometry_msgs::msg::Pose2D pose2; + pose2.x = 4.0; + pose2.y = 5.0; + pose2.theta = M_PI; + + path2d.poses.push_back(pose1); + path2d.poses.push_back(pose2); + + nav_msgs::msg::Path path = pathToPath(path2d); + EXPECT_EQ(path.poses.size(), 2ul); + EXPECT_EQ(path.poses[0].pose.position.x, 1.0); + EXPECT_EQ(path.poses[0].pose.position.y, 2.0); + + tf2::Quaternion quat; + quat.setRPY(0, 0, M_PI / 2.0); + EXPECT_EQ(path.poses[0].pose.orientation.w, quat.w()); + EXPECT_EQ(path.poses[0].pose.orientation.x, quat.x()); + EXPECT_EQ(path.poses[0].pose.orientation.y, quat.x()); + EXPECT_EQ(path.poses[0].pose.orientation.z, quat.z()); + + EXPECT_EQ(path.poses[1].pose.position.x, 4.0); + EXPECT_EQ(path.poses[1].pose.position.y, 5.0); + quat.setRPY(0, 0, M_PI); + EXPECT_EQ(path.poses[1].pose.orientation.w, quat.w()); + EXPECT_EQ(path.poses[1].pose.orientation.x, quat.x()); + EXPECT_EQ(path.poses[1].pose.orientation.y, quat.x()); + EXPECT_EQ(path.poses[1].pose.orientation.z, quat.z()); +} + +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_dwb_controller/nav_2d_utils/test/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt new file mode 100644 index 0000000000..ba9740520f --- /dev/null +++ b/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt @@ -0,0 +1,9 @@ +ament_add_gtest(2d_utils_tests 2d_utils_test.cpp) +target_link_libraries(2d_utils_tests conversions) + +ament_add_gtest(path_ops_tests path_ops_test.cpp) +target_link_libraries(path_ops_tests path_ops) + +ament_add_gtest(tf_help_tests tf_help_test.cpp) +target_link_libraries(tf_help_tests tf_help conversions) + diff --git a/nav2_dwb_controller/nav_2d_utils/test/path_ops_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/path_ops_test.cpp new file mode 100644 index 0000000000..eac7e2bc16 --- /dev/null +++ b/nav2_dwb_controller/nav_2d_utils/test/path_ops_test.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Wilco Bonestroo + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include "gtest/gtest.h" +#include "nav_2d_utils/path_ops.hpp" + +using std::sqrt; +using nav_2d_utils::adjustPlanResolution; + +TEST(path_ops_test, AdjustResolutionEmpty) +{ + nav_2d_msgs::msg::Path2D in; + nav_2d_msgs::msg::Path2D out = adjustPlanResolution(in, 2.0); + EXPECT_EQ(out.poses.size(), 0ul); +} + +TEST(path_ops_test, AdjustResolutionSimple) +{ + nav_2d_msgs::msg::Path2D in; + const float RESOLUTION = 20.0; + + geometry_msgs::msg::Pose2D pose1; + pose1.x = 0.0; + pose1.y = 0.0; + geometry_msgs::msg::Pose2D pose2; + pose2.x = 100.0; + pose2.y = 0.0; + + in.poses.push_back(pose1); + in.poses.push_back(pose2); + + nav_2d_msgs::msg::Path2D out = adjustPlanResolution(in, RESOLUTION); + float length = 100; + ulong number_of_points = ceil(length / (2 * RESOLUTION)); + EXPECT_EQ(out.poses.size(), number_of_points); + float max_length = length / (number_of_points - 1); + + for (unsigned int i = 1; i < out.poses.size(); i++) { + pose1 = out.poses[i - 1]; + pose2 = out.poses[i]; + + double sq_dist = (pose1.x - pose2.x) * (pose1.x - pose2.x) + + (pose1.y - pose2.y) * (pose1.y - pose2.y); + + EXPECT_TRUE(sqrt(sq_dist) <= max_length); + } +} diff --git a/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp b/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp new file mode 100644 index 0000000000..4095567473 --- /dev/null +++ b/nav2_dwb_controller/nav_2d_utils/test/tf_help_test.cpp @@ -0,0 +1,98 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Wilco Bonestroo + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include + +#include "gtest/gtest.h" +#include "nav_2d_utils/tf_help.hpp" + +TEST(TF_Help, TransformToSelf) { + bool result; + + std::shared_ptr tf; + std::string frame = "frame_id"; + geometry_msgs::msg::PoseStamped in_pose; + in_pose.header.frame_id = "frame_id"; + in_pose.pose.position.x = 1.0; + in_pose.pose.position.y = 2.0; + in_pose.pose.position.z = 3.0; + tf2::Quaternion qt; + qt.setRPY(0.5, 1.0, 1.5); + in_pose.pose.orientation.w = qt.w(); + in_pose.pose.orientation.x = qt.x(); + in_pose.pose.orientation.y = qt.y(); + in_pose.pose.orientation.z = qt.z(); + + geometry_msgs::msg::PoseStamped out_pose; + rclcpp::Duration transform_tolerance(0, 500); + + result = nav_2d_utils::transformPose(tf, frame, in_pose, out_pose, transform_tolerance); + + EXPECT_TRUE(result); + EXPECT_EQ(out_pose.header.frame_id, "frame_id"); + EXPECT_EQ(out_pose.pose.position.x, 1.0); + EXPECT_EQ(out_pose.pose.position.y, 2.0); + EXPECT_EQ(out_pose.pose.position.z, 3.0); + EXPECT_EQ(out_pose.pose.orientation.w, qt.w()); + EXPECT_EQ(out_pose.pose.orientation.x, qt.x()); + EXPECT_EQ(out_pose.pose.orientation.y, qt.y()); + EXPECT_EQ(out_pose.pose.orientation.z, qt.z()); +} + +TEST(TF_Help, EmptyBuffer) { + auto clock = std::make_shared(RCL_ROS_TIME); + auto buffer = std::make_shared(clock); + + std::string frame = "frame_id"; + geometry_msgs::msg::PoseStamped in_pose; + in_pose.header.frame_id = "other_frame_id"; + in_pose.pose.position.x = 1.0; + in_pose.pose.position.y = 2.0; + in_pose.pose.position.z = 3.0; + tf2::Quaternion qt; + qt.setRPY(0.5, 1.0, 1.5); + in_pose.pose.orientation.w = qt.w(); + in_pose.pose.orientation.x = qt.x(); + in_pose.pose.orientation.y = qt.y(); + in_pose.pose.orientation.z = qt.z(); + + geometry_msgs::msg::PoseStamped out_pose; + rclcpp::Duration transform_tolerance(0, 500); + + bool result; + result = nav_2d_utils::transformPose(buffer, frame, in_pose, out_pose, transform_tolerance); + + EXPECT_FALSE(result); +} diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt index ff06d31cc5..5f4b3f3c23 100644 --- a/nav2_lifecycle_manager/CMakeLists.txt +++ b/nav2_lifecycle_manager/CMakeLists.txt @@ -9,10 +9,13 @@ find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(bondcpp REQUIRED) +find_package(diagnostic_updater REQUIRED) nav2_package() @@ -35,9 +38,12 @@ set(dependencies rclcpp rclcpp_action rclcpp_lifecycle + rclcpp_components std_msgs std_srvs tf2_geometry_msgs + bondcpp + diagnostic_updater ) ament_target_dependencies(${library_name} @@ -56,6 +62,8 @@ ament_target_dependencies(lifecycle_manager ${dependencies} ) +rclcpp_components_register_nodes(${library_name} "nav2_lifecycle_manager::LifecycleManager") + install(TARGETS ${library_name} ARCHIVE DESTINATION lib diff --git a/nav2_lifecycle_manager/README.md b/nav2_lifecycle_manager/README.md index d2112266b5..9a0905006c 100644 --- a/nav2_lifecycle_manager/README.md +++ b/nav2_lifecycle_manager/README.md @@ -1,11 +1,12 @@ ### 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()```. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-lifecycle.html) for additional parameter descriptions. ### 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.), but it is meant to be called on bringup through a production system application. -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. +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. These are all transitioned in ordered groups for bringup transitions, and reverse ordered groups for shutdown transitions. The lifecycle manager has a default nodes list for all the nodes that it manages. This list can be changed using the lifecycle manager’s _“node_names”_ parameter. 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 384f33e018..edd3454ffe 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2022 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. @@ -18,23 +19,29 @@ #include #include #include +#include #include #include #include "nav2_util/lifecycle_service_client.hpp" +#include "nav2_util/node_thread.hpp" #include "rclcpp/rclcpp.hpp" #include "std_srvs/srv/empty.hpp" #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp" #include "std_srvs/srv/trigger.hpp" +#include "bondcpp/bond.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" + namespace nav2_lifecycle_manager { +using namespace std::chrono_literals; // NOLINT 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 @@ -42,16 +49,18 @@ class LifecycleManager : public rclcpp::Node public: /** * @brief A constructor for nav2_lifecycle_manager::LifecycleManager + * @param options Additional options to control creation of the node. */ - LifecycleManager(); + explicit LifecycleManager(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief A destructor for nav2_lifecycle_manager::LifecycleManager */ ~LifecycleManager(); protected: - // The ROS node to use when calling lifecycle services - rclcpp::Node::SharedPtr service_client_node_; + // Callback group used by services and timers + rclcpp::CallbackGroup::SharedPtr callback_group_; + std::unique_ptr service_thread_; // The services provided by this node rclcpp::Service::SharedPtr manager_srv_; @@ -93,7 +102,7 @@ class LifecycleManager : public rclcpp::Node * @brief Reset all the managed nodes. * @return true or false */ - bool reset(); + bool reset(bool hard_reset = false); /** * @brief Pause all the managed nodes. * @return true or false @@ -121,15 +130,49 @@ class LifecycleManager : public rclcpp::Node */ void destroyLifecycleServiceClients(); + // Support function for creating bond timer + /** + * @brief Support function for creating bond timer + */ + void createBondTimer(); + + // Support function for creating bond connection + /** + * @brief Support function for creating bond connections + */ + bool createBondConnection(const std::string & node_name); + + // Support function for killing bond connections + /** + * @brief Support function for killing bond connections + */ + void destroyBondTimer(); + + // Support function for checking on bond connections + /** + * @ brief Support function for checking on bond connections + * will take down system if there's something non-responsive + */ + void checkBondConnections(); + + // Support function for checking if bond connections come back after respawn + /** + * @ brief Support function for checking on bond connections + * will bring back the system if something goes from non-responsive to responsive + */ + void checkBondRespawnConnection(); + /** * @brief For a node, transition to the new target state */ - bool changeStateForNode(const std::string & node_name, std::uint8_t transition); + bool changeStateForNode( + const std::string & node_name, + std::uint8_t transition); /** * @brief For each node in the map, transition to the new target state */ - bool changeStateForAllNodes(std::uint8_t transition); + bool changeStateForAllNodes(std::uint8_t transition, bool hard_change = false); // Convenience function to highlight the output on the console /** @@ -137,6 +180,21 @@ class LifecycleManager : public rclcpp::Node */ void message(const std::string & msg); + // Diagnostics functions + /** + * @brief function to check if the Nav2 system is active + */ + void CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat); + + // Timer thread to look at bond connections + rclcpp::TimerBase::SharedPtr init_timer_; + rclcpp::TimerBase::SharedPtr bond_timer_; + rclcpp::TimerBase::SharedPtr bond_respawn_timer_; + std::chrono::milliseconds bond_timeout_; + + // A map of all nodes to check bond connection + std::map> bond_map_; + // A map of all nodes to be controlled std::map> node_map_; @@ -150,8 +208,13 @@ class LifecycleManager : public rclcpp::Node // Whether to automatically start up the system bool autostart_; + bool attempt_respawn_reconnection_; bool system_active_{false}; + diagnostic_updater::Updater diagnostics_updater_; + + rclcpp::Time bond_respawn_start_time_{0}; + rclcpp::Duration bond_respawn_max_duration_{10s}; }; } // namespace nav2_lifecycle_manager 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 bb597fbd42..c5f2be8bb6 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,12 @@ class LifecycleManagerClient public: /** * @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); + explicit LifecycleManagerClient( + const std::string & name, + std::shared_ptr parent_node); // Client-side interface to the Nav2 lifecycle manager /** @@ -110,8 +115,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 d56290206e..d258e30154 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.7 + 1.1.0 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 @@ -18,7 +18,9 @@ std_msgs std_srvs tf2_geometry_msgs + bondcpp nav2_common + diagnostic_updater geometry_msgs lifecycle_msgs @@ -28,10 +30,14 @@ rclcpp_lifecycle std_msgs std_srvs + bondcpp tf2_geometry_msgs + diagnostic_updater 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 ccd9faaeab..f18436f21f 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2022 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. @@ -31,30 +32,44 @@ using nav2_util::LifecycleServiceClient; namespace nav2_lifecycle_manager { -LifecycleManager::LifecycleManager() -: Node("lifecycle_manager") +LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) +: Node("lifecycle_manager", options), diagnostics_updater_(this) { RCLCPP_INFO(get_logger(), "Creating"); // 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); + declare_parameter("bond_respawn_max_duration", 10.0); + declare_parameter("attempt_respawn_reconnection", true); node_names_ = get_parameter("node_names").as_string_array(); get_parameter("autostart", autostart_); + double bond_timeout_s; + get_parameter("bond_timeout", bond_timeout_s); + bond_timeout_ = std::chrono::duration_cast( + std::chrono::duration(bond_timeout_s)); + double respawn_timeout_s; + get_parameter("bond_respawn_max_duration", respawn_timeout_s); + bond_respawn_max_duration_ = rclcpp::Duration::from_seconds(respawn_timeout_s); + + get_parameter("attempt_respawn_reconnection", attempt_respawn_reconnection_); + + callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); manager_srv_ = create_service( get_name() + std::string("/manage_nodes"), - std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3)); + std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3), + rmw_qos_profile_services_default, + callback_group_); is_active_srv_ = create_service( get_name() + std::string("/is_active"), - std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3)); - - auto options = rclcpp::NodeOptions().arguments( - {"--ros-args", "-r", std::string("__node:=") + get_name() + "_service_client", "--"}); - service_client_node_ = std::make_shared("_", options); + std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3), + rmw_qos_profile_services_default, + callback_group_); transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE; transition_state_map_[Transition::TRANSITION_CLEANUP] = State::PRIMARY_STATE_UNCONFIGURED; @@ -70,16 +85,32 @@ LifecycleManager::LifecycleManager() transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] = std::string("Shutting down "); - createLifecycleServiceClients(); - - if (autostart_) { - startup(); - } + init_timer_ = this->create_wall_timer( + 0s, + [this]() -> void { + init_timer_->cancel(); + createLifecycleServiceClients(); + if (autostart_) { + init_timer_ = this->create_wall_timer( + 0s, + [this]() -> void { + init_timer_->cancel(); + startup(); + }, + callback_group_); + } + auto executor = std::make_shared(); + executor->add_callback_group(callback_group_, get_node_base_interface()); + service_thread_ = std::make_unique(executor); + }); + diagnostics_updater_.setHardwareID("Nav2"); + diagnostics_updater_.add("Nav2 Health", this, &LifecycleManager::CreateActiveDiagnostic); } LifecycleManager::~LifecycleManager() { - RCLCPP_INFO(get_logger(), "Destroying"); + RCLCPP_INFO(get_logger(), "Destroying %s", get_name()); + service_thread_.reset(); } void @@ -116,13 +147,23 @@ LifecycleManager::isActiveCallback( response->success = system_active_; } +void +LifecycleManager::CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (system_active_) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Nav2 is active"); + } else { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Nav2 is inactive"); + } +} + void 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()); } } @@ -135,10 +176,41 @@ LifecycleManager::destroyLifecycleServiceClients() } } +bool +LifecycleManager::createBondConnection(const std::string & node_name) +{ + const double timeout_ns = + std::chrono::duration_cast(bond_timeout_).count(); + const double timeout_s = timeout_ns / 1e9; + + if (bond_map_.find(node_name) == bond_map_.end() && bond_timeout_.count() > 0.0) { + bond_map_[node_name] = + std::make_shared("bond", node_name, shared_from_this()); + bond_map_[node_name]->setHeartbeatTimeout(timeout_s); + bond_map_[node_name]->setHeartbeatPeriod(0.10); + bond_map_[node_name]->start(); + if ( + !bond_map_[node_name]->waitUntilFormed( + rclcpp::Duration(rclcpp::Duration::from_nanoseconds(timeout_ns / 2)))) + { + RCLCPP_ERROR( + get_logger(), + "Server %s was unable to be reached after %0.2fs by bond. " + "This server may be misconfigured.", + node_name.c_str(), timeout_s); + return false; + } + RCLCPP_INFO(get_logger(), "Server %s connected with bond.", node_name.c_str()); + } + + return true; +} + bool LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t transition) { message(transition_label_map_[transition] + node_name); + if (!node_map_[node_name]->change_state(transition) || !(node_map_[node_name]->get_state() == transition_state_map_[transition])) { @@ -146,24 +218,45 @@ LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t return false; } + if (transition == Transition::TRANSITION_ACTIVATE) { + return createBondConnection(node_name); + } else if (transition == Transition::TRANSITION_DEACTIVATE) { + bond_map_.erase(node_name); + } + return true; } bool -LifecycleManager::changeStateForAllNodes(std::uint8_t transition) +LifecycleManager::changeStateForAllNodes(std::uint8_t transition, bool hard_change) { + // Hard change will continue even if a node fails if (transition == Transition::TRANSITION_CONFIGURE || transition == Transition::TRANSITION_ACTIVATE) { for (auto & node_name : node_names_) { - if (!changeStateForNode(node_name, transition)) { + try { + if (!changeStateForNode(node_name, transition) && !hard_change) { + return false; + } + } catch (const std::runtime_error & e) { + RCLCPP_ERROR( + get_logger(), + "Failed to change state for node: %s. Exception: %s.", node_name.c_str(), e.what()); return false; } } } else { std::vector::reverse_iterator rit; for (rit = node_names_.rbegin(); rit != node_names_.rend(); ++rit) { - if (!changeStateForNode(*rit, transition)) { + try { + if (!changeStateForNode(*rit, transition) && !hard_change) { + return false; + } + } catch (const std::runtime_error & e) { + RCLCPP_ERROR( + get_logger(), + "Failed to change state for node: %s. Exception: %s.", (*rit).c_str(), e.what()); return false; } } @@ -192,46 +285,57 @@ LifecycleManager::startup() } message("Managed nodes are active"); system_active_ = true; + createBondTimer(); return true; } bool LifecycleManager::shutdown() { + system_active_ = false; + destroyBondTimer(); + message("Shutting down managed nodes..."); shutdownAllNodes(); destroyLifecycleServiceClients(); message("Managed nodes have been shut down"); - system_active_ = false; return true; } bool -LifecycleManager::reset() +LifecycleManager::reset(bool hard_reset) { + system_active_ = false; + destroyBondTimer(); + message("Resetting managed nodes..."); // Should transition in reverse order - if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE) || - !changeStateForAllNodes(Transition::TRANSITION_CLEANUP)) + if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE, hard_reset) || + !changeStateForAllNodes(Transition::TRANSITION_CLEANUP, hard_reset)) { - RCLCPP_ERROR(get_logger(), "Failed to reset nodes: aborting reset"); - return false; + if (!hard_reset) { + RCLCPP_ERROR(get_logger(), "Failed to reset nodes: aborting reset"); + return false; + } } + message("Managed nodes have been reset"); - system_active_ = false; return true; } bool LifecycleManager::pause() { + system_active_ = false; + destroyBondTimer(); + message("Pausing managed nodes..."); if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE)) { RCLCPP_ERROR(get_logger(), "Failed to pause nodes: aborting pause"); return false; } + message("Managed nodes have been paused"); - system_active_ = false; return true; } @@ -243,14 +347,122 @@ LifecycleManager::resume() RCLCPP_ERROR(get_logger(), "Failed to resume nodes: aborting resume"); return false; } + message("Managed nodes are active"); system_active_ = true; + createBondTimer(); return true; } -// TODO(mjeronimo): This is used to emphasize the major events during system bring-up and -// shutdown so that the messgaes can be easily seen among the log output. We should replace -// this with a ROS2-supported way of highlighting console output, if possible. +void +LifecycleManager::createBondTimer() +{ + if (bond_timeout_.count() <= 0) { + return; + } + + message("Creating bond timer..."); + bond_timer_ = this->create_wall_timer( + 200ms, + std::bind(&LifecycleManager::checkBondConnections, this), + callback_group_); +} + +void +LifecycleManager::destroyBondTimer() +{ + if (bond_timer_) { + message("Terminating bond timer..."); + bond_timer_->cancel(); + bond_timer_.reset(); + } +} + +void +LifecycleManager::checkBondConnections() +{ + if (!system_active_ || !rclcpp::ok() || bond_map_.empty()) { + return; + } + + for (auto & node_name : node_names_) { + if (!rclcpp::ok()) { + return; + } + + if (bond_map_[node_name]->isBroken()) { + message( + std::string( + "Have not received a heartbeat from " + node_name + ".")); + + // if one is down, bring them all down + RCLCPP_ERROR( + get_logger(), + "CRITICAL FAILURE: SERVER %s IS DOWN after not receiving a heartbeat for %i ms." + " Shutting down related nodes.", + node_name.c_str(), static_cast(bond_timeout_.count())); + reset(true); // hard reset to transition all still active down + // if a server crashed, it won't get cleared due to failed transition, clear manually + bond_map_.clear(); + + // Initialize the bond respawn timer to check if server comes back online + // after a failure, within a maximum timeout period. + if (attempt_respawn_reconnection_) { + bond_respawn_timer_ = this->create_wall_timer( + 1s, + std::bind(&LifecycleManager::checkBondRespawnConnection, this), + callback_group_); + } + return; + } + } +} + +void +LifecycleManager::checkBondRespawnConnection() +{ + // First attempt in respawn, start maximum duration to respawn + if (bond_respawn_start_time_.nanoseconds() == 0) { + bond_respawn_start_time_ = now(); + } + + // Note: system_active_ is inverted since this should be in a failure + // condition. If another outside user actives the system again, this should not process. + if (system_active_ || !rclcpp::ok() || node_names_.empty()) { + bond_respawn_start_time_ = rclcpp::Time(0); + bond_respawn_timer_.reset(); + return; + } + + // Check number of live connections after a bond failure + int live_servers = 0; + const int max_live_servers = node_names_.size(); + for (auto & node_name : node_names_) { + if (!rclcpp::ok()) { + return; + } + + try { + node_map_[node_name]->get_state(); // Only won't throw if the server exists + live_servers++; + } catch (...) { + break; + } + } + + // If all are alive, kill timer and retransition system to active + // Else, check if maximum timeout has occurred + if (live_servers == max_live_servers) { + message("Successfully re-established connections from server respawns, starting back up."); + bond_respawn_start_time_ = rclcpp::Time(0); + bond_respawn_timer_.reset(); + startup(); + } else if (now() - bond_respawn_start_time_ >= bond_respawn_max_duration_) { + message("Failed to re-establish connection from a server crash after maximum timeout."); + bond_respawn_start_time_ = rclcpp::Time(0); + bond_respawn_timer_.reset(); + } +} #define ANSI_COLOR_RESET "\x1b[0m" #define ANSI_COLOR_BLUE "\x1b[34m" @@ -262,3 +474,6 @@ LifecycleManager::message(const std::string & msg) } } // namespace nav2_lifecycle_manager + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_lifecycle_manager::LifecycleManager) diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index c7931539f7..632eb1aa8e 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -19,24 +19,28 @@ #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, + std::shared_ptr parent_node) { 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"); + // Use parent node for service call and logging + node_ = parent_node; // 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_); } bool @@ -73,27 +77,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 +110,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_lifecycle_manager/src/main.cpp b/nav2_lifecycle_manager/src/main.cpp index a07cfa4235..94268ae2f3 100644 --- a/nav2_lifecycle_manager/src/main.cpp +++ b/nav2_lifecycle_manager/src/main.cpp @@ -21,7 +21,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); - rclcpp::spin(node->get_node_base_interface()); + rclcpp::spin(node); rclcpp::shutdown(); return 0; diff --git a/nav2_lifecycle_manager/test/CMakeLists.txt b/nav2_lifecycle_manager/test/CMakeLists.txt index e931a5ce19..96cbaa5f7a 100644 --- a/nav2_lifecycle_manager/test/CMakeLists.txt +++ b/nav2_lifecycle_manager/test/CMakeLists.txt @@ -14,7 +14,28 @@ ament_add_test(test_lifecycle GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_lifecycle_test.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 180 + TIMEOUT 20 ENV TEST_EXECUTABLE=$ ) + +ament_add_gtest_executable(test_bond_gtest + test_bond.cpp +) + +target_link_libraries(test_bond_gtest + ${library_name} +) + +ament_target_dependencies(test_bond_gtest + ${dependencies} +) + +ament_add_test(test_bond + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_bond_test.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 20 + ENV + TEST_EXECUTABLE=$ +) diff --git a/nav2_lifecycle_manager/test/launch_bond_test.py b/nav2_lifecycle_manager/test/launch_bond_test.py new file mode 100755 index 0000000000..061375db45 --- /dev/null +++ b/nav2_lifecycle_manager/test/launch_bond_test.py @@ -0,0 +1,57 @@ +#! /usr/bin/env python3 +# 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. + +import os +import sys + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_test', + output='screen', + parameters=[{'use_sim_time': False}, + {'autostart': False}, + {'node_names': ['bond_tester']}]), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_bond_gtest', + 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_lifecycle_manager/test/launch_lifecycle_test.py b/nav2_lifecycle_manager/test/launch_lifecycle_test.py index 822a442357..c77d58d76a 100755 --- a/nav2_lifecycle_manager/test/launch_lifecycle_test.py +++ b/nav2_lifecycle_manager/test/launch_lifecycle_test.py @@ -32,6 +32,7 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': False}, {'autostart': False}, + {'bond_timeout': 0.0}, {'node_names': ['lifecycle_node_test']}]), ]) diff --git a/nav2_lifecycle_manager/test/test_bond.cpp b/nav2_lifecycle_manager/test/test_bond.cpp new file mode 100644 index 0000000000..38c772e6b1 --- /dev/null +++ b/nav2_lifecycle_manager/test/test_bond.cpp @@ -0,0 +1,199 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_lifecycle_manager/lifecycle_manager.hpp" +#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +// minimal lifecycle node implementing bond as in rest of navigation servers +class TestLifecycleNode : public nav2_util::LifecycleNode +{ +public: + TestLifecycleNode(bool bond, std::string name) + : nav2_util::LifecycleNode(name) + { + state = ""; + enable_bond = bond; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Configured!"); + state = "configured"; + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Activated!"); + state = "activated"; + if (enable_bond) { + createBond(); + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Deactivated!"); + state = "deactivated"; + if (enable_bond) { + destroyBond(); + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Cleanup!"); + state = "cleaned up"; + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Shutdown!"); + state = "shut down"; + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is encountered an error!"); + state = "errored"; + return CallbackReturn::SUCCESS; + } + + bool bondAllocated() + { + return bond_ ? true : false; + } + + void breakBond() + { + bond_->breakBond(); + } + + std::string getState() + { + return state; + } + + bool isBondEnabled() + { + return enable_bond; + } + + bool isBondConnected() + { + return bondAllocated() ? !bond_->isBroken() : false; + } + + std::string state; + bool enable_bond; +}; + +class TestFixture +{ +public: + TestFixture(bool bond, std::string node_name) + { + lf_node_ = std::make_shared(bond, node_name); + lf_thread_ = std::make_unique(lf_node_->get_node_base_interface()); + } + + std::shared_ptr lf_node_; + std::unique_ptr lf_thread_; +}; + +TEST(LifecycleBondTest, POSITIVE) +{ + // let the lifecycle server come up + rclcpp::Rate(1).sleep(); + + auto node = std::make_shared("lifecycle_manager_test_service_client"); + nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node); + + // create node, should be up now + auto fixture = TestFixture(true, "bond_tester"); + auto bond_tester = fixture.lf_node_; + + EXPECT_TRUE(client.startup()); + + // check if bond is connected after being activated + rclcpp::Rate(5).sleep(); + EXPECT_TRUE(bond_tester->isBondConnected()); + EXPECT_EQ(bond_tester->getState(), "activated"); + + bond_tester->breakBond(); + + // bond should be disconnected now and lifecycle manager should know and react to reset + rclcpp::Rate(5).sleep(); + EXPECT_EQ( + nav2_lifecycle_manager::SystemStatus::INACTIVE, + client.is_active(std::chrono::nanoseconds(1000000000))); + EXPECT_FALSE(bond_tester->isBondConnected()); + EXPECT_EQ(bond_tester->getState(), "cleaned up"); + + // check that bringing up again is OK + EXPECT_TRUE(client.startup()); + EXPECT_EQ(bond_tester->getState(), "activated"); + EXPECT_TRUE(bond_tester->isBondConnected()); + EXPECT_EQ( + nav2_lifecycle_manager::SystemStatus::ACTIVE, + client.is_active(std::chrono::nanoseconds(1000000000))); + + // clean state for next test. + EXPECT_TRUE(client.reset()); + EXPECT_FALSE(bond_tester->isBondConnected()); + EXPECT_EQ(bond_tester->getState(), "cleaned up"); +} + +TEST(LifecycleBondTest, NEGATIVE) +{ + auto node = std::make_shared("lifecycle_manager_test_service_client"); + nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node); + + // create node, now without bond setup to connect to. Should fail because no bond + auto fixture = TestFixture(false, "bond_tester"); + auto bond_tester = fixture.lf_node_; + EXPECT_FALSE(client.startup()); + EXPECT_FALSE(bond_tester->isBondEnabled()); + EXPECT_EQ( + nav2_lifecycle_manager::SystemStatus::INACTIVE, + client.is_active(std::chrono::nanoseconds(1000000000))); +} + +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_lifecycle_manager/test/test_lifecycle_manager.cpp b/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp index 983a34ee64..96114f98a5 100644 --- a/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp @@ -82,11 +82,12 @@ class LifecycleClientTestFixture TEST(LifecycleClientTest, BasicTest) { LifecycleClientTestFixture fix; - nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test"); - EXPECT_TRUE(client.startup()); + auto node = std::make_shared("lifecycle_manager_test_service_client"); + nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node); EXPECT_EQ( nav2_lifecycle_manager::SystemStatus::TIMEOUT, client.is_active(std::chrono::nanoseconds(1000))); + EXPECT_TRUE(client.startup()); EXPECT_EQ( nav2_lifecycle_manager::SystemStatus::ACTIVE, client.is_active(std::chrono::nanoseconds(1000000000))); @@ -104,7 +105,8 @@ TEST(LifecycleClientTest, BasicTest) TEST(LifecycleClientTest, WithoutFixture) { - nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test"); + auto node = std::make_shared("lifecycle_manager_test_service_client"); + nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test", node); EXPECT_EQ( nav2_lifecycle_manager::SystemStatus::TIMEOUT, client.is_active(std::chrono::nanoseconds(1000))); diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 9243742486..c24e5e5c4d 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(nav_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) @@ -25,6 +26,8 @@ set(map_saver_cli_executable map_saver_cli) set(map_saver_server_executable map_saver_server) +set(costmap_filter_info_server_executable costmap_filter_info_server) + add_executable(${map_server_executable} src/map_server/main.cpp) @@ -34,6 +37,9 @@ add_executable(${map_saver_cli_executable} add_executable(${map_saver_server_executable} src/map_saver/main_server.cpp) +add_executable(${costmap_filter_info_server_executable} + src/costmap_filter_info/main.cpp) + set(map_io_library_name map_io) set(library_name ${map_server_executable}_core) @@ -44,7 +50,8 @@ add_library(${map_io_library_name} SHARED add_library(${library_name} SHARED src/map_server/map_server.cpp - src/map_saver/map_saver.cpp) + src/map_saver/map_saver.cpp + src/costmap_filter_info/costmap_filter_info_server.cpp) set(map_io_dependencies yaml_cpp_vendor @@ -55,6 +62,7 @@ set(map_io_dependencies set(map_server_dependencies rclcpp rclcpp_lifecycle + rclcpp_components nav_msgs nav2_msgs yaml_cpp_vendor @@ -63,6 +71,7 @@ set(map_server_dependencies set(map_saver_dependencies rclcpp + rclcpp_lifecycle nav_msgs nav2_msgs nav2_util) @@ -76,6 +85,9 @@ ament_target_dependencies(${map_saver_cli_executable} ament_target_dependencies(${map_saver_server_executable} ${map_saver_dependencies}) +ament_target_dependencies(${costmap_filter_info_server_executable} + ${map_saver_dependencies}) + ament_target_dependencies(${library_name} ${map_server_dependencies}) @@ -99,6 +111,9 @@ target_link_libraries(${map_saver_cli_executable} target_link_libraries(${map_saver_server_executable} ${library_name}) +target_link_libraries(${costmap_filter_info_server_executable} + ${library_name}) + target_include_directories(${map_io_library_name} SYSTEM PRIVATE ${GRAPHICSMAGICKCPP_INCLUDE_DIRS}) @@ -110,6 +125,9 @@ if(WIN32) YAML_CPP_DLL) endif() +rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapSaver") +rclcpp_components_register_nodes(${library_name} "nav2_map_server::MapServer") + install(TARGETS ${library_name} ${map_io_library_name} ARCHIVE DESTINATION lib @@ -118,6 +136,7 @@ install(TARGETS install(TARGETS ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable} + ${costmap_filter_info_server_executable} RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ @@ -140,4 +159,5 @@ ament_export_libraries( ${library_name} ${map_io_library_name} ) +ament_export_dependencies(${map_io_dependencies} ${map_server_dependencies}) ament_package() diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index 8a81a2ddc8..64ef980be9 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -1,15 +1,9 @@ # Map Server -The `Map Server` provides maps to the rest of the Navigation2 system using both topic and -service interfaces. +The `Map Server` provides maps to the rest of the Nav2 system using both topic and +service interfaces. Map server will expose maps on the node bringup, but can also change maps using a `load_map` service during run-time, as well as save maps using a `save_map` server. -## Changes from ROS1 Navigation Map Server - -While the nav2 map server provides the same general function as the nav1 map server, the new -code has some changes to accomodate ROS2 as well as some architectural improvements. - -In addition, there is now two new "load_map" and "save_map" services which can be used to -dynamically load and save a map. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-map-server.html) for additional parameter descriptions. ### Architecture @@ -62,7 +56,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/include/nav2_map_server/costmap_filter_info_server.hpp b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp new file mode 100644 index 0000000000..99311deff7 --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp @@ -0,0 +1,79 @@ +// Copyright (c) 2020 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. + +#ifndef NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_ +#define NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" + +namespace nav2_map_server +{ + +class CostmapFilterInfoServer : public nav2_util::LifecycleNode +{ +public: + /** + * @brief Constructor for the nav2_map_server::CostmapFilterInfoServer + */ + CostmapFilterInfoServer(); + /** + * @brief Destructor for the nav2_map_server::CostmapFilterInfoServer + */ + ~CostmapFilterInfoServer(); + +protected: + /** + * @brief Creates CostmapFilterInfo publisher and forms published message from ROS parameters + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief Publishes a CostmapFilterInfo message + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Deactivates publisher + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Resets publisher + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called when in Shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + +private: + rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; + + std::unique_ptr msg_; +}; // CostmapFilterInfoServer + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index 9c9b57d292..6863844c7b 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -37,8 +37,9 @@ class MapSaver : public nav2_util::LifecycleNode public: /** * @brief Constructor for the nav2_map_server::MapSaver + * @param options Additional options to control creation of the node. */ - MapSaver(); + explicit MapSaver(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Destructor for the nav2_map_server::MapServer diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index 700b1753ea..20ab257c4e 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -38,8 +38,9 @@ class MapServer : public nav2_util::LifecycleNode public: /** * @brief A constructor for nav2_map_server::MapServer + * @param options Additional options to control creation of the node. */ - MapServer(); + explicit MapServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief A Destructor for nav2_map_server::MapServer diff --git a/nav2_map_server/launch/map_saver_server.launch.py b/nav2_map_server/launch/map_saver_server.launch.py index 094c171a65..53731569bb 100644 --- a/nav2_map_server/launch/map_saver_server.launch.py +++ b/nav2_map_server/launch/map_saver_server.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): lifecycle_nodes = ['map_saver'] use_sim_time = True autostart = True - save_map_timeout = 2000 + save_map_timeout = 2.0 free_thresh_default = 0.25 occupied_thresh_default = 0.65 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index c66428fb35..b719f972a3 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.7 + 1.1.0 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp new file mode 100644 index 0000000000..7b1c618315 --- /dev/null +++ b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp @@ -0,0 +1,107 @@ +// Copyright (c) 2020 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. + +// TODO(AlexeyMerzlyakov): This dummy info publisher should be removed +// after Semantic Map Server having the same functionality will be developed. + +#include "nav2_map_server/costmap_filter_info_server.hpp" + +#include +#include +#include + +namespace nav2_map_server +{ + +CostmapFilterInfoServer::CostmapFilterInfoServer() +: nav2_util::LifecycleNode("costmap_filter_info_server") +{ + declare_parameter("filter_info_topic", "costmap_filter_info"); + declare_parameter("type", 0); + declare_parameter("mask_topic", "filter_mask"); + declare_parameter("base", 0.0); + declare_parameter("multiplier", 1.0); +} + +CostmapFilterInfoServer::~CostmapFilterInfoServer() +{ +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + std::string filter_info_topic = get_parameter("filter_info_topic").as_string(); + + publisher_ = this->create_publisher( + filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + msg_ = std::make_unique(); + msg_->header.frame_id = ""; + msg_->header.stamp = now(); + msg_->type = get_parameter("type").as_int(); + msg_->filter_mask_topic = get_parameter("mask_topic").as_string(); + msg_->base = static_cast(get_parameter("base").as_double()); + msg_->multiplier = static_cast(get_parameter("multiplier").as_double()); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + publisher_->on_activate(); + publisher_->publish(std::move(msg_)); + + // create bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + publisher_->on_deactivate(); + + // destroy bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + publisher_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + + return nav2_util::CallbackReturn::SUCCESS; +} + +} // namespace nav2_map_server diff --git a/nav2_map_server/src/costmap_filter_info/main.cpp b/nav2_map_server/src/costmap_filter_info/main.cpp new file mode 100644 index 0000000000..e7b48f613e --- /dev/null +++ b/nav2_map_server/src/costmap_filter_info/main.cpp @@ -0,0 +1,32 @@ +// Copyright (c) 2020 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. + +#include + +#include "nav2_map_server/costmap_filter_info_server.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char * argv[]) +{ + auto logger = rclcpp::get_logger("costmap_filter_info_server"); + + RCLCPP_INFO(logger, "This is costmap filter info publisher"); + + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 73aa121fff..95f49ae788 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -46,6 +46,7 @@ #include "yaml-cpp/yaml.h" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/LinearMath/Quaternion.h" +#include "nav2_util/occ_grid_values.hpp" #ifdef _WIN32 // https://github.com/rtv/Stage/blob/master/replace/dirname.c @@ -220,20 +221,20 @@ void loadMapFromFile( switch (load_parameters.mode) { case MapMode::Trinary: if (load_parameters.occupied_thresh < occ) { - map_cell = 100; + map_cell = nav2_util::OCC_GRID_OCCUPIED; } else if (occ < load_parameters.free_thresh) { - map_cell = 0; + map_cell = nav2_util::OCC_GRID_FREE; } else { - map_cell = -1; + map_cell = nav2_util::OCC_GRID_UNKNOWN; } break; case MapMode::Scale: if (pixel.alphaQuantum() != OpaqueOpacity) { - map_cell = -1; + map_cell = nav2_util::OCC_GRID_UNKNOWN; } else if (load_parameters.occupied_thresh < occ) { - map_cell = 100; + map_cell = nav2_util::OCC_GRID_OCCUPIED; } else if (occ < load_parameters.free_thresh) { - map_cell = 0; + map_cell = nav2_util::OCC_GRID_FREE; } else { map_cell = std::rint( (occ - load_parameters.free_thresh) / @@ -242,10 +243,12 @@ void loadMapFromFile( break; case MapMode::Raw: { double occ_percent = std::round(shade * 255); - if (0 <= occ_percent && occ_percent <= 100) { + if (nav2_util::OCC_GRID_FREE <= occ_percent && + occ_percent <= nav2_util::OCC_GRID_OCCUPIED) + { map_cell = static_cast(occ_percent); } else { - map_cell = -1; + map_cell = nav2_util::OCC_GRID_UNKNOWN; } break; } diff --git a/nav2_map_server/src/map_saver/main_cli.cpp b/nav2_map_server/src/map_saver/main_cli.cpp index 89c95e057e..09cf039f44 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_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 563e0d5290..d79005a567 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -35,28 +35,27 @@ #include #include #include +#include using namespace std::placeholders; namespace nav2_map_server { -MapSaver::MapSaver() -: nav2_util::LifecycleNode("map_saver", "", true) +MapSaver::MapSaver(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("map_saver", "", options) { RCLCPP_INFO(get_logger(), "Creating"); save_map_timeout_ = std::make_shared( - std::chrono::milliseconds(declare_parameter("save_map_timeout", 2000))); + rclcpp::Duration::from_seconds(declare_parameter("save_map_timeout", 2.0))); free_thresh_default_ = declare_parameter("free_thresh_default", 0.25), occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65); - // false only of foxy for backwards compatibility - map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", false); + map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", true); } MapSaver::~MapSaver() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn @@ -79,6 +78,10 @@ nav2_util::CallbackReturn MapSaver::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); + + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -86,6 +89,10 @@ nav2_util::CallbackReturn MapSaver::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); + + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -93,6 +100,9 @@ nav2_util::CallbackReturn MapSaver::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); + + save_map_service_.reset(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -139,9 +149,6 @@ bool MapSaver::saveMapTopicToFile( map_topic_loc.c_str(), save_parameters_loc.map_file_name.c_str()); try { - // Pointer to map message received in the subscription callback - nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = nullptr; - // Correct map_topic_loc if necessary if (map_topic_loc == "") { map_topic_loc = "map"; @@ -166,44 +173,51 @@ bool MapSaver::saveMapTopicToFile( save_parameters_loc.occupied_thresh = occupied_thresh_default_; } + std::promise prom; + std::future future_result = prom.get_future(); // A callback function that receives map message from subscribed topic - auto mapCallback = [&map_msg]( + auto mapCallback = [&prom]( const nav_msgs::msg::OccupancyGrid::SharedPtr msg) -> void { - map_msg = msg; + prom.set_value(msg); }; - // Add new subscription for incoming map topic. - // Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode - // as a map listener. - rclcpp::QoS map_qos = rclcpp::SystemDefaultsQoS(); // initialize to default + rclcpp::QoS map_qos(10); // initialize to default if (map_subscribe_transient_local_) { - map_qos = rclcpp::QoS(10); map_qos.transient_local(); map_qos.reliable(); map_qos.keep_last(1); } - auto map_sub = rclcpp_node_->create_subscription( - map_topic_loc, map_qos, mapCallback); - - rclcpp::Time start_time = now(); - while (rclcpp::ok()) { - if ((now() - start_time) > *save_map_timeout_) { - RCLCPP_ERROR(get_logger(), "Failed to save the map: timeout"); - return false; - } - - if (map_msg) { - // Map message received. Saving it to file - if (saveMapToFile(*map_msg, save_parameters_loc)) { - RCLCPP_INFO(get_logger(), "Map saved successfully"); - return true; - } else { - RCLCPP_ERROR(get_logger(), "Failed to save the map"); - return false; - } - } - - rclcpp::sleep_for(std::chrono::milliseconds(100)); + + // Create new CallbackGroup for map_sub + auto callback_group = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + + auto option = rclcpp::SubscriptionOptions(); + option.callback_group = callback_group; + auto map_sub = create_subscription( + map_topic_loc, map_qos, mapCallback, option); + + // Create SingleThreadedExecutor to spin map_sub in callback_group + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_callback_group(callback_group, get_node_base_interface()); + // Spin until map message received + auto timeout = save_map_timeout_->to_chrono(); + auto status = executor.spin_until_future_complete(future_result, timeout); + if (status != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Failed to spin map subscription"); + return false; + } + // map_sub is no more needed + map_sub.reset(); + // Map message received. Saving it to file + nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = future_result.get(); + if (saveMapToFile(*map_msg, save_parameters_loc)) { + RCLCPP_INFO(get_logger(), "Map saved successfully"); + return true; + } else { + RCLCPP_ERROR(get_logger(), "Failed to save the map"); + return false; } } catch (std::exception & e) { RCLCPP_ERROR(get_logger(), "Failed to save the map: %s", e.what()); @@ -214,3 +228,10 @@ bool MapSaver::saveMapTopicToFile( } } // namespace nav2_map_server + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::MapSaver) diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index d633af10a3..5920e095be 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -62,20 +62,19 @@ using namespace std::placeholders; namespace nav2_map_server { -MapServer::MapServer() -: nav2_util::LifecycleNode("map_server") +MapServer::MapServer(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("map_server", "", options) { 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"); } MapServer::~MapServer() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn @@ -129,6 +128,9 @@ MapServer::on_activate(const rclcpp_lifecycle::State & /*state*/) auto occ_grid = std::make_unique(msg_); occ_pub_->publish(std::move(occ_grid)); + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -139,6 +141,9 @@ MapServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) occ_pub_->on_deactivate(); + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -230,3 +235,10 @@ void MapServer::updateMsgHeader() } } // namespace nav2_map_server + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_map_server::MapServer) diff --git a/nav2_map_server/test/component/test_map_saver_node.cpp b/nav2_map_server/test/component/test_map_saver_node.cpp index 864ca4c3e3..909675ae8a 100644 --- a/nav2_map_server/test/component/test_map_saver_node.cpp +++ b/nav2_map_server/test/component/test_map_saver_node.cpp @@ -13,11 +13,14 @@ // See the License for the specific language governing permissions and // limitations under the License. + #include -#include -#include + #include #include +#include // NOLINT + +#include "rclcpp/rclcpp.hpp" #include "test_constants/test_constants.h" #include "nav2_map_server/map_saver.hpp" @@ -59,6 +62,8 @@ class MapSaverTestFixture : public ::testing::Test { lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE); lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP); + lifecycle_client_.reset(); + node_.reset(); } template 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 c08107f72f..9e69672cde 100644 --- a/nav2_map_server/test/component/test_map_saver_publisher.cpp +++ b/nav2_map_server/test/component/test_map_saver_publisher.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include "rclcpp/rclcpp.hpp" #include "nav2_map_server/map_io.hpp" @@ -23,7 +22,6 @@ #define TEST_DIR TEST_DIRECTORY -using namespace std::chrono_literals; using namespace nav2_map_server; // NOLINT using std::experimental::filesystem::path; @@ -34,7 +32,8 @@ class TestPublisher : public rclcpp::Node : Node("map_publisher") { std::string pub_map_file = path(TEST_DIR) / path(g_valid_yaml_file); - LOAD_MAP_STATUS status = loadMapFromYaml(pub_map_file, msg_); + 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.c_str()); return; @@ -43,19 +42,11 @@ class TestPublisher : public rclcpp::Node map_pub_ = create_publisher( "map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - - timer_ = create_wall_timer(300ms, std::bind(&TestPublisher::mapPublishCallback, this)); + map_pub_->publish(msg); } protected: - void mapPublishCallback() - { - map_pub_->publish(msg_); - } - rclcpp::Publisher::SharedPtr map_pub_; - rclcpp::TimerBase::SharedPtr timer_; - nav_msgs::msg::OccupancyGrid msg_; }; int main(int argc, char ** argv) diff --git a/nav2_map_server/test/component/test_map_server_node.cpp b/nav2_map_server/test/component/test_map_server_node.cpp index 6fa14d9b98..d034ca3c68 100644 --- a/nav2_map_server/test/component/test_map_server_node.cpp +++ b/nav2_map_server/test/component/test_map_server_node.cpp @@ -13,10 +13,12 @@ // limitations under the License. #include -#include -#include + #include #include +#include // NOLINT + +#include #include "test_constants/test_constants.h" #include "nav2_map_server/map_server.hpp" @@ -59,6 +61,8 @@ class MapServerTestFixture : public ::testing::Test lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE); lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP); lifecycle_client_->change_state(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + lifecycle_client_.reset(); + node_.reset(); } template diff --git a/nav2_map_server/test/map_saver_params.yaml b/nav2_map_server/test/map_saver_params.yaml index 25d6b88b2c..c36361266d 100644 --- a/nav2_map_server/test/map_saver_params.yaml +++ b/nav2_map_server/test/map_saver_params.yaml @@ -1,5 +1,5 @@ map_saver: ros__parameters: - save_map_timeout: 1000 + save_map_timeout: 5.0 free_thresh_default: 0.196 occupied_thresh_default: 0.65 diff --git a/nav2_map_server/test/test_launch_files/map_saver_node.launch.py b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py index 9c061439c7..7287f73594 100644 --- a/nav2_map_server/test/test_launch_files/map_saver_node.launch.py +++ b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): - map_publisher = os.path.dirname(os.getenv('TEST_EXECUTABLE')) + '/test_map_saver_publisher' + map_publisher = f"{os.path.dirname(os.getenv('TEST_EXECUTABLE'))}/test_map_saver_publisher" ld = LaunchDescription() diff --git a/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt index e0326d462c..095a6509ae 100644 --- a/nav2_map_server/test/unit/CMakeLists.txt +++ b/nav2_map_server/test/unit/CMakeLists.txt @@ -9,3 +9,14 @@ target_link_libraries(test_map_io ${map_io_library_name} stdc++fs ) + +# costmap_filter_info_server unit test +ament_add_gtest(test_costmap_filter_info_server + test_costmap_filter_info_server.cpp +) + +ament_target_dependencies(test_costmap_filter_info_server rclcpp) + +target_link_libraries(test_costmap_filter_info_server + ${library_name} +) diff --git a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp new file mode 100644 index 0000000000..168fbc9f7b --- /dev/null +++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp @@ -0,0 +1,146 @@ +// Copyright (c) 2020 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. + +#include + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "nav2_map_server/costmap_filter_info_server.hpp" + +using namespace std::chrono_literals; + +typedef std::recursive_mutex mutex_t; + +static const char FILTER_INFO_TOPIC[] = "filter_info"; +static const int TYPE = 1; +static const char MASK_TOPIC[] = "mask"; +static const double BASE = 0.1; +static const double MULTIPLIER = 0.2; + +static const double EPSILON = std::numeric_limits::epsilon(); + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer +{ +public: + void start() + { + on_configure(get_current_state()); + on_activate(get_current_state()); + } + + void stop() + { + on_deactivate(get_current_state()); + on_cleanup(get_current_state()); + on_shutdown(get_current_state()); + } +}; + +class InfoServerTester : public ::testing::Test +{ +public: + InfoServerTester() + : info_server_(nullptr), info_(nullptr), subscription_(nullptr) + { + access_ = new mutex_t(); + + info_server_ = std::make_shared(); + try { + info_server_->set_parameter(rclcpp::Parameter("filter_info_topic", FILTER_INFO_TOPIC)); + info_server_->set_parameter(rclcpp::Parameter("type", TYPE)); + info_server_->set_parameter(rclcpp::Parameter("mask_topic", MASK_TOPIC)); + info_server_->set_parameter(rclcpp::Parameter("base", BASE)); + info_server_->set_parameter(rclcpp::Parameter("multiplier", MULTIPLIER)); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + info_server_->get_logger(), + "Error while setting parameters for CostmapFilterInfoServer: %s", ex.what()); + throw; + } + + info_server_->start(); + + subscription_ = info_server_->create_subscription( + FILTER_INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&InfoServerTester::infoCallback, this, std::placeholders::_1)); + } + + ~InfoServerTester() + { + info_server_->stop(); + info_server_.reset(); + subscription_.reset(); + } + + bool isReceived() + { + std::lock_guard guard(*getMutex()); + if (info_) { + return true; + } else { + return false; + } + } + + mutex_t * getMutex() + { + return access_; + } + +protected: + std::shared_ptr info_server_; + nav2_msgs::msg::CostmapFilterInfo::SharedPtr info_; + +private: + void infoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg) + { + std::lock_guard guard(*getMutex()); + info_ = msg; + } + + rclcpp::Subscription::SharedPtr subscription_; + + mutex_t * access_; +}; + +TEST_F(InfoServerTester, testCostmapFilterInfoPublish) +{ + rclcpp::Time start_time = info_server_->now(); + while (!isReceived()) { + rclcpp::spin_some(info_server_->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + // Waiting no more than 5 seconds + ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms)); + } + + // Checking received CostmapFilterInfo for consistency + EXPECT_EQ(info_->type, TYPE); + EXPECT_EQ(info_->filter_mask_topic, MASK_TOPIC); + EXPECT_NEAR(info_->base, BASE, EPSILON); + EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON); +} diff --git a/nav2_minimal.dockerfile b/nav2_minimal.dockerfile index 9616e754a7..baa0eac506 100644 --- a/nav2_minimal.dockerfile +++ b/nav2_minimal.dockerfile @@ -6,7 +6,7 @@ # docker build -t nav2:latest \ # --build-arg UNDERLAY_MIXINS \ # --build-arg OVERLAY_MIXINS ./ -ARG FROM_IMAGE=ros:foxy-ros-base +ARG FROM_IMAGE=ros:humble-ros-base # multi-stage for caching FROM $FROM_IMAGE AS cache @@ -27,16 +27,16 @@ COPY ./ src/navigation2 # copy manifests for caching WORKDIR /opt RUN find ./ -name "package.xml" | \ - xargs cp --parents -t /tmp + xargs cp --parents -t /tmp # multi-stage for building FROM $FROM_IMAGE AS build # install CI dependencies RUN apt-get update && apt-get install -q -y \ - ccache \ - lcov \ - && rm -rf /var/lib/apt/lists/* + ccache \ + lcov \ + && rm -rf /var/lib/apt/lists/* # copy underlay manifests ENV UNDERLAY_WS /opt/underlay_ws @@ -45,10 +45,10 @@ WORKDIR $UNDERLAY_WS # install underlay dependencies RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ - apt-get update && rosdep install -q -y \ - --from-paths src \ - --ignore-src \ - && rm -rf /var/lib/apt/lists/* + apt-get update && rosdep install -q -y \ + --from-paths src \ + --ignore-src \ + && rm -rf /var/lib/apt/lists/* # copy underlay source COPY --from=cache $UNDERLAY_WS ./ @@ -57,15 +57,15 @@ COPY --from=cache $UNDERLAY_WS ./ ARG UNDERLAY_MIXINS="release ccache" ARG FAIL_ON_BUILD_FAILURE=True RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ - colcon build \ - --symlink-install \ - --mixin \ - $UNDERLAY_MIXINS \ - --event-handlers console_direct+ \ - || touch build_failed && \ - if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \ - exit 1; \ - fi + colcon build \ + --symlink-install \ + --mixin \ + $UNDERLAY_MIXINS \ + --event-handlers console_direct+ \ + || touch build_failed && \ + if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \ + exit 1; \ + fi # copy overlay manifests ENV OVERLAY_WS /opt/overlay_ws @@ -73,13 +73,14 @@ COPY --from=cache /tmp/overlay_ws $OVERLAY_WS WORKDIR $OVERLAY_WS COPY ./minimal_repos.txt ./ +COPY ./nav2_pkgs.txt ./ # install overlay dependencies RUN . $UNDERLAY_WS/install/setup.sh && \ - apt-get update && rosdep install -q -y \ - --from-paths $(awk '$0="src/navigation2/"$0' ./minimal_repos.txt) \ - --ignore-src \ - && rm -rf /var/lib/apt/lists/* + apt-get update && rosdep install -q -y \ + --from-paths $(awk '$0="src/navigation2/"$0' ./minimal_repos.txt) \ + --ignore-src \ + && rm -rf /var/lib/apt/lists/* # copy overlay source COPY --from=cache $OVERLAY_WS ./ @@ -87,15 +88,15 @@ COPY --from=cache $OVERLAY_WS ./ # build overlay source ARG OVERLAY_MIXINS="release ccache" RUN . $UNDERLAY_WS/install/setup.sh && \ - colcon build \ - --symlink-install \ - --mixin \ - $OVERLAY_MIXINS \ - --packages-select $(cat ./minimal_repos.txt) nav_2d_msgs nav_2d_utils \ - || touch build_failed && \ - if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \ - exit 1; \ - fi + colcon build \ + --symlink-install \ + --mixin \ + $OVERLAY_MIXINS \ + --packages-select $(cat ./nav2_pkgs.txt) $(cat ./minimal_repos.txt) \ + || touch build_failed && \ + if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \ + exit 1; \ + fi # source overlay from entrypoint # RUN sed --in-place \ @@ -109,13 +110,11 @@ ENV OVERLAY_WS /opt/overlay_ws WORKDIR $OVERLAY_WS/src COPY debian/create_debians.sh $OVERLAY_WS/src/create_debians.sh -# COPY umd_mission/docker/debian/50-my-packages.list /etc/ros/rosdep/sources.list.d/50-my-packages.list -# COPY umd_mission/docker/debian/rosdep.yaml /rosdep.yaml -RUN . /opt/ros/foxy/setup.sh \ +RUN . /opt/ros/humble/setup.sh \ && . /opt/overlay_ws/install/setup.sh \ && . /opt/underlay_ws/install/setup.sh \ && ./create_debians.sh \ && rm -rf $OVERLAY_WS $UNDERLAY_WS -WORKDIR /opt/ros/foxy +WORKDIR /opt/ros/humble diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index c3891475b6..6cfe55f717 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -16,6 +16,8 @@ nav2_package() rosidl_generate_interfaces(${PROJECT_NAME} "msg/Costmap.msg" "msg/CostmapMetaData.msg" + "msg/CostmapFilterInfo.msg" + "msg/SpeedLimit.msg" "msg/VoxelGrid.msg" "msg/BehaviorTreeStatusChange.msg" "msg/BehaviorTreeLog.msg" @@ -23,6 +25,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ParticleCloud.msg" "msg/PathAndBoundary.msg" "srv/GetCostmap.srv" + "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" "srv/ClearCostmapAroundRobot.srv" "srv/ClearEntireCostmap.srv" @@ -31,11 +34,15 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SaveMap.srv" "action/BackUp.action" "action/ComputePathToPose.action" + "action/ComputePathThroughPoses.action" + "action/DriveOnHeading.action" + "action/SmoothPath.action" "action/FollowPath.action" "action/NavigateToPose.action" + "action/NavigateThroughPoses.action" "action/Wait.action" "action/Spin.action" - "action/DummyRecovery.action" + "action/DummyBehavior.action" "action/FollowWaypoints.action" "action/ComputePath.action" DEPENDENCIES builtin_interfaces geographic_msgs geometry_msgs std_msgs action_msgs nav_msgs diff --git a/nav2_msgs/README.md b/nav2_msgs/README.md index 0ca22a2340..868c049f0a 100644 --- a/nav2_msgs/README.md +++ b/nav2_msgs/README.md @@ -1,5 +1,3 @@ # 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. - -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. +The `nav2_msgs` package is a set of messages, services, and actions for the `Nav2` system. `Nav2` also makes use of the standard `nav_msgs`, where appropriate. diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action index 996eb83864..9937302578 100644 --- a/nav2_msgs/action/BackUp.action +++ b/nav2_msgs/action/BackUp.action @@ -1,8 +1,10 @@ #goal definition geometry_msgs/Point target float32 speed +builtin_interfaces/Duration time_allowance --- #result definition builtin_interfaces/Duration total_elapsed_time --- +#feedback definition float32 distance_traveled diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action new file mode 100644 index 0000000000..a1f609a9e0 --- /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 false, use current robot pose as path start, if true, use start above instead +--- +#result definition +nav_msgs/Path path +builtin_interfaces/Duration planning_time +--- +#feedback definition diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index add60a824d..a052c552ab 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,9 +1,11 @@ #goal definition -geometry_msgs/PoseStamped pose +geometry_msgs/PoseStamped goal +geometry_msgs/PoseStamped start string planner_id +bool use_start # If false, use current robot pose as path start, if true, use start above instead --- #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time --- -#feedback +#feedback definition diff --git a/nav2_msgs/action/DriveOnHeading.action b/nav2_msgs/action/DriveOnHeading.action new file mode 100644 index 0000000000..9937302578 --- /dev/null +++ b/nav2_msgs/action/DriveOnHeading.action @@ -0,0 +1,10 @@ +#goal definition +geometry_msgs/Point target +float32 speed +builtin_interfaces/Duration time_allowance +--- +#result definition +builtin_interfaces/Duration total_elapsed_time +--- +#feedback definition +float32 distance_traveled diff --git a/nav2_msgs/action/DummyRecovery.action b/nav2_msgs/action/DummyBehavior.action similarity index 84% rename from nav2_msgs/action/DummyRecovery.action rename to nav2_msgs/action/DummyBehavior.action index 78fabe0b81..4b8db815d2 100644 --- a/nav2_msgs/action/DummyRecovery.action +++ b/nav2_msgs/action/DummyBehavior.action @@ -4,4 +4,4 @@ std_msgs/String command #result definition builtin_interfaces/Duration total_elapsed_time --- -#feedback +#feedback definition diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 8b9384492c..5462faa239 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -1,10 +1,11 @@ #goal definition nav_msgs/Path path string controller_id +string goal_checker_id --- #result definition std_msgs/Empty result --- -#feedback +#feedback definition float32 distance_to_goal float32 speed diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index dfa7517a5b..c4b4764821 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -4,5 +4,5 @@ geometry_msgs/PoseStamped[] poses #result definition int32[] missed_waypoints --- -#feedback +#feedback definition uint32 current_waypoint diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action new file mode 100644 index 0000000000..e2a57f25a9 --- /dev/null +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -0,0 +1,14 @@ +#goal definition +geometry_msgs/PoseStamped[] poses +string behavior_tree +--- +#result definition +std_msgs/Empty result +--- +#feedback definition +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 792bee20ad..b707792241 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -5,7 +5,9 @@ string behavior_tree #result definition std_msgs/Empty result --- +#feedback definition 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/action/SmoothPath.action b/nav2_msgs/action/SmoothPath.action new file mode 100644 index 0000000000..18560d70b9 --- /dev/null +++ b/nav2_msgs/action/SmoothPath.action @@ -0,0 +1,12 @@ +#goal definition +nav_msgs/Path path +string smoother_id +builtin_interfaces/Duration max_smoothing_duration +bool check_for_collisions +--- +#result definition +nav_msgs/Path path +builtin_interfaces/Duration smoothing_duration +bool was_completed +--- +#feedback definition diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action index 2f498edceb..3e1c0aadef 100644 --- a/nav2_msgs/action/Spin.action +++ b/nav2_msgs/action/Spin.action @@ -1,7 +1,9 @@ #goal definition float32 target_yaw +builtin_interfaces/Duration time_allowance --- #result definition builtin_interfaces/Duration total_elapsed_time --- +#feedback definition float32 angular_distance_traveled diff --git a/nav2_msgs/action/Wait.action b/nav2_msgs/action/Wait.action index ed651226c6..8cf7119431 100644 --- a/nav2_msgs/action/Wait.action +++ b/nav2_msgs/action/Wait.action @@ -4,5 +4,5 @@ builtin_interfaces/Duration time #result definition builtin_interfaces/Duration total_elapsed_time --- -#feedback +#feedback definition builtin_interfaces/Duration time_left diff --git a/nav2_msgs/msg/CostmapFilterInfo.msg b/nav2_msgs/msg/CostmapFilterInfo.msg new file mode 100644 index 0000000000..fb9d9af4ca --- /dev/null +++ b/nav2_msgs/msg/CostmapFilterInfo.msg @@ -0,0 +1,14 @@ +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 (m/s) +uint8 type +# Name of filter mask topic +string filter_mask_topic +# Multiplier base offset and multiplier coefficient for conversion of OccGrid. +# Used to convert OccupancyGrid data values to filter space values. +# data -> into some other number space: +# space = data * multiplier + base +float32 base +float32 multiplier diff --git a/nav2_msgs/msg/SpeedLimit.msg b/nav2_msgs/msg/SpeedLimit.msg new file mode 100644 index 0000000000..ea9630fc91 --- /dev/null +++ b/nav2_msgs/msg/SpeedLimit.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +# Setting speed limit in percentage if true or in absolute values in false case +bool percentage +# Maximum allowed speed (in percent of maximum robot speed or in m/s depending +# on "percentage" value). When no-limit it is set to 0.0 +float64 speed_limit diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index f66ac4b624..5c2d97d754 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,8 +2,8 @@ nav2_msgs - 0.4.7 - Messages and service files for the navigation2 stack + 1.1.0 + Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski Carlos Orduno diff --git a/nav2_msgs/srv/ClearCostmapAroundRobot.srv b/nav2_msgs/srv/ClearCostmapAroundRobot.srv index 150ca61300..e3b41bf584 100644 --- a/nav2_msgs/srv/ClearCostmapAroundRobot.srv +++ b/nav2_msgs/srv/ClearCostmapAroundRobot.srv @@ -1,6 +1,5 @@ -# Clears the costmap within a window around the robot +# Clears the costmap within a distance -float32 window_size_x -float32 window_size_y +float32 reset_distance --- std_msgs/Empty response diff --git a/nav2_msgs/srv/IsPathValid.srv b/nav2_msgs/srv/IsPathValid.srv new file mode 100644 index 0000000000..8d6dc3b38d --- /dev/null +++ b/nav2_msgs/srv/IsPathValid.srv @@ -0,0 +1,6 @@ +#Determine if the current path is still valid + +nav_msgs/Path path +--- +bool is_valid +int32[] invalid_pose_indices diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt index 1e84cb02f9..e956384a6b 100644 --- a/nav2_navfn_planner/CMakeLists.txt +++ b/nav2_navfn_planner/CMakeLists.txt @@ -54,9 +54,6 @@ ament_target_dependencies(${library_name} ${dependencies} ) -# prevent pluginlib from using boost -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) install(TARGETS ${library_name} @@ -75,7 +72,9 @@ install(FILES global_planner_plugin.xml 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_navfn_planner/README.md b/nav2_navfn_planner/README.md index 90bb5467b3..ad980d1765 100644 --- a/nav2_navfn_planner/README.md +++ b/nav2_navfn_planner/README.md @@ -1,19 +1,7 @@ # Navfn Planner -The NavfnPlanner is a plugin for the Nav2 Planner server. +The NavfnPlanner is a global planner plugin for the Nav2 Planner server. It implements the Navigation Function planner with either A\* or Dij. expansions. It is largely equivalent to its counterpart in ROS 1 Navigation. The Navfn planner assumes a circular robot (or a robot that can be approximated as circular for the purposes of global path planning) and operates on a weighted costmap. -It provides the equivalent functionality to a [GlobalPlanner](http://wiki.ros.org/nav_core#BaseGlobalPlanner) in ROS1 [MoveBase](http://wiki.ros.org/move_base). +The `global_planner` package from ROS (1) is a refactor on NavFn to make it more easily understandable, but it lacks in run-time performance and introduces suboptimal behaviors. As NavFn has been extremely stable for about 10 years at the time of porting, the maintainers felt no compelling reason to port over another, largely equivalent (but poorer functioning) planner. -## Status -Currently, NavfnPlanner's core algorithm is a direct port from the ROS1 MoveBase [Navfn](http://wiki.ros.org/navfn) planner. The Navfn planning algorithm is based on the [Global Dynamic Window Approach](https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf). - -## Characteristics - -In Dijkstra mode (`use_astar = false`) Dijkstra's search algorithm is guaranteed to find the shortest path under any condition. -In A* mode (`use_astar = true`) A*'s search algorithm is not guaranteed to find the shortest path, however it uses a heuristic to expand the potential field towards the goal. - -The Navfn planner assumes a circular robot and operates on a costmap. - -## Next Steps -- Implement additional planners based on optimal control, potential field or other graph search algorithms that require transformation of the world model to other representations (topological, tree map, etc.) to confirm sufficient generalization. [Issue #225](http://github.com/ros-planning/navigation2/issues/225) -- Implement planners for non-holonomic robots. [Issue #225](http://github.com/ros-planning/navigation2/issues/225) +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-navfn.html) for additional parameter descriptions. 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 4a50cd0244..76bc107f6d 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -32,6 +32,7 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" namespace nav2_navfn_planner { @@ -39,58 +40,113 @@ 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( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + 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 nav2_msgs::PathAndBoundary of the generated path + */ nav2_msgs::msg::PathAndBoundary createPlan( const geometry_msgs::msg::PoseStamped &start, const nav_msgs::msg::Path &goal, const int &robots) override; + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav2_msgs::PathAndBoundary of the generated path + */ nav2_msgs::msg::PathAndBoundary createPlan( const geographic_msgs::msg::GeoPoseStamped &start, const geographic_msgs::msg::GeoPath &goal, const int &robots) 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 - bool computePotential(const geometry_msgs::msg::Point &world_point); - - // Compute a plan to a goal from a potential - must call computePotential first + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & goal, double tolerance, + nav_msgs::msg::Path & plan); + + /** + * @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); + + /** + * @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 + const geometry_msgs::msg::Pose & goal, + nav_msgs::msg::Path & plan); + + /** + * @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); + 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 - double getPointPotential(const geometry_msgs::msg::Point &world_point); + /** + * @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 // - must call computePotential first @@ -98,26 +154,51 @@ namespace nav2_navfn_planner // 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) + const geometry_msgs::msg::Pose & p1, + const geometry_msgs::msg::Pose & p2) { double dx = p1.position.x - p2.position.x; double dy = p1.position.y - p2.position.y; return dx * dx + dy * dy; } - // Transform a point from world to map frame - bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my); - - // Transform a point from map to world frame - void mapToWorld(double mx, double my, double &wx, double &wy); - - // Set the corresponding cell cost to be free space + /** + * @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); + + /** + * @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); + + /** + * @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 @@ -126,17 +207,20 @@ namespace nav2_navfn_planner // TF buffer std::shared_ptr tf_; - // node ptr - nav2_util::LifecycleNode::SharedPtr node_; + // Clock + rclcpp::Clock::SharedPtr clock_; + + // Logger + rclcpp::Logger logger_{rclcpp::get_logger("NavfnPlanner")}; // Global Costmap - nav2_costmap_2d::Costmap2D *costmap_; + nav2_costmap_2d::Costmap2D * costmap_; // The global frame of the costmap std::string global_frame_, name_; // Whether or not the planner should be allowed to plan through unknown space - bool allow_unknown_; + bool allow_unknown_, use_final_approach_orientation_; // If the goal is obstructed, the tolerance specifies how many meters the planner // can relax the constraint in x and y before failing @@ -145,15 +229,18 @@ namespace nav2_navfn_planner // 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_; + // parent node weak ptr + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; /** * @brief Callback executed when a paramter change is detected - * @param event ParameterEvent message + * @param parameters list of changed parameters */ - void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); }; } // namespace nav2_navfn_planner diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 2be8ab3491..21bd59f86c 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.7 + 1.1.0 TODO Steve Macenski Carlos Orduno @@ -29,6 +29,7 @@ ament_lint_common ament_lint_auto + ament_cmake_gtest ament_cmake diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 2d478c4a04..7dee5bfa1d 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -40,6 +40,7 @@ #include "nav2_msgs/msg/path_and_boundary.hpp" using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT using nav2_util::declare_parameter_if_not_declared; using rcl_interfaces::msg::ParameterType; using std::placeholders::_1; @@ -55,62 +56,74 @@ namespace nav2_navfn_planner NavfnPlanner::~NavfnPlanner() { RCLCPP_INFO( - node_->get_logger(), "Destroying plugin %s of type NavfnPlanner", + logger_, "Destroying plugin %s of type NavfnPlanner", name_.c_str()); } void NavfnPlanner::configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, std::shared_ptr tf, std::shared_ptr costmap_ros) { - node_ = parent; tf_ = tf; name_ = name; costmap_ = costmap_ros->getCostmap(); global_frame_ = costmap_ros->getGlobalFrameID(); + node_ = parent; + auto node = parent.lock(); + clock_ = node->get_clock(); + logger_ = node->get_logger(); + RCLCPP_INFO( - node_->get_logger(), "Configuring plugin %s of type NavfnPlanner", + logger_, "Configuring plugin %s of type NavfnPlanner", name_.c_str()); // Initialize parameters // Declare this plugin's parameters - declare_parameter_if_not_declared(node_, name + ".tolerance", rclcpp::ParameterValue(0.5)); - node_->get_parameter(name + ".tolerance", tolerance_); - declare_parameter_if_not_declared(node_, name + ".use_astar", rclcpp::ParameterValue(false)); - node_->get_parameter(name + ".use_astar", use_astar_); - declare_parameter_if_not_declared(node_, name + ".allow_unknown", rclcpp::ParameterValue(true)); - node_->get_parameter(name + ".allow_unknown", allow_unknown_); + declare_parameter_if_not_declared(node, name + ".tolerance", rclcpp::ParameterValue(0.5)); + node->get_parameter(name + ".tolerance", tolerance_); + declare_parameter_if_not_declared(node, name + ".use_astar", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".use_astar", use_astar_); + declare_parameter_if_not_declared(node, name + ".allow_unknown", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".allow_unknown", allow_unknown_); + declare_parameter_if_not_declared( + node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".use_final_approach_orientation", use_final_approach_orientation_); // Create a planner based on the new costmap size planner_ = std::make_unique( - costmap_->getSizeInCellsX(), - costmap_->getSizeInCellsY()); + costmap_->getSizeInCellsX(), + costmap_->getSizeInCellsY()); } void NavfnPlanner::activate() { RCLCPP_INFO( - node_->get_logger(), "Activating plugin %s of type NavfnPlanner", + logger_, "Activating plugin %s of type NavfnPlanner", name_.c_str()); + // Add callback for dynamic parameters + auto node = node_.lock(); + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&NavfnPlanner::dynamicParametersCallback, this, _1)); } void NavfnPlanner::deactivate() { RCLCPP_INFO( - node_->get_logger(), "Deactivating plugin %s of type NavfnPlanner", + logger_, "Deactivating plugin %s of type NavfnPlanner", name_.c_str()); + dyn_params_handler_.reset(); } void NavfnPlanner::cleanup() { RCLCPP_INFO( - node_->get_logger(), "Cleaning up plugin %s of type NavfnPlanner", + logger_, "Cleaning up plugin %s of type NavfnPlanner", name_.c_str()); planner_.reset(); } @@ -121,6 +134,10 @@ namespace nav2_navfn_planner const int &robots) { (void)robots; + #ifdef BENCHMARK_TESTING + steady_clock::time_point a = steady_clock::now(); + #endif + nav2_msgs::msg::PathAndBoundary path_and_boundary; // Update planner based on the new costmap size if (isPlannerOutOfDate()) @@ -132,14 +149,49 @@ namespace nav2_navfn_planner nav_msgs::msg::Path path; + // Corner case of the start(x,y) = goal(x,y) + if (start.pose.position.x == goal.poses[0].pose.position.x && + start.pose.position.y == goal.poses[0].pose.position.y) + { + unsigned int mx, my; + costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + if (costmap_->getCost(mx, my) == nav2_costmap_2d::LETHAL_OBSTACLE) { + RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); + return path_and_boundary; + } + path.header.stamp = clock_->now(); + path.header.frame_id = global_frame_; + geometry_msgs::msg::PoseStamped pose; + pose.header = path.header; + pose.pose.position.z = 0.0; + + pose.pose = start.pose; + // if we have a different start and goal orientation, set the unique path pose to the goal + // orientation, unless use_final_approach_orientation=true where we need it to be the start + // orientation to avoid movement from the local planner + if (start.pose.orientation != goal.poses[0].pose.orientation && !use_final_approach_orientation_) { + pose.pose.orientation = goal.poses[0].pose.orientation; + } + path.poses.push_back(pose); + path_and_boundary.path_local.push_back(path); + return path_and_boundary; + } + if (!makePlan(start.pose, goal.poses[0].pose, tolerance_, path)) { RCLCPP_WARN( - node_->get_logger(), "%s: failed to create plan with " + logger_, "%s: failed to create plan with " "tolerance %.2f.", name_.c_str(), tolerance_); } path_and_boundary.path_local.push_back(path); + + #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 << std::endl; + #endif + return path_and_boundary; } @@ -169,14 +221,14 @@ namespace nav2_navfn_planner bool NavfnPlanner::makePlan( - const geometry_msgs::msg::Pose &start, - const geometry_msgs::msg::Pose &goal, double tolerance, - nav_msgs::msg::Path &plan) + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & goal, double tolerance, + nav_msgs::msg::Path & plan) { // clear the plan, just in case plan.poses.clear(); - plan.header.stamp = node_->now(); + plan.header.stamp = clock_->now(); plan.header.frame_id = global_frame_; // TODO(orduno): add checks for start and goal reference frame -- should be in global frame @@ -185,17 +237,16 @@ namespace nav2_navfn_planner double wy = start.position.y; RCLCPP_DEBUG( - node_->get_logger(), "Making plan from (%.2f,%.2f) to (%.2f,%.2f)", - start.position.x, start.position.y, goal.position.x, goal.position.y); + logger_, "Making plan from (%.2f,%.2f) to (%.2f,%.2f)", + start.position.x, start.position.y, goal.position.x, goal.position.y); unsigned int mx, my; - if (!worldToMap(wx, wy, mx, my)) - { + if (!worldToMap(wx, wy, mx, my)) { RCLCPP_WARN( - node_->get_logger(), - "Cannot create a plan: the robot's start position is off the global" - " costmap. Planning will always fail, are you sure" - " the robot has been properly localized?"); + logger_, + "Cannot create a plan: the robot's start position is off the global" + " costmap. Planning will always fail, are you sure" + " the robot has been properly localized?"); return false; } @@ -206,8 +257,8 @@ namespace nav2_navfn_planner // make sure to resize the underlying array that Navfn uses planner_->setNavArr( - costmap_->getSizeInCellsX(), - costmap_->getSizeInCellsY()); + costmap_->getSizeInCellsX(), + costmap_->getSizeInCellsY()); planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_); @@ -220,12 +271,11 @@ namespace nav2_navfn_planner wx = goal.position.x; wy = goal.position.y; - if (!worldToMap(wx, wy, mx, my)) - { + if (!worldToMap(wx, wy, mx, my)) { RCLCPP_WARN( - node_->get_logger(), - "The goal sent to the planner is off the global costmap." - " Planning will always fail to this goal."); + logger_, + "The goal sent to the planner is off the global costmap." + " Planning will always fail to this goal."); return false; } @@ -238,12 +288,9 @@ namespace nav2_navfn_planner planner_->setStart(map_goal); planner_->setGoal(map_start); - if (use_astar_) - { + if (use_astar_) { planner_->calcNavFnAstar(); - } - else - { + } else { planner_->calcNavFnDijkstra(true); } @@ -254,28 +301,22 @@ namespace nav2_navfn_planner p = goal; double potential = getPointPotential(p.position); - if (potential < POT_HIGH) - { + if (potential < POT_HIGH) { // Goal is reachable by itself best_pose = p; found_legal = true; - } - else - { + } else { // Goal is not reachable. Trying to find nearest to the goal // reachable point within its tolerance region double best_sdist = std::numeric_limits::max(); p.position.y = goal.position.y - tolerance; - while (p.position.y <= goal.position.y + tolerance) - { + while (p.position.y <= goal.position.y + tolerance) { p.position.x = goal.position.x - tolerance; - while (p.position.x <= goal.position.x + tolerance) - { + while (p.position.x <= goal.position.x + tolerance) { potential = getPointPotential(p.position); double sdist = squared_distance(p, goal); - if (potential < POT_HIGH && sdist < best_sdist) - { + if (potential < POT_HIGH && sdist < best_sdist) { best_sdist = sdist; best_pose = p; found_legal = true; @@ -286,19 +327,41 @@ namespace nav2_navfn_planner } } - if (found_legal) - { + if (found_legal) { // extract the plan - if (getPlanFromPotential(best_pose, plan)) - { + if (getPlanFromPotential(best_pose, plan)) { smoothApproachToGoal(best_pose, plan); - } - else - { + + // If use_final_approach_orientation=true, interpolate the last pose orientation from the + // previous pose to set the orientation to the 'final approach' orientation of the robot so + // it does not rotate. + // And deal with corner case of plan of length 1 + if (use_final_approach_orientation_) { + size_t plan_size = plan.poses.size(); + if (plan_size == 1) { + plan.poses.back().pose.orientation = start.orientation; + } else if (plan_size > 1) { + double dx, dy, theta; + auto last_pose = plan.poses.back().pose.position; + auto approach_pose = plan.poses[plan_size - 2].pose.position; + // Deal with the case of NavFn producing a path with two equal last poses + if (std::abs(last_pose.x - approach_pose.x) < 0.0001 && + std::abs(last_pose.y - approach_pose.y) < 0.0001 && plan_size > 2) + { + approach_pose = plan.poses[plan_size - 3].pose.position; + } + dx = last_pose.x - approach_pose.x; + dy = last_pose.y - approach_pose.y; + theta = atan2(dy, dx); + plan.poses.back().pose.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(theta); + } + } + } else { RCLCPP_ERROR( - node_->get_logger(), - "Failed to create a plan from potential when a legal" - " potential was found. This shouldn't happen."); + logger_, + "Failed to create a plan from potential when a legal" + " potential was found. This shouldn't happen."); } } @@ -307,18 +370,17 @@ namespace nav2_navfn_planner void NavfnPlanner::smoothApproachToGoal( - const geometry_msgs::msg::Pose &goal, - nav_msgs::msg::Path &plan) + const geometry_msgs::msg::Pose & goal, + nav_msgs::msg::Path & plan) { // Replace the last pose of the computed path if it's actually further away // to the second to last pose than the goal pose. - if (plan.poses.size() >= 2) - { + if (plan.poses.size() >= 2) { auto second_to_last_pose = plan.poses.end()[-2]; auto last_pose = plan.poses.back(); if ( - squared_distance(last_pose.pose, second_to_last_pose.pose) > - squared_distance(goal, second_to_last_pose.pose)) + squared_distance(last_pose.pose, second_to_last_pose.pose) > + squared_distance(goal, second_to_last_pose.pose)) { plan.poses.back().pose = goal; return; @@ -331,8 +393,8 @@ namespace nav2_navfn_planner bool NavfnPlanner::getPlanFromPotential( - const geometry_msgs::msg::Pose &goal, - nav_msgs::msg::Path &plan) + const geometry_msgs::msg::Pose & goal, + nav_msgs::msg::Path & plan) { // clear the plan, just in case plan.poses.clear(); @@ -343,12 +405,11 @@ namespace nav2_navfn_planner // the potential has already been computed, so we won't update our copy of the costmap unsigned int mx, my; - if (!worldToMap(wx, wy, mx, my)) - { + if (!worldToMap(wx, wy, mx, my)) { RCLCPP_WARN( - node_->get_logger(), - "The goal sent to the navfn planner is off the global costmap." - " Planning will always fail to this goal."); + logger_, + "The goal sent to the navfn planner is off the global costmap." + " Planning will always fail to this goal."); return false; } @@ -358,22 +419,25 @@ namespace nav2_navfn_planner planner_->setStart(map_goal); - int path_len = planner_->calcPath(costmap_->getSizeInCellsX() * 4); - if (path_len == 0) - { + const int & max_cycles = (costmap_->getSizeInCellsX() >= costmap_->getSizeInCellsY()) ? + (costmap_->getSizeInCellsX() * 4) : (costmap_->getSizeInCellsY() * 4); + + int path_len = planner_->calcPath(max_cycles); + if (path_len == 0) { return false; } auto cost = planner_->getLastPathCost(); - RCLCPP_DEBUG(node_->get_logger(), "Path found, %d steps, %f cost\n", path_len, cost); + RCLCPP_DEBUG( + logger_, + "Path found, %d steps, %f cost\n", path_len, cost); // extract the plan - float *x = planner_->getPathX(); - float *y = planner_->getPathY(); + float * x = planner_->getPathX(); + float * y = planner_->getPathY(); int len = planner_->getPathLen(); - for (int i = len - 1; i >= 0; --i) - { + for (int i = len - 1; i >= 0; --i) { // convert the plan to world coordinates double world_x, world_y; mapToWorld(x[i], y[i], world_x, world_y); @@ -393,11 +457,10 @@ namespace nav2_navfn_planner } double - NavfnPlanner::getPointPotential(const geometry_msgs::msg::Point &world_point) + NavfnPlanner::getPointPotential(const geometry_msgs::msg::Point & world_point) { unsigned int mx, my; - if (!worldToMap(world_point.x, world_point.y, mx, my)) - { + if (!worldToMap(world_point.x, world_point.y, mx, my)) { return std::numeric_limits::max(); } @@ -443,32 +506,31 @@ namespace nav2_navfn_planner // } bool - NavfnPlanner::worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) + NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) { - if (wx < costmap_->getOriginX() || wy < costmap_->getOriginY()) - { + if (wx < costmap_->getOriginX() || wy < costmap_->getOriginY()) { return false; } mx = static_cast( - std::round((wx - costmap_->getOriginX()) / costmap_->getResolution())); + std::round((wx - costmap_->getOriginX()) / costmap_->getResolution())); my = static_cast( - std::round((wy - costmap_->getOriginY()) / costmap_->getResolution())); + std::round((wy - costmap_->getOriginY()) / costmap_->getResolution())); - if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY()) - { + if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY()) { return true; } RCLCPP_ERROR( - node_->get_logger(), "worldToMap failed: mx,my: %d,%d, size_x,size_y: %d,%d", mx, my, - costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + logger_, + "worldToMap failed: mx,my: %d,%d, size_x,size_y: %d,%d", mx, my, + costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); return false; } void - NavfnPlanner::mapToWorld(double mx, double my, double &wx, double &wy) + NavfnPlanner::mapToWorld(double mx, double my, double & wx, double & wy) { wx = costmap_->getOriginX() + mx * costmap_->getResolution(); wy = costmap_->getOriginY() + my * costmap_->getResolution(); @@ -482,30 +544,33 @@ namespace nav2_navfn_planner 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; + rcl_interfaces::msg::SetParametersResult + NavfnPlanner::dynamicParametersCallback(std::vector parameters) + { + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == name_ + ".tolerance") { + tolerance_ = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == name_ + ".use_astar") { + use_astar_ = parameter.as_bool(); + } else if (name == name_ + ".allow_unknown") { + allow_unknown_ = parameter.as_bool(); + } else if (name == name_ + ".use_final_approach_orientation") { + use_final_approach_orientation_ = parameter.as_bool(); + } } } + result.successful = true; + return result; } -} -} // namespace nav2_navfn_planner + } // namespace nav2_navfn_planner -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(nav2_navfn_planner::NavfnPlanner, nav2_core::GlobalPlanner) + #include "pluginlib/class_list_macros.hpp" + PLUGINLIB_EXPORT_CLASS(nav2_navfn_planner::NavfnPlanner, nav2_core::GlobalPlanner) diff --git a/nav2_navfn_planner/test/CMakeLists.txt b/nav2_navfn_planner/test/CMakeLists.txt new file mode 100644 index 0000000000..d415d906ef --- /dev/null +++ b/nav2_navfn_planner/test/CMakeLists.txt @@ -0,0 +1,10 @@ +# Test dynamic parameters +ament_add_gtest(test_dynamic_parameters + test_dynamic_parameters.cpp +) +ament_target_dependencies(test_dynamic_parameters + ${dependencies} +) +target_link_libraries(test_dynamic_parameters + ${library_name} +) diff --git a/nav2_navfn_planner/test/test_dynamic_parameters.cpp b/nav2_navfn_planner/test/test_dynamic_parameters.cpp new file mode 100644 index 0000000000..b4cded3097 --- /dev/null +++ b/nav2_navfn_planner/test/test_dynamic_parameters.cpp @@ -0,0 +1,64 @@ +// 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 "gtest/gtest.h" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_navfn_planner/navfn_planner.hpp" +#include "rclcpp/rclcpp.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(NavfnTest, testDynamicParameter) +{ + auto node = std::make_shared("Navfntest"); + auto costmap = std::make_shared("global_costmap"); + costmap->on_configure(rclcpp_lifecycle::State()); + auto planner = + std::make_unique(); + auto tf = std::make_shared(node->get_clock()); + planner->configure(node, "test", tf, costmap); + planner->activate(); + + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.tolerance", 1.0), + rclcpp::Parameter("test.use_astar", true), + rclcpp::Parameter("test.allow_unknown", true), + rclcpp::Parameter("test.use_final_approach_orientation", true)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + EXPECT_EQ(node->get_parameter("test.tolerance").as_double(), 1.0); + EXPECT_EQ(node->get_parameter("test.use_astar").as_bool(), true); + EXPECT_EQ(node->get_parameter("test.allow_unknown").as_bool(), true); + EXPECT_EQ(node->get_parameter("test.use_final_approach_orientation").as_bool(), true); +} diff --git a/nav2_pkgs.txt b/nav2_pkgs.txt new file mode 100644 index 0000000000..116b11b2d3 --- /dev/null +++ b/nav2_pkgs.txt @@ -0,0 +1,7 @@ +dwb_msgs +dwb_core +dwb_plugins +costmap_queue +dwb_critics +nav_2d_msgs +nav_2d_utils \ No newline at end of file diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt index 3e5ca6d3d2..dc20add754 100644 --- a/nav2_planner/CMakeLists.txt +++ b/nav2_planner/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(nav2_util REQUIRED) @@ -32,6 +33,7 @@ set(dependencies rclcpp rclcpp_action rclcpp_lifecycle + rclcpp_components std_msgs visualization_msgs nav2_util @@ -54,8 +56,6 @@ ament_target_dependencies(${library_name} ${dependencies} ) -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - add_executable(${executable_name} src/main.cpp ) @@ -66,8 +66,7 @@ ament_target_dependencies(${executable_name} ${dependencies} ) -# prevent pluginlib from using boost -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +rclcpp_components_register_nodes(${library_name} "nav2_planner::PlannerServer") install(TARGETS ${library_name} ARCHIVE DESTINATION lib @@ -85,7 +84,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_planner/README.md b/nav2_planner/README.md index 0d4447b103..3a17c825d4 100644 --- a/nav2_planner/README.md +++ b/nav2_planner/README.md @@ -1,5 +1,9 @@ # Nav2 Planner -The Nav2 planner is a [planning module](../doc/requirements/requirements.md) that implements the `nav2_behavior_tree::ComputePathToPose` interface. +The Nav2 planner is a Task Server in Nav2 that implements the `nav2_behavior_tree::ComputePathToPose` interface. -A planning module implementing the `nav2_behavior_tree::ComputePathToPose` interface is responsible for generating a feasible path given start and end robot poses. It loads a map of potential planner plugins like NavFn to do the path generation in different user-defined situations. +A planning module implementing the `nav2_behavior_tree::ComputePathToPose` interface is responsible for generating a feasible path given start and end robot poses. It loads a map of potential planner plugins to do the path generation in different user-defined situations. + +See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. + +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-planner-server.html) for additional parameter descriptions and a [tutorial about writing planner plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html). diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 2eee89479e..4791d8d104 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -27,6 +28,7 @@ #include "nav2_msgs/msg/path_and_boundary.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/compute_path.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" @@ -37,29 +39,32 @@ #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_core/global_planner.hpp" +#include "nav2_msgs/srv/is_path_valid.hpp" namespace nav2_planner { - /** +/** * @class nav2_planner::PlannerServer * @brief An action server implements the behavior tree's ComputePath * interface and hosts various plugins of different algorithms to compute plans. */ - class PlannerServer : public nav2_util::LifecycleNode - { - public: - /** +class PlannerServer : public nav2_util::LifecycleNode +{ +public: + /** * @brief A constructor for nav2_planner::PlannerServer + * @param options Additional options to control creation of the node. */ - PlannerServer(); - /** + explicit PlannerServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /** * @brief A destructor for nav2_planner::PlannerServer */ - ~PlannerServer(); + ~PlannerServer(); - using PlannerMap = std::unordered_map; + using PlannerMap = std::unordered_map; - /** + /** * @brief Method to get plan from the desired plugin * @param start starting pose * @param goal goal request @@ -67,86 +72,192 @@ namespace nav2_planner * @param robots number of robots * @return Paths and boundaries */ - template - nav2_msgs::msg::PathAndBoundary getPlan( - const PoseType &start, - const PathType &goal, - const std::string &planner_id, - const int &robots); + template + nav2_msgs::msg::PathAndBoundary getPlan( + const PoseType &start, + const PathType &goal, + const std::string &planner_id, + const int &robots); - protected: - /** +protected: + /** * @brief Configure member variables and initializes planner * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override; - /** + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override; + /** * @brief Activate member variables * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override; - /** + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override; + /** * @brief Deactivate member variables * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override; - /** + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override; + /** * @brief Reset member variables * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override; - /** + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override; + + /** * @brief Called when in shutdown state * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ - nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override; + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + using ActionT = nav2_msgs::action::ComputePath; + using ActionServer = nav2_util::SimpleActionServer; + + // Our action server implements the ComputePath action + std::unique_ptr action_server_; + + /** + * @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); - using ActionT = nav2_msgs::action::ComputePath; - using ActionServer = nav2_util::SimpleActionServer; + /** + * @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); - // Our action server implements the ComputePath action - 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); + + /** * @brief The action server callback which calls planner to get the path + * ComputePathToPose */ - void computePlan(); + void computePlan(); - /** + /** + * @brief The action server callback which calls planner to get the path + * ComputePathThroughPoses + */ + void computePlanThroughPoses(); + + /** + * @brief The service callback to determine if the path is still valid + * @param request to the service + * @param response from the service + */ + void isPathValid( + const std::shared_ptr request, + std::shared_ptr response); + + /** * @brief Publish a path for visualization purposes * @param path_boundary Reference to Global Path */ - void publishPlan(const nav2_msgs::msg::PathAndBoundary &path_boundary); + void publishPlan(const nav2_msgs::msg::PathAndBoundary &path_boundary); + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::mutex dynamic_params_lock_; - // Planner - PlannerMap planners_; - pluginlib::ClassLoader gp_loader_; - std::vector default_ids_; - std::vector default_types_; - std::vector planner_ids_; - std::vector planner_types_; - double max_planner_duration_; - std::string planner_ids_concat_; + // Planner + PlannerMap planners_; + pluginlib::ClassLoader gp_loader_; + std::vector default_ids_; + std::vector default_types_; + std::vector planner_ids_; + std::vector planner_types_; + double max_planner_duration_; + std::string planner_ids_concat_; - // Clock - rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; + // Clock + rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; - // TF buffer - std::shared_ptr tf_; + // TF buffer + std::shared_ptr tf_; - // Global Costmap - std::shared_ptr costmap_ros_; - std::unique_ptr costmap_thread_; - nav2_costmap_2d::Costmap2D *costmap_; + // Global Costmap + std::shared_ptr costmap_ros_; + std::unique_ptr costmap_thread_; + nav2_costmap_2d::Costmap2D *costmap_; - // Publishers for the path - rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; - }; + // Publishers for the path + rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; + + // Service to deterime if the path is valid + rclcpp::Service::SharedPtr is_path_valid_service_; +}; } // namespace nav2_planner diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index e762c67d0e..d9902074df 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.7 + 1.1.0 TODO Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index fd8790ed14..dece6f3745 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 @@ -26,349 +27,533 @@ #include "builtin_interfaces/msg/duration.hpp" #include "nav2_util/costmap.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_planner/planner_server.hpp" using namespace std::chrono_literals; +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; namespace nav2_planner { - PlannerServer::PlannerServer() - : nav2_util::LifecycleNode("nav2_planner", "", true), - gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), - default_ids_{"GridBased"}, - default_types_{"nav2_navfn_planner/NavfnPlanner"}, - costmap_(nullptr) - { - RCLCPP_INFO(get_logger(), "Creating"); +PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) + : nav2_util::LifecycleNode("planner_server", "", options), + gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), + default_ids_{"GridBased"}, + default_types_{"nav2_navfn_planner/NavfnPlanner"}, + costmap_(nullptr) +{ + RCLCPP_INFO(get_logger(), "Creating"); + + // Declare this node's parameters + declare_parameter("planner_plugins", default_ids_); + declare_parameter("expected_planner_frequency", 1.0); - // Declare this node's parameters - declare_parameter("planner_plugins", default_ids_); - declare_parameter("expected_planner_frequency", 20.0); + get_parameter("planner_plugins", planner_ids_); + if (planner_ids_ == default_ids_) { + for (size_t i = 0; i < default_ids_.size(); ++i) { + declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); + } + } - // Setup the global costmap - costmap_ros_ = std::make_shared( - "global_costmap", std::string{get_namespace()}, "global_costmap"); + // Setup the global costmap + costmap_ros_ = std::make_shared( + "global_costmap", std::string{get_namespace()}, "global_costmap"); - // Launch a thread to run the costmap node - costmap_thread_ = std::make_unique(costmap_ros_); + // Launch a thread to run the costmap node + costmap_thread_ = std::make_unique(costmap_ros_); +} + +PlannerServer::~PlannerServer() +{ + RCLCPP_INFO(get_logger(), "Destroying"); + planners_.clear(); + costmap_thread_.reset(); +} + +nav2_util::CallbackReturn +PlannerServer::on_configure(const rclcpp_lifecycle::State &state) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + costmap_ros_->on_configure(state); + costmap_ = costmap_ros_->getCostmap(); + + RCLCPP_DEBUG( + get_logger(), "Costmap size: %d,%d", + costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + + tf_ = costmap_ros_->getTfBuffer(); + + planner_types_.resize(planner_ids_.size()); + + auto node = shared_from_this(); + + for (size_t i = 0; i != planner_ids_.size(); i++) { + try { + planner_types_[i] = nav2_util::get_plugin_type_param( + node, planner_ids_[i]); + nav2_core::GlobalPlanner::Ptr planner = + gp_loader_.createUniqueInstance(planner_types_[i]); + RCLCPP_INFO( + get_logger(), "Created global planner plugin %s of type %s", + planner_ids_[i].c_str(), planner_types_[i].c_str()); + planner->configure(node, planner_ids_[i], tf_, costmap_ros_); + planners_.insert({planner_ids_[i], planner}); + } catch (const pluginlib::PluginlibException &ex) { + RCLCPP_FATAL( + get_logger(), "Failed to create global planner. Exception: %s", + ex.what()); + return nav2_util::CallbackReturn::FAILURE; + } } - PlannerServer::~PlannerServer() - { - RCLCPP_INFO(get_logger(), "Destroying"); - planners_.clear(); - costmap_thread_.reset(); + for (size_t i = 0; i != planner_ids_.size(); i++){ + planner_ids_concat_ += planner_ids_[i] + std::string(" "); } - nav2_util::CallbackReturn - PlannerServer::on_configure(const rclcpp_lifecycle::State &state) - { - RCLCPP_INFO(get_logger(), "Configuring"); + RCLCPP_INFO( + get_logger(), + "Planner Server has %s planners available.", planner_ids_concat_.c_str()); - costmap_ros_->on_configure(state); - costmap_ = costmap_ros_->getCostmap(); + double expected_planner_frequency; + get_parameter("expected_planner_frequency", expected_planner_frequency); + if (expected_planner_frequency > 0) { + max_planner_duration_ = 1 / expected_planner_frequency; + } else { + RCLCPP_WARN( + get_logger(), + "The expected planner frequency parameter is %.4f Hz. The value should to be greater" + " than 0.0 to turn on duration overrrun warning messages", + expected_planner_frequency); + max_planner_duration_ = 0.0; + } - RCLCPP_DEBUG( - get_logger(), "Costmap size: %d,%d", - costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + // Initialize pubs & subs + plan_publisher_ = create_publisher("plan", 1); - tf_ = costmap_ros_->getTfBuffer(); + // Create the action server that we implement with our navigateToPose method + action_server_ = std::make_unique( + node, + "compute_path_to_pose", + std::bind(&PlannerServer::computePlan, this)); - get_parameter("planner_plugins", planner_ids_); - if (planner_ids_ == default_ids_) - { - for (size_t i = 0; i < default_ids_.size(); ++i) - { - declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); - } - } - planner_types_.resize(planner_ids_.size()); + return nav2_util::CallbackReturn::SUCCESS; +} - auto node = shared_from_this(); +nav2_util::CallbackReturn +PlannerServer::on_activate(const rclcpp_lifecycle::State &state) +{ + RCLCPP_INFO(get_logger(), "Activating"); - for (size_t i = 0; i != planner_ids_.size(); i++) - { - try - { - planner_types_[i] = nav2_util::get_plugin_type_param( - node, planner_ids_[i]); - nav2_core::GlobalPlanner::Ptr planner = - gp_loader_.createUniqueInstance(planner_types_[i]); - RCLCPP_INFO( - get_logger(), "Created global planner plugin %s of type %s", - planner_ids_[i].c_str(), planner_types_[i].c_str()); - planner->configure(node, planner_ids_[i], tf_, costmap_ros_); - planners_.insert({planner_ids_[i], planner}); - } - catch (const pluginlib::PluginlibException &ex) - { - RCLCPP_FATAL( - get_logger(), "Failed to create global planner. Exception: %s", - ex.what()); - return nav2_util::CallbackReturn::FAILURE; - } - } + plan_publisher_->on_activate(); + action_server_->activate(); + costmap_ros_->on_activate(state); - for (size_t i = 0; i != planner_ids_.size(); i++) - { - planner_ids_concat_ += planner_ids_[i] + std::string(" "); - } + PlannerMap::iterator it; + for (it = planners_.begin(); it != planners_.end(); ++it) { + it->second->activate(); + } - RCLCPP_INFO( - get_logger(), - "Planner Server has %s planners available.", planner_ids_concat_.c_str()); + auto node = shared_from_this(); - double expected_planner_frequency; - get_parameter("expected_planner_frequency", expected_planner_frequency); - if (expected_planner_frequency > 0) - { - max_planner_duration_ = 1 / expected_planner_frequency; - } - else - { - RCLCPP_WARN( - get_logger(), - "The expected planner frequency parameter is %.4f Hz. The value should to be greater" - " than 0.0 to turn on duration overrrun warning messages", - expected_planner_frequency); - max_planner_duration_ = 0.0; - } + is_path_valid_service_ = node->create_service( + "is_path_valid", + std::bind( + &PlannerServer::isPathValid, this, + std::placeholders::_1, std::placeholders::_2)); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&PlannerServer::dynamicParametersCallback, this, _1)); + + // create bond connection + createBond(); - // Initialize pubs & subs - plan_publisher_ = create_publisher("plan", 1); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +PlannerServer::on_deactivate(const rclcpp_lifecycle::State &state) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); - // Create the action server that we implement with our navigateToPose method - action_server_ = std::make_unique( - rclcpp_node_, - "compute_path_to_pose", - std::bind(&PlannerServer::computePlan, this)); + action_server_->deactivate(); + plan_publisher_->on_deactivate(); + costmap_ros_->on_deactivate(state); - return nav2_util::CallbackReturn::SUCCESS; + PlannerMap::iterator it; + for (it = planners_.begin(); it != planners_.end(); ++it) { + it->second->deactivate(); } - nav2_util::CallbackReturn - PlannerServer::on_activate(const rclcpp_lifecycle::State &state) - { - RCLCPP_INFO(get_logger(), "Activating"); + dyn_params_handler_.reset(); - plan_publisher_->on_activate(); - action_server_->activate(); - costmap_ros_->on_activate(state); + // destroy bond connection + destroyBond(); - PlannerMap::iterator it; - for (it = planners_.begin(); it != planners_.end(); ++it) - { - it->second->activate(); - } + return nav2_util::CallbackReturn::SUCCESS; +} - return nav2_util::CallbackReturn::SUCCESS; - } +nav2_util::CallbackReturn +PlannerServer::on_cleanup(const rclcpp_lifecycle::State &state) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); - nav2_util::CallbackReturn - PlannerServer::on_deactivate(const rclcpp_lifecycle::State &state) - { - RCLCPP_INFO(get_logger(), "Deactivating"); + action_server_.reset(); + plan_publisher_.reset(); + tf_.reset(); + costmap_ros_->on_cleanup(state); - action_server_->deactivate(); - plan_publisher_->on_deactivate(); - costmap_ros_->on_deactivate(state); + PlannerMap::iterator it; + for (it = planners_.begin(); it != planners_.end(); ++it) { + it->second->cleanup(); + } + planners_.clear(); + costmap_ = nullptr; + return nav2_util::CallbackReturn::SUCCESS; +} +nav2_util::CallbackReturn +PlannerServer::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} - PlannerMap::iterator it; - for (it = planners_.begin(); it != planners_.end(); ++it) - { - it->second->deactivate(); - } +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; +} - return nav2_util::CallbackReturn::SUCCESS; +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(); } +} - nav2_util::CallbackReturn - PlannerServer::on_cleanup(const rclcpp_lifecycle::State &state) - { - RCLCPP_INFO(get_logger(), "Cleaning up"); +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; + } - action_server_.reset(); - plan_publisher_.reset(); - tf_.reset(); - costmap_ros_->on_cleanup(state); + return false; +} - PlannerMap::iterator it; - for (it = planners_.begin(); it != planners_.end(); ++it) - { - it->second->cleanup(); - } - planners_.clear(); - costmap_ = nullptr; +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(); + } +} - return nav2_util::CallbackReturn::SUCCESS; +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; } - nav2_util::CallbackReturn - PlannerServer::on_shutdown(const rclcpp_lifecycle::State &) + 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_INFO(get_logger(), "Shutting down"); - return nav2_util::CallbackReturn::SUCCESS; + RCLCPP_WARN( + get_logger(), "Could not transform the start or goal pose in the costmap frame"); + action_server->terminate_current(); + return false; } - void - PlannerServer::computePlan() - { - auto start_time = steady_clock_.now(); + return true; +} - // Initialize the ComputePath goal and result - auto goal = action_server_->get_current_goal(); - auto result = std::make_shared(); +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; + } - try - { - if (action_server_ == nullptr || !action_server_->is_server_active()) - { - RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); - return; - } + RCLCPP_DEBUG( + get_logger(), + "Found valid path of size %lu to (%.2f, %.2f)", + path.poses.size(), goal.pose.position.x, + goal.pose.position.y); - if (action_server_->is_cancel_requested()) - { - RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); - action_server_->terminate_all(); - return; - } + return true; +} - auto start_local = goal->start_local; - auto goals_local = goal->goals_local; - auto start_global = goal->start_global; - auto goals_global = goal->goals_global; +void +PlannerServer::computePlan() +{ + std::lock_guard lock(dynamic_params_lock_); - if (!goal->use_start) - { - // TODO: We might have to revisit this about getRobotPose if we are using GPS - if (!costmap_ros_->getRobotPose(start_local)) - { - action_server_->terminate_current(); - return; - } - } + auto start_time = steady_clock_.now(); - if (action_server_->is_preempt_requested()) - { - goal = action_server_->accept_pending_goal(); - } + // Initialize the ComputePath goal and result + auto goal = action_server_->get_current_goal(); + auto result = std::make_shared(); - if (goal->mode == nav2_msgs::action::ComputePath::Goal::LOCAL) - { - RCLCPP_INFO( - get_logger(), - "Local mode selected."); - result->path_and_boundary = getPlan(start_local, goals_local, - goal->planner_id, goal->robots); - } - else if (goal->mode == nav2_msgs::action::ComputePath::Goal::GLOBAL) - { - RCLCPP_INFO( - get_logger(), - "Global mode selected."); - result->path_and_boundary = getPlan(start_global, goals_global, - goal->planner_id, goal->robots); - } + try { + if (isServerInactive(action_server_) || isCancelRequested(action_server_)) { + return; + } + + // waitForCostmap(); - if (result->path_and_boundary.path_local.empty() && - result->path_and_boundary.path_global.empty()) + getPreemptedGoalIfRequested(action_server_, goal); + + auto start_local = goal->start_local; + auto goals_local = goal->goals_local; + auto start_global = goal->start_global; + auto goals_global = goal->goals_global; + + if (!goal->use_start) + { + // TODO: We might have to revisit this about getRobotPose if we are using GPS + if (!costmap_ros_->getRobotPose(start_local)) { - RCLCPP_WARN( - get_logger(), "Planning algorithm %s failed to generate a valid" - " path", - goal->planner_id.c_str()); action_server_->terminate_current(); return; } + } - RCLCPP_DEBUG( + if (goal->mode == nav2_msgs::action::ComputePath::Goal::LOCAL) + { + RCLCPP_INFO( get_logger(), - "Found valid path of size"); + "Local mode selected."); + result->path_and_boundary = getPlan(start_local, goals_local, + goal->planner_id, goal->robots); + } + else if (goal->mode == nav2_msgs::action::ComputePath::Goal::GLOBAL) + { + RCLCPP_INFO( + get_logger(), + "Global mode selected."); + result->path_and_boundary = getPlan(start_global, goals_global, + goal->planner_id, goal->robots); + } - publishPlan(result->path_and_boundary); + if (result->path_and_boundary.path_local.empty() && + result->path_and_boundary.path_global.empty()) + { + RCLCPP_WARN( + get_logger(), "Planning algorithm %s failed to generate a valid" + " path", + goal->planner_id.c_str()); + action_server_->terminate_current(); + return; + } - auto cycle_duration = steady_clock_.now() - start_time; - result->planning_time = cycle_duration; + RCLCPP_DEBUG( + get_logger(), + "Found valid path of size"); - if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) - { - RCLCPP_WARN( - 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()); - } + publishPlan(result->path_and_boundary); - action_server_->succeeded_current(result); - } - catch (std::exception &ex) + 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(), "%s plugin failed to plan calculation: \"%s\"", - goal->planner_id.c_str(), ex.what()); - // TODO(orduno): provide information about fail error to parent task, - // for example: couldn't get costmap update - action_server_->terminate_current(); + 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()); + } + + action_server_->succeeded_current(result); + } + catch (std::exception &ex) + { + RCLCPP_WARN( + get_logger(), "%s plugin failed to plan calculation: \"%s\"", + goal->planner_id.c_str(), ex.what()); + // TODO(orduno): provide information about fail error to parent task, + // for example: couldn't get costmap update + action_server_->terminate_current(); + } +} + +template +nav2_msgs::msg::PathAndBoundary +PlannerServer::getPlan( + const PoseType &start, + const PathType &goals, + const std::string &planner_id, + const int &robots) +{ + RCLCPP_DEBUG( + get_logger(), "Attempting to a find path from goal(s) to start"); + + // RCLCPP_DEBUG( + // get_logger(), "Attempting to a find path from (%.2f, %.2f) to " + // "(%.2f, %.2f).", + // start.pose.position.x, start.pose.position.y, + // goal.pose.position.x, goal.pose.position.y); + + if (planners_.find(planner_id) != planners_.end()) { + return planners_[planner_id]->createPlan(start, goals, robots); + } else { + if (planners_.size() == 1 && planner_id.empty()) { + RCLCPP_WARN_ONCE( + get_logger(), "No planners specified in action call. " + "Server will use only plugin %s in server." + " This warning will appear once.", + planner_ids_concat_.c_str()); + return planners_[planners_.begin()->first]->createPlan(start, goals, robots); + } else { + RCLCPP_ERROR( + get_logger(), "planner %s is not a valid planner. " + "Planner names are: %s", + planner_id.c_str(), + planner_ids_concat_.c_str()); } } - template - nav2_msgs::msg::PathAndBoundary - PlannerServer::getPlan( - const PoseType &start, - const PathType &goals, - const std::string &planner_id, - const int &robots) + return nav2_msgs::msg::PathAndBoundary(); +} + +void +PlannerServer::publishPlan(const nav2_msgs::msg::PathAndBoundary &path_boundary) +{ + auto msg = std::make_unique(path_boundary); + if ( + plan_publisher_->is_activated() && + this->count_subscribers(plan_publisher_->get_topic_name()) > 0) { - RCLCPP_DEBUG( - get_logger(), "Attempting to a find path from goal(s) to start"); + plan_publisher_->publish(std::move(msg)); + } +} - // RCLCPP_DEBUG( - // get_logger(), "Attempting to a find path from (%.2f, %.2f) to " - // "(%.2f, %.2f).", - // start.pose.position.x, start.pose.position.y, - // goal.pose.position.x, goal.pose.position.y); +void PlannerServer::isPathValid( + const std::shared_ptr request, + std::shared_ptr response) +{ + response->is_valid = true; - if (planners_.find(planner_id) != planners_.end()) - { - return planners_[planner_id]->createPlan(start, goals, robots); - } - else - { - if (planners_.size() == 1 && planner_id.empty()) - { - RCLCPP_WARN_ONCE( - get_logger(), "No planners specified in action call. " - "Server will use only plugin %s in server." - " This warning will appear once.", - planner_ids_concat_.c_str()); - return planners_[planners_.begin()->first]->createPlan(start, goals, robots); + if (request->path.poses.empty()) { + response->is_valid = false; + return; + } + + geometry_msgs::msg::PoseStamped current_pose; + unsigned int closest_point_index = 0; + if (costmap_ros_->getRobotPose(current_pose)) { + float current_distance = std::numeric_limits::max(); + float closest_distance = current_distance; + geometry_msgs::msg::Point current_point = current_pose.pose.position; + for (unsigned int i = 0; i < request->path.poses.size(); ++i) { + geometry_msgs::msg::Point path_point = request->path.poses[i].pose.position; + + current_distance = nav2_util::geometry_utils::euclidean_distance( + current_point, + path_point); + + if (current_distance < closest_distance) { + closest_point_index = i; + closest_distance = current_distance; } - else + } + + /** + * The lethal check starts at the closest point to avoid points that have already been passed + * and may have become occupied + */ + unsigned int mx = 0; + unsigned int my = 0; + for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) { + costmap_->worldToMap( + request->path.poses[i].pose.position.x, + request->path.poses[i].pose.position.y, mx, my); + unsigned int cost = costmap_->getCost(mx, my); + + if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { - RCLCPP_ERROR( - get_logger(), "planner %s is not a valid planner. " - "Planner names are: %s", - planner_id.c_str(), - planner_ids_concat_.c_str()); + response->is_valid = false; } } - - return nav2_msgs::msg::PathAndBoundary(); } +} - void - PlannerServer::publishPlan(const nav2_msgs::msg::PathAndBoundary &path_boundary) - { - auto msg = std::make_unique(path_boundary); - if ( - plan_publisher_->is_activated() && - this->count_subscribers(plan_publisher_->get_topic_name()) > 0) - { - plan_publisher_->publish(std::move(msg)); +rcl_interfaces::msg::SetParametersResult +PlannerServer::dynamicParametersCallback(std::vector parameters) +{ + std::lock_guard lock(dynamic_params_lock_); + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == "expected_planner_frequency") { + if (parameter.as_double() > 0) { + max_planner_duration_ = 1 / parameter.as_double(); + } else { + RCLCPP_WARN( + get_logger(), + "The expected planner frequency parameter is %.4f Hz. The value should to be greater" + " than 0.0 to turn on duration overrrun warning messages", parameter.as_double()); + max_planner_duration_ = 0.0; + } + } } } -} // namespace nav2_planner + result.successful = true; + return result; +} + +} // namespace nav2_planner + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_planner::PlannerServer) diff --git a/nav2_planner/test/CMakeLists.txt b/nav2_planner/test/CMakeLists.txt new file mode 100644 index 0000000000..d415d906ef --- /dev/null +++ b/nav2_planner/test/CMakeLists.txt @@ -0,0 +1,10 @@ +# Test dynamic parameters +ament_add_gtest(test_dynamic_parameters + test_dynamic_parameters.cpp +) +ament_target_dependencies(test_dynamic_parameters + ${dependencies} +) +target_link_libraries(test_dynamic_parameters + ${library_name} +) diff --git a/nav2_planner/test/test_dynamic_parameters.cpp b/nav2_planner/test/test_dynamic_parameters.cpp new file mode 100644 index 0000000000..8215e80e43 --- /dev/null +++ b/nav2_planner/test/test_dynamic_parameters.cpp @@ -0,0 +1,90 @@ +// 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 "gtest/gtest.h" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_planner/planner_server.hpp" +#include "rclcpp/rclcpp.hpp" + +class PlannerShim : public nav2_planner::PlannerServer +{ +public: + PlannerShim() + : nav2_planner::PlannerServer(rclcpp::NodeOptions()) + { + } + + // Since we cannot call configure/activate due to costmaps + // requiring TF + void setDynamicCallback() + { + auto node = shared_from_this(); + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&PlannerShim::dynamicParamsShim, this, std::placeholders::_1)); + } + + rcl_interfaces::msg::SetParametersResult + dynamicParamsShim(std::vector parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + dynamicParametersCallback(parameters); + return result; + } +}; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(WPTest, test_dynamic_parameters) +{ + auto planner = std::make_shared(); + planner->setDynamicCallback(); + + auto rec_param = std::make_shared( + planner->get_node_base_interface(), planner->get_node_topics_interface(), + planner->get_node_graph_interface(), + planner->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("expected_planner_frequency", 100.0)}); + + rclcpp::spin_until_future_complete( + planner->get_node_base_interface(), + results); + + EXPECT_EQ(planner->get_parameter("expected_planner_frequency").as_double(), 100.0); + + // test edge case for = 0 + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("expected_planner_frequency", -1.0)}); + + rclcpp::spin_until_future_complete( + planner->get_node_base_interface(), + results); + + EXPECT_EQ(planner->get_parameter("expected_planner_frequency").as_double(), -1.0); +} diff --git a/nav2_recoveries/CHANGELOG.rst b/nav2_recoveries/CHANGELOG.rst deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/nav2_recoveries/README.md b/nav2_recoveries/README.md deleted file mode 100644 index e5f93a0f99..0000000000 --- a/nav2_recoveries/README.md +++ /dev/null @@ -1,120 +0,0 @@ -**Warning**: As with the rest of `nav2`, this package is still in development and only works with Turtlebot 3 at the moment. Currently collision avoidance has not been integrated. The user is advised to not use this feature on a physical robot for safety reasons. As of now, this feature should only be used in simulations. - ---- - -# Recoveries - -The `nav2_recoveries` package implements, as the name suggests, a module for executing simple controlled robot movements such as rotating on its own axis or moving linearly. - -The package defines: -- A `Recovery` template which is used as a base class to implement specific recovery. -- The `BackUp`, `Spin` and `Stop` recoveries. - -## Overview - -*Recovery* define simple predictable movements that components can leverage for defining more complex behavior. For example, `nav2` uses recoveries for executing recovery behaviors, such as the ones defined on the [BtNavigator](../nav2_bt_navigator/README.md##Recovery-Behavior-Trees). - -Currently the package provides the following recoveries: -- **Spin** performs an in-place rotation by a given angle. -- **Back Up** performs an linear translation by a given distance. -- **Stop** brings the robot to a stationary state. - -## Implementation - -The module is implemented as a single node containing multiple recoveries and follows the `nav2` [task hierarchy](../nav2_tasks/README.md#Overview). Each recovery is defined as a `nav2_task` with corresponding *command* and *result* message definitions. - -The `Recovery` base class manages the task server, provides a robot interface and calls the recovery's update functions. - -To gain insight into the package lets go over how to implement and execute a new recoveries. - -### Defining a recovery - -In this section we'll go over how to define a new recovery and implement the corresponding recovery. - -The first step is to provide the [task definition](../nav2_tasks/README.md) inside the `nav2_tasks` package. For example, lets define a `SomeRecovery` task interface, i.e. the types of messages to use for the command and result, as well as the client and server. - -```cpp -namespace nav2_tasks -{ - -using SomeRecoveryCommand = geometry_msgs::msg::Point; -using SomeRecoveryResult = std_msgs::msg::Empty; - -using SomeRecoveryTaskClient = TaskClient; -using SomeRecoveryTaskServer = TaskServer; - -template<> -inline const char * getTaskName() -{ - return "SomeRecoveryTask"; -} - -} // namespace nav2_tasks -``` - -For this example we arbitrarily pick `geometry_msgs::msg::Point` and `std_msgs::msg::Empty` as message types for command and result. - -Next we define the class for our new recovery. This class should derive from `Recovery` and use the command and result messages defined on the corresponding task. - -```cpp -class SomeRecovery : public Recovery -``` - -On the implementation of `SomeRecovery` all we do is override `onRun` and `onCycleUpdate`. - -```cpp -using nav2_tasks - -TaskStatus SomeRecovery::onRun(const SomeRecoveryCommand::SharedPtr command) -{ - /* onRun code */ -} - -TaskStatus SomeRecovery::onCycleUpdate(SomeRecoveryResult & result) -{ - /* onCycleUpdate code */ -} -``` - -The `onRun` method is the entry point for the recovery and here we should: -- Catch the command. -- Perform checks before the main execution loop. -- Possibly do some initialization. -- Return a `nav2_tasks::TaskStatus` given the initial checks. - -The `onCycleUpdate` method is called periodically until it returns `FAILED` or `SUCCEEDED`, here we should: -- Set the robot in motion. -- Perform some unit of work. -- Check if the robot state, determine if work completed -- Return a `nav2_tasks::TaskStatus`. - -### Defining the recovery's client - -Recoveries use the `nav2_tasks` interface, so we need to define the task client: - -```cpp -nav2_tasks::TaskClient some_recovery_task_client; -``` - -To send requests we create the command and sent it over the client: - -```cpp -SomeRecoveryCommand::SharedPtr command; -// Fill command -some_recovery_task_client.sendCommand(command) -``` - -### (optional) Define the Behavior Tree action node - -For using recoveries within a behavior tree such as [bt_navigator](../nav2_bt_navigator/README.md##Navigation-Behavior-Trees), then a corresponding *action node* needs to be defined. Checkout `nav2_tasks` for examples on how to implement one. - -## Plans - -- Check for collision before executing a recovery. Issues [379](https://github.com/ros-planning/navigation2/issues/379) and [533](https://github.com/ros-planning/navigation2/issues/533). -- Remove the stop recovery, move the funcionality to the robot class. Issue [575](https://github.com/ros-planning/navigation2/issues/575) -- Consider moving `nav2_recoveries` altogether to the `nav2_robot` package. Issue [378](https://github.com/ros-planning/navigation2/issues/378). -- Depending on the feedback from the community we might want to develop this package to include a wide variety of recoveries (arcs) to support all kinds of task, navigation (lattice-based), docking, etc. -- Define smooth transitions between motions. Issue [411](https://github.com/ros-planning/navigation2/issues/411). -- Make the existing recoveries configurable for other robots. - -Refer to Github for an up-to-date [list](https://github.com/ros-planning/navigation2/issues?q=is%3Aopen+is%3Aissue+label%3Anav2_recoveries). diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp deleted file mode 100644 index b429c8bdcf..0000000000 --- a/nav2_recoveries/plugins/back_up.cpp +++ /dev/null @@ -1,145 +0,0 @@ -// Copyright (c) 2018 Intel Corporation, 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. - -#include -#include -#include -#include - -#include "back_up.hpp" -#include "nav2_util/node_utils.hpp" - -using namespace std::chrono_literals; - -namespace nav2_recoveries -{ - -BackUp::BackUp() -: Recovery(), - feedback_(std::make_shared()) -{ -} - -BackUp::~BackUp() -{ -} - -void BackUp::onConfigure() -{ - nav2_util::declare_parameter_if_not_declared( - node_, - "simulate_ahead_time", rclcpp::ParameterValue(2.0)); - node_->get_parameter("simulate_ahead_time", simulate_ahead_time_); -} - -Status BackUp::onRun(const std::shared_ptr command) -{ - if (command->target.y != 0.0 || command->target.z != 0.0) { - RCLCPP_INFO( - node_->get_logger(), "Backing up in Y and Z not supported, " - "will only move in X."); - } - - command_x_ = command->target.x; - command_speed_ = command->speed; - - if (!nav2_util::getCurrentPose( - initial_pose_, *tf_, global_frame_, robot_base_frame_, - transform_tolerance_)) - { - RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available."); - return Status::FAILED; - } - - return Status::SUCCEEDED; -} - -Status BackUp::onCycleUpdate() -{ - geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose( - current_pose, *tf_, global_frame_, robot_base_frame_, - transform_tolerance_)) - { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); - return Status::FAILED; - } - - double diff_x = initial_pose_.pose.position.x - current_pose.pose.position.x; - double diff_y = initial_pose_.pose.position.y - current_pose.pose.position.y; - double distance = sqrt(diff_x * diff_x + diff_y * diff_y); - - feedback_->distance_traveled = distance; - action_server_->publish_feedback(feedback_); - - if (distance >= abs(command_x_)) { - stopRobot(); - return Status::SUCCEEDED; - } - - // TODO(mhpanah): cmd_vel value should be passed as a parameter - auto cmd_vel = std::make_unique(); - cmd_vel->linear.y = 0.0; - cmd_vel->angular.z = 0.0; - command_x_ < 0 ? cmd_vel->linear.x = -command_speed_ : cmd_vel->linear.x = command_speed_; - - geometry_msgs::msg::Pose2D pose2d; - pose2d.x = current_pose.pose.position.x; - pose2d.y = current_pose.pose.position.y; - pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - - if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) { - stopRobot(); - RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting BackUp"); - return Status::SUCCEEDED; - } - - vel_pub_->publish(std::move(cmd_vel)); - - return Status::RUNNING; -} - -bool BackUp::isCollisionFree( - const double & distance, - geometry_msgs::msg::Twist * cmd_vel, - geometry_msgs::msg::Pose2D & pose2d) -{ - // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments - int cycle_count = 0; - 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 = 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.) { - break; - } - - if (!collision_checker_->isCollisionFree(pose2d)) { - return false; - } - } - return true; -} - -} // namespace nav2_recoveries - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(nav2_recoveries::BackUp, nav2_core::Recovery) diff --git a/nav2_recoveries/plugins/back_up.hpp b/nav2_recoveries/plugins/back_up.hpp deleted file mode 100644 index ac219e65a7..0000000000 --- a/nav2_recoveries/plugins/back_up.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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. - -#ifndef NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_ -#define NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_ - -#include -#include - -#include "nav2_recoveries/recovery.hpp" -#include "nav2_msgs/action/back_up.hpp" - -namespace nav2_recoveries -{ -using BackUpAction = nav2_msgs::action::BackUp; - -class BackUp : public Recovery -{ -public: - BackUp(); - ~BackUp(); - - Status onRun(const std::shared_ptr command) override; - - Status onCycleUpdate() override; - -protected: - bool isCollisionFree( - const double & distance, - geometry_msgs::msg::Twist * cmd_vel, - geometry_msgs::msg::Pose2D & pose2d); - - void onConfigure() override; - - double min_linear_vel_; - double max_linear_vel_; - double linear_acc_lim_; - - geometry_msgs::msg::PoseStamped initial_pose_; - double command_x_; - double command_speed_; - double simulate_ahead_time_; - - BackUpAction::Feedback::SharedPtr feedback_; -}; - -} // namespace nav2_recoveries - -#endif // NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_ diff --git a/nav2_recoveries/recovery_plugin.xml b/nav2_recoveries/recovery_plugin.xml deleted file mode 100644 index b8ccaca0ad..0000000000 --- a/nav2_recoveries/recovery_plugin.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/nav2_recoveries/test/CMakeLists.txt b/nav2_recoveries/test/CMakeLists.txt deleted file mode 100644 index 98cb9a6ec0..0000000000 --- a/nav2_recoveries/test/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -ament_add_gtest(test_recoveries - test_recoveries.cpp -) - -ament_target_dependencies(test_recoveries - ${dependencies} -) diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index ffe79adf8e..6c17481925 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -11,8 +11,10 @@ find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) nav2_package() +set(CMAKE_CXX_STANDARD 17) include_directories( include @@ -27,6 +29,7 @@ set(dependencies nav2_util nav2_core tf2 + tf2_geometry_msgs ) set(library_name nav2_regulated_pure_pursuit_controller) @@ -34,9 +37,6 @@ 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} ) @@ -62,7 +62,6 @@ endif() ament_export_include_directories(include) ament_export_libraries(${library_name}) ament_export_dependencies(${dependencies}) -ament_export_definitions("PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(nav2_core nav2_regulated_pure_pursuit_controller.xml) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 285797499c..5a34f68a33 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -22,18 +22,18 @@ In order to simply book-keeping, the global path is continuously pruned to the c 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). +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. 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. @@ -44,13 +44,13 @@ An unintended tertiary benefit of scaling the linear velocities by curvature is Mixing the proximity and curvature regulated linear velocities with the time-scaled collision checker, we see a near-perfect combination allowing the regulated pure pursuit algorithm to handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. +Note: The maximum allowed time to collision is thresholded by the lookahead point, starting in Humble. This is such that collision checking isn't significantly overshooting the path, which can cause issues in constrained environments. For example, if there were a straight-line path going towards a wall that then turned left, if this parameter was set to high, then it would detect a collision past the point of actual robot intended motion. Thusly, if a robot is moving fast, selecting further out lookahead points is not only a matter of behavioral stability for Pure Pursuit, but also gives a robot further predictive collision detection capabilities. The max allowable time parameter is still in place for slow commands, as described in detail above. + ## Configuration | Parameter | Description | |-----|----| | `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 | @@ -60,7 +60,7 @@ Mixing the proximity and curvature regulated linear velocities with the time-sca | `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 | +| `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions, limited to maximum distance of lookahead distance selected | | `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | | `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | | `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles | @@ -71,8 +71,8 @@ Mixing the proximity and curvature regulated linear velocities with the time-sca | `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 | -| `goal_dist_tol` | XY tolerance from goal to rotate to the goal heading, if `use_rotate_to_heading` is enabled. This should match or be smaller than the `GoalChecker`'s translational goal tolerance. | - +| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | +| `use_interpolation` | Enables interpolation between poses on the path for lookahead point selection. Helps sparse paths to avoid inducing discontinuous commanded velocities. Set this to false for a potential performance boost, at the expense of smooth control. | Example fully-described XML with default parameter values: @@ -85,7 +85,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: "goal_checker" controller_plugins: ["FollowPath"] progress_checker: @@ -100,8 +100,6 @@ controller_server: 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 @@ -111,7 +109,7 @@ controller_server: 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 + max_allowed_time_to_collision_up_to_carrot: 1.0 use_regulated_linear_velocity_scaling: true use_cost_regulated_linear_velocity_scaling: false regulated_linear_scaling_min_radius: 0.9 @@ -119,7 +117,8 @@ controller_server: use_rotate_to_heading: true rotate_to_heading_min_angle: 0.785 max_angular_accel: 3.2 - goal_dist_tol: 0.25 + max_robot_pose_search_dist: 10.0 + use_interpolation: false cost_scaling_dist: 0.3 cost_scaling_gain: 1.0 inflation_cost_scaling_factor: 3.0 @@ -130,7 +129,7 @@ controller_server: | 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. | +| `lookahead_arc` | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision_up_to_carrot`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. | Note: The `lookahead_arc` is also a really great speed indicator, when "full" to carrot or max time, you know you're at full speed. If 20% less, you can tell the robot is approximately 20% below maximum speed. Think of it as the collision checking bounds but also a speed guage. diff --git a/nav2_regulated_pure_pursuit_controller/doc/circle-segment-intersection.ipynb b/nav2_regulated_pure_pursuit_controller/doc/circle-segment-intersection.ipynb new file mode 100644 index 0000000000..550238704e --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/doc/circle-segment-intersection.ipynb @@ -0,0 +1,170 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "97dbdadd-7a94-4939-8ed5-c8551b662917", + "metadata": {}, + "source": [ + "# Circle Segment Intersection (for interpolation)\n", + "Here is an interactive plot that demonstrates the functionality of the formula to calculate the intersection of a line segment, and a circle centered at the origin." + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "id": "d31dc723-a6dc-400d-8b31-fe84ea6d5e45", + "metadata": {}, + "outputs": [ + { + "data": { + "application/vnd.jupyter.widget-view+json": { + "model_id": "cbfad4e8309a4ee2bef53994add83330", + "version_major": 2, + "version_minor": 0 + }, + "text/plain": [ + "VBox(children=(Label(value='A and B can be moved with the mouse. One must be inside the circle, and one must b…" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "from bqplot import *\n", + "import numpy as np\n", + "import ipywidgets as widgets\n", + "\n", + "\n", + "def circle_segment_intersection(p1, p2, r):\n", + " x1, y1 = p1\n", + " x2, y2 = p2\n", + " dx = x2 - x1\n", + " dy = y2 - y1\n", + " dr2 = dx ** 2 + dy ** 2\n", + " D = x1 * y2 - x2 * y1\n", + " d1 = x1 ** 2 + y1 ** 2\n", + " d2 = x2 ** 2 + y2 ** 2\n", + " dd = d2 - d1\n", + " sqrt_term = np.sqrt(r ** 2 * dr2 - D ** 2)\n", + " x = (D * dy + np.copysign(1.0, dd) * dx * sqrt_term) / dr2\n", + " y = (-D * dx + np.copysign(1.0, dd) * dy * sqrt_term) / dr2\n", + " return x, y\n", + "\n", + "\n", + "MAX = 5.0\n", + "x_sc = LinearScale(min=-MAX, max=MAX)\n", + "y_sc = LinearScale(min=-MAX, max=MAX)\n", + "\n", + "ax_x = Axis(label=\"x\", scale=x_sc, tick_format=\"0.0f\")\n", + "ax_y = Axis(label=\"y\", scale=y_sc, orientation=\"vertical\", tick_format=\"0.0f\")\n", + "\n", + "points = Scatter(\n", + " names=[\"A\", \"B\"], x=[0.0, 3.0], y=[2.0, 4.0], scales={\"x\": x_sc, \"y\": y_sc}, enable_move=True\n", + ")\n", + "\n", + "\n", + "def get_circle(r):\n", + " t = np.linspace(0, 2 * np.pi)\n", + " x = r * np.cos(t)\n", + " y = r * np.sin(t)\n", + " return x, y\n", + "\n", + "radius_slider = widgets.FloatSlider(min=0.0, max=MAX, value=3.0, description=\"Circle radius\")\n", + "circle_x, circle_y = get_circle(radius_slider.value)\n", + "\n", + "circle = Lines(x=circle_x, y=circle_y, scales={\"x\": x_sc, \"y\": y_sc}, colors=[\"green\"])\n", + "\n", + "x1, x2 = points.x\n", + "y1, y2 = points.y\n", + "xi, yi = circle_segment_intersection((x1, y1), (x2, y2), radius_slider.value)\n", + "\n", + "intersection = Scatter(\n", + " names=[\"C\"],\n", + " x=[xi],\n", + " y=[yi],\n", + " scales={\"x\": x_sc, \"y\": y_sc},\n", + " enable_move=False,\n", + " colors=[\"purple\"],\n", + ")\n", + "\n", + "fig = Figure(axes=[ax_x, ax_y], marks=[circle, points, intersection])\n", + "\n", + "fig.max_aspect_ratio = 1\n", + "fig.min_aspect_ratio = 1\n", + "\n", + "\n", + "def both_inside_or_both_outside_circle(points, r):\n", + " x1, x2 = points.x\n", + " y1, y2 = points.y\n", + " d1 = x1 ** 2 + y1 ** 2\n", + " d2 = x2 ** 2 + y2 ** 2\n", + " if d1 < r ** 2 and d2 < r ** 2:\n", + " return True\n", + " elif d1 > r ** 2 and d2 > r ** 2:\n", + " return True\n", + " else:\n", + " return False\n", + "\n", + "\n", + "def update_circle(message):\n", + " circle_x, circle_y = get_circle(radius_slider.value)\n", + " circle.x = circle_x\n", + " circle.y = circle_y\n", + " update_intersection(message)\n", + "\n", + "\n", + "def update_intersection(message):\n", + " x1, x2 = points.x\n", + " y1, y2 = points.y\n", + " r = radius_slider.value\n", + " if both_inside_or_both_outside_circle(points, r):\n", + " circle.colors = [\"red\"]\n", + " intersection.x = []\n", + " intersection.y = []\n", + " else:\n", + " circle.colors = [\"green\"]\n", + " xi, yi = circle_segment_intersection((x1, y1), (x2, y2), r)\n", + " intersection.x = [xi]\n", + " intersection.y = [yi]\n", + "\n", + "\n", + "points.observe(update_intersection, [\"x\", \"y\"])\n", + "\n", + "radius_slider.observe(update_circle, \"value\")\n", + "\n", + "widgets.VBox(\n", + " [\n", + " widgets.Label(\n", + " \"A and B can be moved with the mouse. One must be inside the circle, and one must be outside.\",\n", + " fixed=True,\n", + " ),\n", + " radius_slider,\n", + " fig,\n", + " ]\n", + ")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 7cb3fafa97..9224921de2 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -13,19 +13,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ -#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ +#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 +#include "nav2_costmap_2d/footprint_collision_checker.hpp" #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 @@ -56,9 +59,9 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param costmap_ros Costmap2DROS object of environment */ void configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) override; + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; /** * @brief Cleanup controller state machine @@ -84,12 +87,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller * * @param pose Current robot pose * @param velocity Current robot velocity - * @param results Output param, if not NULL, will be filled in with full evaluation results + * @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) override; + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * /*goal_checker*/) override; /** * @brief nav2_core setPlan - Sets the global plan @@ -97,13 +101,25 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ 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 + * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint + * Points ineligible to be selected as a lookahead point if they are any of the following: + * - Outside the local_costmap (collision avoidance cannot be assured) * @param pose pose to transform * @return Path in new frame */ - nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose); + nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose); /** * @brief Transform a pose to another frame. @@ -155,7 +171,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @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, + void rotateToHeading( + double & linear_vel, double & angular_vel, const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed); /** @@ -164,20 +181,25 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param carrot_pose Pose of carrot * @param linear_vel linear velocity to forward project * @param angular_vel angular velocity to forward project + * @param carrot_dist Distance to the carrot for PP * @return Whether collision is imminent */ bool isCollisionImminent( const geometry_msgs::msg::PoseStamped &, - const double &, const double &); + const double &, const double &, + const double &); /** - * @brief Whether point is in collision + * @brief checks for collision at projected pose * @param x Pose of pose x * @param y Pose of pose y + * @param theta orientation of Yaw * @return Whether in collision */ - bool inCollision(const double & x, const double & y); - + bool inCollision( + const double & x, + const double & y, + const double & theta); /** * @brief Cost at a point * @param x Pose of pose x @@ -198,7 +220,21 @@ class RegulatedPurePursuitController : public nav2_core::Controller 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); + const double & pose_cost, double & linear_vel, double & sign); + + /** + * @brief Find the intersection a circle and a line segment. + * This assumes the circle is centered at the origin. + * If no intersection is found, a floating point error will occur. + * @param p1 first endpoint of line segment + * @param p2 second endpoint of line segment + * @param r radius of circle + * @return point of intersection + */ + static geometry_msgs::msg::Point circleSegmentIntersection( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r); /** * @brief Get lookahead point @@ -208,13 +244,35 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + /** + * @brief checks for the cusp position + * @param pose Pose input to determine the cusp position + * @return robot distance from the cusp + */ + double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan); + + /** + * Get the greatest extent of the costmap in meters from the center. + * @return max of distance from center in meters to edge of costmap + */ + double getCostmapMaxExtent() const; + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; 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")}; + rclcpp::Clock::SharedPtr clock_; - double desired_linear_vel_; + double desired_linear_vel_, base_desired_linear_vel_; double lookahead_dist_; double rotate_to_heading_angular_vel_; double max_lookahead_dist_; @@ -222,10 +280,9 @@ class RegulatedPurePursuitController : public nav2_core::Controller double lookahead_time_; 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_; + double max_allowed_time_to_collision_up_to_carrot_; bool use_regulated_linear_velocity_scaling_; bool use_cost_regulated_linear_velocity_scaling_; double cost_scaling_dist_; @@ -237,13 +294,23 @@ class RegulatedPurePursuitController : public nav2_core::Controller double max_angular_accel_; double rotate_to_heading_min_angle_; double goal_dist_tol_; + bool allow_reversing_; + double max_robot_pose_search_dist_; + bool use_interpolation_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; - std::shared_ptr> carrot_pub_; + std::shared_ptr> + carrot_pub_; std::shared_ptr> carrot_arc_pub_; + std::unique_ptr> + collision_checker_; + + // Dynamic parameters handler + std::mutex mutex_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; }; } // namespace nav2_regulated_pure_pursuit_controller -#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 14c4e1c1d9..cffd2c1f49 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 0.4.7 + 1.1.0 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh @@ -19,6 +19,7 @@ nav2_msgs pluginlib tf2 + tf2_geometry_msgs ament_cmake_gtest ament_lint_common @@ -26,6 +27,7 @@ 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 index f71b519751..6a9a97c6b7 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -15,64 +15,47 @@ #include #include +#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" - -using std::hypot; +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" 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 - -double clamp(double value, double min, double max) { - if (value < min) return min; - if (value > max) return max; - return value; -} +using rcl_interfaces::msg::ParameterType; namespace nav2_regulated_pure_pursuit_controller { -/** - * Find element in iterator with the minimum calculated value - */ -template -Iter min_by(Iter begin, Iter end, Getter getCompareVal) +void RegulatedPurePursuitController::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) { - 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; - } + auto node = parent.lock(); + node_ = parent; + if (!node) { + throw nav2_core::PlannerException("Unable to lock node!"); } - return lowest_it; -} -void RegulatedPurePursuitController::configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) -{ costmap_ros_ = costmap_ros; costmap_ = costmap_ros_->getCostmap(); tf_ = tf; plugin_name_ = name; logger_ = node->get_logger(); + clock_ = node->get_clock(); 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)); @@ -94,13 +77,13 @@ void RegulatedPurePursuitController::configure( 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)); + node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + 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)); + 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( @@ -118,45 +101,90 @@ void RegulatedPurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( - node, plugin_name_ + ".goal_dist_tol", rclcpp::ParameterValue(0.25)); + node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_robot_pose_search_dist", + rclcpp::ParameterValue(getCostmapMaxExtent())); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_interpolation", + rclcpp::ParameterValue(true)); node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); + base_desired_linear_vel_ = desired_linear_vel_; 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_ + ".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", + 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_ + ".min_approach_linear_velocity", + min_approach_linear_velocity_); + node->get_parameter( + plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + max_allowed_time_to_collision_up_to_carrot_); + 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_ + ".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(plugin_name_ + ".goal_dist_tol", goal_dist_tol_); + node->get_parameter(plugin_name_ + ".allow_reversing", allow_reversing_); node->get_parameter("controller_frequency", control_frequency); + node->get_parameter( + plugin_name_ + ".max_robot_pose_search_dist", + max_robot_pose_search_dist_); + node->get_parameter( + plugin_name_ + ".use_interpolation", + use_interpolation_); 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."); + 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; } + /** Possible to drive in reverse direction if and only if + "use_rotate_to_heading" parameter is set to false **/ + + if (use_rotate_to_heading_ && allow_reversing_) { + RCLCPP_WARN( + logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. By default setting use_rotate_to_heading true"); + allow_reversing_ = 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); + + // initialize collision checker and set costmap + collision_checker_ = std::make_unique>(costmap_); + collision_checker_->setCostmap(costmap_); } void RegulatedPurePursuitController::cleanup() @@ -181,6 +209,12 @@ void RegulatedPurePursuitController::activate() global_path_pub_->on_activate(); carrot_pub_->on_activate(); carrot_arc_pub_->on_activate(); + // Add callback for dynamic parameters + auto node = node_.lock(); + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &RegulatedPurePursuitController::dynamicParametersCallback, + this, std::placeholders::_1)); } void RegulatedPurePursuitController::deactivate() @@ -193,6 +227,7 @@ void RegulatedPurePursuitController::deactivate() global_path_pub_->on_deactivate(); carrot_pub_->on_deactivate(); carrot_arc_pub_->on_deactivate(); + dyn_params_handler_.reset(); } std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( @@ -206,14 +241,15 @@ std::unique_ptr RegulatedPurePursuitController return carrot_msg; } -double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) +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 = clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + lookahead_dist = fabs(speed.linear.x) * lookahead_time_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); } return lookahead_dist; @@ -221,13 +257,37 @@ double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs: geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & speed) + const geometry_msgs::msg::Twist & speed, + nav2_core::GoalChecker * goal_checker) { + std::lock_guard lock_reinit(mutex_); + + // 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); + double lookahead_dist = getLookAheadDistance(speed); + + // Check for reverse driving + if (allow_reversing_) { + // Cusp check + double dist_to_cusp = findVelocitySignChange(transformed_plan); + + // if the lookahead distance is further than the cusp, use the cusp distance instead + if (dist_to_cusp < lookahead_dist) { + lookahead_dist = dist_to_cusp; + } + } + auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); carrot_pub_->publish(createCarrotMsg(carrot_pose)); @@ -245,6 +305,12 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; } + // Setting the velocity direction + double sign = 1.0; + if (allow_reversing_) { + sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; + } + linear_vel = desired_linear_vel_; // Make sure we're in compliance with basic constraints @@ -258,14 +324,15 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity applyConstraints( fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, curvature, speed, - costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel); + costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel, sign); - // Apply curvature to angular velocity after constraining linear velocity - angular_vel = linear_vel * curvature; + // Apply curvature to angular velocity after constraining linear velocity + angular_vel = sign * linear_vel * curvature; } // Collision checking on this velocity heading - if (isCollisionImminent(pose, linear_vel, angular_vel)) { + const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); + if (isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { throw nav2_core::PlannerException("RegulatedPurePursuitController detected collision ahead!"); } @@ -305,7 +372,42 @@ void RegulatedPurePursuitController::rotateToHeading( 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 = clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); + angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); +} + +geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r) +{ + // Formula for intersection of a line with a circle centered at the origin, + // modified to always return the point that is on the segment between the two points. + // https://mathworld.wolfram.com/Circle-LineIntersection.html + // This works because the poses are transformed into the robot frame. + // This can be derived from solving the system of equations of a line and a circle + // which results in something that is just a reformulation of the quadratic formula. + // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at + // https://www.desmos.com/calculator/td5cwbuocd + double x1 = p1.x; + double x2 = p2.x; + double y1 = p1.y; + double y2 = p2.y; + + double dx = x2 - x1; + double dy = y2 - y1; + double dr2 = dx * dx + dy * dy; + double D = x1 * y2 - x2 * y1; + + // Augmentation to only return point within segment + double d1 = x1 * x1 + y1 * y1; + double d2 = x2 * x2 + y2 * y2; + double dd = d2 - d1; + + geometry_msgs::msg::Point p; + double sqrt_term = std::sqrt(r * r * dr2 - D * D); + p.x = (D * dy + std::copysign(1.0, dd) * dx * sqrt_term) / dr2; + p.y = (-D * dx + std::copysign(1.0, dd) * dy * sqrt_term) / dr2; + return p; } geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( @@ -321,6 +423,21 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin // 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()); + } else if (use_interpolation_ && goal_pose_it != transformed_plan.poses.begin()) { + // Find the point on the line segment between the two poses + // that is exactly the lookahead distance away from the robot pose (the origin) + // This can be found with a closed form for the intersection of a segment and a circle + // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle, + // and goal_pose is guaranteed to be outside the circle. + auto prev_pose_it = std::prev(goal_pose_it); + auto point = circleSegmentIntersection( + prev_pose_it->pose.position, + goal_pose_it->pose.position, lookahead_dist); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = prev_pose_it->header.frame_id; + pose.header.stamp = goal_pose_it->header.stamp; + pose.pose.position = point; + return pose; } return *goal_pose_it; @@ -328,13 +445,17 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin bool RegulatedPurePursuitController::isCollisionImminent( const geometry_msgs::msg::PoseStamped & robot_pose, - const double & linear_vel, const double & angular_vel) + const double & linear_vel, const double & angular_vel, + const double & carrot_dist) { // 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)) { + if (inCollision( + robot_pose.pose.position.x, robot_pose.pose.position.y, + tf2::getYaw(robot_pose.pose.orientation))) + { return true; } @@ -346,20 +467,31 @@ bool RegulatedPurePursuitController::isCollisionImminent( 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() / fabs(linear_vel); + double projection_time = 0.0; + if (fabs(linear_vel) < 0.01 && fabs(angular_vel) > 0.01) { + // rotating to heading at goal or toward path + // Equation finds the angular distance required for the largest + // part of the robot radius to move to another costmap cell: + // theta_min = 2.0 * sin ((res/2) / r_max) + // via isosceles triangle r_max-r_max-resolution, + // dividing by angular_velocity gives us a timestep. + double max_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius(); + projection_time = + 2.0 * sin((costmap_->getResolution() / 2) / max_radius) / fabs(angular_vel); + } else { + // Normal path tracking + projection_time = costmap_->getResolution() / fabs(linear_vel); + } + const geometry_msgs::msg::Point & robot_xy = robot_pose.pose.position; 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); + // only forward simulate within time requested int i = 1; - while (true) { - // only forward simulate within time requested - if (i * projection_time > max_allowed_time_to_collision_) { - break; - } - + while (i * projection_time < max_allowed_time_to_collision_up_to_carrot_) { i++; // apply velocity at curr_pose over distance @@ -367,14 +499,19 @@ bool RegulatedPurePursuitController::isCollisionImminent( curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); curr_pose.theta += projection_time * angular_vel; + // check if past carrot pose, where no longer a thoughtfully valid command + if (hypot(curr_pose.x - robot_xy.x, curr_pose.y - robot_xy.y) > carrot_dist) { + break; + } + // 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)) { + // check for collision at the projected pose + if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) { carrot_arc_pub_->publish(arc_pts_msg); return true; } @@ -385,24 +522,47 @@ bool RegulatedPurePursuitController::isCollisionImminent( return false; } -bool RegulatedPurePursuitController::inCollision(const double & x, const double & y) +bool RegulatedPurePursuitController::inCollision( + const double & x, + const double & y, + const double & theta) { unsigned int mx, my; - costmap_->worldToMap(x, y, mx, my); - unsigned char cost = costmap_->getCost(mx, my); + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 30000, + "The dimensions of the costmap is too small to successfully check for " + "collisions as far ahead as requested. Proceed at your own risk, slow the robot, or " + "increase your costmap size."); + return false; + } - if (costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) { - return cost >= INSCRIBED_INFLATED_OBSTACLE && cost != NO_INFORMATION; - } else { - return cost >= INSCRIBED_INFLATED_OBSTACLE; + double footprint_cost = collision_checker_->footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint()); + if (footprint_cost == static_cast(NO_INFORMATION) && + costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) + { + return false; } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost >= static_cast(LETHAL_OBSTACLE); } double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) { unsigned int mx, my; - costmap_->worldToMap(x, y, mx, my); + + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_FATAL( + logger_, + "The dimensions of the costmap is too small to fully include your robot's footprint, " + "thusly the robot cannot proceed further"); + throw nav2_core::PlannerException( + "RegulatedPurePursuitController: Dimensions of the costmap are too small " + "to encapsulate the robot footprint at current speeds!"); + } unsigned char cost = costmap_->getCost(mx, my); return static_cast(cost); @@ -411,7 +571,7 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double 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) + const double & pose_cost, double & linear_vel, double & sign) { double curvature_vel = linear_vel; double cost_vel = linear_vel; @@ -431,7 +591,7 @@ void RegulatedPurePursuitController::applyConstraints( { 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; + 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_; @@ -443,8 +603,11 @@ void RegulatedPurePursuitController::applyConstraints( 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()) { + // end of the path. We'll scale linear velocity by error to slow to a smooth stop. + // This expression is eq. to (1) holding time to goal, t, constant using the theoretical + // lookahead distance and proposed velocity and (2) using t with the actual lookahead + // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). + if (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_) { @@ -457,8 +620,9 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = std::min(linear_vel, approach_vel); } - // Limit linear velocities to be valid and kinematically feasible, v = v0 + a * dt - linear_vel = clamp(linear_vel, 0.0, desired_linear_vel_); + // Limit linear velocities to be valid + linear_vel = std::clamp(fabs(linear_vel), 0.0, desired_linear_vel_); + linear_vel = sign * linear_vel; } void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) @@ -466,6 +630,24 @@ 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) { @@ -475,29 +657,32 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( // 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)) - { + 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; + double max_costmap_extent = getCostmapMaxExtent(); + + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_); // First find the closest pose on the path to the robot + // bounded by when the path turns around (if it does) so we don't get a pose from a later + // portion of the path auto transformation_begin = - min_by( - global_plan_.poses.begin(), global_plan_.poses.end(), + nav2_util::geometry_utils::min_by( + global_plan_.poses.begin(), closest_pose_upper_bound, [&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. + // Find points up to max_transform_dist so we only 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; + transformation_begin, global_plan_.poses.end(), + [&](const auto & pose) { + return euclidean_distance(pose, robot_pose) > max_costmap_extent; }); // Lambda to transform a PoseStamped from global frame to local @@ -531,6 +716,36 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( return transformed_plan; } +double RegulatedPurePursuitController::findVelocitySignChange( + const nav_msgs::msg::Path & transformed_plan) +{ + // Iterating through the transformed global path to determine the position of the cusp + for (unsigned int pose_id = 1; pose_id < transformed_plan.poses.size() - 1; ++pose_id) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = transformed_plan.poses[pose_id].pose.position.x - + transformed_plan.poses[pose_id - 1].pose.position.x; + double oa_y = transformed_plan.poses[pose_id].pose.position.y - + transformed_plan.poses[pose_id - 1].pose.position.y; + double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x - + transformed_plan.poses[pose_id].pose.position.x; + double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y - + transformed_plan.poses[pose_id].pose.position.y; + + /* Checking for the existance of cusp, in the path, using the dot product + and determine it's distance from the robot. If there is no cusp in the path, + then just determine the distance to the goal location. */ + if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) { + // returning the distance if there is a cusp + // The transformed path is in the robots frame, so robot is at the origin + return hypot( + transformed_plan.poses[pose_id].pose.position.x, + transformed_plan.poses[pose_id].pose.position.y); + } + } + + return std::numeric_limits::max(); +} + bool RegulatedPurePursuitController::transformPose( const std::string frame, const geometry_msgs::msg::PoseStamped & in_pose, @@ -551,7 +766,99 @@ bool RegulatedPurePursuitController::transformPose( return false; } -} // namespace nav2_pure_pursuit_controller +double RegulatedPurePursuitController::getCostmapMaxExtent() const +{ + const double max_costmap_dim_meters = std::max( + costmap_->getSizeInMetersX(), costmap_->getSizeInMetersX()); + return max_costmap_dim_meters / 2.0; +} + + +rcl_interfaces::msg::SetParametersResult +RegulatedPurePursuitController::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(mutex_); + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".inflation_cost_scaling_factor") { + if (parameter.as_double() <= 0.0) { + RCLCPP_WARN( + logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Ignoring parameter update."); + continue; + } + inflation_cost_scaling_factor_ = parameter.as_double(); + } else if (name == plugin_name_ + ".desired_linear_vel") { + desired_linear_vel_ = parameter.as_double(); + base_desired_linear_vel_ = parameter.as_double(); + } else if (name == plugin_name_ + ".lookahead_dist") { + lookahead_dist_ = parameter.as_double(); + } else if (name == plugin_name_ + ".max_lookahead_dist") { + max_lookahead_dist_ = parameter.as_double(); + } else if (name == plugin_name_ + ".min_lookahead_dist") { + min_lookahead_dist_ = parameter.as_double(); + } else if (name == plugin_name_ + ".lookahead_time") { + lookahead_time_ = parameter.as_double(); + } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { + rotate_to_heading_angular_vel_ = parameter.as_double(); + } else if (name == plugin_name_ + ".min_approach_linear_velocity") { + min_approach_linear_velocity_ = parameter.as_double(); + } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { + max_allowed_time_to_collision_up_to_carrot_ = parameter.as_double(); + } else if (name == plugin_name_ + ".cost_scaling_dist") { + cost_scaling_dist_ = parameter.as_double(); + } else if (name == plugin_name_ + ".cost_scaling_gain") { + cost_scaling_gain_ = parameter.as_double(); + } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") { + regulated_linear_scaling_min_radius_ = parameter.as_double(); + } else if (name == plugin_name_ + ".transform_tolerance") { + double transform_tolerance = parameter.as_double(); + transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") { + regulated_linear_scaling_min_speed_ = parameter.as_double(); + } else if (name == plugin_name_ + ".max_angular_accel") { + max_angular_accel_ = parameter.as_double(); + } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") { + rotate_to_heading_min_angle_ = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { + use_velocity_scaled_lookahead_dist_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { + use_regulated_linear_velocity_scaling_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { + use_cost_regulated_linear_velocity_scaling_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_rotate_to_heading") { + if (parameter.as_bool() && allow_reversing_) { + RCLCPP_WARN( + logger_, "Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. Rejecting parameter update."); + continue; + } + use_rotate_to_heading_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".allow_reversing") { + if (use_rotate_to_heading_ && parameter.as_bool()) { + RCLCPP_WARN( + logger_, "Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. Rejecting parameter update."); + continue; + } + allow_reversing_ = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_regulated_pure_pursuit_controller // Register this controller as a nav2_core plugin PLUGINLIB_EXPORT_CLASS( diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt index aee6297fcb..3e8d4c5b0a 100644 --- a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -1,6 +1,7 @@ # tests for regulated PP ament_add_gtest(test_regulated_pp test_regulated_pp.cpp + path_utils/path_utils.cpp ) ament_target_dependencies(test_regulated_pp ${dependencies} @@ -8,3 +9,7 @@ ament_target_dependencies(test_regulated_pp target_link_libraries(test_regulated_pp ${library_name} ) + +# Path utils test +ament_add_gtest(test_path_utils path_utils/test_path_utils.cpp path_utils/path_utils.cpp) +ament_target_dependencies(test_path_utils nav_msgs geometry_msgs tf2_geometry_msgs) diff --git a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp new file mode 100644 index 0000000000..4b516ec1f3 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp @@ -0,0 +1,88 @@ +// Copyright (c) 2022 Adam Aposhian +// +// 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 "path_utils.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace path_utils +{ + +void append_transform_to_path( + nav_msgs::msg::Path & path, + tf2::Transform & relative_transform) +{ + // Add a new empty pose + path.poses.emplace_back(); + // Get the previous, last pose (after the emplace_back so the reference isn't invalidated) + auto & previous_pose = *(path.poses.end() - 2); + auto & new_pose = path.poses.back(); + + // get map_transform of previous_pose + tf2::Transform map_transform; + tf2::fromMsg(previous_pose.pose, map_transform); + + tf2::Transform full_transform; + full_transform.mult(map_transform, relative_transform); + + tf2::toMsg(full_transform, new_pose.pose); + new_pose.header.frame_id = previous_pose.header.frame_id; +} + +void Straight::append(nav_msgs::msg::Path & path, double spacing) const +{ + auto num_points = std::floor(length_ / spacing); + path.poses.reserve(path.poses.size() + num_points); + tf2::Transform translation(tf2::Quaternion::getIdentity(), tf2::Vector3(spacing, 0.0, 0.0)); + for (size_t i = 1; i <= num_points; ++i) { + append_transform_to_path(path, translation); + } +} + +double chord_length(double radius, double radians) +{ + return 2 * radius * sin(radians / 2); +} + +void Arc::append(nav_msgs::msg::Path & path, double spacing) const +{ + double length = radius_ * std::abs(radians_); + size_t num_points = std::floor(length / spacing); + double radians_per_step = radians_ / num_points; + tf2::Transform transform( + tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), radians_per_step), + tf2::Vector3(chord_length(radius_, std::abs(radians_per_step)), 0.0, 0.0)); + path.poses.reserve(path.poses.size() + num_points); + for (size_t i = 0; i < num_points; ++i) { + append_transform_to_path(path, transform); + } +} + +nav_msgs::msg::Path generate_path( + geometry_msgs::msg::PoseStamped start, + double spacing, + std::initializer_list> segments) +{ + nav_msgs::msg::Path path; + path.header = start.header; + path.poses.push_back(start); + for (const auto & segment : segments) { + segment->append(path, spacing); + } + return path; +} + +} // namespace path_utils diff --git a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp new file mode 100644 index 0000000000..c079e21614 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp @@ -0,0 +1,111 @@ +// Copyright (c) 2022 FireFly Automatix +// +// 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. +// +// Author: Adam Aposhian + +#ifndef PATH_UTILS__PATH_UTILS_HPP_ +#define PATH_UTILS__PATH_UTILS_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" + +namespace path_utils +{ + +/** + * Build human-readable test paths + */ +class PathSegment +{ +public: + virtual void append(nav_msgs::msg::Path & path, double spacing) const = 0; + virtual ~PathSegment() {} +}; + +class Arc : public PathSegment +{ +public: + explicit Arc(double radius, double radians) + : radius_(radius), radians_(radians) {} + void append(nav_msgs::msg::Path & path, double spacing) const override; + +private: + double radius_; + double radians_; +}; + +class Straight : public PathSegment +{ +public: + explicit Straight(double length) + : length_(length) {} + void append(nav_msgs::msg::Path & path, double spacing) const override; + +private: + double length_; +}; + +class LeftTurn : public Arc +{ +public: + explicit LeftTurn(double radius) + : Arc(radius, M_PI_2) {} +}; + +class RightTurn : public Arc +{ +public: + explicit RightTurn(double radius) + : Arc(radius, -M_PI_2) {} +}; + +class LeftTurnAround : public Arc +{ +public: + explicit LeftTurnAround(double radius) + : Arc(radius, M_PI) {} +}; + +class RightTurnAround : public Arc +{ +public: + explicit RightTurnAround(double radius) + : Arc(radius, -M_PI) {} +}; + +class LeftCircle : public Arc +{ +public: + explicit LeftCircle(double radius) + : Arc(radius, 2.0 * M_PI) {} +}; + +class RightCircle : public Arc +{ +public: + explicit RightCircle(double radius) + : Arc(radius, -2.0 * M_PI) {} +}; + +nav_msgs::msg::Path generate_path( + geometry_msgs::msg::PoseStamped start, + double spacing, + std::initializer_list> segments); + +} // namespace path_utils + +#endif // PATH_UTILS__PATH_UTILS_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/test/path_utils/test_path_utils.cpp b/nav2_regulated_pure_pursuit_controller/test/path_utils/test_path_utils.cpp new file mode 100644 index 0000000000..795555521a --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/path_utils/test_path_utils.cpp @@ -0,0 +1,128 @@ +// Copyright (c) 2022 Adam Aposhian +// +// 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 "path_utils.hpp" +#include "gtest/gtest.h" + +using namespace path_utils; // NOLINT + +TEST(PathUtils, test_generate_straight) +{ + geometry_msgs::msg::PoseStamped start; + start.header.frame_id = "test_frame"; + + constexpr double path_length = 2.0; + constexpr double spacing = 1.0; + + auto path = generate_path( + start, spacing, { + std::make_unique(path_length) + }); + EXPECT_EQ(path.poses.size(), 3u); + for (const auto & pose : path.poses) { + EXPECT_EQ(pose.header.frame_id, start.header.frame_id); + } + EXPECT_DOUBLE_EQ(path.poses[0].pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(path.poses[0].pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(path.poses[0].pose.position.z, 0.0); + + EXPECT_NEAR(path.poses[1].pose.position.x, 1.0, 0.1); + EXPECT_NEAR(path.poses[1].pose.position.y, 0.0, 0.1); + EXPECT_NEAR(path.poses[1].pose.position.z, 0.0, 0.1); + + EXPECT_NEAR(path.poses[2].pose.position.x, 2.0, 0.1); + EXPECT_NEAR(path.poses[2].pose.position.y, 0.0, 0.1); + EXPECT_NEAR(path.poses[2].pose.position.z, 0.0, 0.1); +} + +TEST(PathUtils, test_half_turn) +{ + // Start at a more interesting place, turned the other way + geometry_msgs::msg::PoseStamped start; + start.header.frame_id = "map"; + start.pose.position.x = 1.0; + start.pose.position.y = -1.0; + start.pose.orientation.x = 0.0; + start.pose.orientation.y = 0.0; + start.pose.orientation.z = 1.0; + start.pose.orientation.w = 0.0; + + constexpr double spacing = 0.1; + constexpr double radius = 2.0; + + auto path = generate_path( + start, spacing, { + std::make_unique(radius), + }); + constexpr double expected_path_length = M_PI * radius; + EXPECT_NEAR(path.poses.size(), 1 + static_cast(expected_path_length / spacing), 10); + for (const auto & pose : path.poses) { + EXPECT_EQ(pose.header.frame_id, start.header.frame_id); + } + + // Check the last pose + auto & last_pose = path.poses.back(); + auto & last_position = last_pose.pose.position; + EXPECT_NEAR(last_position.x, 1.0, 0.2); + EXPECT_NEAR(last_position.y, 3.0, 0.2); + EXPECT_DOUBLE_EQ(last_position.z, 0.0); + + // Should be facing forward now + auto & last_orientation = last_pose.pose.orientation; + EXPECT_NEAR(last_orientation.x, 0.0, 0.1); + EXPECT_NEAR(last_orientation.y, 0.0, 0.1); + EXPECT_NEAR(last_orientation.z, 0.0, 0.1); + EXPECT_NEAR(last_orientation.w, 1.0, 0.1); +} + +TEST(PathUtils, test_generate_all) +{ + geometry_msgs::msg::PoseStamped start; + start.header.frame_id = "map"; + + constexpr double spacing = 0.1; + + auto path = generate_path( + start, spacing, { + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0, 2 * M_PI), // another circle + }); + constexpr double expected_path_length = 1.0 + 2.0 * (M_PI_2 + M_PI_2) + 2.0 * (M_PI) +3.0 * + (2.0 * M_PI); + EXPECT_NEAR(path.poses.size(), 1 + static_cast(expected_path_length / spacing), 50); + for (const auto & pose : path.poses) { + EXPECT_EQ(pose.header.frame_id, start.header.frame_id); + } + + // Check the last pose + auto & last_pose = path.poses.back(); + auto & last_position = last_pose.pose.position; + EXPECT_NEAR(last_position.x, 3.0, 0.5); + EXPECT_NEAR(last_position.y, 6.0, 0.5); + EXPECT_DOUBLE_EQ(last_position.z, 0.0); + + auto & last_orientation = last_pose.pose.orientation; + EXPECT_NEAR(last_orientation.x, 0.0, 0.1); + EXPECT_NEAR(last_orientation.y, 0.0, 0.1); + EXPECT_NEAR(last_orientation.z, 0.0, 0.1); + EXPECT_NEAR(last_orientation.w, 1.0, 0.1); +} diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 9389b43bff..9f65a8554f 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -16,12 +16,16 @@ #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 "path_utils/path_utils.hpp" #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_core/exceptions.hpp" class RclCppFixture { @@ -34,33 +38,41 @@ RclCppFixture g_rclcppfixture; class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController { public: - BasicAPIRPP() : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {}; + BasicAPIRPP() + : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {} - nav_msgs::msg::Path getPlan() {return global_plan_;}; + nav_msgs::msg::Path getPlan() {return global_plan_;} - double getSpeed() {return desired_linear_vel_;}; + 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;}; + 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;} double getLookAheadDistanceWrapper(const geometry_msgs::msg::Twist & twist) { return getLookAheadDistance(twist); - }; + } + + static geometry_msgs::msg::Point circleSegmentIntersectionWrapper( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r) + { + return circleSegmentIntersection(p1, p2, r); + } 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) @@ -73,20 +85,34 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure return shouldRotateToGoalHeading(carrot_pose); } - void rotateToHeadingWrapper(double & linear_vel, double & angular_vel, + 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) + 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 & sign) { - return applyConstraints(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + return applyConstraints( + dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + linear_vel, sign); } + double findVelocitySignChangeWrapper( + const nav_msgs::msg::Path & transformed_plan) + { + return findVelocitySignChange(transformed_plan); + } + + nav_msgs::msg::Path transformGlobalPlanWrapper( + const geometry_msgs::msg::PoseStamped & pose) + { + return transformGlobalPlan(pose); + } }; TEST(RegulatedPurePursuitTest, basicAPI) @@ -98,6 +124,7 @@ TEST(RegulatedPurePursuitTest, basicAPI) // instantiate auto ctrl = std::make_shared(); + costmap->on_configure(rclcpp_lifecycle::State()); ctrl->configure(node, name, tf, costmap); ctrl->activate(); ctrl->deactivate(); @@ -110,6 +137,18 @@ TEST(RegulatedPurePursuitTest, basicAPI) 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) @@ -128,6 +167,162 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) EXPECT_EQ(rtn->point.z, 0.01); } +TEST(RegulatedPurePursuitTest, findVelocitySignChange) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPPfindVelocitySignChange"); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "smb"; + auto time = node->get_clock()->now(); + pose.header.stamp = time; + pose.pose.position.x = 1.0; + pose.pose.position.y = 0.0; + + nav_msgs::msg::Path path; + path.poses.resize(3); + path.header.frame_id = "smb"; + path.header.stamp = pose.header.stamp; + path.poses[0].pose.position.x = 1.0; + path.poses[0].pose.position.y = 1.0; + path.poses[1].pose.position.x = 2.0; + path.poses[1].pose.position.y = 2.0; + path.poses[2].pose.position.x = -1.0; + path.poses[2].pose.position.y = -1.0; + ctrl->setPlan(path); + auto rtn = ctrl->findVelocitySignChangeWrapper(path); + EXPECT_EQ(rtn, sqrt(8.0)); + + path.poses[2].pose.position.x = 3.0; + path.poses[2].pose.position.y = 3.0; + ctrl->setPlan(path); + rtn = ctrl->findVelocitySignChangeWrapper(path); + EXPECT_EQ(rtn, std::numeric_limits::max()); +} + +using CircleSegmentIntersectionParam = std::tuple< + std::pair, + std::pair, + double, + std::pair +>; + +class CircleSegmentIntersectionTest + : public ::testing::TestWithParam +{}; + +TEST_P(CircleSegmentIntersectionTest, circleSegmentIntersection) +{ + auto pair1 = std::get<0>(GetParam()); + auto pair2 = std::get<1>(GetParam()); + auto r = std::get<2>(GetParam()); + auto expected_pair = std::get<3>(GetParam()); + auto pair_to_point = [](std::pair p) -> geometry_msgs::msg::Point { + geometry_msgs::msg::Point point; + point.x = p.first; + point.y = p.second; + point.z = 0.0; + return point; + }; + auto p1 = pair_to_point(pair1); + auto p2 = pair_to_point(pair2); + auto actual = BasicAPIRPP::circleSegmentIntersectionWrapper(p1, p2, r); + auto expected_point = pair_to_point(expected_pair); + EXPECT_DOUBLE_EQ(actual.x, expected_point.x); + EXPECT_DOUBLE_EQ(actual.y, expected_point.y); + // Expect that the intersection point is actually r away from the origin + EXPECT_DOUBLE_EQ(r, std::hypot(actual.x, actual.y)); +} + +INSTANTIATE_TEST_SUITE_P( + InterpolationTest, + CircleSegmentIntersectionTest, + testing::Values( + // Origin to the positive X axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {2.0, 0.0}, + 1.0, + {1.0, 0.0} +}, + // Origin to hte negative X axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-2.0, 0.0}, + 1.0, + {-1.0, 0.0} +}, + // Origin to the positive Y axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, 2.0}, + 1.0, + {0.0, 1.0} +}, + // Origin to the negative Y axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, -2.0}, + 1.0, + {0.0, -1.0} +}, + // non-origin to the X axis with non-unit circle, with the second point inside + CircleSegmentIntersectionParam{ + {4.0, 0.0}, + {-1.0, 0.0}, + 2.0, + {2.0, 0.0} +}, + // non-origin to the Y axis with non-unit circle, with the second point inside + CircleSegmentIntersectionParam{ + {0.0, 4.0}, + {0.0, -0.5}, + 2.0, + {0.0, 2.0} +}, + // origin to the positive X axis, on the circle + CircleSegmentIntersectionParam{ + {2.0, 0.0}, + {0.0, 0.0}, + 2.0, + {2.0, 0.0} +}, + // origin to the positive Y axis, on the circle + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, 2.0}, + 2.0, + {0.0, 2.0} +}, + // origin to the upper-right quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {6.0, 8.0}, + 5.0, + {3.0, 4.0} +}, + // origin to the lower-left quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-6.0, -8.0}, + 5.0, + {-3.0, -4.0} +}, + // origin to the upper-left quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-6.0, 8.0}, + 5.0, + {-3.0, 4.0} +}, + // origin to the lower-right quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {6.0, -8.0}, + 5.0, + {3.0, -4.0} +} +)); + TEST(RegulatedPurePursuitTest, lookaheadAPI) { auto ctrl = std::make_shared(); @@ -135,6 +330,8 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) std::string name = "PathFollower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); ctrl->configure(node, name, tf, costmap); geometry_msgs::msg::Twist twist; @@ -175,7 +372,12 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) auto pt = ctrl->getLookAheadPointWrapper(dist, path); EXPECT_EQ(pt.pose.position.x, 1.0); - // test getting next closest point + // test getting next closest point without interpolation + node->set_parameter( + rclcpp::Parameter( + name + ".use_interpolation", + rclcpp::ParameterValue(false))); + ctrl->configure(node, name, tf, costmap); dist = 3.8; pt = ctrl->getLookAheadPointWrapper(dist, path); EXPECT_EQ(pt.pose.position.x, 4.0); @@ -184,6 +386,16 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) dist = 100.0; pt = ctrl->getLookAheadPointWrapper(dist, path); EXPECT_EQ(pt.pose.position.x, 9.0); + + // test interpolation + node->set_parameter( + rclcpp::Parameter( + name + ".use_interpolation", + rclcpp::ParameterValue(true))); + ctrl->configure(node, name, tf, costmap); + dist = 3.8; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 3.8); } TEST(RegulatedPurePursuitTest, rotateTests) @@ -193,6 +405,8 @@ TEST(RegulatedPurePursuitTest, rotateTests) std::string name = "PathFollower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); ctrl->configure(node, name, tf, costmap); // shouldRotateToPath @@ -262,6 +476,8 @@ TEST(RegulatedPurePursuitTest, applyConstraints) std::string name = "PathFollower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); ctrl->configure(node, name, tf, costmap); double dist_error = 0.0; @@ -270,25 +486,29 @@ TEST(RegulatedPurePursuitTest, applyConstraints) 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(); + double sign = 1.0; // test curvature regulation (default) curr_speed.linear.x = 0.25; - ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + ctrl->applyConstraintsWrapper( + dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + linear_vel, sign); 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); + ctrl->applyConstraintsWrapper( + dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + linear_vel, sign); 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); + ctrl->applyConstraintsWrapper( + dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + linear_vel, sign); EXPECT_NEAR(linear_vel, 0.25, 0.01); // min out by curvature @@ -301,24 +521,449 @@ TEST(RegulatedPurePursuitTest, applyConstraints) // 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); + // 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); + // 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); + // 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); + // ctrl->applyConstraintsWrapper( + // dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); // EXPECT_NEAR(linear_vel, 0.5, 0.01); } + +TEST(RegulatedPurePursuitTest, testDynamicParameter) +{ + auto node = std::make_shared("Smactest"); + auto costmap = std::make_shared("global_costmap"); + costmap->on_configure(rclcpp_lifecycle::State()); + auto ctrl = + std::make_unique(); + auto tf = std::make_shared(node->get_clock()); + ctrl->configure(node, "test", tf, costmap); + ctrl->activate(); + + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.desired_linear_vel", 1.0), + rclcpp::Parameter("test.lookahead_dist", 7.0), + rclcpp::Parameter("test.max_lookahead_dist", 7.0), + rclcpp::Parameter("test.min_lookahead_dist", 6.0), + rclcpp::Parameter("test.lookahead_time", 1.8), + rclcpp::Parameter("test.rotate_to_heading_angular_vel", 18.0), + rclcpp::Parameter("test.min_approach_linear_velocity", 1.0), + rclcpp::Parameter("test.max_allowed_time_to_collision_up_to_carrot", 2.0), + rclcpp::Parameter("test.cost_scaling_dist", 2.0), + rclcpp::Parameter("test.cost_scaling_gain", 4.0), + rclcpp::Parameter("test.regulated_linear_scaling_min_radius", 10.0), + rclcpp::Parameter("test.transform_tolerance", 30.0), + rclcpp::Parameter("test.max_angular_accel", 3.0), + rclcpp::Parameter("test.rotate_to_heading_min_angle", 0.7), + rclcpp::Parameter("test.regulated_linear_scaling_min_speed", 4.0), + rclcpp::Parameter("test.use_velocity_scaled_lookahead_dist", false), + rclcpp::Parameter("test.use_regulated_linear_velocity_scaling", false), + rclcpp::Parameter("test.use_cost_regulated_linear_velocity_scaling", false), + rclcpp::Parameter("test.allow_reversing", false), + rclcpp::Parameter("test.use_rotate_to_heading", false)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + EXPECT_EQ(node->get_parameter("test.desired_linear_vel").as_double(), 1.0); + EXPECT_EQ(node->get_parameter("test.lookahead_dist").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("test.max_lookahead_dist").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("test.min_lookahead_dist").as_double(), 6.0); + EXPECT_EQ(node->get_parameter("test.lookahead_time").as_double(), 1.8); + EXPECT_EQ(node->get_parameter("test.rotate_to_heading_angular_vel").as_double(), 18.0); + EXPECT_EQ(node->get_parameter("test.min_approach_linear_velocity").as_double(), 1.0); + EXPECT_EQ( + node->get_parameter( + "test.max_allowed_time_to_collision_up_to_carrot").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("test.cost_scaling_dist").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("test.cost_scaling_gain").as_double(), 4.0); + EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_radius").as_double(), 10.0); + EXPECT_EQ(node->get_parameter("test.transform_tolerance").as_double(), 30.0); + EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 3.0); + EXPECT_EQ(node->get_parameter("test.rotate_to_heading_min_angle").as_double(), 0.7); + EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_speed").as_double(), 4.0); + EXPECT_EQ(node->get_parameter("test.use_velocity_scaled_lookahead_dist").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.use_regulated_linear_velocity_scaling").as_bool(), false); + EXPECT_EQ( + node->get_parameter( + "test.use_cost_regulated_linear_velocity_scaling").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.allow_reversing").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.use_rotate_to_heading").as_bool(), false); +} + +class TransformGlobalPlanTest : public ::testing::Test +{ +protected: + void SetUp() override + { + ctrl_ = std::make_shared(); + node_ = std::make_shared("testRPP"); + tf_buffer_ = std::make_shared(node_->get_clock()); + costmap_ = std::make_shared("fake_costmap"); + } + + void configure_costmap(uint16_t width, double resolution) + { + constexpr char costmap_frame[] = "test_costmap_frame"; + constexpr char robot_frame[] = "test_robot_frame"; + + auto results = costmap_->set_parameters( + { + rclcpp::Parameter("global_frame", costmap_frame), + rclcpp::Parameter("robot_base_frame", robot_frame), + rclcpp::Parameter("width", width), + rclcpp::Parameter("height", width), + rclcpp::Parameter("resolution", resolution) + }); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + + rclcpp_lifecycle::State state; + costmap_->on_configure(state); + } + + void configure_controller(double max_robot_pose_search_dist) + { + std::string plugin_name = "test_rpp"; + nav2_util::declare_parameter_if_not_declared( + node_, plugin_name + ".max_robot_pose_search_dist", + rclcpp::ParameterValue(max_robot_pose_search_dist)); + ctrl_->configure(node_, plugin_name, tf_buffer_, costmap_); + } + + void setup_transforms(geometry_msgs::msg::Point & robot_position) + { + transform_time_ = node_->get_clock()->now(); + // Note: transforms go parent to child + + // We will have a separate path and costmap frame for completeness, + // but we will leave them cooincident for convenience. + geometry_msgs::msg::TransformStamped path_to_costmap; + path_to_costmap.header.frame_id = PATH_FRAME; + path_to_costmap.header.stamp = transform_time_; + path_to_costmap.child_frame_id = COSTMAP_FRAME; + path_to_costmap.transform.translation.x = 0.0; + path_to_costmap.transform.translation.y = 0.0; + path_to_costmap.transform.translation.z = 0.0; + + geometry_msgs::msg::TransformStamped costmap_to_robot; + costmap_to_robot.header.frame_id = COSTMAP_FRAME; + costmap_to_robot.header.stamp = transform_time_; + costmap_to_robot.child_frame_id = ROBOT_FRAME; + costmap_to_robot.transform.translation.x = robot_position.x; + costmap_to_robot.transform.translation.y = robot_position.y; + costmap_to_robot.transform.translation.z = robot_position.z; + + tf2_msgs::msg::TFMessage tf_message; + tf_message.transforms = { + path_to_costmap, + costmap_to_robot + }; + for (const auto & transform : tf_message.transforms) { + tf_buffer_->setTransform(transform, "test", false); + } + tf_buffer_->setUsingDedicatedThread(true); // lying to let it do transforms + } + + static constexpr char PATH_FRAME[] = "test_path_frame"; + static constexpr char COSTMAP_FRAME[] = "test_costmap_frame"; + static constexpr char ROBOT_FRAME[] = "test_robot_frame"; + + std::shared_ptr ctrl_; + std::shared_ptr node_; + std::shared_ptr costmap_; + std::shared_ptr tf_buffer_; + rclcpp::Time transform_time_; +}; + +// This tests that not only should nothing get pruned on a costmap +// that contains the entire global_plan, and also that it doesn't skip to the end of the path +// which is closer to the robot pose than the start. +TEST_F(TransformGlobalPlanTest, no_pruning_on_large_costmap) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // A really big costmap + // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); + configure_controller(5.0); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + constexpr double spacing = 0.1; + constexpr double circle_radius = 1.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_EQ(transformed_plan.poses.size(), global_plan.poses.size()); +} + +// This plan shouldn't get pruned because of the costmap, +// but should be half pruned because it is halfway around the circle +TEST_F(TransformGlobalPlanTest, transform_start_selection) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 4.0; // on the other side of the circle + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 0.1; + constexpr double circle_radius = 2.0; // diameter 4 + + // A really big costmap + // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); + // This should just be at least half the circumference: pi*r ~= 6 + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_NEAR(transformed_plan.poses.size(), global_plan.poses.size() / 2, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} + +// This should throw an exception when all poses are outside of the costmap +TEST_F(TransformGlobalPlanTest, all_poses_outside_of_costmap) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = 1000.0; + robot_pose.pose.position.y = 1000.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 0.1; + constexpr double circle_radius = 2.0; // diameter 4 + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(10u, 0.1); + // This should just be at least half the circumference: pi*r ~= 6 + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + EXPECT_THROW(ctrl_->transformGlobalPlanWrapper(robot_pose), nav2_core::PlannerException); +} + +// Should shortcut the circle if the circle is shorter than max_robot_pose_search_dist +TEST_F(TransformGlobalPlanTest, good_circle_shortcut) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 0.1; + constexpr double circle_radius = 2.0; // diameter 4 + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); + // This should just be at least the circumference: 2*pi*r ~= 12 + constexpr double max_robot_pose_search_dist = 15.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_NEAR(transformed_plan.poses.size(), 1, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} + +// Simple costmap pruning on a straight line +TEST_F(TransformGlobalPlanTest, costmap_pruning) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 1.0; + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(20u, 0.5); + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + constexpr double path_length = 100.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(path_length) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_NEAR(transformed_plan.poses.size(), 10u, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} + +// Should prune out later portions of the path that come back into the costmap +TEST_F(TransformGlobalPlanTest, prune_after_leaving_costmap) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 1.0; + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(20u, 0.5); + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + constexpr double path_length = 100.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(path_length), + std::make_unique(1.0), + std::make_unique(path_length) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + // This should be essentially the same as the regular straight path + EXPECT_NEAR(transformed_plan.poses.size(), 10u, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt new file mode 100644 index 0000000000..2e141d5f44 --- /dev/null +++ b/nav2_rotation_shim_controller/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_rotation_shim_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) +find_package(angles 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 + angles +) + +set(library_name nav2_rotation_shim_controller) + +add_library(${library_name} SHARED + src/nav2_rotation_shim_controller.cpp) + +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_rotation_shim_controller.xml) + +ament_package() diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md new file mode 100644 index 0000000000..15185bfa96 --- /dev/null +++ b/nav2_rotation_shim_controller/README.md @@ -0,0 +1,75 @@ +# Nav2 Rotation Shim Controller + +This is a controller (local trajectory planner) that implements a "shim" controller plugin. It was developed by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). + +The Rotation Shim Controller stands between the controller server and the main controller plugin to implement a specific behavior often troublesome for other algorithms. This shim will rotate a robot in place to the rough heading of a newly received path. Afterwards, it will forward all future commands on that path to the main controller. It will take in a ``primary_controller`` parameter, containing the actual controller to use for path tracking once aligned with the path. + +This is useful for situations when working with plugins that are either too specialized or tuned for a particular task that they can fail to adequately solve the full local planning problem performantly. Examples include: + +- Heavily tuning DWB for excellent path tracking makes it difficult to handle large deviations +- TEB behavior tends to "whip" the robot around with small turns, in a somewhat scary way due to the elastic band approach +- Neither TEB or DWB will simply rotate the robot in place to start tracking a new path. They instead perform a small 'spiral out' maneuver that is often clunky with odd velocities. You may prefer a clean and simple rotation in place. + +As such, this controller will check the rough heading difference with respect to the robot and a newly received path. If within a threshold, it will pass the request onto the controller to execute. If it is outside of the threshold, this controller will rotate the robot towards that path heading. Once it is within the tolerance, it will then pass off control-execution from this rotation shim controller onto the primary controller plugin. At this point, the robot is still going to be rotating, allowing the current plugin to take control for a smooth hand off into path tracking. It is recommended to be more generous than strict in the angular threshold to allow for a smoother transition, but should be tuned for a specific application's desired behaviors. + +The Rotation Shim Controller is suitable for: +- Robots that can rotate in place, such as differential and omnidirectional robots. +- Preference to rotate in place rather than 'spiral out' when starting to track a new path that is at a significantly different heading than the robot's current heading. +- Using planners that are non-kinematically feasible, such as NavFn, Theta\*, or Smac 2D (Feasible planners such as Smac Hybrid-A* and State Lattice will start search from the robot's actual starting heading, requiring no rotation). + +This plugin implements the `nav2_core::Controller` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). It will host an internal plugin of your actual path tracker (e.g. MPPI, RPP, DWB, TEB, etc) that will be used after the robot has rotated to the rough starting heading of the path. + +

+ +

+ +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-rotation-shim-controller.html) for additional parameter descriptions. + +## Configuration + +| Parameter | Description | +|-----|----| +| `angular_dist_threshold` | Maximum angular distance, in radians, away from the path heading to trigger rotation until within. | +| `forward_sampling_distance` | Forward distance, in meters, along path to select a sampling point to use to approximate path heading | +| `rotate_to_heading_angular_vel` | Angular rotational velocity, in rad/s, to rotate to the path heading | +| `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading | +| `max_angular_accel` | Maximum angular acceleration for rotation to heading | +| `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. | + +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_plugins: "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_rotation_shim_controller::RotationShimController" + primary_controller: "dwb_core::DWBLocalPlanner" + angular_dist_threshold: 0.785 + forward_sampling_distance: 0.5 + rotate_to_heading_angular_vel: 1.8 + max_angular_accel: 3.2 + simulate_ahead_time: 1.0 + + # DWB parameters + ... + ... + ... +``` diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp new file mode 100644 index 0000000000..d3f96dd25e --- /dev/null +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -0,0 +1,179 @@ +// 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_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_ +#define NAV2_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_core/controller.hpp" +#include "nav2_core/exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "angles/angles.h" + +namespace nav2_rotation_shim_controller +{ + +/** + * @class nav2_rotation_shim_controller::RotationShimController + * @brief Rotate to rough path heading controller shim plugin + */ +class RotationShimController : public nav2_core::Controller +{ +public: + /** + * @brief Constructor for nav2_rotation_shim_controller::RotationShimController + */ + RotationShimController(); + + /** + * @brief Destrructor for nav2_rotation_shim_controller::RotationShimController + */ + ~RotationShimController() 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, std::shared_ptr tf, + 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 + * @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 Finds the point on the path that is roughly the sampling + * point distance away from the robot for use. + * May throw exception if a point at least that far away cannot be found + * @return pt location of the output point + */ + geometry_msgs::msg::PoseStamped getSampledPathPt(); + + /** + * @brief Uses TF to find the location of the sampled path point in base frame + * @param pt location of the sampled path point + * @return location of the pose in base frame + */ + geometry_msgs::msg::Pose transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt); + + /** + * @brief Rotates the robot to the rough heading + * @param angular_distance Angular distance to the goal remaining + * @param pose Starting pose of robot + * @param velocity Starting velocity of robot + * @return Twist command for rotation to rough heading + */ + geometry_msgs::msg::TwistStamped computeRotateToHeadingCommand( + const double & angular_distance, + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity); + + /** + * @brief Checks if rotation is safe + * @param cmd_vel Velocity to check over + * @param angular_distance_to_heading Angular distance to heading requested + * @param pose Starting pose of robot + */ + void isCollisionFree( + const geometry_msgs::msg::TwistStamped & cmd_vel, + const double & angular_distance_to_heading, + const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + std::shared_ptr tf_; + std::string plugin_name_; + rclcpp::Logger logger_ {rclcpp::get_logger("RotationShimController")}; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr costmap_ros_; + std::unique_ptr> + collision_checker_; + + pluginlib::ClassLoader lp_loader_; + nav2_core::Controller::Ptr primary_controller_; + bool path_updated_; + nav_msgs::msg::Path current_path_; + double forward_sampling_distance_, angular_dist_threshold_; + double rotate_to_heading_angular_vel_, max_angular_accel_; + double control_duration_, simulate_ahead_time_; + + // Dynamic parameters handler + std::mutex mutex_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; +}; + +} // namespace nav2_rotation_shim_controller + +#endif // NAV2_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_ diff --git a/nav2_rotation_shim_controller/nav2_rotation_shim_controller.xml b/nav2_rotation_shim_controller/nav2_rotation_shim_controller.xml new file mode 100644 index 0000000000..7b3f9ac52c --- /dev/null +++ b/nav2_rotation_shim_controller/nav2_rotation_shim_controller.xml @@ -0,0 +1,9 @@ + + + + + nav2_rotation_shim_controller + + + + diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml new file mode 100644 index 0000000000..a92ba01bb0 --- /dev/null +++ b/nav2_rotation_shim_controller/package.xml @@ -0,0 +1,34 @@ + + + + nav2_rotation_shim_controller + 1.1.0 + Rotation Shim Controller + Steve Macenski + Apache-2.0 + + ament_cmake + + nav2_common + nav2_core + nav2_util + nav2_costmap_2d + rclcpp + geometry_msgs + nav2_msgs + angles + pluginlib + tf2 + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + nav2_regulated_pure_pursuit_controller + nav2_controller + + + ament_cmake + + + + diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp new file mode 100644 index 0000000000..bc86900f64 --- /dev/null +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -0,0 +1,317 @@ +// 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 + +#include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp" + +using rcl_interfaces::msg::ParameterType; + +namespace nav2_rotation_shim_controller +{ + +RotationShimController::RotationShimController() +: lp_loader_("nav2_core", "nav2_core::Controller"), + primary_controller_(nullptr), + path_updated_(false) +{ +} + +void RotationShimController::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) +{ + plugin_name_ = name; + node_ = parent; + auto node = parent.lock(); + + tf_ = tf; + costmap_ros_ = costmap_ros; + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + std::string primary_controller; + double control_frequency; + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".angular_dist_threshold", rclcpp::ParameterValue(0.785)); // 45 deg + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".forward_sampling_distance", rclcpp::ParameterValue(0.5)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".simulate_ahead_time", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING); + + node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_); + node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_); + node->get_parameter( + plugin_name_ + ".rotate_to_heading_angular_vel", + rotate_to_heading_angular_vel_); + node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); + node->get_parameter(plugin_name_ + ".simulate_ahead_time", simulate_ahead_time_); + + primary_controller = node->get_parameter(plugin_name_ + ".primary_controller").as_string(); + node->get_parameter("controller_frequency", control_frequency); + control_duration_ = 1.0 / control_frequency; + + try { + primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); + RCLCPP_INFO( + logger_, "Created internal controller for rotation shimming: %s of type %s", + plugin_name_.c_str(), primary_controller.c_str()); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + logger_, + "Failed to create internal controller for rotation shimming. Exception: %s", ex.what()); + return; + } + + primary_controller_->configure(parent, name, tf, costmap_ros); + + // initialize collision checker and set costmap + collision_checker_ = std::make_unique>(costmap_ros->getCostmap()); +} + +void RotationShimController::activate() +{ + RCLCPP_INFO( + logger_, + "Activating controller: %s of type " + "nav2_rotation_shim_controller::RotationShimController", + plugin_name_.c_str()); + + primary_controller_->activate(); + + auto node = node_.lock(); + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &RotationShimController::dynamicParametersCallback, + this, std::placeholders::_1)); +} + +void RotationShimController::deactivate() +{ + RCLCPP_INFO( + logger_, + "Deactivating controller: %s of type " + "nav2_rotation_shim_controller::RotationShimController", + plugin_name_.c_str()); + + primary_controller_->deactivate(); + + dyn_params_handler_.reset(); +} + +void RotationShimController::cleanup() +{ + RCLCPP_INFO( + logger_, + "Cleaning up controller: %s of type " + "nav2_rotation_shim_controller::RotationShimController", + plugin_name_.c_str()); + + primary_controller_->cleanup(); + primary_controller_.reset(); +} + +geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * goal_checker) +{ + if (path_updated_) { + std::lock_guard lock_reinit(mutex_); + try { + geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(getSampledPathPt()); + + double angular_distance_to_heading = + std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x); + if (fabs(angular_distance_to_heading) > angular_dist_threshold_) { + RCLCPP_DEBUG( + logger_, + "Robot is not within the new path's rough heading, rotating to heading..."); + return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); + } else { + RCLCPP_DEBUG( + logger_, + "Robot is at the new path's rough heading, passing to controller"); + path_updated_ = false; + } + } catch (const std::runtime_error & e) { + RCLCPP_DEBUG( + logger_, + "Rotation Shim Controller was unable to find a sampling point," + " a rotational collision was detected, or TF failed to transform" + " into base frame! what(): %s", e.what()); + path_updated_ = false; + } + } + + // If at this point, use the primary controller to path track + return primary_controller_->computeVelocityCommands(pose, velocity, goal_checker); +} + +geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() +{ + if (current_path_.poses.size() < 2) { + throw nav2_core::PlannerException( + "Path is too short to find a valid sampled path point for rotation."); + } + + geometry_msgs::msg::Pose start = current_path_.poses.front().pose; + double dx, dy; + + // Find the first point at least sampling distance away + for (unsigned int i = 1; i != current_path_.poses.size(); i++) { + dx = current_path_.poses[i].pose.position.x - start.position.x; + dy = current_path_.poses[i].pose.position.y - start.position.y; + if (hypot(dx, dy) >= forward_sampling_distance_) { + current_path_.poses[i].header.frame_id = current_path_.header.frame_id; + current_path_.poses[i].header.stamp = clock_->now(); // Get current time transformation + return current_path_.poses[i]; + } + } + + throw nav2_core::PlannerException( + std::string( + "Unable to find a sampling point at least %0.2f from the robot," + "passing off to primary controller plugin.", forward_sampling_distance_)); +} + +geometry_msgs::msg::Pose +RotationShimController::transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt) +{ + geometry_msgs::msg::PoseStamped pt_base; + if (!nav2_util::transformPoseInTargetFrame(pt, pt_base, *tf_, costmap_ros_->getBaseFrameID())) { + throw nav2_core::PlannerException("Failed to transform pose to base frame!"); + } + return pt_base.pose; +} + +geometry_msgs::msg::TwistStamped +RotationShimController::computeRotateToHeadingCommand( + const double & angular_distance_to_heading, + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity) +{ + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header = pose.header; + const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0; + const double angular_vel = sign * rotate_to_heading_angular_vel_; + const double & dt = control_duration_; + const double min_feasible_angular_speed = velocity.angular.z - max_angular_accel_ * dt; + const double max_feasible_angular_speed = velocity.angular.z + max_angular_accel_ * dt; + cmd_vel.twist.angular.z = + std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); + + isCollisionFree(cmd_vel, angular_distance_to_heading, pose); + return cmd_vel; +} + +void RotationShimController::isCollisionFree( + const geometry_msgs::msg::TwistStamped & cmd_vel, + const double & angular_distance_to_heading, + const geometry_msgs::msg::PoseStamped & pose) +{ + // Simulate rotation ahead by time in control frequency increments + double simulated_time = 0.0; + double initial_yaw = tf2::getYaw(pose.pose.orientation); + double yaw = 0.0; + double footprint_cost = 0.0; + double remaining_rotation_before_thresh = + fabs(angular_distance_to_heading) - angular_dist_threshold_; + + while (simulated_time < simulate_ahead_time_) { + simulated_time += control_duration_; + yaw = initial_yaw + cmd_vel.twist.angular.z * simulated_time; + + // Stop simulating past the point it would be passed onto the primary controller + if (angles::shortest_angular_distance(yaw, initial_yaw) >= remaining_rotation_before_thresh) { + break; + } + + using namespace nav2_costmap_2d; // NOLINT + footprint_cost = collision_checker_->footprintCostAtPose( + pose.pose.position.x, pose.pose.position.y, + yaw, costmap_ros_->getRobotFootprint()); + + if (footprint_cost == static_cast(NO_INFORMATION) && + costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) + { + throw std::runtime_error("RotationShimController detected a potential collision ahead!"); + } + + if (footprint_cost >= static_cast(LETHAL_OBSTACLE)) { + throw std::runtime_error("RotationShimController detected collision ahead!"); + } + } +} + +void RotationShimController::setPlan(const nav_msgs::msg::Path & path) +{ + path_updated_ = true; + current_path_ = path; + primary_controller_->setPlan(path); +} + +void RotationShimController::setSpeedLimit(const double & speed_limit, const bool & percentage) +{ + primary_controller_->setSpeedLimit(speed_limit, percentage); +} + +rcl_interfaces::msg::SetParametersResult +RotationShimController::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(mutex_); + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".angular_dist_threshold") { + angular_dist_threshold_ = parameter.as_double(); + } else if (name == plugin_name_ + ".forward_sampling_distance") { + forward_sampling_distance_ = parameter.as_double(); + } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { + rotate_to_heading_angular_vel_ = parameter.as_double(); + } else if (name == plugin_name_ + ".max_angular_accel") { + max_angular_accel_ = parameter.as_double(); + } else if (name == plugin_name_ + ".simulate_ahead_time") { + simulate_ahead_time_ = parameter.as_double(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_rotation_shim_controller + +// Register this controller as a nav2_core plugin +PLUGINLIB_EXPORT_CLASS( + nav2_rotation_shim_controller::RotationShimController, + nav2_core::Controller) diff --git a/nav2_rotation_shim_controller/test/CMakeLists.txt b/nav2_rotation_shim_controller/test/CMakeLists.txt new file mode 100644 index 0000000000..ae3161d1a7 --- /dev/null +++ b/nav2_rotation_shim_controller/test/CMakeLists.txt @@ -0,0 +1,13 @@ +# tests +find_package(nav2_controller REQUIRED) + +ament_add_gtest(test_shim_controller + test_shim_controller.cpp +) +ament_target_dependencies(test_shim_controller + ${dependencies} + nav2_controller +) +target_link_libraries(test_shim_controller + ${library_name} +) diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp new file mode 100644 index 0000000000..e1bc30f80e --- /dev/null +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -0,0 +1,352 @@ +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" +#include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp" +#include "tf2_ros/transform_broadcaster.h" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class RotationShimShim : public nav2_rotation_shim_controller::RotationShimController +{ +public: + RotationShimShim() + : nav2_rotation_shim_controller::RotationShimController() + { + } + + nav2_core::Controller::Ptr getPrimaryController() + { + return primary_controller_; + } + + nav_msgs::msg::Path getPath() + { + return current_path_; + } + + bool isPathUpdated() + { + return path_updated_; + } + + geometry_msgs::msg::PoseStamped getSampledPathPtWrapper() + { + return getSampledPathPt(); + } + + geometry_msgs::msg::Pose transformPoseToBaseFrameWrapper(geometry_msgs::msg::PoseStamped pt) + { + return transformPoseToBaseFrame(pt); + } + + geometry_msgs::msg::TwistStamped + computeRotateToHeadingCommandWrapper( + const double & param, + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity) + { + return computeRotateToHeadingCommand(param, pose, velocity); + } +}; + +TEST(RotationShimControllerTest, lifecycleTransitions) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + + // Should not populate primary controller, does not exist + EXPECT_THROW(ctrl->configure(node, name, tf, costmap), std::runtime_error); + EXPECT_EQ(ctrl->getPrimaryController(), nullptr); + + // Add a controller to the setup + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter( + "PathFollower.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"))}); + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + ctrl->configure(node, name, tf, costmap); + EXPECT_NE(ctrl->getPrimaryController(), nullptr); + + ctrl->activate(); + + ctrl->setSpeedLimit(50.0, 2.0); + + ctrl->deactivate(); + ctrl->cleanup(); +} + +TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + + // set a valid primary controller so we can do lifecycle + node->declare_parameter( + "PathFollower.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + // Test state update and path setting + nav_msgs::msg::Path path; + path.header.frame_id = "hi mate!"; + path.poses.resize(10); + path.poses[1].pose.position.x = 0.1; + path.poses[1].pose.position.y = 0.1; + path.poses[2].pose.position.x = 1.0; + path.poses[2].pose.position.y = 1.0; + path.poses[3].pose.position.x = 10.0; + path.poses[3].pose.position.y = 10.0; + EXPECT_EQ(controller->isPathUpdated(), false); + controller->setPlan(path); + EXPECT_EQ(controller->getPath().header.frame_id, std::string("hi mate!")); + EXPECT_EQ(controller->getPath().poses.size(), 10u); + EXPECT_EQ(controller->isPathUpdated(), true); + + // Test getting a sampled point + auto pose = controller->getSampledPathPtWrapper(); + EXPECT_EQ(pose.pose.position.x, 1.0); // default forward sampling is 0.5 + EXPECT_EQ(pose.pose.position.y, 1.0); + + nav_msgs::msg::Path path_invalid_leng; + controller->setPlan(path_invalid_leng); + EXPECT_THROW(controller->getSampledPathPtWrapper(), std::runtime_error); + + nav_msgs::msg::Path path_invalid_dists; + path.poses.resize(10); + controller->setPlan(path_invalid_dists); + EXPECT_THROW(controller->getSampledPathPtWrapper(), std::runtime_error); +} + +TEST(RotationShimControllerTest, rotationAndTransformTests) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + + // set a valid primary controller so we can do lifecycle + node->declare_parameter( + "PathFollower.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + // Test state update and path setting + nav_msgs::msg::Path path; + path.header.frame_id = "fake_frame"; + path.poses.resize(10); + path.poses[1].pose.position.x = 0.1; + path.poses[1].pose.position.y = 0.1; + path.poses[2].pose.position.x = 1.0; + path.poses[2].pose.position.y = 1.0; + path.poses[3].pose.position.x = 10.0; + path.poses[3].pose.position.y = 10.0; + controller->setPlan(path); + + const geometry_msgs::msg::Twist velocity; + EXPECT_EQ( + controller->computeRotateToHeadingCommandWrapper( + 0.7, path.poses[0], velocity).twist.angular.z, 1.8); + EXPECT_EQ( + controller->computeRotateToHeadingCommandWrapper( + -0.7, path.poses[0], velocity).twist.angular.z, -1.8); + + EXPECT_EQ( + controller->computeRotateToHeadingCommandWrapper( + 0.87, path.poses[0], velocity).twist.angular.z, 1.8); + + // in base_link, so should pass through values without issue + geometry_msgs::msg::PoseStamped pt; + pt.pose.position.x = 100.0; + pt.header.frame_id = "base_link"; + pt.header.stamp = rclcpp::Time(); + auto rtn = controller->transformPoseToBaseFrameWrapper(pt); + EXPECT_EQ(rtn.position.x, 100.0); + + // in frame that doesn't exist, shouldn't throw, but should fail + geometry_msgs::msg::PoseStamped pt2; + pt.pose.position.x = 100.0; + pt.header.frame_id = "fake_frame2"; + pt.header.stamp = rclcpp::Time(); + EXPECT_THROW(controller->transformPoseToBaseFrameWrapper(pt2), std::runtime_error); +} + +TEST(RotationShimControllerTest, computeVelocityTests) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto listener = std::make_shared(*tf, node, true); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + auto tf_broadcaster = std::make_shared(node); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "base_link"; + transform.child_frame_id = "odom"; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + tf_broadcaster->sendTransform(transform); + + // set a valid primary controller so we can do lifecycle + node->declare_parameter( + "PathFollower.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + // Test state update and path setting + nav_msgs::msg::Path path; + path.header.frame_id = "fake_frame"; + path.poses.resize(10); + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "base_link"; + geometry_msgs::msg::Twist velocity; + nav2_controller::SimpleGoalChecker checker; + checker.initialize(node, "checker", costmap); + + // send without setting a path - should go to RPP immediately + // then it should throw an exception because the path is empty and invalid + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); + + // Set with a path -- should attempt to find a sampled point but throw exception + // because it cannot be found, then go to RPP and throw exception because it cannot be transformed + controller->setPlan(path); + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); + + path.header.frame_id = "base_link"; + path.poses[1].pose.position.x = 0.1; + path.poses[1].pose.position.y = 0.1; + path.poses[2].pose.position.x = -1.0; + path.poses[2].pose.position.y = -1.0; + path.poses[2].header.frame_id = "base_link"; + path.poses[3].pose.position.x = 10.0; + path.poses[3].pose.position.y = 10.0; + + // this should allow it to find the sampled point, then transform to base_link + // validly because we setup the TF for it. The -1.0 should be selected since default min + // is 0.5 and that should cause a rotation in place + controller->setPlan(path); + tf_broadcaster->sendTransform(transform); + auto effort = controller->computeVelocityCommands(pose, velocity, &checker); + EXPECT_EQ(fabs(effort.twist.angular.z), 1.8); + + path.header.frame_id = "base_link"; + path.poses[1].pose.position.x = 0.1; + path.poses[1].pose.position.y = 0.1; + path.poses[2].pose.position.x = 1.0; + path.poses[2].pose.position.y = 0.0; + path.poses[2].header.frame_id = "base_link"; + path.poses[3].pose.position.x = 10.0; + path.poses[3].pose.position.y = 10.0; + + // this should allow it to find the sampled point, then transform to base_link + // validly because we setup the TF for it. The 1.0 should be selected since default min + // is 0.5 and that should cause a pass off to the RPP controller which will throw + // and exception because the costmap is bogus + controller->setPlan(path); + tf_broadcaster->sendTransform(transform); + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); +} + +TEST(RotationShimControllerTest, testDynamicParameter) +{ + auto node = std::make_shared("ShimControllerTest"); + auto costmap = std::make_shared("global_costmap"); + std::string name = "test"; + auto tf = std::make_shared(node->get_clock()); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + + // set a valid primary controller so we can do lifecycle + node->declare_parameter( + "test.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.angular_dist_threshold", 7.0), + rclcpp::Parameter("test.forward_sampling_distance", 7.0), + rclcpp::Parameter("test.rotate_to_heading_angular_vel", 7.0), + rclcpp::Parameter("test.max_angular_accel", 7.0), + rclcpp::Parameter("test.simulate_ahead_time", 7.0), + rclcpp::Parameter("test.primary_controller", std::string("HI"))}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + EXPECT_EQ(node->get_parameter("test.angular_dist_threshold").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("test.forward_sampling_distance").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("test.rotate_to_heading_angular_vel").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0); +} diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index f344363296..39bdbabae4 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") @@ -33,10 +33,12 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(visualization_msgs REQUIRED) set(nav2_rviz_plugins_headers_to_moc - include/nav2_rviz_plugins/goal_pose_updater - include/nav2_rviz_plugins/goal_common + include/nav2_rviz_plugins/goal_pose_updater.hpp + include/nav2_rviz_plugins/goal_common.hpp 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} ) @@ -87,9 +91,6 @@ target_link_libraries(${library_name} # TODO: Make this specific to this project (not rviz default plugins) target_compile_definitions(${library_name} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") -# prevent pluginlib from using boost -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) install( 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 a3a1c8d5ea..b875c982ef 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -25,12 +25,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" @@ -63,7 +64,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); @@ -77,10 +79,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_; @@ -95,16 +100,30 @@ 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_; - nav2_lifecycle_manager::LifecycleManagerClient client_loc_; + std::shared_ptr client_nav_; + std::shared_ptr client_loc_; QPushButton * start_reset_button_{nullptr}; QPushButton * pause_resume_button_{nullptr}; @@ -112,6 +131,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_; @@ -129,9 +150,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_; @@ -143,6 +165,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_; }; @@ -155,8 +194,8 @@ class InitialThread : public QThread using SystemStatus = nav2_lifecycle_manager::SystemStatus; explicit InitialThread( - nav2_lifecycle_manager::LifecycleManagerClient & client_nav, - nav2_lifecycle_manager::LifecycleManagerClient & client_loc) + std::shared_ptr & client_nav, + std::shared_ptr & client_loc) : client_nav_(client_nav), client_loc_(client_loc) {} @@ -167,14 +206,14 @@ class InitialThread : public QThread while (status_nav == SystemStatus::TIMEOUT) { if (status_nav == SystemStatus::TIMEOUT) { - status_nav = client_nav_.is_active(std::chrono::seconds(1)); + status_nav = client_nav_->is_active(std::chrono::seconds(1)); } } // try to communicate twice, might not actually be up if in SLAM mode bool tried_loc_bringup_once = false; while (status_loc == SystemStatus::TIMEOUT) { - status_loc = client_loc_.is_active(std::chrono::seconds(1)); + status_loc = client_loc_->is_active(std::chrono::seconds(1)); if (tried_loc_bringup_once) { break; } @@ -201,8 +240,8 @@ class InitialThread : public QThread void localizationInactive(); private: - nav2_lifecycle_manager::LifecycleManagerClient client_nav_; - nav2_lifecycle_manager::LifecycleManagerClient client_loc_; + std::shared_ptr client_nav_; + std::shared_ptr client_loc_; }; } // namespace nav2_rviz_plugins 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 0000000000..7647aaf67f --- /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 0000000000..f67a6e0538 --- /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 388cd4345c..5e090d7496 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.7 + 1.1.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 34d27f28d0..197a9a750d 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 97d5e54b09..7fdfff779b 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 e227489171..085bf014b2 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -20,9 +20,12 @@ #include #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; @@ -35,9 +38,7 @@ GoalPoseUpdater GoalUpdater; Nav2Panel::Nav2Panel(QWidget * parent) : Panel(parent), - server_timeout_(10), - client_nav_("lifecycle_manager_navigation"), - client_loc_("lifecycle_manager_localization") + server_timeout_(100) { // Create the control button and its tooltip @@ -46,6 +47,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 @@ -54,9 +57,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
"); @@ -73,8 +77,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"); @@ -84,7 +92,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(); @@ -96,7 +106,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 @@ -110,38 +120,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", "Cancel Waypoint Mode"); + 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_->setObjectName("accumulated"); - accumulated_->assignProperty(start_reset_button_, "text", "Cancel"); - accumulated_->assignProperty(start_reset_button_, "toolTip", cancel_msg); - accumulated_->assignProperty(start_reset_button_, "enabled", true); - - accumulated_->assignProperty(pause_resume_button_, "text", "Pause"); - accumulated_->assignProperty(pause_resume_button_, "enabled", false); - accumulated_->assignProperty(pause_resume_button_, "toolTip", pause_msg); - - accumulated_->assignProperty(navigation_mode_button_, "text", "Start Navigation"); - accumulated_->assignProperty(navigation_mode_button_, "enabled", false); - accumulated_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg); + 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(); @@ -187,7 +217,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_); @@ -195,9 +228,13 @@ 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_->addTransition(start_reset_button_, SIGNAL(clicked()), canceled_); + 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_); @@ -208,7 +245,9 @@ Nav2Panel::Nav2Panel(QWidget * parent) 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); @@ -217,15 +256,32 @@ Nav2Panel::Nav2Panel(QWidget * parent) runningTransition->setTargetState(idle_); running_->addTransition(runningTransition); - ROSActionQTransition * idleAccumulatedTransition = + 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); - idleAccumulatedTransition->setTargetState(accumulated_); - idle_->addTransition(idleAccumulatedTransition); + idleAccumulatedNTPTransition->setTargetState(accumulated_nav_through_poses_); + idle_->addTransition(idleAccumulatedNTPTransition); - ROSActionQTransition * accumulatedTransition = new ROSActionQTransition(QActionState::ACTIVE); - accumulatedTransition->setTargetState(idle_); - accumulated_->addTransition(accumulatedTransition); + ROSActionQTransition * accumulatedNTPTransition = new ROSActionQTransition(QActionState::ACTIVE); + accumulatedNTPTransition->setTargetState(idle_); + accumulated_nav_through_poses_->addTransition(accumulatedNTPTransition); + auto options = rclcpp::NodeOptions().arguments( + {"--ros-args --remap __node:=navigation_dialog_action_client"}); + client_node_ = std::make_shared("_", options); + + client_nav_ = std::make_shared( + "lifecycle_manager_navigation", client_node_); + client_loc_ = std::make_shared( + "lifecycle_manager_localization", client_node_); initial_thread_ = new InitialThread(client_nav_, client_loc_); connect(initial_thread_, &InitialThread::finished, initial_thread_, &QObject::deleteLater); @@ -250,6 +306,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, @@ -271,7 +329,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_); @@ -283,6 +342,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_); @@ -290,10 +351,6 @@ Nav2Panel::Nav2Panel(QWidget * parent) main_layout->setContentsMargins(10, 10, 10, 10); setLayout(main_layout); - auto options = rclcpp::NodeOptions().arguments( - {"--ros-args --remap __node:=navigation_dialog_action_client"}); - client_node_ = std::make_shared("_", options); - navigation_action_client_ = rclcpp_action::create_client( client_node_, @@ -301,9 +358,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( @@ -323,6 +385,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 @@ -339,12 +439,12 @@ Nav2Panel::onPause() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::pause, - &client_nav_, std::placeholders::_1), server_timeout_); + client_nav_.get(), std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::pause, - &client_loc_, std::placeholders::_1), server_timeout_); + client_loc_.get(), std::placeholders::_1), server_timeout_); } void @@ -354,12 +454,12 @@ Nav2Panel::onResume() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::resume, - &client_nav_, std::placeholders::_1), server_timeout_); + client_nav_.get(), std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::resume, - &client_loc_, std::placeholders::_1), server_timeout_); + client_loc_.get(), std::placeholders::_1), server_timeout_); } void @@ -369,12 +469,12 @@ Nav2Panel::onStartup() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::startup, - &client_nav_, std::placeholders::_1), server_timeout_); + client_nav_.get(), std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::startup, - &client_loc_, std::placeholders::_1), server_timeout_); + client_loc_.get(), std::placeholders::_1), server_timeout_); } void @@ -384,12 +484,12 @@ Nav2Panel::onShutdown() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::reset, - &client_nav_, std::placeholders::_1), server_timeout_); + client_nav_.get(), std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::reset, - &client_loc_, std::placeholders::_1), server_timeout_); + client_loc_.get(), std::placeholders::_1), server_timeout_); timer_.stop(); } @@ -428,6 +528,18 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame) void Nav2Panel::onCancelButtonPressed() { + 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_); @@ -436,32 +548,44 @@ Nav2Panel::onCancelButtonPressed() rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); - return; + } else { + waypoint_follower_goal_handle_.reset(); } } - if (navigation_goal_handle_) { - 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() { @@ -471,7 +595,7 @@ Nav2Panel::onAccumulating() void Nav2Panel::timerEvent(QTimerEvent * event) { - if (state_machine_.configuration().contains(accumulated_)) { + 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"); @@ -482,6 +606,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) @@ -523,7 +668,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; } @@ -531,7 +676,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) { @@ -543,7 +688,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); @@ -564,6 +711,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) { @@ -572,7 +771,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; } @@ -580,10 +779,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); @@ -652,7 +857,7 @@ Nav2Panel::updateWpNavigationMarkers() arrow_marker.color.g = 255; arrow_marker.color.b = 0; arrow_marker.color.a = 1.0f; - arrow_marker.lifetime = rclcpp::Duration(0); + arrow_marker.lifetime = rclcpp::Duration(0s); arrow_marker.frame_locked = false; marker_array->markers.push_back(arrow_marker); @@ -670,7 +875,7 @@ Nav2Panel::updateWpNavigationMarkers() circle_marker.color.g = 0; circle_marker.color.b = 0; circle_marker.color.a = 1.0f; - circle_marker.lifetime = rclcpp::Duration(0); + circle_marker.lifetime = rclcpp::Duration(0s); circle_marker.frame_locked = false; marker_array->markers.push_back(circle_marker); @@ -689,7 +894,7 @@ Nav2Panel::updateWpNavigationMarkers() marker_text.color.g = 255; marker_text.color.b = 0; marker_text.color.a = 1.0f; - marker_text.lifetime = rclcpp::Duration(0); + marker_text.lifetime = rclcpp::Duration(0s); marker_text.frame_locked = false; marker_text.text = "wp_" + std::to_string(i + 1); marker_array->markers.push_back(marker_text); @@ -704,6 +909,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 0000000000..702a6e50dc --- /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 0000000000..f04abad5a6 --- /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 0000000000..c4f398f77d --- /dev/null +++ b/nav2_simple_commander/README.md @@ -0,0 +1,121 @@ +# 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 + +See its [API Guide Page](https://navigation.ros.org/commander_api/index.html) for additional parameter descriptions. + +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, behavior_tree='') | Requests the robot to drive through a set of poses (list of `PoseStamped`).| +| goToPose(pose, behavior_tree='') | 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. | +| followPath(path, controller_id='', goal_checker_id='') | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | +| spin(spin_dist=1.57, time_allowance=10) | Requests the robot to performs an in-place rotation by a given angle. | +| backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10) | Requests the robot to back up by a given distance. | +| cancelTask() | Cancel an ongoing task request.| +| isTaskComplete() | Checks if task is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | +| getFeedback() | Gets feedback from task, returns action server feedback object. | +| getResult() | Gets final result of task, to be called after `isTaskComplete` returns `True`. Returns action server result object. | +| getPath(start, goal, planner_id='', use_start=False) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | +| getPathThroughPoses(start, goals, planner_id='', use_start=False) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. | +| smoothPath(path, smoother_id='', max_duration=2.0, check_for_collision=False) | Smooths a given `nav_msgs/msg/Path` path. | +| changeMap(map_filepath) | Requests a change from the current map to `map_filepath`'s yaml. | +| clearAllCostmaps() | Clears both the global and local costmaps. | +| 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(navigator='bt_navigator, localizer='amcl') | 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. Custom navigator and localizer nodes can be specified | +| 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. | +| destroyNode() | Releases the resources used by the object. | + +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()` +... +path = nav.getPath(init_pose, goal_pose) +smoothed_path = nav.smoothPath(path) +... +nav.goToPose(goal_pose) +while not nav.isTaskComplete(): + feedback = nav.getFeedback() + if feedback.navigation_duration > 600: + nav.cancelTask() +... +result = nav.getResult() +if result == TaskResult.SUCCEEDED: + print('Goal succeeded!') +elif result == TaskResult.CANCELED: + print('Goal was canceled!') +elif result == TaskResult.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. +- `example_follow_path.py` - Demonstrates the path following capabilities of the navigator, as well as a number of auxiliary methods such as path smoothing. +## 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/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py new file mode 100644 index 0000000000..41624dff6f --- /dev/null +++ b/nav2_simple_commander/launch/follow_path_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_follow_path', + 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/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py new file mode 100644 index 0000000000..3cfbd1a2c3 --- /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 0000000000..11f39b9a37 --- /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 0000000000..7d019884fe --- /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 0000000000..5978570243 --- /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/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py new file mode 100644 index 0000000000..0d50500034 --- /dev/null +++ b/nav2_simple_commander/launch/recoveries_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='demo_recoveries', + 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 0000000000..55ef0fe65a --- /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 0000000000..70cf78d99d --- /dev/null +++ b/nav2_simple_commander/launch/warehouse.world @@ -0,0 +1,683 @@ + + + + + 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 + + + + + + + + /tf:=tf + + + 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 0000000000..eddcc0d4b5 --- /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 0000000000..d3a925a252 Binary files /dev/null and b/nav2_simple_commander/media/readme.gif differ diff --git a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/__init__.py b/nav2_simple_commander/nav2_simple_commander/__init__.py similarity index 100% rename from nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/__init__.py rename to nav2_simple_commander/nav2_simple_commander/__init__.py 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 0000000000..429e11b68f --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -0,0 +1,100 @@ +#! /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, TaskResult +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.isTaskComplete(): + 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 == TaskResult.SUCCEEDED: + print('Inspection of shelves complete! Returning to start...') + elif result == TaskResult.CANCELED: + print('Inspection of shelving was canceled. Returning to start...') + elif result == TaskResult.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.isTaskComplete(): + 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 0000000000..6a7a177acb --- /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, TaskResult + +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(f'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.isTaskComplete(): + 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 == TaskResult.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 == TaskResult.CANCELED: + print(f'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 == TaskResult.FAILED: + print(f'Task at {request_item_location} failed!') + exit(-1) + + while not navigator.isTaskComplete(): + pass + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py new file mode 100644 index 0000000000..0313f956ff --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py @@ -0,0 +1,105 @@ +#! /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, TaskResult +import rclpy +from rclpy.duration import Duration + + +""" +Basic recoveries demo. In this demonstration, the robot navigates +to a dead-end where recoveries such as backup and spin are used +to get out of it. +""" + + +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) + + # Wait for navigation to fully activate + navigator.waitUntilNav2Active() + + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = 6.13 + goal_pose.pose.position.y = 1.90 + goal_pose.pose.orientation.w = 1.0 + + navigator.goToPose(goal_pose) + + i = 0 + while not navigator.isTaskComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print( + f'Estimated time of arrival to destination is: \ + {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9}' + ) + + # Robot hit a dead end, back it up + print('Robot hit a dead end, backing up...') + navigator.backup(backup_dist=0.5, backup_speed=0.1) + + i = 0 + while not navigator.isTaskComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print(f'Distance traveled: {feedback.distance_traveled}') + + # Turn it around + print('Spinning robot around...') + navigator.spin(spin_dist=3.14) + + i = 0 + while not navigator.isTaskComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print(f'Spin angle traveled: {feedback.angular_distance_traveled}') + + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Dead end confirmed! Returning to start...') + elif result == TaskResult.CANCELED: + print('Recovery was canceled. Returning to start...') + elif result == TaskResult.FAILED: + print('Recovering from dead end failed! Returning to start...') + + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + navigator.goToPose(initial_pose) + while not navigator.isTaskComplete(): + 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 0000000000..f727cc8d72 --- /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, TaskResult + +import rclpy +from rclpy.duration import Duration + + +""" +Basic security route patrol demo. In this demonstration, the expectation +is that there are security cameras mounted on the robots recording or being +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.isTaskComplete(): + 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.cancelTask() + + # If at end of route, reverse the route to restart + security_route.reverse() + + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Route complete! Restarting...') + elif result == TaskResult.CANCELED: + print('Security route was canceled, exiting.') + exit(1) + elif result == TaskResult.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_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py new file mode 100644 index 0000000000..963dacf942 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -0,0 +1,93 @@ +#! /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, TaskResult +import rclpy + + +""" +Basic navigation demo to follow a given path after smoothing +""" + + +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) + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # 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 = -3.0 + goal_pose.pose.position.y = -2.0 + goal_pose.pose.orientation.w = 1.0 + + # Get the path, smooth it + path = navigator.getPath(initial_pose, goal_pose) + smoothed_path = navigator.smoothPath(path) + + # Follow path + navigator.followPath(smoothed_path) + + i = 0 + while not navigator.isTaskComplete(): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated distance remaining to goal position: ' + + '{0:.3f}'.format(feedback.distance_to_goal) + + '\nCurrent speed of the robot: ' + + '{0:.3f}'.format(feedback.speed)) + + # Do something depending on the return code + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Goal succeeded!') + elif result == TaskResult.CANCELED: + print('Goal was canceled!') + elif result == TaskResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py new file mode 100644 index 0000000000..58c47c75e1 --- /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, TaskResult +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.isTaskComplete(): + ################################################ + # + # 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.cancelTask() + + # 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 == TaskResult.SUCCEEDED: + print('Goal succeeded!') + elif result == TaskResult.CANCELED: + print('Goal was canceled!') + elif result == TaskResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py new file mode 100644 index 0000000000..58dee813c9 --- /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, TaskResult +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.isTaskComplete(): + ################################################ + # + # 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.cancelTask() + + # 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 == TaskResult.SUCCEEDED: + print('Goal succeeded!') + elif result == TaskResult.CANCELED: + print('Goal was canceled!') + elif result == TaskResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py new file mode 100644 index 0000000000..9d02341a66 --- /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, TaskResult +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.isTaskComplete(): + ################################################ + # + # 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.cancelTask() + + # 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 == TaskResult.SUCCEEDED: + print('Goal succeeded!') + elif result == TaskResult.CANCELED: + print('Goal was canceled!') + elif result == TaskResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py new file mode 100644 index 0000000000..1026f8f75d --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -0,0 +1,547 @@ +#! /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 builtin_interfaces.msg import Duration +from geometry_msgs.msg import Point +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import BackUp, Spin +from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose +from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose +from nav2_msgs.action import SmoothPath +from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes + +import rclpy +from rclpy.action import ActionClient +from rclpy.duration import Duration as rclpyDuration +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy +from rclpy.qos import QoSProfile, QoSReliabilityPolicy + + +class TaskResult(Enum): + UNKNOWN = 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.follow_path_client = ActionClient(self, FollowPath, 'follow_path') + 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.smoother_client = ActionClient(self, SmoothPath, 'smooth_path') + self.spin_client = ActionClient(self, Spin, 'spin') + self.backup_client = ActionClient(self, BackUp, 'backup') + 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 destroyNode(self): + self.destroy_node() + + def destroy_node(self): + self.nav_through_poses_client.destroy() + self.nav_to_pose_client.destroy() + self.follow_waypoints_client.destroy() + self.follow_path_client.destroy() + self.compute_path_to_pose_client.destroy() + self.compute_path_through_poses_client.destroy() + self.smoother_client.destroy() + self.spin_client.destroy() + self.backup_client.destroy() + super().destroy_node() + + 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, behavior_tree=''): + """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 + goal_msg.behavior_tree = behavior_tree + + self.info(f'Navigating with {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(f'Goal with {len(poses)} poses was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def goToPose(self, pose, behavior_tree=''): + """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 + goal_msg.behavior_tree = behavior_tree + + 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(f'Following {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(f'Following {len(poses)} waypoints request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def spin(self, spin_dist=1.57, time_allowance=10): + self.debug("Waiting for 'Spin' action server") + while not self.spin_client.wait_for_server(timeout_sec=1.0): + self.info("'Spin' action server not available, waiting...") + goal_msg = Spin.Goal() + goal_msg.target_yaw = spin_dist + goal_msg.time_allowance = Duration(sec=time_allowance) + + self.info(f'Spinning to angle {goal_msg.target_yaw}....') + send_goal_future = self.spin_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('Spin request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): + self.debug("Waiting for 'Backup' action server") + while not self.backup_client.wait_for_server(timeout_sec=1.0): + self.info("'Backup' action server not available, waiting...") + goal_msg = BackUp.Goal() + goal_msg.target = Point(x=float(backup_dist)) + goal_msg.speed = backup_speed + goal_msg.time_allowance = Duration(sec=time_allowance) + + self.info(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....') + send_goal_future = self.backup_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('Backup request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def followPath(self, path, controller_id='', goal_checker_id=''): + """Send a `FollowPath` action request.""" + self.debug("Waiting for 'FollowPath' action server") + while not self.follow_path_client.wait_for_server(timeout_sec=1.0): + self.info("'FollowPath' action server not available, waiting...") + + goal_msg = FollowPath.Goal() + goal_msg.path = path + goal_msg.controller_id = controller_id + goal_msg.goal_checker_id = goal_checker_id + + self.info('Executing path...') + send_goal_future = self.follow_path_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('Follow path was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def cancelTask(self): + """Cancel pending task request of any type.""" + self.info('Canceling current task.') + if self.result_future: + future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, future) + return + + def isTaskComplete(self): + """Check if the task 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(f'Task with failed with status code: {self.status}') + return True + else: + # Timed out, still processing, not complete yet + return False + + self.debug('Task 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 TaskResult.SUCCEEDED + elif self.status == GoalStatus.STATUS_ABORTED: + return TaskResult.FAILED + elif self.status == GoalStatus.STATUS_CANCELED: + return TaskResult.CANCELED + else: + return TaskResult.UNKNOWN + + def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): + """Block until the full navigation system is up and running.""" + self._waitForNodeToActivate(localizer) + if localizer == 'amcl': + self._waitForInitialPose() + self._waitForNodeToActivate(navigator) + self.info('Nav2 is ready for use!') + return + + def getPath(self, start, goal, planner_id='', use_start=False): + """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.start = start + goal_msg.goal = goal + goal_msg.planner_id = planner_id + goal_msg.use_start = use_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(f'Getting path failed with status code: {self.status}') + return None + + return self.result_future.result().result.path + + def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): + """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.start = start + goal_msg.goals = goals + goal_msg.planner_id = planner_id + goal_msg.use_start = use_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(f'Getting path failed with status code: {self.status}') + return None + + return self.result_future.result().result.path + + def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): + """Send a `SmoothPath` action request.""" + self.debug("Waiting for 'SmoothPath' action server") + while not self.smoother_client.wait_for_server(timeout_sec=1.0): + self.info("'SmoothPath' action server not available, waiting...") + + goal_msg = SmoothPath.Goal() + goal_msg.path = path + goal_msg.max_smoothing_duration = rclpyDuration(seconds=max_duration).to_msg() + goal_msg.smoother_id = smoother_id + goal_msg.check_for_collisions = check_for_collision + + self.info('Smoothing path...') + send_goal_future = self.smoother_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('Smooth 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(f'Getting path failed with status code: {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(f'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(f'{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(f'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(f'{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(f'Waiting for {node_name} to become active..') + node_service = f'{node_name}/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info(f'{node_service} service not available, waiting...') + + req = GetState.Request() + state = 'unknown' + while state != 'active': + self.debug(f'Getting {node_name} state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label + self.debug(f'Result of get_state: {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 0000000000..c4e7c94158 --- /dev/null +++ b/nav2_simple_commander/package.xml @@ -0,0 +1,24 @@ + + + + nav2_simple_commander + 1.1.0 + An importable library for writing mobile robot applications in python3 + steve + Apache-2.0 + + rclpy + geometry_msgs + nav2_msgs + action_msgs + lifecycle_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/nav2_bringup/nav2_gazebo_spawner/resource/nav2_gazebo_spawner b/nav2_simple_commander/resource/nav2_simple_commander similarity index 100% rename from nav2_bringup/nav2_gazebo_spawner/resource/nav2_gazebo_spawner rename to nav2_simple_commander/resource/nav2_simple_commander diff --git a/nav2_simple_commander/setup.cfg b/nav2_simple_commander/setup.cfg new file mode 100644 index 0000000000..040b9290ca --- /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 0000000000..ab19a26e47 --- /dev/null +++ b/nav2_simple_commander/setup.py @@ -0,0 +1,38 @@ +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', + 'example_follow_path = nav2_simple_commander.example_follow_path: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', + 'demo_recoveries = nav2_simple_commander.demo_recoveries:main', + ], + }, +) diff --git a/nav2_simple_commander/test/test_copyright.py b/nav2_simple_commander/test/test_copyright.py new file mode 100644 index 0000000000..cc8ff03f79 --- /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 0000000000..27ee1078ff --- /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 0000000000..b234a3840f --- /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 new file mode 100644 index 0000000000..c21e856da8 --- /dev/null +++ b/nav2_smac_planner/CMakeLists.txt @@ -0,0 +1,163 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_smac_planner) + +set(CMAKE_BUILD_TYPE Release) #Debug, Release + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(pluginlib REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(angles REQUIRED) +find_package(ompl REQUIRED) +find_package(OpenMP REQUIRED) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + +if(MSVC) + add_compile_definitions(_USE_MATH_DEFINES) +else() + add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) +endif() + +include_directories( + include + ${OMPL_INCLUDE_DIRS} + ${OpenMP_INCLUDE_DIRS} +) + +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) + +set(dependencies + rclcpp + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + nav_msgs + geometry_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + nav2_core + pluginlib + angles + eigen3_cmake_module +) + +# Hybrid plugin +add_library(${library_name} SHARED + src/smac_planner_hybrid.cpp + src/a_star.cpp + src/collision_checker.cpp + src/smoother.cpp + src/analytic_expansion.cpp + src/node_hybrid.cpp + src/node_lattice.cpp + src/costmap_downsampler.cpp + src/node_2d.cpp + src/node_basic.cpp +) + +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} + ${dependencies} +) + +# 2D plugin +add_library(${library_name}_2d SHARED + src/smac_planner_2d.cpp + src/a_star.cpp + src/smoother.cpp + src/collision_checker.cpp + src/analytic_expansion.cpp + src/node_hybrid.cpp + src/node_lattice.cpp + src/costmap_downsampler.cpp + src/node_2d.cpp + src/node_basic.cpp +) + +target_link_libraries(${library_name}_2d ${OMPL_LIBRARIES}) +target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS}) + +ament_target_dependencies(${library_name}_2d + ${dependencies} +) + +# Lattice plugin +add_library(${library_name}_lattice SHARED + src/smac_planner_lattice.cpp + src/a_star.cpp + src/smoother.cpp + src/collision_checker.cpp + src/analytic_expansion.cpp + src/node_hybrid.cpp + src/node_lattice.cpp + src/costmap_downsampler.cpp + src/node_2d.cpp + src/node_basic.cpp +) + +target_link_libraries(${library_name}_lattice ${OMPL_LIBRARIES}) +target_include_directories(${library_name}_lattice PUBLIC ${Eigen3_INCLUDE_DIRS}) + +ament_target_dependencies(${library_name}_lattice + ${dependencies} +) + +pluginlib_export_plugin_description_file(nav2_core smac_plugin_hybrid.xml) +pluginlib_export_plugin_description_file(nav2_core smac_plugin_2d.xml) +pluginlib_export_plugin_description_file(nav2_core smac_plugin_lattice.xml) + +install(TARGETS ${library_name} ${library_name}_2d ${library_name}_lattice + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY lattice_primitives/sample_primitives DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name} ${library_name}_2d ${library_name}_lattice) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md new file mode 100644 index 0000000000..086403edf0 --- /dev/null +++ b/nav2_smac_planner/README.md @@ -0,0 +1,232 @@ +# Smac Planner + +The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 3 distinct plugins: +- `SmacPlannerHybrid`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models (legged, ackermann and car models). + - `SmacPlannerLattice`: a highly optimized fully reconfigurable State Lattice implementation supporting configurable minimum control sets, with provided control sets for Ackermann, Legged, Differential and Omnidirectional 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. Additional template for planning also could be made available using it. +- `CollisionChecker`: Collision check based on a robot's radius or footprint. +- `Smoother`: A simple path smoother to smooth out 2D, Hybrid-A\*, and State Lattice paths. + +We have users reporting using this on: +- Delivery robots +- Industrial robots +- Vertical farming +- Solar farms + +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smac-planner.html) for additional parameter descriptions. + +## 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 **circular** differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support **legged, cars, car-like, and ackermann vehicles** using the `SmacPlannerHybrid` plugin which implements a Hybrid-A\* planner. We support **non-circular, arbitrary shaped, any model vehicles** using the `SmacPlannerLattice` plugin which implements a State Lattice planner. It contains control sets and generators for ackermann, legged, differential drive and omnidirectional vehicles, but you may provide your own for another robot type or to have different planning behaviors. The last two plugins are also useful for curvature constrained or kinematically feasible 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 `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), with modifications to the heuristic, traversal functions to increase path quality without needing expensive optimization-based smoothing. + +The `SmacPlannerLattice` implements the State Lattice planner. While we do not implement it precisely the same way as [Optimal, Smooth, Nonholonomic MobileRobot Motion Planning in State Lattices](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2007_1/pivtoraiko_mihail_2007_1.pdf) (with control sets found using [Generating Near Minimal Spanning Control Sets for Constrained Motion Planning in Discrete State Spaces](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2005_1/pivtoraiko_mihail_2005_1.pdf)), it is sufficiently similar it may be used as a good reference. Additional optimizations for on-approach analytic expansions and improved heuristic functions were used, largely matching those of Hybrid-A\* to allow them to share these optimized implementations to drive search towards the goal, faster. + +In summary... + +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 requiring kinematically feasible planning with SE2 collision checking +- Legged robots + +The `SmacPlannerLattice` is designed to work with: +- Arbitrary shaped, non-circular robots requiring kinematically feasible planning with SE2 collision checking using the full capabilities of the drivetrain +- Flexibility to use other robot model types or with provided non-circular differental, ackermann, and omni support + +The `SmacPlanner2D` is designed to work with: +- Circular, differential or omnidirectional robots +- Relatively small robots with respect to environment size (e.g. RC car in a hallway or large robot in a convention center) that can be approximated by circular footprint. + +## Features + +We further improve on Hybrid-A\* 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). +- Gradient-descent, basic but fast smoother +- Faster planning than original paper by highly optimizing the template A\* algorithm. +- Faster planning via custom precomputed heuristic, motion primitive, and other functions. +- 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. +- High unit and integration test coverage, doxygen documentation. +- Uses modern C++14 language features and individual components are easily reusable. +- 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 subsequent 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 subsequent replanning 20x or more. + +Most of these features (multi-resolution, models, smoother, etc) are also available in the `SmacPlanner2D` and `SmacPlannerLattice` plugins. + +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. + +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 new Smoothing Server as the Cost-aware smoother. This smoother has been replaced by a simpler optimization 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. It will through terminate when paths become in collision with the environment. If you would like to use this smoother, however, it is available in the smoother server, though it will take some additional compute time. + +## Metrics + +The original Hybrid-A\* implementation boasted planning times of 50-300ms for planning across 102,400 cell maps with 72 angular bins. We see much faster results in our evaluations: + +- **2-20ms** for planning across 147,456 (1.4x larger) cell maps with 72 angular bins. +- **30-200ms** for planning across 344,128 (3.3x larger) cell map with 72 angular bins. + +An example of the 3 planners can be seen below, planning a roughly 75 m path. +- 2D A* computed the path in 243ms (Panel 1) +- Hybrid-A* computed the path in 144ms (Panel 2) +- State Lattice computed the path in 113ms (Panel 3) +- For reference: NavFn compute the path in 146ms, including some nasty path discontinuity artifacts + +![alt text](test/3planners.png) + +## Design + +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 3 nodes by default currently. The 2D node template (`Node2D`) which does 2D grid-search with either 4 or 8-connected neighborhoods. 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. We also provide the Lattice (`NodeLattice`) node for state lattice planning making use of the wider range of velocity options available to differential and omnidirectional robots. 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 (not available for State Lattice due to the lattices generated are dependent on costmap resolution). For the `SmacPlannerHybrid` and `SmacPlannerLattice` 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. + +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 place. + +``` +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: True + + 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: 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) + analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius + 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.0 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0 + non_straight_penalty: 1.20 # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # 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. + retrospective_penalty: 0.025 # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0 + rotation_penalty: 5.0 # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings. + 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. + allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. + smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1e-10 + do_refinement: true # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further +``` + +## Topics + +| Topic | Type | +|-----------------|-------------------| +| unsmoothed_path | nav_msgs/Path | + + +## Install + +``` +sudo apt-get install ros--nav2-smac-planner +``` + +## Etc (Important Side Notes) + +### Potential Fields + +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 / 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 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**. + +### Hybrid-A* and State Lattice Turning Radius' + +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. + +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. + +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 the Hybrid-A\*, State Lattice, 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. + +### Penalty Function Tuning + +The penalty function defaults are tuned for all of the planners based on a 5cm costmap. While some change in this should not largely impact default behavior, it may be good to tune for your specific application and needs. The default values were tuned to have decent out of the box behavior for a large number of platforms and resolutions. In most situations, you should not need to play with them. + +**However**, due to the nature of the State Lattice planner being able to use any number of custom generated minimum control sets, this planner may require more tuning to get good behavior. The defaults for State Lattice were generated using the 5cm Ackermann files you can find in this package as initial examples. After a change in formulation for the Hybrid-A* planner, the default of change penalty off seems to produce good results, but please tune to your application need and run-time speed requirements. + +When tuning, the "reasonable" range for each penalty is listed below. While you may obviously tune outside of these ranges, I've found that they offer a good trade-off and outside of these ranges behaviors get suboptimal quickly. +- Cost: 1.7 - 6.0 +- Non-Straight: 1.0 - 1.3 +- Change: 0.0 - 0.3 +- Reverse: 1.3 - 5.0 + +Note that change penalty must be greater than 0.0. The Non-staight, reverse, and cost penalties must be greater than 1.0, strictly. + +### 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 new file mode 100644 index 0000000000..2af454bb53 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -0,0 +1,266 @@ +// 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. + +#ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_ +#define NAV2_SMAC_PLANNER__A_STAR_HPP_ + +#include +#include +#include +#include +#include +#include +#include "Eigen/Core" + +#include "nav2_costmap_2d/costmap_2d.hpp" + +#include "nav2_smac_planner/analytic_expansion.hpp" +#include "nav2_smac_planner/node_2d.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_smac_planner/node_lattice.hpp" +#include "nav2_smac_planner/node_basic.hpp" +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/constants.hpp" + +namespace nav2_smac_planner +{ + +/** + * @class nav2_smac_planner::AStarAlgorithm + * @brief An A* implementation for planning in a costmap. Templated based on the Node type. + */ +template +class AStarAlgorithm +{ +public: + typedef NodeT * NodePtr; + typedef std::unordered_map Graph; + typedef std::vector NodeVector; + typedef std::pair> NodeElement; + typedef typename NodeT::Coordinates Coordinates; + typedef typename NodeT::CoordinateVector CoordinateVector; + typedef typename NodeVector::iterator NeighborIterator; + typedef std::function NodeGetter; + + /** + * @struct nav2_smac_planner::NodeComparator + * @brief Node comparison for priority queue sorting + */ + struct NodeComparator + { + bool operator()(const NodeElement & a, const NodeElement & b) const + { + return a.first > b.first; + } + }; + + typedef std::priority_queue, NodeComparator> NodeQueue; + + /** + * @brief A constructor for nav2_smac_planner::PlannerServer + * @param neighborhood The type of neighborhood to use for search (4 or 8 connected) + */ + explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); + + /** + * @brief A destructor for nav2_smac_planner::AStarAlgorithm + */ + ~AStarAlgorithm(); + + /** + * @brief Initialization of the planner with defaults + * @param allow_unknown Allow search in unknown space, good for navigation while mapping + * @param max_iterations Maximum number of iterations to use while expanding search + * @param max_on_approach_iterations Maximum number of iterations before returning a valid + * path once within thresholds to refine path + * comes at more compute time but smoother paths. + * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns + * false after this timeout + */ + void initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations, + const double & max_planning_time, + const float & lookup_table_size, + const unsigned int & dim_3_size); + + /** + * @brief Creating path from given costmap, start, and goal + * @param path Reference to a vector of indicies of generated path + * @param num_iterations Reference to number of iterations to create plan + * @param tolerance Reference to tolerance in costmap nodes + * @return if plan was successful + */ + bool createPath(CoordinateVector & path, int & num_iterations, const float & tolerance); + + /** + * @brief Sets the collision checker to use + * @param collision_checker Collision checker to use for checking state validity + */ + void setCollisionChecker(GridCollisionChecker * collision_checker); + + /** + * @brief Set the goal for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + */ + void setGoal( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3); + + /** + * @brief Set the starting pose for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + */ + void setStart( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3); + + /** + * @brief Get maximum number of iterations to plan + * @return Reference to Maximum iterations parameter + */ + int & getMaxIterations(); + + /** + * @brief Get pointer reference to starting node + * @return Node pointer reference to starting node + */ + NodePtr & getStart(); + + /** + * @brief Get pointer reference to goal node + * @return Node pointer reference to goal node + */ + NodePtr & getGoal(); + + /** + * @brief Get maximum number of on-approach iterations after within threshold + * @return Reference to Maximum on-appraoch iterations parameter + */ + int & getOnApproachMaxIterations(); + + /** + * @brief Get tolerance, in node nodes + * @return Reference to tolerance parameter + */ + float & getToleranceHeuristic(); + + /** + * @brief Get size of graph in X + * @return Size in X + */ + unsigned int & getSizeX(); + + /** + * @brief Get size of graph in Y + * @return Size in Y + */ + unsigned int & getSizeY(); + + /** + * @brief Get number of angle quantization bins (SE2) or Z coordinate (XYZ) + * @return Number of angle bins / Z dimension + */ + unsigned int & getSizeDim3(); + +protected: + /** + * @brief Get pointer to next goal in open set + * @return Node pointer reference to next heuristically scored node + */ + inline NodePtr getNextNode(); + + /** + * @brief Get pointer to next goal in open set + * @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); + + /** + * @brief Adds node to graph + * @param cost The cost to sort into the open set of the node + * @param node Node pointer reference to add to open set + */ + inline NodePtr addToGraph(const unsigned int & index); + + /** + * @brief Check if this node is the goal node + * @param node Node pointer to check if its the goal node + * @return if node is goal + */ + inline bool isGoal(NodePtr & node); + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + inline float getHeuristicCost(const NodePtr & node); + + /** + * @brief Check if inputs to planner are valid + * @return Are valid + */ + inline bool areInputsValid(); + + /** + * @brief Clear hueristic queue of nodes to search + */ + inline void clearQueue(); + + /** + * @brief Clear graph of nodes searched + */ + inline void clearGraph(); + + int _timing_interval = 5000; + + bool _traverse_unknown; + int _max_iterations; + int _max_on_approach_iterations; + double _max_planning_time; + float _tolerance; + unsigned int _x_size; + unsigned int _y_size; + unsigned int _dim3_size; + SearchInfo _search_info; + + Coordinates _goal_coordinates; + NodePtr _start; + NodePtr _goal; + + Graph _graph; + NodeQueue _queue; + + MotionModel _motion_model; + NodeHeuristicPair _best_heuristic_node; + + GridCollisionChecker * _collision_checker; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr> _expander; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__A_STAR_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp new file mode 100644 index 0000000000..fa1b2f1fdd --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -0,0 +1,133 @@ +// 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__ANALYTIC_EXPANSION_HPP_ +#define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ + +#include +#include +#include +#include + +#include "nav2_smac_planner/node_2d.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_smac_planner/node_lattice.hpp" +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/constants.hpp" + +namespace nav2_smac_planner +{ + +template +class AnalyticExpansion +{ +public: + typedef NodeT * NodePtr; + typedef typename NodeT::Coordinates Coordinates; + typedef std::function NodeGetter; + + /** + * @struct nav2_smac_planner::AnalyticExpansion::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; + + /** + * @brief Constructor for analytic expansion object + */ + AnalyticExpansion( + const MotionModel & motion_model, + const SearchInfo & search_info, + const bool & traverse_unknown, + const unsigned int & dim_3_size); + + /** + * @brief Sets the collision checker and costmap to use in expansion validation + * @param collision_checker Collision checker to use + */ + void setCollisionChecker(GridCollisionChecker * collision_checker); + + /** + * @brief Attempt an analytic path completion + * @param node The node to start the analytic path from + * @param goal The goal node to plan to + * @param getter Gets a node at a set of coordinates + * @param iterations Iterations to run over + * @param best_cost Best heuristic cost to propertionally expand more closer to the goal + * @return Node pointer reference to goal node if successful, else + * return nullptr + */ + NodePtr tryAnalyticExpansion( + const NodePtr & current_node, + const NodePtr & goal_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 goal The goal node to plan to + * @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 NodePtr & goal, + 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 goal The goal node to plan to + * @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 NodePtr & goal, + const AnalyticExpansionNodes & expanded_nodes); + + /** + * @brief Takes an expanded nodes to clean up, if necessary, of any state + * information that may be poluting it from a prior search iteration + * @param expanded_nodes Expanded node to clean up from search + */ + void cleanNode(const NodePtr & nodes); + +protected: + MotionModel _motion_model; + SearchInfo _search_info; + bool _traverse_unknown; + unsigned int _dim_3_size; + GridCollisionChecker * _collision_checker; + std::list> _detached_nodes; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp new file mode 100644 index 0000000000..e6520f4bd2 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -0,0 +1,124 @@ +// 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 "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_smac_planner/constants.hpp" + +#ifndef NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_ +#define NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_ + +namespace nav2_smac_planner +{ + +/** + * @class nav2_smac_planner::GridCollisionChecker + * @brief A costmap grid collision checker + */ +class GridCollisionChecker + : public nav2_costmap_2d::FootprintCollisionChecker +{ +public: + /** + * @brief A constructor for nav2_smac_planner::GridCollisionChecker + * for use when regular bin intervals are appropriate + * @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, + unsigned int num_quantizations); + + /** + * @brief A constructor for nav2_smac_planner::GridCollisionChecker + * for use when irregular bin intervals are appropriate + * @param costmap The costmap to collision check against + * @param angles The vector of possible angle bins to precompute for + * orientations for to speed up collision checking, in radians + */ + // GridCollisionChecker( + // nav2_costmap_2d::Costmap2D * costmap, + // std::vector & angles); + + /** + * @brief Set the footprint to use with collision checker + * @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, + const double & possible_inscribed_cost); + + /** + * @brief Check if in collision with costmap and footprint at pose + * @param x X coordinate of pose to check against + * @param y Y coordinate of pose to check against + * @param theta Angle bin number of pose to check against (NOT radians) + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const float & x, + const float & y, + const float & theta, + const bool & traverse_unknown); + + /** + * @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); + + /** + * @brief Get cost at footprint pose in costmap + * @return the cost at the pose in costmap + */ + float getCost(); + + /** + * @brief Get the angles of the precomputed footprint orientations + * @return the ordered vector of angles corresponding to footprints + */ + std::vector & getPrecomputedAngles() + { + return angles_; + } + +private: + /** + * @brief Check if value outside the range + * @param min Minimum value of the range + * @param max Maximum value of the range + * @param value the value to check if it is within the range + * @return boolean if in range or not + */ + bool outsideRange(const unsigned int & max, const float & value); + +protected: + std::vector oriented_footprints_; + nav2_costmap_2d::Footprint unoriented_footprint_; + double footprint_cost_; + bool footprint_is_radius_; + std::vector angles_; + double possible_inscribed_cost_{-1}; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp new file mode 100644 index 0000000000..8bd0db5575 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -0,0 +1,75 @@ +// 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 NAV2_SMAC_PLANNER__CONSTANTS_HPP_ +#define NAV2_SMAC_PLANNER__CONSTANTS_HPP_ + +#include + +namespace nav2_smac_planner +{ +enum class MotionModel +{ + UNKNOWN = 0, + VON_NEUMANN = 1, + MOORE = 2, + DUBIN = 3, + REEDS_SHEPP = 4, + STATE_LATTICE = 5, +}; + +inline std::string toString(const MotionModel & n) +{ + switch (n) { + case MotionModel::VON_NEUMANN: + return "Von Neumann"; + case MotionModel::MOORE: + return "Moore"; + case MotionModel::DUBIN: + return "Dubin"; + case MotionModel::REEDS_SHEPP: + return "Reeds-Shepp"; + case MotionModel::STATE_LATTICE: + return "State Lattice"; + default: + return "Unknown"; + } +} + +inline MotionModel fromString(const std::string & n) +{ + if (n == "VON_NEUMANN") { + return MotionModel::VON_NEUMANN; + } else if (n == "MOORE") { + return MotionModel::MOORE; + } else if (n == "DUBIN") { + 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.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 + +#endif // NAV2_SMAC_PLANNER__CONSTANTS_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp b/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp new file mode 100644 index 0000000000..332507d7c2 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp @@ -0,0 +1,118 @@ +// Copyright (c) 2020, Carlos Luis +// +// 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__COSTMAP_DOWNSAMPLER_HPP_ +#define NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ + +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_smac_planner/constants.hpp" + +namespace nav2_smac_planner +{ + +/** + * @class nav2_smac_planner::CostmapDownsampler + * @brief A costmap downsampler for more efficient path planning + */ +class CostmapDownsampler +{ +public: + /** + * @brief A constructor for CostmapDownsampler + */ + CostmapDownsampler(); + + /** + * @brief A destructor for CostmapDownsampler + */ + ~CostmapDownsampler(); + + /** + * @brief Configure the downsampled costmap object and the ROS publisher + * @param node Lifecycle node pointer + * @param global_frame The ID of the global frame used by the costmap + * @param topic_name The name of the topic to publish the downsampled costmap + * @param costmap The costmap we want to downsample + * @param downsampling_factor Multiplier for the costmap resolution + * @param use_min_cost_neighbor If true, min function is used instead of max for downsampling + */ + void on_configure( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & global_frame, + const std::string & topic_name, + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor, + const bool & use_min_cost_neighbor = false); + + /** + * @brief Activate the publisher of the downsampled costmap + */ + void on_activate(); + + /** + * @brief Deactivate the publisher of the downsampled costmap + */ + void on_deactivate(); + + /** + * @brief Cleanup the publisher of the downsampled costmap + */ + void on_cleanup(); + + /** + * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap + * @param downsampling_factor Multiplier for the costmap resolution + * @return A ptr to the downsampled costmap + */ + nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor); + + /** + * @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version + */ + void resizeCostmap(); + +protected: + /** + * @brief Update the sizes X-Y of the costmap and its downsampled version + */ + void updateCostmapSize(); + + /** + * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell + * @param new_mx The X-coordinate of the cell in the new costmap + * @param new_my The Y-coordinate of the cell in the new costmap + */ + void setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my); + + unsigned int _size_x; + unsigned int _size_y; + unsigned int _downsampled_size_x; + unsigned int _downsampled_size_y; + unsigned int _downsampling_factor; + bool _use_min_cost_neighbor; + float _downsampled_resolution; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _downsampled_costmap; + std::unique_ptr _downsampled_costmap_pub; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp new file mode 100644 index 0000000000..4150deaee7 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -0,0 +1,284 @@ +// 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 NAV2_SMAC_PLANNER__NODE_2D_HPP_ +#define NAV2_SMAC_PLANNER__NODE_2D_HPP_ + +#include +#include +#include +#include +#include +#include +#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 +{ + +/** + * @class nav2_smac_planner::Node2D + * @brief Node2D implementation for graph + */ +class Node2D +{ +public: + typedef Node2D * NodePtr; + typedef std::unique_ptr> Graph; + typedef std::vector NodeVector; + + /** + * @class nav2_smac_planner::Node2D::Coordinates + * @brief Node2D implementation of coordinate structure + */ + struct Coordinates + { + Coordinates() {} + Coordinates(const float & x_in, const float & y_in) + : x(x_in), y(y_in) + {} + + float x, y; + }; + typedef std::vector CoordinateVector; + + /** + * @brief A constructor for nav2_smac_planner::Node2D + * @param index The index of this node for self-reference + */ + explicit Node2D(const unsigned int index); + + /** + * @brief A destructor for nav2_smac_planner::Node2D + */ + ~Node2D(); + + /** + * @brief operator== for comparisons + * @param Node2D right hand side node reference + * @return If cell indicies are equal + */ + bool operator==(const Node2D & rhs) + { + return this->_index == rhs._index; + } + + /** + * @brief Reset method for new search + */ + void reset(); + /** + * @brief Gets the accumulated cost at this node + * @return accumulated cost + */ + inline float & getAccumulatedCost() + { + return _accumulated_cost; + } + + /** + * @brief Sets the accumulated cost at this node + * @param reference to accumulated cost + */ + inline void setAccumulatedCost(const float & cost_in) + { + _accumulated_cost = cost_in; + } + + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline float & getCost() + { + 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 + */ + inline bool & wasVisited() + { + return _was_visited; + } + + /** + * @brief Sets if cell has been visited in search + */ + 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; + } + + /** + * @brief Gets cell index + * @return Reference to cell index + */ + inline unsigned int & getIndex() + { + return _index; + } + + /** + * @brief Check if this node is valid + * @param traverse_unknown If we can explore unknown nodes on the graph + * @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); + + /** + * @brief get traversal cost from this node to child node + * @param child Node pointer to this node's child + * @return traversal cost + */ + float getTraversalCost(const NodePtr & child); + + /** + * @brief Get index + * @param x x coordinate of point to get index of + * @param y y coordinate of point to get index of + * @param width width of costmap + * @return index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & width) + { + return x + y * width; + } + + /** + * @brief Get index + * @param Index Index of point + * @param width width of costmap + * @param angles angle bins to use (must be 1 or throws exception) + * @return coordinates of point + */ + static inline Coordinates getCoords( + const unsigned int & index, const unsigned int & width, const unsigned int & angles) + { + if (angles != 1) { + throw std::runtime_error("Node type Node2D does not have a valid angle quantization."); + } + + 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 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 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 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 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 + */ + void getNeighbors( + std::function & validity_checker, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); + + /** + * @brief Set the starting pose for planning, as a node index + * @param path Reference to a vector of indicies of generated path + * @return whether the path was able to be backtraced + */ + bool backtracePath(CoordinateVector & path); + + Node2D * parent; + static float cost_travel_multiplier; + static std::vector _neighbors_grid_offsets; + +private: + float _cell_cost; + float _accumulated_cost; + unsigned int _index; + bool _was_visited; + bool _is_queued; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__NODE_2D_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp new file mode 100644 index 0000000000..407cde5c79 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -0,0 +1,83 @@ +// 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 NAV2_SMAC_PLANNER__NODE_BASIC_HPP_ +#define NAV2_SMAC_PLANNER__NODE_BASIC_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/StateSpace.h" + +#include "nav2_smac_planner/constants.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_smac_planner/node_lattice.hpp" +#include "nav2_smac_planner/node_2d.hpp" +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/collision_checker.hpp" + +namespace nav2_smac_planner +{ + +/** + * @class nav2_smac_planner::NodeBasic + * @brief NodeBasic implementation for priority queue insertion + */ +template +class NodeBasic +{ +public: + /** + * @brief A constructor for nav2_smac_planner::NodeBasic + * @param cost_in The costmap cost at this node + * @param index The index of this node for self-reference + */ + explicit NodeBasic(const unsigned int index) + : index(index), + graph_node_ptr(nullptr) + { + } + + /** + * @brief Take a NodeBasic and populate it with any necessary state + * cached in the queue for NodeT. + * @param node NodeT ptr to populate metadata into NodeBasic + */ + void populateSearchNode(NodeT * & node); + + /** + * @brief Take a NodeBasic and populate it with any necessary state + * cached in the queue for NodeTs. + * @param node Search node (basic) object to initialize internal node + * with state + */ + void processSearchNode(); + + typename NodeT::Coordinates pose; // Used by NodeHybrid and NodeLattice + NodeT * graph_node_ptr; + MotionPrimitive * prim_ptr; // Used by NodeLattice + unsigned int index, motion_index; + bool backward; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__NODE_BASIC_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp new file mode 100644 index 0000000000..18bf38576f --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -0,0 +1,467 @@ +// 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 NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ +#define NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/StateSpace.h" + +#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; + +typedef std::pair ObstacleHeuristicElement; +struct ObstacleHeuristicComparator +{ + bool operator()(const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b) const + { + return a.first > b.first; + } +}; + +typedef std::vector ObstacleHeuristicQueue; + +// Must forward declare +class NodeHybrid; + +/** + * @struct nav2_smac_planner::HybridMotionTable + * @brief A table of motion primitives and related functions + */ +struct HybridMotionTable +{ + /** + * @brief A constructor for nav2_smac_planner::HybridMotionTable + */ + HybridMotionTable() {} + + /** + * @brief Initializing using Dubin model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initDubin( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Initializing using Reeds-Shepp model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initReedsShepp( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Get projections of motion models + * @param node Ptr to NodeHybrid + * @return A set of motion poses + */ + MotionPoses getProjections(const NodeHybrid * node); + + /** + * @brief Get the angular bin to use from a raw orientation + * @param theta Angle in radians + * @return bin index of closest angle to request + */ + unsigned int getClosestAngularBin(const double & theta); + + /** + * @brief Get the raw orientation from an angular bin + * @param bin_idx Index of the bin + * @return Raw orientation in radians + */ + float getAngleFromBin(const unsigned int & bin_idx); + + MotionModel motion_model = MotionModel::UNKNOWN; + 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; + float travel_distance_reward; + ompl::base::StateSpacePtr state_space; + std::vector> delta_xs; + std::vector> delta_ys; + std::vector trig_values; +}; + +/** + * @class nav2_smac_planner::NodeHybrid + * @brief NodeHybrid implementation for graph, Hybrid-A* + */ +class NodeHybrid +{ +public: + typedef NodeHybrid * NodePtr; + typedef std::unique_ptr> Graph; + typedef std::vector NodeVector; + + /** + * @class nav2_smac_planner::NodeHybrid::Coordinates + * @brief NodeHybrid implementation of coordinate structure + */ + struct Coordinates + { + /** + * @brief A constructor for nav2_smac_planner::NodeHybrid::Coordinates + */ + 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 + */ + Coordinates(const float & x_in, const float & y_in, const float & theta_in) + : 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::NodeHybrid + * @param index The index of this node for self-reference + */ + explicit NodeHybrid(const unsigned int index); + + /** + * @brief A destructor for nav2_smac_planner::NodeHybrid + */ + ~NodeHybrid(); + + /** + * @brief operator== for comparisons + * @param NodeHybrid right hand side node reference + * @return If cell indicies are equal + */ + bool operator==(const NodeHybrid & rhs) + { + return this->_index == rhs._index; + } + + /** + * @brief setting continuous coordinate search poses (in partial-cells) + * @param Pose pose + */ + inline void setPose(const Coordinates & pose_in) + { + pose = pose_in; + } + + /** + * @brief Reset method for new search + */ + void reset(); + + /** + * @brief Gets the accumulated cost at this node + * @return accumulated cost + */ + inline float & getAccumulatedCost() + { + return _accumulated_cost; + } + + /** + * @brief Sets the accumulated cost at this node + * @param reference to accumulated cost + */ + inline void setAccumulatedCost(const float & cost_in) + { + _accumulated_cost = cost_in; + } + + /** + * @brief Sets the motion primitive index used to achieve node in search + * @param reference to motion primitive idx + */ + inline void setMotionPrimitiveIndex(const unsigned int & idx) + { + _motion_primitive_index = idx; + } + + /** + * @brief Gets the motion primitive index used to achieve node in search + * @return reference to motion primitive idx + */ + inline unsigned int & getMotionPrimitiveIndex() + { + return _motion_primitive_index; + } + + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline float & getCost() + { + return _cell_cost; + } + + /** + * @brief Gets if cell has been visited in search + * @param If cell was visited + */ + inline bool & wasVisited() + { + return _was_visited; + } + + /** + * @brief Sets if cell has been visited in search + */ + inline void visited() + { + _was_visited = true; + } + + /** + * @brief Gets cell index + * @return Reference to cell index + */ + inline unsigned int & getIndex() + { + return _index; + } + + /** + * @brief Check if this node is valid + * @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); + + /** + * @brief Get traversal cost of parent node to child node + * @param child Node pointer to child + * @return traversal cost + */ + float getTraversalCost(const NodePtr & child); + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @param width Width of costmap + * @param angle_quantization Number of theta bins + * @return Index + */ + 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) + { + return angle + x * angle_quantization + y * width * angle_quantization; + } + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @return Index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & angle) + { + return getIndex( + x, y, angle, motion_table.size_x, + motion_table.num_angle_quantization); + } + + /** + * @brief Get coordinates at index + * @param index Index of point + * @param width Width of costmap + * @param angle_quantization Theta size of costmap + * @return Coordinates + */ + static inline Coordinates getCoords( + const unsigned int & index, + const unsigned int & width, const unsigned int & angle_quantization) + { + return Coordinates( + (index / angle_quantization) % width, // x + index / (angle_quantization * width), // y + index % angle_quantization); // theta + } + + /** + * @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 nav2_costmap_2d::Costmap2D * costmap); + + /** + * @brief Initialize motion models + * @param motion_model Motion model enum to use + * @param size_x Size of X of graph + * @param size_y Size of y of graph + * @param angle_quantization Size of theta bins of graph + * @param search_info Search info to use + */ + static void initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & angle_quantization, + SearchInfo & search_info); + + /** + * @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 float getObstacleHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const double & cost_penalty); + + /** + * @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 & start_x, const unsigned int & start_y, + const unsigned int & goal_x, const unsigned int & goal_y); + + /** + * @brief Retrieve all valid neighbors of a node. + * @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 + */ + void getNeighbors( + std::function & validity_checker, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); + + /** + * @brief Set the starting pose for planning, as a node index + * @param path Reference to a vector of indicies of generated path + * @return whether the path was able to be backtraced + */ + bool backtracePath(CoordinateVector & path); + + NodeHybrid * parent; + Coordinates pose; + + // 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 ObstacleHeuristicQueue 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; + unsigned int _motion_primitive_index; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp new file mode 100644 index 0000000000..b40624efd6 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -0,0 +1,429 @@ +// 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 NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_ +#define NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "nlohmann/json.hpp" +#include "ompl/base/StateSpace.h" +#include "angles/angles.h" + +#include "nav2_smac_planner/constants.hpp" +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_smac_planner/utils.hpp" + +namespace nav2_smac_planner +{ + +// forward declare +class NodeLattice; +class NodeHybrid; + +/** + * @struct nav2_smac_planner::LatticeMotionTable + * @brief A table of motion primitives and related functions + */ +struct LatticeMotionTable +{ + /** + * @brief A constructor for nav2_smac_planner::LatticeMotionTable + */ + LatticeMotionTable() {} + + /** + * @brief Initializing state lattice planner's motion model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initMotionModel( + unsigned int & size_x_in, + SearchInfo & search_info); + + /** + * @brief Get projections of motion models + * @param node Ptr to NodeLattice + * @return A set of motion poses + */ + MotionPrimitivePtrs getMotionPrimitives(const NodeLattice * node); + + /** + * @brief Get file metadata needed + * @param lattice_filepath Filepath to the lattice file + * @return A set of metadata containing the number of angular bins + * and the global coordinates minimum turning radius of the primitives + * for use in analytic expansion and heuristic calculation. + */ + static LatticeMetadata getLatticeMetadata(const std::string & lattice_filepath); + + /** + * @brief Get the angular bin to use from a raw orientation + * @param theta Angle in radians + * @return bin index of closest angle to request + */ + unsigned int getClosestAngularBin(const double & theta); + + /** + * @brief Get the raw orientation from an angular bin + * @param bin_idx Index of the bin + * @return Raw orientation in radians + */ + float & getAngleFromBin(const unsigned int & bin_idx); + + unsigned int size_x; + unsigned int num_angle_quantization; + float change_penalty; + float non_straight_penalty; + float cost_penalty; + float reverse_penalty; + float travel_distance_reward; + float rotation_penalty; + bool allow_reverse_expansion; + std::vector> motion_primitives; + ompl::base::StateSpacePtr state_space; + std::vector trig_values; + std::string current_lattice_filepath; + LatticeMetadata lattice_metadata; +}; + +/** + * @class nav2_smac_planner::NodeLattice + * @brief NodeLattice implementation for graph, Hybrid-A* + */ +class NodeLattice +{ +public: + typedef NodeLattice * NodePtr; + typedef std::unique_ptr> Graph; + typedef std::vector NodeVector; + typedef NodeHybrid::Coordinates Coordinates; + typedef NodeHybrid::CoordinateVector CoordinateVector; + + /** + * @brief A constructor for nav2_smac_planner::NodeLattice + * @param index The index of this node for self-reference + */ + explicit NodeLattice(const unsigned int index); + + /** + * @brief A destructor for nav2_smac_planner::NodeLattice + */ + ~NodeLattice(); + + /** + * @brief operator== for comparisons + * @param NodeLattice right hand side node reference + * @return If cell indicies are equal + */ + bool operator==(const NodeLattice & rhs) + { + return this->_index == rhs._index; + } + + /** + * @brief setting continuous coordinate search poses (in partial-cells) + * @param Pose pose + */ + inline void setPose(const Coordinates & pose_in) + { + pose = pose_in; + } + + /** + * @brief Reset method for new search + */ + void reset(); + + /** + * @brief Sets the motion primitive used to achieve node in search + * @param pointer to motion primitive + */ + inline void setMotionPrimitive(MotionPrimitive * prim) + { + _motion_primitive = prim; + } + + /** + * @brief Gets the motion primitive used to achieve node in search + * @return pointer to motion primitive + */ + inline MotionPrimitive * & getMotionPrimitive() + { + return _motion_primitive; + } + + /** + * @brief Gets the accumulated cost at this node + * @return accumulated cost + */ + inline float & getAccumulatedCost() + { + return _accumulated_cost; + } + + /** + * @brief Sets the accumulated cost at this node + * @param reference to accumulated cost + */ + inline void setAccumulatedCost(const float & cost_in) + { + _accumulated_cost = cost_in; + } + + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline float & getCost() + { + return _cell_cost; + } + + /** + * @brief Gets if cell has been visited in search + * @param If cell was visited + */ + inline bool & wasVisited() + { + return _was_visited; + } + + /** + * @brief Sets if cell has been visited in search + */ + inline void visited() + { + _was_visited = true; + } + + /** + * @brief Gets cell index + * @return Reference to cell index + */ + inline unsigned int & getIndex() + { + return _index; + } + + /** + * @brief Sets that this primitive is moving in reverse + */ + inline void backwards(bool back = true) + { + _backwards = back; + } + + /** + * @brief Gets if this primitive is moving in reverse + * @return backwards If moving in reverse + */ + inline bool isBackward() + { + return _backwards; + } + + /** + * @brief Check if this node is valid + * @param traverse_unknown If we can explore unknown nodes on the graph + * @param collision_checker Collision checker object to aid in validity checking + * @param primitive Optional argument if needing to check over a primitive + * not only a terminal pose + * @param is_backwards Optional argument if needed to check if prim expansion is + * in reverse + * @return whether this node is valid and collision free + */ + bool isNodeValid( + const bool & traverse_unknown, + GridCollisionChecker * collision_checker, + MotionPrimitive * primitive = nullptr, + bool is_backwards = false); + + /** + * @brief Get traversal cost of parent node to child node + * @param child Node pointer to child + * @return traversal cost + */ + float getTraversalCost(const NodePtr & child); + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @return Index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & angle) + { + // Hybrid-A* and State Lattice share a coordinate system + return NodeHybrid::getIndex( + x, y, angle, motion_table.size_x, + motion_table.num_angle_quantization); + } + + /** + * @brief Get coordinates at index + * @param index Index of point + * @param width Width of costmap + * @param angle_quantization Theta size of costmap + * @return Coordinates + */ + static inline Coordinates getCoords( + const unsigned int & index, + const unsigned int & width, const unsigned int & angle_quantization) + { + // Hybrid-A* and State Lattice share a coordinate system + return NodeHybrid::Coordinates( + (index / angle_quantization) % width, // x + index / (angle_quantization * width), // y + index % angle_quantization); // theta + } + + /** + * @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 nav2_costmap_2d::Costmap2D * costmap); + + /** + * @brief Initialize motion models + * @param motion_model Motion model enum to use + * @param size_x Size of X of graph + * @param size_y Size of y of graph + * @param angle_quantization Size of theta bins of graph + * @param search_info Search info to use + */ + static void initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & angle_quantization, + SearchInfo & search_info); + + /** + * @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 wavefront heuristic + * @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 & start_x, const unsigned int & start_y, + const unsigned int & goal_x, const unsigned int & goal_y) + { + // State Lattice and Hybrid-A* share this heuristics + NodeHybrid::resetObstacleHeuristic(costmap, start_x, start_y, goal_x, goal_y); + } + + /** + * @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 float getObstacleHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const double & cost_penalty) + { + return NodeHybrid::getObstacleHeuristic(node_coords, goal_coords, cost_penalty); + } + + /** + * @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 Retrieve all valid neighbors of a node. + * @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 + */ + void getNeighbors( + std::function & validity_checker, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); + + /** + * @brief Set the starting pose for planning, as a node index + * @param path Reference to a vector of indicies of generated path + * @return whether the path was able to be backtraced + */ + bool backtracePath(CoordinateVector & path); + + NodeLattice * parent; + Coordinates pose; + static LatticeMotionTable motion_table; + // 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; + MotionPrimitive * _motion_primitive; + bool _backwards; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__NODE_LATTICE_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 new file mode 100644 index 0000000000..c499d0796f --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -0,0 +1,128 @@ +// 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 NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ +#define NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ + +#include +#include +#include +#include + +#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" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2/utils.h" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +namespace nav2_smac_planner +{ + +class SmacPlanner2D : public nav2_core::GlobalPlanner +{ +public: + /** + * @brief constructor + */ + SmacPlanner2D(); + + /** + * @brief destructor + */ + ~SmacPlanner2D(); + + /** + * @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; + + /** + * @brief Cleanup lifecycle node + */ + void cleanup() override; + + /** + * @brief Activate lifecycle node + */ + void activate() override; + + /** + * @brief Deactivate lifecycle node + */ + void deactivate() override; + + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav2_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: + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + std::unique_ptr> _a_star; + GridCollisionChecker _collision_checker; + std::unique_ptr _smoother; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _costmap_downsampler; + rclcpp::Clock::SharedPtr _clock; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner2D")}; + std::string _global_frame, _name; + float _tolerance; + int _downsampling_factor; + bool _downsample_costmap; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + double _max_planning_time; + bool _allow_unknown; + int _max_iterations; + int _max_on_approach_iterations; + bool _use_final_approach_orientation; + SearchInfo _search_info; + std::string _motion_model_for_search; + MotionModel _motion_model; + std::mutex _mutex; + rclcpp_lifecycle::LifecycleNode::WeakPtr _node; + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp new file mode 100644 index 0000000000..51f6608b5f --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -0,0 +1,130 @@ +// 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 NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ +#define NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ + +#include +#include +#include + +#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" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2/utils.h" + +namespace nav2_smac_planner +{ + +class SmacPlannerHybrid : public nav2_core::GlobalPlanner +{ +public: + /** + * @brief constructor + */ + SmacPlannerHybrid(); + + /** + * @brief destructor + */ + ~SmacPlannerHybrid(); + + /** + * @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; + + /** + * @brief Cleanup lifecycle node + */ + void cleanup() override; + + /** + * @brief Activate lifecycle node + */ + void activate() override; + + /** + * @brief Deactivate lifecycle node + */ + void deactivate() override; + + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav2_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: + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + std::unique_ptr> _a_star; + GridCollisionChecker _collision_checker; + std::unique_ptr _smoother; + rclcpp::Clock::SharedPtr _clock; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerHybrid")}; + nav2_costmap_2d::Costmap2D * _costmap; + std::shared_ptr _costmap_ros; + std::unique_ptr _costmap_downsampler; + std::string _global_frame, _name; + float _lookup_table_dim; + float _tolerance; + bool _downsample_costmap; + int _downsampling_factor; + double _angle_bin_size; + unsigned int _angle_quantizations; + bool _allow_unknown; + int _max_iterations; + SearchInfo _search_info; + double _max_planning_time; + double _lookup_table_size; + double _minimum_turning_radius_global_coords; + std::string _motion_model_for_search; + MotionModel _motion_model; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + std::mutex _mutex; + rclcpp_lifecycle::LifecycleNode::WeakPtr _node; + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp new file mode 100644 index 0000000000..c34204b18e --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -0,0 +1,122 @@ +// 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__SMAC_PLANNER_LATTICE_HPP_ +#define NAV2_SMAC_PLANNER__SMAC_PLANNER_LATTICE_HPP_ + +#include +#include +#include + +#include "nav2_smac_planner/a_star.hpp" +#include "nav2_smac_planner/smoother.hpp" +#include "nav2_smac_planner/utils.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2/utils.h" + +namespace nav2_smac_planner +{ + +class SmacPlannerLattice : public nav2_core::GlobalPlanner +{ +public: + /** + * @brief constructor + */ + SmacPlannerLattice(); + + /** + * @brief destructor + */ + ~SmacPlannerLattice(); + + /** + * @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; + + /** + * @brief Cleanup lifecycle node + */ + void cleanup() override; + + /** + * @brief Activate lifecycle node + */ + void activate() override; + + /** + * @brief Deactivate lifecycle node + */ + void deactivate() override; + + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav2_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: + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + std::unique_ptr> _a_star; + GridCollisionChecker _collision_checker; + std::unique_ptr _smoother; + rclcpp::Clock::SharedPtr _clock; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerLattice")}; + nav2_costmap_2d::Costmap2D * _costmap; + std::shared_ptr _costmap_ros; + MotionModel _motion_model; + LatticeMetadata _metadata; + std::string _global_frame, _name; + SearchInfo _search_info; + bool _allow_unknown; + int _max_iterations; + float _tolerance; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + double _max_planning_time; + double _lookup_table_size; + std::mutex _mutex; + rclcpp_lifecycle::LifecycleNode::WeakPtr _node; + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_LATTICE_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp new file mode 100644 index 0000000000..8c2adc5d29 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -0,0 +1,243 @@ +// 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__SMOOTHER_HPP_ +#define NAV2_SMAC_PLANNER__SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/constants.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "angles/angles.h" +#include "tf2/utils.h" +#include "ompl/base/StateSpace.h" +#include "ompl/base/spaces/DubinsStateSpace.h" + +namespace nav2_smac_planner +{ + +/** + * @class nav2_smac_planner::PathSegment + * @brief A segment of a path in start/end indices + */ +struct PathSegment +{ + unsigned int start; + unsigned int end; +}; + +/** + * @struct nav2_smac_planner::BoundaryPoints + * @brief Set of boundary condition points from expansion + */ +struct BoundaryPoints +{ + /** + * @brief A constructor for BoundaryPoints + */ + BoundaryPoints(double & x_in, double & y_in, double & theta_in) + : x(x_in), y(y_in), theta(theta_in) + {} + + double x; + double y; + double theta; +}; + +/** + * @struct nav2_smac_planner::BoundaryExpansion + * @brief Boundary expansion state + */ +struct BoundaryExpansion +{ + double path_end_idx{0.0}; + double expansion_path_length{0.0}; + double original_path_length{0.0}; + std::vector pts; + bool in_collision{false}; +}; + +typedef std::vector BoundaryExpansions; +typedef std::vector::iterator PathIterator; +typedef std::vector::reverse_iterator ReversePathIterator; + +/** + * @class nav2_smac_planner::Smoother + * @brief A path smoother implementation + */ +class Smoother +{ +public: + /** + * @brief A constructor for nav2_smac_planner::Smoother + */ + explicit Smoother(const SmootherParams & params); + + /** + * @brief A destructor for nav2_smac_planner::Smoother + */ + ~Smoother() {} + + /** + * @brief Initialization of the smoother + * @param min_turning_radius Minimum turning radius (m) + * @param motion_model Motion model type + */ + void initialize( + const double & min_turning_radius); + + /** + * @brief Smoother API method + * @param path Reference to path + * @param costmap Pointer to minimal costmap + * @param max_time Maximum time to compute, stop early if over limit + * @return If smoothing was successful + */ + bool smooth( + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time); + +protected: + /** + * @brief Smoother method - does the smoothing on a segment + * @param path Reference to path + * @param reversing_segment Return if this is a reversing segment + * @param costmap Pointer to minimal costmap + * @param max_time Maximum time to compute, stop early if over limit + * @return If smoothing was successful + */ + bool smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time); + + /** + * @brief Get the field value for a given dimension + * @param msg Current pose to sample + * @param dim Dimension ID of interest + * @return dim value + */ + inline double getFieldByDim( + const geometry_msgs::msg::PoseStamped & msg, + const unsigned int & dim); + + /** + * @brief Set the field value for a given dimension + * @param msg Current pose to sample + * @param dim Dimension ID of interest + * @param value to set the dimention to for the pose + */ + inline void setFieldByDim( + geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, + const double & value); + + /** + * @brief Finds the starting and end indices of path segments where + * the robot is traveling in the same direction (e.g. forward vs reverse) + * @param path Path in which to look for cusps + * @return Set of index pairs for each segment of the path in a given direction + */ + std::vector findDirectionalPathSegments(const nav_msgs::msg::Path & path); + + /** + * @brief Enforced minimum curvature boundary conditions on plan output + * the robot is traveling in the same direction (e.g. forward vs reverse) + * @param start_pose Start pose of the feasible path to maintain + * @param path Path to modify for curvature constraints on start / end of path + * @param costmap Costmap to check for collisions + * @param reversing_segment Whether this path segment is in reverse + */ + void enforceStartBoundaryConditions( + const geometry_msgs::msg::Pose & start_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment); + + /** + * @brief Enforced minimum curvature boundary conditions on plan output + * the robot is traveling in the same direction (e.g. forward vs reverse) + * @param end_pose End pose of the feasible path to maintain + * @param path Path to modify for curvature constraints on start / end of path + * @param costmap Costmap to check for collisions + * @param reversing_segment Whether this path segment is in reverse + */ + void enforceEndBoundaryConditions( + const geometry_msgs::msg::Pose & end_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment); + + /** + * @brief Given a set of boundary expansion, find the one which is shortest + * such that it is least likely to contain a loop-de-loop when working with + * close-by primitive markers. Instead, select a further away marker which + * generates a shorter ` + * @param boundary_expansions Set of boundary expansions + * @return Idx of the shorest boundary expansion option + */ + unsigned int findShortestBoundaryExpansionIdx(const BoundaryExpansions & boundary_expansions); + + /** + * @brief Populate a motion model expansion from start->end into expansion + * @param start Start pose of the feasible path to maintain + * @param end End pose of the feasible path to maintain + * @param expansion Expansion object to populate + * @param costmap Costmap to check for collisions + * @param reversing_segment Whether this path segment is in reverse + */ + void findBoundaryExpansion( + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & end, + BoundaryExpansion & expansion, + const nav2_costmap_2d::Costmap2D * costmap); + + /** + * @brief Generates boundary expansions with end idx at least strategic + * distances away, using either Reverse or (Forward) Path Iterators. + * @param start iterator to start search in path for + * @param end iterator to end search for + * @return Boundary expansions with end idxs populated + */ + template + BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end); + + /** + * @brief For a given path, update the path point orientations based on smoothing + * @param path Path to approximate the path orientation in + * @param reversing_segment Return if this is a reversing segment + */ + inline void updateApproximatePathOrientations( + nav_msgs::msg::Path & path, + bool & reversing_segment); + + double min_turning_rad_, tolerance_, data_w_, smooth_w_; + int max_its_, refinement_ctr_; + bool is_holonomic_, do_refinement_; + MotionModel motion_model_; + ompl::base::StateSpacePtr state_space_; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__SMOOTHER_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp new file mode 100644 index 0000000000..f2cd73d33a --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -0,0 +1,169 @@ +// 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 NAV2_SMAC_PLANNER__TYPES_HPP_ +#define NAV2_SMAC_PLANNER__TYPES_HPP_ + +#include +#include +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_smac_planner +{ + +typedef std::pair NodeHeuristicPair; + +/** + * @struct nav2_smac_planner::SearchInfo + * @brief Search properties and penalties + */ +struct SearchInfo +{ + float minimum_turning_radius; + float non_straight_penalty; + float change_penalty; + float reverse_penalty; + float cost_penalty; + float retrospective_penalty; + float rotation_penalty; + float analytic_expansion_ratio; + float analytic_expansion_max_length; + std::string lattice_filepath; + bool cache_obstacle_heuristic; + bool allow_reverse_expansion; +}; + +/** + * @struct nav2_smac_planner::SmootherParams + * @brief Parameters for the smoother + */ +struct SmootherParams +{ + /** + * @brief A constructor for nav2_smac_planner::SmootherParams + */ + SmootherParams() + : holonomic_(false) + { + } + + /** + * @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_); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "do_refinement", rclcpp::ParameterValue(true)); + node->get_parameter(local_name + "do_refinement", do_refinement_); + } + + double tolerance_; + int max_its_; + double w_data_; + double w_smooth_; + bool holonomic_; + bool do_refinement_; +}; + +/** + * @struct nav2_smac_planner::MotionPose + * @brief A struct for poses in motion primitives + */ +struct MotionPose +{ + /** + * @brief A constructor for nav2_smac_planner::MotionPose + */ + MotionPose() {} + + /** + * @brief A constructor for nav2_smac_planner::MotionPose + * @param x X pose + * @param y Y pose + * @param theta Angle of pose + */ + MotionPose(const float & x, const float & y, const float & theta) + : _x(x), _y(y), _theta(theta) + {} + + MotionPose operator-(const MotionPose & p2) + { + return MotionPose(this->_x - p2._x, this->_y - p2._y, this->_theta - p2._theta); + } + + float _x; + float _y; + float _theta; +}; + +typedef std::vector MotionPoses; + +/** + * @struct nav2_smac_planner::LatticeMetadata + * @brief A struct of all lattice metadata + */ +struct LatticeMetadata +{ + float min_turning_radius; + float grid_resolution; + unsigned int number_of_headings; + std::vector heading_angles; + unsigned int number_of_trajectories; + std::string motion_model; +}; + +/** + * @struct nav2_smac_planner::MotionPrimitive + * @brief A struct of all motion primitive data + */ +struct MotionPrimitive +{ + unsigned int trajectory_id; + float start_angle; + float end_angle; + float turning_radius; + float trajectory_length; + float arc_length; + float straight_length; + bool left_turn; + MotionPoses poses; +}; + +typedef std::vector MotionPrimitives; +typedef std::vector MotionPrimitivePtrs; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__TYPES_HPP_ 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 0000000000..fcb267e980 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -0,0 +1,159 @@ +// 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 radians +* @param theta continuous bin coordinates angle +* @return quaternion orientation in map frame +*/ +inline geometry_msgs::msg::Quaternion getWorldOrientation( + const float & theta) +{ + // theta is in radians already + tf2::Quaternion q; + q.setEuler(0.0, 0.0, theta); + 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; +} + +/** + * @brief convert json to lattice metadata + * @param[in] json json object + * @param[out] lattice meta data + */ +inline void fromJsonToMetaData(const nlohmann::json & json, LatticeMetadata & lattice_metadata) +{ + json.at("turning_radius").get_to(lattice_metadata.min_turning_radius); + json.at("grid_resolution").get_to(lattice_metadata.grid_resolution); + json.at("num_of_headings").get_to(lattice_metadata.number_of_headings); + json.at("heading_angles").get_to(lattice_metadata.heading_angles); + json.at("number_of_trajectories").get_to(lattice_metadata.number_of_trajectories); + json.at("motion_model").get_to(lattice_metadata.motion_model); +} + +/** + * @brief convert json to pose + * @param[in] json json object + * @param[out] pose + */ +inline void fromJsonToPose(const nlohmann::json & json, MotionPose & pose) +{ + pose._x = json[0]; + pose._y = json[1]; + pose._theta = json[2]; +} + +/** + * @brief convert json to motion primitive + * @param[in] json json object + * @param[out] motion primitive + */ +inline void fromJsonToMotionPrimitive( + const nlohmann::json & json, MotionPrimitive & motion_primitive) +{ + json.at("trajectory_id").get_to(motion_primitive.trajectory_id); + json.at("start_angle_index").get_to(motion_primitive.start_angle); + json.at("end_angle_index").get_to(motion_primitive.end_angle); + json.at("trajectory_radius").get_to(motion_primitive.turning_radius); + json.at("trajectory_length").get_to(motion_primitive.trajectory_length); + json.at("arc_length").get_to(motion_primitive.arc_length); + json.at("straight_length").get_to(motion_primitive.straight_length); + json.at("left_turn").get_to(motion_primitive.left_turn); + + for (unsigned int i = 0; i < json["poses"].size(); i++) { + MotionPose pose; + fromJsonToPose(json["poses"][i], pose); + motion_primitive.poses.push_back(pose); + } +} + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__UTILS_HPP_ diff --git a/nav2_smac_planner/lattice_primitives/README.md b/nav2_smac_planner/lattice_primitives/README.md new file mode 100644 index 0000000000..0d9b6982fa --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/README.md @@ -0,0 +1,168 @@ +# Lattice Primitive Generator +## Contents + +- **[About](#about)** +- **[Setup](#setup)** +- **[Usage](#usage)** +- **[Parameters](#parameters)** +- **[Output file structure](#output-file-structure)** +- **[How it Works](#how-it-works)** +
+ +## About +The scripts in this folder are used to generate the minimum control set for the state lattice planner. This work is based on [Generating Near Minimal Control Sets for Constrained Motion Planning in Discrete State Spaces](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2005_1/pivtoraiko_mihail_2005_1.pdf). An example of the trajectories for a grid resolution of 5cm and turning radius of 0.5m is shown below. + +![ ](docs/all_trajectories.png) + +## Setup +To install the required python packages run the following command + +``` +pip install -r requirements.txt +``` + +## Usage +Run the primitive generator by using the following command +``` +python3 generate_motion_primitives.py [--config] [--output] [--visualizations] +``` + +To adjust the settings to fit your particular needs you can edit the parameters in the [config.json](config.json) file. Alternatively, you can create your own file and pass it in using the --config flag. + +The output file can be specified by passing in a path with the --output flag. The default is set to save in a file called output.json in the same directory as this README. + +The directory to save the visualizations can be specified by passing in a path with the --visualizations flag. + +## Parameters ## +Note: None of these parameters have defaults. They all must be specified through the [config.json](config.json) file. + +**motion_model** (string) + +The type of motion model used. Accepted values: +- `ackermann`: Only forward and reversing trajectories +- `diff`: Forward moving trajectories + rotation in place by a single angular bin +- `omni`: Forward moving trajectories + rotation in place by a single angular bin + sideways sliding motions +
+
+ +**turning_radius** (float) + +The minimum turning radius of the robot (in meters). Typical values for a service robot range from 0.4 to 1.0m. +
+
+ +**grid_resolution** (float) + +The resolution of the grid (in meters) used to create the lattice. If the grid resolution is too large proportionally to the turning radius then the generator will not return a good result. This should be the same as your costmap resolution. +
+
+ +**stopping_threshold** (float) + +Number of consecutive iterations without a valid trajectory before stopping the search. A value too low may mean that you stop searching too early. A value too high will cause the search to take longer. We found that stopping after 5 iterations produced the best results. +
+
+ +**num_of_headings** (float) + +Number of discrete angular headings used. Due to the way discretization is done this number should be restricted to multiples of 8. Angles are not generated uniformly but instead generated in a way to facilitate straight lines. See [angle discretization](#angle-discretization) for more details. We believe 16 headings is a good number for most use cases. +
+
+ +## Output file structure +The output file is a JSON file and contains the following fields: + +**version** + +The version number of the lattice generator that created the output file + +**date_generated** + +The date the output file was generated. Format: YYYY-MM-DD + +**lattice_metadata** + +A dictionary that contains information about the generated lattice. Most of this data comes from the config file used when generating the primitives. More information on each field is given in the [Parameters](#parameters) section. Includes the following fields: +- **motion_model** +- **turning_radius** (meters) +- **grid_resolution** (meters) +- **stopping_threshold** +- **num_of_headings** +- **heading_angles** + - A list of the heading angles (in radians) that are used in the primitives +- **number_of_trajectories** + - The total number of trajectories contained in the output file + +**primitives** + +A list of dictionaries where each dictionary represents an individual motion primitive. Each motion primitive contains the following fields: +- **trajectory_id** + - The id associated with the primitive +- **start_angle_index** + - The start angle of the primitive represented as an index for the heading_angle list given in the lattice_metadata +- **end_angle_index** + - The end angle of the primitive represented as an index for the heading_angle list given in the lattice_metadata +- **left_turn** + - A boolean value that is true if the path curves to the left. Straight paths default to true. +- **trajectory_radius** (meters) + - The radius of the circle that was used to create the arc portion of a primitive. trajectory_radius is 0 if the primitive is purely a straight line +- **trajectory_length** (meters) + - The length of the primitive +- **arc_length** (meters) + - The length of the arc portion of a primitive. arc_length is 0 if the primitive is a straight line +- **straight_length** (meters) + - The length of the straight line portion of a primitive. straight_length is 0 if the primitive is a pure arc. +- **poses** + - A list where each entry is a list containing three values: x, y, and yaw (radians) + +## How it works +This section describes how the various portions of the generation algorithm works. + +### Angle Discretization +Dividing a full turn into uniform angular headings presents several problems. The biggest problem is that it will create angles for which a straight trajectory does not land nicely on an endpoint that aligns with the grid. Instead we discretize the angles in a way that ensures straight paths will land on endpoints aligned with the grid. + +![ ](docs/angle_discretization.png) + +The image shows how the angular headings are generated. The same idea can be extended to a higher number of headings. As a result, the number of headings parameter is restricted to multiples of 8. + +### Trajectory Generator +1. Create two lines. Line 1 passes through start point with angle of start angle, and line 2 passes through the end point with angle of end angle + +2. Find the intersection point I of lines 1 and 2 + +3. Calculate the distance beween I and the origin (let this be d1). Also calculate the distance between I and the end point (let this be d2) + +4. If d1 and d2 are equal then proceed to step 5. Otherwise, create intermediate points for each line that are min(d1, d2) distance away along the lines from I. So there should be an intermediate point on line 1 and an intermediate point on line 2. One of these intermediate points should align with either the origin or the end point by nature of how the distance was calculated. + +5. Create perpindicular lines for line 1 and line 2 that pass through the respective intermediate points. The intersection of these perpindicular lines is the centre of the circle whose arc represents the curved portion of the trajectory. + +6. Finally, if needed append straight segments to the path to ensure we start at the origin and end at the end point. + + +There are several checks we need to make to ensure a valid trajectory is generated: +- If the start and end angles are parallel then the lines must overlap +- The intersection point must occur before the end point on line 2 and after the origin on line 1 +- The radius of the generated trajectory must be less than the user supplied minimum turning radius + +### Lattice Generator +The lattice generator is generally based on the generation of the control set as described in [Generating Near Minimal Control Sets for Constrained Motion Planning in Discrete State Spaces](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2005_1/pivtoraiko_mihail_2005_1.pdf). However, some changes were made to the above method. A brief outline of the implemented method is given below: + +1. Create a wavefront that begins a minimum trajectory length away from the origin. + + - The minimum trajectory length is defined as the length a trajectory needs to move from one discrete heading to the next. (Since the headings are not separated equally we use the smallest heading change) + +2. Generate paths to all points on this wavefront for all possible end heading angles. + +3. When a path is generated it is checked to ensure it does not pass "close" to another path. If it does it is removed, otherwise it remains in the set + + - "Close" is defined to be within half the grid resolution for length and half the average angular bin size for angular rotation + +4. Steps 2-3 are repeated for the next wavefront which is a grid resolution step further away from the origin. + +5. Steps 1-4 are repeated untill all trajectories are being removed. The generator will continue for a few more wavefront steps until N wavefronts have been searched with no new trajectories. At this point the generator terminates and returns the computed minimal set. + + - The number N is the stopping_threshold parameter + +6. Steps 1-5 are repeated for all possible start angles between 0 and 90. + +7. The resulting control set will only contain trajectories in quadrant 1. To get the final control set we exploit symmetry across the axess and flip the trajectories in different ways. \ No newline at end of file diff --git a/nav2_smac_planner/lattice_primitives/config.json b/nav2_smac_planner/lattice_primitives/config.json new file mode 100644 index 0000000000..27671c337d --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/config.json @@ -0,0 +1,7 @@ +{ + "motion_model": "ackermann", + "turning_radius": 0.5, + "grid_resolution": 0.05, + "stopping_threshold": 5, + "num_of_headings": 16 +} \ No newline at end of file diff --git a/nav2_smac_planner/lattice_primitives/constants.py b/nav2_smac_planner/lattice_primitives/constants.py new file mode 100644 index 0000000000..d389e48cc8 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/constants.py @@ -0,0 +1,15 @@ +# Copyright (c) 2021, Matthew Booker +# +# 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. + +VERSION = 1.0 diff --git a/nav2_smac_planner/lattice_primitives/docs/all_trajectories.png b/nav2_smac_planner/lattice_primitives/docs/all_trajectories.png new file mode 100644 index 0000000000..5888a84f8d Binary files /dev/null and b/nav2_smac_planner/lattice_primitives/docs/all_trajectories.png differ diff --git a/nav2_smac_planner/lattice_primitives/docs/angle_discretization.png b/nav2_smac_planner/lattice_primitives/docs/angle_discretization.png new file mode 100644 index 0000000000..47602245ef Binary files /dev/null and b/nav2_smac_planner/lattice_primitives/docs/angle_discretization.png differ diff --git a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py new file mode 100644 index 0000000000..e70c235e83 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py @@ -0,0 +1,260 @@ +# Copyright (c) 2021, Matthew Booker +# +# 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. + +import argparse +from datetime import datetime +import json +import logging +from pathlib import Path +import time + +import constants +from lattice_generator import LatticeGenerator + +import matplotlib.pyplot as plt +import numpy as np + + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) + + +def handle_arg_parsing(): + """ + Handle the parsing of arguments. + + Returns + ------- + argparse.Namespace + An object containing all parsed arguments + + """ + parser = argparse.ArgumentParser(description='Generate motionprimitives ' + "for Nav2's State " + 'Lattice Planner') + parser.add_argument('--config', + type=Path, + default='./config.json', + help='The config file containing the ' + 'parameters to be used') + parser.add_argument('--output', + type=Path, + default='./output.json', + help='The output file containing the ' + 'trajectory data') + parser.add_argument('--visualizations', + type=Path, + default='./visualizations', + help='The output folder where the ' + 'visualizations of the trajectories will be saved') + + return parser.parse_args() + + +def create_heading_angle_list(minimal_set_trajectories: dict) -> list: + """ + Create a sorted list of heading angles from the minimal trajectory set. + + Args: + ---- + minimal_set_trajectories: dict + The minimal spanning set + + Returns + ------- + list + A sorted list of heading angles + + """ + heading_angles = set(minimal_set_trajectories.keys()) + return sorted(heading_angles, key=lambda x: (x < 0, x)) + + +def read_config(config_path) -> dict: + """ + Read in the user defined parameters via JSON. + + Args: + ---- + config_path: Path + Path to the config file + + Returns + ------- + dict + Dictionary containing the user defined parameters + + """ + with open(config_path) as config_file: + config = json.load(config_file) + + return config + + +def create_header(config: dict, minimal_set_trajectories: dict) -> dict: + """ + Create a dict containing all the fields to populate the header with. + + Args: + ---- + config: dict + The dict containing user specified parameters + minimal_set_trajectories: dict + The minimal spanning set + + Returns + ------- + dict + A dictionary containing the fields to populate the header with + + """ + header_dict = { + 'version': constants.VERSION, + 'date_generated': datetime.today().strftime('%Y-%m-%d'), + 'lattice_metadata': {}, + 'primitives': [], + } + + for key, value in config.items(): + header_dict['lattice_metadata'][key] = value + + heading_angles = create_heading_angle_list(minimal_set_trajectories) + adjusted_heading_angles = [angle + 2*np.pi if angle < 0 else angle for angle in heading_angles] + + header_dict['lattice_metadata']['heading_angles'] = adjusted_heading_angles + + return header_dict + + +def write_to_json(output_path: Path, minimal_set_trajectories: dict, config: dict) -> None: + """ + Write the minimal spanning set to an output file. + + Args: + ---- + output_path: Path + The output file for the json data + minimal_set_trajectories: dict + The minimal spanning set + config: dict + The dict containing user specified parameters + + """ + output_dict = create_header(config, minimal_set_trajectories) + + trajectory_start_angles = list(minimal_set_trajectories.keys()) + + heading_angle_list = create_heading_angle_list(minimal_set_trajectories) + heading_lookup = {angle: idx for idx, angle in + enumerate(heading_angle_list)} + + idx = 0 + for start_angle in sorted(trajectory_start_angles, + key=lambda x: (x < 0, x)): + + for trajectory in sorted( + minimal_set_trajectories[start_angle], + key=lambda x: x.parameters.end_angle + ): + + traj_info = {} + traj_info['trajectory_id'] = idx + traj_info['start_angle_index'] = heading_lookup[trajectory.parameters.start_angle] + traj_info['end_angle_index'] = heading_lookup[trajectory.parameters.end_angle] + traj_info['left_turn'] = bool(trajectory.parameters.left_turn) + traj_info['trajectory_radius'] = \ + trajectory.parameters.turning_radius + traj_info['trajectory_length'] = round( + trajectory.parameters.total_length, 5 + ) + traj_info['arc_length'] = round( + trajectory.parameters.arc_length, + 5 + ) + traj_info['straight_length'] = round( + trajectory.parameters.start_straight_length + + trajectory.parameters.end_straight_length, + 5, + ) + traj_info['poses'] = trajectory.path.to_output_format() + + output_dict['primitives'].append(traj_info) + idx += 1 + + output_dict['lattice_metadata']['number_of_trajectories'] = idx + + with open(output_path, 'w') as output_file: + json.dump(output_dict, output_file, indent='\t') + + +def save_visualizations(visualizations_folder: Path, minimal_set_trajectories: dict) -> None: + """ + Draw the visualizations for every trajectory and save it as an image. + + Args: + ---- + visualizations_folder: Path + The path to the folder for where to save the images + minimal_set_trajectories: dict + The minimal spanning set + + """ + # Create the directory if it doesnt exist + visualizations_folder.mkdir(exist_ok=True) + + for start_angle in minimal_set_trajectories.keys(): + + for trajectory in minimal_set_trajectories[start_angle]: + plt.plot(trajectory.path.xs, trajectory.path.ys, 'b') + + plt.grid(True) + plt.axis('square') + left_x, right_x = plt.xlim() + left_y, right_y = plt.ylim() + + output_path = visualizations_folder / 'all_trajectories.png' + plt.savefig(output_path) + plt.clf() + + for start_angle in minimal_set_trajectories.keys(): + + angle_in_deg = np.rad2deg(start_angle) + + if start_angle < 0 or start_angle > np.pi / 2: + continue + + for trajectory in minimal_set_trajectories[start_angle]: + plt.plot(trajectory.path.xs, trajectory.path.ys, 'b') + plt.xlim(left_x, right_x) + plt.ylim(left_y, right_y) + + plt.grid(True) + + output_path = visualizations_folder / f'{angle_in_deg}.png' + plt.savefig(output_path) + plt.clf() + + +if __name__ == '__main__': + + args = handle_arg_parsing() + config = read_config(args.config) + + start = time.time() + lattice_gen = LatticeGenerator(config) + minimal_set_trajectories = lattice_gen.run() + print(f'Finished Generating. Took {time.time() - start} seconds') + + write_to_json(args.output, minimal_set_trajectories, config) + save_visualizations(args.visualizations, minimal_set_trajectories) diff --git a/nav2_smac_planner/lattice_primitives/helper.py b/nav2_smac_planner/lattice_primitives/helper.py new file mode 100644 index 0000000000..24d3cf9140 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/helper.py @@ -0,0 +1,127 @@ +# Copyright (c) 2021, Matthew Booker +# +# 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. + +import numpy as np + + +def normalize_angle(angle): + """ + Normalize the angle to between [0, 2pi). + + Args: + angle: float + The angle to normalize in radians + + Returns + ------- + The normalized angle in the range [0,2pi) + + """ + while angle >= 2*np.pi: + angle -= 2*np.pi + + while angle < 0: + angle += 2*np.pi + + return angle + + +def angle_difference(angle_1, angle_2, left_turn=None): + """ + Calculate the difference between two angles based on a given direction. + + Args: + angle_1: float + The starting angle in radians + angle_2: float + The ending angle in radians + left_turn: bool + The direction of turn. True if left, false if right + and None if smallest angular difference should be + returned + + Returns + ------- + The angular difference between the two angles according to + the specified turn direction + + """ + if left_turn is None: + dif = abs(angle_1 - angle_2) + + return dif if dif <= np.pi else 2 * np.pi - dif + + elif left_turn: + + if angle_2 >= angle_1: + return abs(angle_1 - angle_2) + else: + return 2 * np.pi - abs(angle_1 - angle_2) + + else: + if angle_1 >= angle_2: + return abs(angle_1 - angle_2) + else: + return 2 * np.pi - abs(angle_1 - angle_2) + + +def interpolate_yaws(start_angle, end_angle, left_turn, steps): + """ + Create equally spaced yaws between two angles. + + Args: + start_angle: float + The starting angle + end_angle: float + The ending angle + left_turn: bool + The direction of turn. True if left, False otherwise + steps: int + The number of yaws to generate between start and end + angle + + Returns + ------- + An array of yaws starting at start angle and ending at end + angle with steps number of angles between them + + """ + if left_turn: + if start_angle > end_angle: + end_angle += 2 * np.pi + else: + if end_angle > start_angle: + end_angle -= 2 * np.pi + + yaws = np.linspace(start_angle, end_angle, steps) + yaws = np.vectorize(normalize_angle)(yaws) + + return yaws + + +def get_rotation_matrix(angle): + """ + Return a rotation matrix that is equivalent to a 2D rotation of angle. + + Args: + angle: float + The angle to create a rotation matrix for + + Returns + ------- + A 2x2 matrix representing a 2D rotation by angle + + """ + return np.array([[np.cos(angle), -np.sin(angle)], + [np.sin(angle), np.cos(angle)]]) diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py new file mode 100644 index 0000000000..3bcf735de8 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py @@ -0,0 +1,745 @@ +# Copyright (c) 2021, Matthew Booker +# +# 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. + +from collections import defaultdict +from enum import Enum + +from helper import angle_difference, interpolate_yaws + +import numpy as np + +from rtree import index + +from trajectory import Path, Trajectory, TrajectoryParameters + +from trajectory_generator import TrajectoryGenerator + + +class LatticeGenerator: + """ + Handles all the logic for computing the minimal control set. + + Computes the minimal control set for a vehicle given its parameters. + Includes handling the propogating and searching along wavefronts as + well as determining if a trajectory is part of the minimal set based + on previously added trajectories. + """ + + class MotionModel(Enum): + """An Enum used for determining the motion model to use.""" + + ACKERMANN = 1 + DIFF = 2 + OMNI = 3 + + class Flip(Enum): + """An Enum used for determining how a trajectory should be flipped.""" + + X = 1 + Y = 2 + BOTH = 3 + + def __init__(self, config: dict): + """Init the lattice generator from the user supplied config.""" + self.trajectory_generator = TrajectoryGenerator(config) + self.grid_resolution = config['grid_resolution'] + self.turning_radius = config['turning_radius'] + self.stopping_threshold = config['stopping_threshold'] + self.num_of_headings = config['num_of_headings'] + self.headings = \ + self._get_heading_discretization(config['num_of_headings']) + + self.motion_model = self.MotionModel[config['motion_model'].upper()] + + self.DISTANCE_THRESHOLD = 0.5 * self.grid_resolution + self.ROTATION_THRESHOLD = 0.5 * (2 * np.pi / self.num_of_headings) + + def _get_wave_front_points(self, pos: int) -> np.array: + """ + Calculate the end points that lie on the wave front. + + Uses the user supplied grid resolution to calculate the + valid end points that lie on a wave front at a discrete + interval away from the origin. + + Args: + pos: int + The number of discrete intervals of grid resolution + away from the origin to generate the wave points at + + Returns + ------- + np.array + An array of coordinates + + """ + positions = [] + + max_point_coord = self.grid_resolution * pos + + for i in range(pos): + varying_point_coord = self.grid_resolution * i + + # Change the y and keep x at max + positions.append((max_point_coord, varying_point_coord)) + + # Change the x and keep y at max + positions.append((varying_point_coord, max_point_coord)) + + # Append the corner + positions.append((max_point_coord, max_point_coord)) + + return np.array(positions) + + def _get_heading_discretization(self, number_of_headings: int) -> list: + """ + Calculate the heading discretization based on the number of headings. + + Does not uniformly generate headings but instead generates a set of + discrete headings that is better suited for straight line trajectories. + + Args: + number_of_headings: int + The number of headings to discretize a 360 degree turn into + + Returns + ------- + list + A list of headings in radians + + """ + max_val = int((((number_of_headings + 4) / 4) - 1) / 2) + + outer_edge_x = [] + outer_edge_y = [] + + # Generate points that lie on the perimeter of the surface + # of a square with sides of length max_val + for i in range(-max_val, max_val + 1): + outer_edge_x.extend([i, i]) + outer_edge_y.extend([-max_val, max_val]) + + if i != max_val and i != -max_val: + outer_edge_x.extend([-max_val, max_val]) + outer_edge_y.extend([i, i]) + + return sorted([np.arctan2(j, i) for i, j in zip(outer_edge_x, outer_edge_y)]) + + def _point_to_line_distance(self, p1: np.array, p2: np.array, q: np.array) -> float: + """ + Return the shortest distance from a point to a line segment. + + Args: + p1: np.array(2,) + Start point of line segment + p2: np.array(2,) + End point of line segment + q: np.array(2,) + Point to get distance away from line of + + Returns + ------- + float + The shortest distance between q and line segment p1p2 + + """ + # Get back the l2-norm without the square root + l2 = np.inner(p1 - p2, p1 - p2) + + if l2 == 0: + return np.linalg.norm(p1 - q) + + # Ensure t lies in [0, 1] + t = max(0, min(1, np.dot(q - p1, p2 - p1) / l2)) + projected_point = p1 + t * (p2 - p1) + + return np.linalg.norm(q - projected_point) + + def _is_minimal_trajectory( + self, trajectory: Trajectory, prior_end_poses: index.Rtree + ) -> bool: + """ + Determine wheter a trajectory is a minimal trajectory. + + Uses an RTree for speedup. + + Args: + trajectory: Trajectory + The trajectory to check + prior_end_poses: RTree + An RTree holding the current minimal set of trajectories + + Returns + ------- + bool + True if the trajectory is a minimal trajectory otherwise false + + """ + # Iterate over line segments in the trajectory + for x1, y1, x2, y2, yaw in zip( + trajectory.path.xs[:-1], + trajectory.path.ys[:-1], + trajectory.path.xs[1:], + trajectory.path.ys[1:], + trajectory.path.yaws[:-1], + ): + + p1 = np.array([x1, y1]) + p2 = np.array([x2, y2]) + + # Create a bounding box search region + # around the line segment + left_bb = min(x1, x2) - self.DISTANCE_THRESHOLD + right_bb = max(x1, x2) + self.DISTANCE_THRESHOLD + top_bb = max(y1, y2) + self.DISTANCE_THRESHOLD + bottom_bb = min(y1, y2) - self.DISTANCE_THRESHOLD + + # For any previous end points in the search region we + # check the distance to that point and the angle + # difference. If they are within threshold then this + # trajectory can be composed from a previous trajectory + for prior_end_pose in prior_end_poses.intersection( + (left_bb, bottom_bb, right_bb, top_bb), objects='raw' + ): + if ( + self._point_to_line_distance(p1, p2, prior_end_pose[:-1]) + < self.DISTANCE_THRESHOLD + and angle_difference(yaw, prior_end_pose[-1]) + < self.ROTATION_THRESHOLD + ): + return False + + return True + + def _compute_min_trajectory_length(self) -> float: + """ + Compute the minimum trajectory length for the given parameters. + + The minimum trajectory length is defined as the length needed + for the sharpest possible turn to move from 0 degrees to the next + discrete heading. Since the distance between headings is not uniform + we take the smallest possible difference. + + Returns + ------- + float + The minimal length of a trajectory + + """ + # Compute arc length for a turn that moves from 0 degrees to + # the minimum heading difference + heading_diff = [ + abs(self.headings[i + 1] - self.headings[i]) + for i in range(len(self.headings) - 1) + ] + + return self.turning_radius * min(heading_diff) + + def _generate_minimal_spanning_set(self) -> dict: + """ + Generate the minimal spanning set. + + Iteratves over all possible trajectories and keeps only those that + are part of the minimal set. + + Returns + ------- + dict + A dictionary where the key is the start_angle and the value is + a list of trajectories that begin at that angle + + """ + quadrant1_end_poses = defaultdict(list) + + # Since we only compute for quadrant 1 we only need headings between + # 0 and 90 degrees + initial_headings = sorted( + filter(lambda x: 0 <= x and x <= np.pi / 2, self.headings) + ) + + # Use the minimum trajectory length to find the starting wave front + min_trajectory_length = self._compute_min_trajectory_length() + wave_front_start_pos = int( + np.round(min_trajectory_length / self.grid_resolution) + ) + + for start_heading in initial_headings: + iterations_without_trajectory = 0 + + prior_end_poses = index.Index() + + wave_front_cur_pos = wave_front_start_pos + + # To get target headings: sort headings radially and remove those + # that are more than 90 degrees away + target_headings = sorted( + self.headings, key=lambda x: (abs(x - start_heading), -x) + ) + target_headings = list( + filter(lambda x: abs(start_heading - x) <= np.pi / 2, target_headings) + ) + + while iterations_without_trajectory < self.stopping_threshold: + iterations_without_trajectory += 1 + + # Generate x,y coordinates for current wave front + positions = self._get_wave_front_points(wave_front_cur_pos) + + for target_point in positions: + for target_heading in target_headings: + # Use 10% of grid separation for finer granularity + # when checking if trajectory overlaps another already + # seen trajectory + trajectory = self.trajectory_generator.generate_trajectory( + target_point, + start_heading, + target_heading, + 0.1 * self.grid_resolution, + ) + + if trajectory is not None: + # Check if path overlaps something in minimal + # spanning set + if self._is_minimal_trajectory(trajectory, prior_end_poses): + + # Add end pose to minimal set + new_end_pose = np.array( + [target_point[0], target_point[1], target_heading] + ) + + quadrant1_end_poses[start_heading].append( + (target_point, target_heading) + ) + + # Create a new bounding box in the RTree + # for this trajectory + left_bb = target_point[0] - self.DISTANCE_THRESHOLD + right_bb = target_point[0] + self.DISTANCE_THRESHOLD + bottom_bb = target_point[1] - self.DISTANCE_THRESHOLD + top_bb = target_point[1] + self.DISTANCE_THRESHOLD + + prior_end_poses.insert( + 0, + (left_bb, bottom_bb, right_bb, top_bb), + new_end_pose, + ) + + iterations_without_trajectory = 0 + + wave_front_cur_pos += 1 + + # Once we have found the minimal trajectory set for quadrant 1 + # we can leverage symmetry to create the complete minimal set + return self._create_complete_minimal_spanning_set(quadrant1_end_poses) + + def _flip_angle(self, angle: float, flip_type: Flip) -> float: + """ + Return the the appropriate flip of the angle in self.headings. + + Args: + angle: float + The angle to flip + flip_type: Flip + Whether to flip acrpss X axis, Y axis, or both + + Returns + ------- + float + The angle in self.heading that is the appropriate flip + + """ + angle_idx = self.headings.index(angle) + + if flip_type == self.Flip.X: + heading_idx = (self.num_of_headings / 2 - 1) - angle_idx - 1 + elif flip_type == self.Flip.Y: + heading_idx = self.num_of_headings - angle_idx - 2 + elif flip_type == self.Flip.BOTH: + heading_idx = ( + angle_idx - (self.num_of_headings / 2) + ) % self.num_of_headings + else: + raise Exception(f'Unsupported flip type: {flip_type}') + + return self.headings[int(heading_idx)] + + def _create_complete_minimal_spanning_set( + self, single_quadrant_minimal_set: dict + ) -> dict: + """ + Create the full minimal spanning set from a single quadrant set. + + Exploits the symmetry between the quadrants to create the full set. + This is done by flipping every trajectory in the first quadrant across + either the X-axis, Y-axis, or both axes. + + Args: + single_quadrant_minimal_set: dict + The minimal set for quadrant 1 (positive x and positive y) + + Returns + ------- + dict + The complete minimal spanning set containing the trajectories + in all quadrants + + """ + all_trajectories = defaultdict(list) + + for start_angle in single_quadrant_minimal_set.keys(): + + for end_point, end_angle in single_quadrant_minimal_set[start_angle]: + + x, y = end_point + + # Prevent double adding trajectories that lie on axes + # (i.e. start and end angle are either both 0 or both pi/2) + if start_angle == 0 and end_angle == 0: + unflipped_start_angle = 0.0 + flipped_x_start_angle = np.pi + + unflipped_end_angle = 0.0 + flipped_x_end_angle = np.pi + + # Generate trajectories from calculated parameters + unflipped_trajectory = ( + self.trajectory_generator.generate_trajectory( + np.array([x, y]), + unflipped_start_angle, + unflipped_end_angle, + self.grid_resolution, + ) + ) + flipped_x_trajectory = ( + self.trajectory_generator.generate_trajectory( + np.array([-x, -y]), + flipped_x_start_angle, + flipped_x_end_angle, + self.grid_resolution, + ) + ) + + all_trajectories[ + unflipped_trajectory.parameters.start_angle + ].append(unflipped_trajectory) + + all_trajectories[ + flipped_x_trajectory.parameters.start_angle + ].append(flipped_x_trajectory) + + elif abs(start_angle) == np.pi / 2 and abs(end_angle) == np.pi / 2: + unflipped_start_angle = np.pi / 2 + flipped_y_start_angle = -np.pi / 2 + + unflipped_end_angle = np.pi / 2 + flipped_y_end_angle = -np.pi / 2 + + # Generate trajectories from calculated parameters + unflipped_trajectory = ( + self.trajectory_generator.generate_trajectory( + np.array([-x, y]), + unflipped_start_angle, + unflipped_end_angle, + self.grid_resolution, + ) + ) + + flipped_y_trajectory = ( + self.trajectory_generator.generate_trajectory( + np.array([x, -y]), + flipped_y_start_angle, + flipped_y_end_angle, + self.grid_resolution, + ) + ) + + all_trajectories[ + unflipped_trajectory.parameters.start_angle + ].append(unflipped_trajectory) + all_trajectories[ + flipped_y_trajectory.parameters.start_angle + ].append(flipped_y_trajectory) + + else: + unflipped_start_angle = start_angle + flipped_x_start_angle = self._flip_angle(start_angle, self.Flip.X) + flipped_y_start_angle = self._flip_angle(start_angle, self.Flip.Y) + flipped_xy_start_angle = self._flip_angle( + start_angle, self.Flip.BOTH + ) + + unflipped_end_angle = end_angle + flipped_x_end_angle = self._flip_angle(end_angle, self.Flip.X) + flipped_y_end_angle = self._flip_angle(end_angle, self.Flip.Y) + flipped_xy_end_angle = self._flip_angle(end_angle, self.Flip.BOTH) + + # Generate trajectories from calculated parameters + unflipped_trajectory = ( + self.trajectory_generator.generate_trajectory( + np.array([x, y]), + unflipped_start_angle, + unflipped_end_angle, + self.grid_resolution, + ) + ) + flipped_x_trajectory = ( + self.trajectory_generator.generate_trajectory( + np.array([-x, y]), + flipped_x_start_angle, + flipped_x_end_angle, + self.grid_resolution, + ) + ) + flipped_y_trajectory = ( + self.trajectory_generator.generate_trajectory( + np.array([x, -y]), + flipped_y_start_angle, + flipped_y_end_angle, + self.grid_resolution, + ) + ) + flipped_xy_trajectory = ( + self.trajectory_generator.generate_trajectory( + np.array([-x, -y]), + flipped_xy_start_angle, + flipped_xy_end_angle, + self.grid_resolution, + ) + ) + + all_trajectories[ + unflipped_trajectory.parameters.start_angle + ].append(unflipped_trajectory) + all_trajectories[ + flipped_x_trajectory.parameters.start_angle + ].append(flipped_x_trajectory) + all_trajectories[ + flipped_y_trajectory.parameters.start_angle + ].append(flipped_y_trajectory) + all_trajectories[ + flipped_xy_trajectory.parameters.start_angle + ].append(flipped_xy_trajectory) + + return all_trajectories + + def _handle_motion_model(self, spanning_set: dict) -> dict: + """ + Add the appropriate motions for the user supplied motion model. + + Ackerman: No additional trajectories + + Diff: In place turns to the right and left + + Omni: Diff + Sliding movements to right and left + + Args: + spanning set: dict + The minimal spanning set + + Returns + ------- + dict + The minimal spanning set with additional trajectories based + on the motion model + + """ + if self.motion_model == self.MotionModel.ACKERMANN: + return spanning_set + + elif self.motion_model == self.MotionModel.DIFF: + diff_spanning_set = self._add_in_place_turns(spanning_set) + return diff_spanning_set + + elif self.motion_model == self.MotionModel.OMNI: + omni_spanning_set = self._add_in_place_turns(spanning_set) + omni_spanning_set = self._add_horizontal_motions(omni_spanning_set) + return omni_spanning_set + + else: + print('No handling implemented for Motion Model: ' + + f'{self.motion_model}') + raise NotImplementedError + + def _add_in_place_turns(self, spanning_set: dict) -> dict: + """ + Add in place turns to the spanning set. + + In place turns are trajectories with only a rotational component and + only shift a single angular heading step + + Args: + spanning_set: dict + The minimal spanning set + + Returns + ------- + dict + The minimal spanning set containing additional in place turns + for each start angle + + """ + all_angles = sorted(spanning_set.keys()) + + for idx, start_angle in enumerate(all_angles): + prev_angle_idx = idx - 1 if idx - 1 >= 0 else len(all_angles) - 1 + next_angle_idx = idx + 1 if idx + 1 < len(all_angles) else 0 + + prev_angle = all_angles[prev_angle_idx] + next_angle = all_angles[next_angle_idx] + + left_turn_params = TrajectoryParameters.no_arc( + end_point=np.array([0, 0]), + start_angle=start_angle, + end_angle=next_angle, + ) + right_turn_params = TrajectoryParameters.no_arc( + end_point=np.array([0, 0]), + start_angle=start_angle, + end_angle=prev_angle, + ) + + # Calculate number of steps needed to rotate by roughly 10 degrees + # for each pose + angle_dif = angle_difference(start_angle, next_angle) + steps = int(round(angle_dif / np.deg2rad(10))) + 1 + + position = np.full(steps, 0) + left_yaws = interpolate_yaws(start_angle, next_angle, True, steps) + right_yaws = interpolate_yaws(start_angle, prev_angle, False, steps) + + left_turn_path = Path(xs=position, ys=position, yaws=left_yaws) + right_turn_path = Path(xs=position, ys=position, yaws=right_yaws) + + left_turn = Trajectory(parameters=left_turn_params, path=left_turn_path) + right_turn = Trajectory(parameters=right_turn_params, path=right_turn_path) + + spanning_set[start_angle].append(left_turn) + spanning_set[start_angle].append(right_turn) + + return spanning_set + + def _add_horizontal_motions(self, spanning_set: dict) -> dict: + """ + Add horizontal sliding motions to the spanning set. + + The horizontal sliding motions are simply straight line trajectories + at 90 degrees to every start angle in the spanning set. The yaw of these + trajectories is the same as the start angle for which it is generated. + + Args: + spanning_set: dict + The minimal spanning set + + Returns + ------- + dict + The minimal spanning set containing additional sliding motions + for each start angle + + """ + # Calculate the offset in the headings list that represents an + # angle change of 90 degrees + idx_offset = int(self.num_of_headings / 4) + + for idx, angle in enumerate(self.headings): + + # Copy the straight line trajectory for the start angle that + # is 90 degrees to the left + left_angle_idx = int((idx + idx_offset) % self.num_of_headings) + left_angle = self.headings[left_angle_idx] + left_trajectories = spanning_set[left_angle] + left_straight_trajectory = next( + t for t in left_trajectories if t.parameters.end_angle == left_angle + ) + + # Copy the straight line trajectory for the start angle that + # is 90 degrees to the right + right_angle_idx = int((idx - idx_offset) % self.num_of_headings) + right_angle = self.headings[right_angle_idx] + right_trajectories = spanning_set[right_angle] + right_straight_trajectory = next( + t for t in right_trajectories if t.parameters.end_angle == right_angle + ) + + yaws = np.full( + len(left_straight_trajectory.path.xs), angle, dtype=np.float64 + ) + + # Create a new set of parameters that represents + # the left sliding motion + parmas_l = left_straight_trajectory.parameters + left_motion_parameters = TrajectoryParameters( + parmas_l.turning_radius, + parmas_l.x_offset, + parmas_l.y_offset, + parmas_l.end_point, + angle, + angle, + parmas_l.left_turn, + parmas_l.arc_start_point, + parmas_l.arc_end_point, + ) + + # Create a new set of parameters that represents + # the right sliding motion + params_r = right_straight_trajectory.parameters + right_motion_parameters = TrajectoryParameters( + params_r.turning_radius, + params_r.x_offset, + params_r.y_offset, + params_r.end_point, + angle, + angle, + params_r.left_turn, + parmas_l.arc_start_point, + parmas_l.arc_end_point, + ) + + left_motion = Trajectory( + parameters=left_motion_parameters, + path=Path( + xs=left_straight_trajectory.path.xs, + ys=left_straight_trajectory.path.ys, + yaws=yaws, + ), + ) + + right_motion = Trajectory( + parameters=right_motion_parameters, + path=Path( + xs=right_straight_trajectory.path.xs, + ys=right_straight_trajectory.path.ys, + yaws=yaws, + ), + ) + + spanning_set[angle].append(left_motion) + spanning_set[angle].append(right_motion) + + return spanning_set + + def run(self): + """ + Run the lattice generator. + + Returns + ------- + dict + The minimal spanning set including additional motions for the + specified motion model + + """ + complete_spanning_set = self._generate_minimal_spanning_set() + + return self._handle_motion_model(complete_spanning_set) diff --git a/nav2_smac_planner/lattice_primitives/requirements.txt b/nav2_smac_planner/lattice_primitives/requirements.txt new file mode 100644 index 0000000000..c76f9d4ba6 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/requirements.txt @@ -0,0 +1,3 @@ +numpy>=1.17.4 +matplotlib>=3.1.2 +Rtree>=0.9.7 \ No newline at end of file diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json new file mode 100644 index 0000000000..0df406dfdc --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json @@ -0,0 +1,3932 @@ +{ + "version": 1.0, + "date_generated": "2022-03-17", + "lattice_metadata": { + "motion_model": "ackermann", + "turning_radius": 0.5, + "grid_resolution": 0.05, + "stopping_threshold": 5, + "num_of_headings": 16, + "heading_angles": [ + 0.0, + 0.4636476090008061, + 0.7853981633974483, + 1.1071487177940904, + 1.5707963267948966, + 2.0344439357957027, + 2.356194490192345, + 2.677945044588987, + 3.141592653589793, + 3.6052402625905993, + 3.9269908169872414, + 4.2487413713838835, + 4.71238898038469, + 5.176036589385496, + 5.497787143782138, + 5.81953769817878 + ], + "number_of_trajectories": 80 + }, + "primitives": [ + { + "trajectory_id": 0, + "start_angle_index": 0, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.04981, + -0.00236, + 6.18831988221979 + ], + [ + 0.09917, + -0.00944, + 6.093454457259993 + ], + [ + 0.14765, + -0.02115, + 5.998589032300196 + ], + [ + 0.19479, + -0.03741, + 5.903723607340399 + ], + [ + 0.24018, + -0.05805, + 5.808858182380602 + ], + [ + 0.28341, + -0.08291, + 5.713992757420805 + ], + [ + 0.3241, + -0.11175, + 5.619127332461009 + ], + [ + 0.36187, + -0.14431, + 5.524261907501212 + ], + [ + 0.39638, + -0.1803, + 5.429396482541415 + ], + [ + 0.42733, + -0.2194, + 5.334531057581619 + ], + [ + 0.45444, + -0.26126, + 5.239665632621822 + ], + [ + 0.47769, + -0.30538, + 5.176036589385496 + ], + [ + 0.5, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 1, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.05254, + -0.00218, + 6.200401159939406 + ], + [ + 0.10472, + -0.00869, + 6.117617012699226 + ], + [ + 0.15619, + -0.0195, + 6.034832865459045 + ], + [ + 0.20658, + -0.03452, + 5.952048718218864 + ], + [ + 0.25556, + -0.05366, + 5.869264570978684 + ], + [ + 0.30295, + -0.07648, + 5.81953769817878 + ], + [ + 0.35, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 2, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.05, + 0.0, + 0.0 + ], + [ + 0.1, + 0.0, + 0.0 + ], + [ + 0.15, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 3, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.05254, + 0.00218, + 0.0827841472401804 + ], + [ + 0.10472, + 0.00869, + 0.1655682944803608 + ], + [ + 0.15619, + 0.0195, + 0.2483524417205412 + ], + [ + 0.20658, + 0.03452, + 0.3311365889607216 + ], + [ + 0.25556, + 0.05366, + 0.413920736200902 + ], + [ + 0.30295, + 0.07648, + 0.4636476090008061 + ], + [ + 0.35, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 4, + "start_angle_index": 0, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.04981, + 0.00236, + 0.0948654249597968 + ], + [ + 0.09917, + 0.00944, + 0.1897308499195936 + ], + [ + 0.14765, + 0.02115, + 0.2845962748793904 + ], + [ + 0.19479, + 0.03741, + 0.3794616998391872 + ], + [ + 0.24018, + 0.05805, + 0.4743271247989839 + ], + [ + 0.28341, + 0.08291, + 0.5691925497587808 + ], + [ + 0.3241, + 0.11175, + 0.6640579747185776 + ], + [ + 0.36187, + 0.14431, + 0.7589233996783744 + ], + [ + 0.39638, + 0.1803, + 0.8537888246381711 + ], + [ + 0.42733, + 0.2194, + 0.9486542495979677 + ], + [ + 0.45444, + 0.26126, + 1.0435196745577644 + ], + [ + 0.47769, + 0.30538, + 1.1071487177940904 + ], + [ + 0.5, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 5, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.04747, + 0.02076, + 0.360614807000627 + ], + [ + 0.09683, + 0.03652, + 0.2575820050004478 + ], + [ + 0.14755, + 0.04712, + 0.15454920300026875 + ], + [ + 0.19909, + 0.05245, + 0.051516401000089584 + ], + [ + 0.25091, + 0.05245, + 6.231668906179497 + ], + [ + 0.30245, + 0.04712, + 6.128636104179318 + ], + [ + 0.35317, + 0.03652, + 6.025603302179138 + ], + [ + 0.40253, + 0.02076, + 5.922570500178959 + ], + [ + 0.45, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 6, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.04729, + 0.0212, + 0.37934804372793224 + ], + [ + 0.09619, + 0.03835, + 0.29504847845505844 + ], + [ + 0.14636, + 0.05131, + 0.21074891318218458 + ], + [ + 0.19745, + 0.06001, + 0.12644934790931073 + ], + [ + 0.24909, + 0.06437, + 0.04214978263643687 + ], + [ + 0.30091, + 0.06437, + 6.2410355245431495 + ], + [ + 0.35255, + 0.06001, + 6.156735959270275 + ], + [ + 0.40364, + 0.05131, + 6.0724363939974015 + ], + [ + 0.45381, + 0.03835, + 5.988136828724528 + ], + [ + 0.50271, + 0.0212, + 5.903837263451654 + ], + [ + 0.55, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 7, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.04705, + 0.02352, + 0.4636476090008061 + ], + [ + 0.09444, + 0.04634, + 0.413920736200902 + ], + [ + 0.14342, + 0.06548, + 0.3311365889607216 + ], + [ + 0.19381, + 0.0805, + 0.24835244172054122 + ], + [ + 0.24528, + 0.09131, + 0.16556829448036087 + ], + [ + 0.29746, + 0.09782, + 0.08278414724018052 + ], + [ + 0.35, + 0.1, + 0.0 + ] + ] + }, + { + "trajectory_id": 8, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + 0.025, + 0.4636476090008061 + ], + [ + 0.1, + 0.05, + 0.4636476090008061 + ], + [ + 0.15, + 0.075, + 0.4636476090008061 + ], + [ + 0.2, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 9, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.04409, + 0.0241, + 0.536596705651906 + ], + [ + 0.08631, + 0.05134, + 0.6095458023030058 + ], + [ + 0.12643, + 0.08159, + 0.6824948989541056 + ], + [ + 0.16424, + 0.11469, + 0.7554439956052055 + ], + [ + 0.2, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 10, + "start_angle_index": 2, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03617, + 0.03288, + 0.6899154709776123 + ], + [ + 0.07532, + 0.06215, + 0.5944327785577764 + ], + [ + 0.11707, + 0.08756, + 0.4989500861379405 + ], + [ + 0.16106, + 0.10888, + 0.4034673937181046 + ], + [ + 0.20688, + 0.1259, + 0.30798470129826866 + ], + [ + 0.25412, + 0.13848, + 0.21250200887843262 + ], + [ + 0.30234, + 0.1465, + 0.11701931645859664 + ], + [ + 0.3511, + 0.14988, + 0.021536624038760666 + ], + [ + 0.4, + 0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 11, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03576, + 0.03531, + 0.7554439956052055 + ], + [ + 0.07357, + 0.06841, + 0.6824948989541056 + ], + [ + 0.11369, + 0.09866, + 0.6095458023030058 + ], + [ + 0.15591, + 0.1259, + 0.5365967056519059 + ], + [ + 0.2, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 12, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + 0.0375, + 0.7853981633974483 + ], + [ + 0.075, + 0.075, + 0.7853981633974483 + ], + [ + 0.1125, + 0.1125, + 0.7853981633974483 + ], + [ + 0.15, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 13, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03531, + 0.03576, + 0.8153523311896911 + ], + [ + 0.06841, + 0.07357, + 0.8883014278407909 + ], + [ + 0.09866, + 0.11369, + 0.9612505244918907 + ], + [ + 0.1259, + 0.15591, + 1.0341996211429907 + ], + [ + 0.15, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 14, + "start_angle_index": 2, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03288, + 0.03617, + 0.8808808558172843 + ], + [ + 0.06215, + 0.07532, + 0.9763635482371201 + ], + [ + 0.08756, + 0.11707, + 1.071846240656956 + ], + [ + 0.10888, + 0.16106, + 1.167328933076792 + ], + [ + 0.1259, + 0.20688, + 1.262811625496628 + ], + [ + 0.13848, + 0.25412, + 1.358294317916464 + ], + [ + 0.1465, + 0.30234, + 1.4537770103363 + ], + [ + 0.14988, + 0.3511, + 1.549259702756136 + ], + [ + 0.15, + 0.4, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 15, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.0241, + 0.04409, + 1.0341996211429905 + ], + [ + 0.05134, + 0.08631, + 0.9612505244918907 + ], + [ + 0.08159, + 0.12643, + 0.8883014278407909 + ], + [ + 0.11469, + 0.16424, + 0.8153523311896911 + ], + [ + 0.15, + 0.2, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 16, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + 0.05, + 1.1071487177940904 + ], + [ + 0.05, + 0.1, + 1.1071487177940904 + ], + [ + 0.075, + 0.15, + 1.1071487177940904 + ], + [ + 0.1, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 17, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.02352, + 0.04705, + 1.1071487177940904 + ], + [ + 0.04634, + 0.09444, + 1.1568755905939945 + ], + [ + 0.06548, + 0.14342, + 1.239659737834175 + ], + [ + 0.0805, + 0.19381, + 1.3224438850743554 + ], + [ + 0.09131, + 0.24528, + 1.4052280323145356 + ], + [ + 0.09782, + 0.29746, + 1.488012179554716 + ], + [ + 0.1, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 18, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.02076, + 0.04747, + 1.2101815197942696 + ], + [ + 0.03652, + 0.09683, + 1.3132143217944487 + ], + [ + 0.04712, + 0.14755, + 1.4162471237946277 + ], + [ + 0.05245, + 0.19909, + 1.519279925794807 + ], + [ + 0.05245, + 0.25091, + 1.622312727794986 + ], + [ + 0.04712, + 0.30245, + 1.7253455297951654 + ], + [ + 0.03652, + 0.35317, + 1.8283783317953444 + ], + [ + 0.02076, + 0.40253, + 1.9314111337955238 + ], + [ + 0.0, + 0.45, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 19, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.0212, + 0.04729, + 1.1914482830669642 + ], + [ + 0.03835, + 0.09619, + 1.2757478483398381 + ], + [ + 0.05131, + 0.14636, + 1.3600474136127119 + ], + [ + 0.06001, + 0.19745, + 1.4443469788855858 + ], + [ + 0.06437, + 0.24909, + 1.5286465441584596 + ], + [ + 0.06437, + 0.30091, + 1.6129461094313333 + ], + [ + 0.06001, + 0.35255, + 1.6972456747042073 + ], + [ + 0.05131, + 0.40364, + 1.7815452399770813 + ], + [ + 0.03835, + 0.45381, + 1.8658448052499552 + ], + [ + 0.0212, + 0.50271, + 1.9501443705228287 + ], + [ + 0.0, + 0.55, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 20, + "start_angle_index": 4, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.00236, + 0.04981, + 1.4759309018350997 + ], + [ + 0.00944, + 0.09917, + 1.381065476875303 + ], + [ + 0.02115, + 0.14765, + 1.2862000519155061 + ], + [ + 0.03741, + 0.19479, + 1.1913346269557095 + ], + [ + 0.05805, + 0.24018, + 1.0964692019959126 + ], + [ + 0.08291, + 0.28341, + 1.0016037770361157 + ], + [ + 0.11175, + 0.3241, + 0.9067383520763189 + ], + [ + 0.14431, + 0.36187, + 0.8118729271165221 + ], + [ + 0.1803, + 0.39638, + 0.7170075021567255 + ], + [ + 0.2194, + 0.42733, + 0.6221420771969288 + ], + [ + 0.26126, + 0.45444, + 0.5272766522371322 + ], + [ + 0.30538, + 0.47769, + 0.4636476090008061 + ], + [ + 0.35, + 0.5, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 21, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.00218, + 0.05254, + 1.488012179554716 + ], + [ + 0.00869, + 0.10472, + 1.4052280323145356 + ], + [ + 0.0195, + 0.15619, + 1.3224438850743554 + ], + [ + 0.03452, + 0.20658, + 1.239659737834175 + ], + [ + 0.05366, + 0.25556, + 1.1568755905939945 + ], + [ + 0.07648, + 0.30295, + 1.1071487177940904 + ], + [ + 0.1, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 22, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 1.5707963267948966 + ], + [ + 0.0, + 0.15, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 23, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.00218, + 0.05254, + 1.653580474035077 + ], + [ + -0.00869, + 0.10472, + 1.7363646212752575 + ], + [ + -0.0195, + 0.15619, + 1.8191487685154377 + ], + [ + -0.03452, + 0.20658, + 1.9019329157556182 + ], + [ + -0.05366, + 0.25556, + 1.9847170629957986 + ], + [ + -0.07648, + 0.30295, + 2.0344439357957027 + ], + [ + -0.1, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 24, + "start_angle_index": 4, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.00236, + 0.04981, + 1.6656617517546934 + ], + [ + -0.00944, + 0.09917, + 1.76052717671449 + ], + [ + -0.02115, + 0.14765, + 1.855392601674287 + ], + [ + -0.03741, + 0.19479, + 1.9502580266340837 + ], + [ + -0.05805, + 0.24018, + 2.0451234515938803 + ], + [ + -0.08291, + 0.28341, + 2.1399888765536774 + ], + [ + -0.11175, + 0.3241, + 2.234854301513474 + ], + [ + -0.14431, + 0.36187, + 2.3297197264732707 + ], + [ + -0.1803, + 0.39638, + 2.4245851514330674 + ], + [ + -0.2194, + 0.42733, + 2.519450576392864 + ], + [ + -0.26126, + 0.45444, + 2.6143160013526607 + ], + [ + -0.30538, + 0.47769, + 2.677945044588987 + ], + [ + -0.35, + 0.5, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 25, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.02076, + 0.04747, + 1.9314111337955235 + ], + [ + -0.03652, + 0.09683, + 1.8283783317953444 + ], + [ + -0.04712, + 0.14755, + 1.7253455297951654 + ], + [ + -0.05245, + 0.19909, + 1.622312727794986 + ], + [ + -0.05245, + 0.25091, + 1.519279925794807 + ], + [ + -0.04712, + 0.30245, + 1.4162471237946277 + ], + [ + -0.03652, + 0.35317, + 1.3132143217944487 + ], + [ + -0.02076, + 0.40253, + 1.2101815197942694 + ], + [ + 0.0, + 0.45, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 26, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.0212, + 0.04729, + 1.950144370522829 + ], + [ + -0.03835, + 0.09619, + 1.865844805249955 + ], + [ + -0.05131, + 0.14636, + 1.7815452399770813 + ], + [ + -0.06001, + 0.19745, + 1.6972456747042073 + ], + [ + -0.06437, + 0.24909, + 1.6129461094313335 + ], + [ + -0.06437, + 0.30091, + 1.5286465441584598 + ], + [ + -0.06001, + 0.35255, + 1.4443469788855858 + ], + [ + -0.05131, + 0.40364, + 1.3600474136127119 + ], + [ + -0.03835, + 0.45381, + 1.275747848339838 + ], + [ + -0.0212, + 0.50271, + 1.1914482830669644 + ], + [ + 0.0, + 0.55, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 27, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.02352, + 0.04705, + 2.0344439357957027 + ], + [ + -0.04634, + 0.09444, + 1.9847170629957986 + ], + [ + -0.06548, + 0.14342, + 1.9019329157556182 + ], + [ + -0.0805, + 0.19381, + 1.8191487685154377 + ], + [ + -0.09131, + 0.24528, + 1.7363646212752575 + ], + [ + -0.09782, + 0.29746, + 1.653580474035077 + ], + [ + -0.1, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 28, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + 0.05, + 2.0344439357957027 + ], + [ + -0.05, + 0.1, + 2.0344439357957027 + ], + [ + -0.075, + 0.15, + 2.0344439357957027 + ], + [ + -0.1, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 29, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.0241, + 0.04409, + 2.1073930324468026 + ], + [ + -0.05134, + 0.08631, + 2.1803421290979026 + ], + [ + -0.08159, + 0.12643, + 2.253291225749002 + ], + [ + -0.11469, + 0.16424, + 2.326240322400102 + ], + [ + -0.15, + 0.2, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 30, + "start_angle_index": 6, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03288, + 0.03617, + 2.260711797772509 + ], + [ + -0.06215, + 0.07532, + 2.165229105352673 + ], + [ + -0.08756, + 0.11707, + 2.069746412932837 + ], + [ + -0.10888, + 0.16106, + 1.9742637205130011 + ], + [ + -0.1259, + 0.20688, + 1.8787810280931652 + ], + [ + -0.13848, + 0.25412, + 1.7832983356733292 + ], + [ + -0.1465, + 0.30234, + 1.6878156432534932 + ], + [ + -0.14988, + 0.3511, + 1.5923329508336572 + ], + [ + -0.15, + 0.4, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 31, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03531, + 0.03576, + 2.326240322400102 + ], + [ + -0.06841, + 0.07357, + 2.253291225749002 + ], + [ + -0.09866, + 0.11369, + 2.1803421290979026 + ], + [ + -0.1259, + 0.15591, + 2.1073930324468026 + ], + [ + -0.15, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 32, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + 0.0375, + 2.356194490192345 + ], + [ + -0.075, + 0.075, + 2.356194490192345 + ], + [ + -0.1125, + 0.1125, + 2.356194490192345 + ], + [ + -0.15, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 33, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03576, + 0.03531, + 2.3861486579845876 + ], + [ + -0.07357, + 0.06841, + 2.4590977546356876 + ], + [ + -0.11369, + 0.09866, + 2.532046851286787 + ], + [ + -0.15591, + 0.1259, + 2.604995947937887 + ], + [ + -0.2, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 34, + "start_angle_index": 6, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03617, + 0.03288, + 2.4516771826121806 + ], + [ + -0.07532, + 0.06215, + 2.547159875032017 + ], + [ + -0.11707, + 0.08756, + 2.6426425674518526 + ], + [ + -0.16106, + 0.10888, + 2.7381252598716888 + ], + [ + -0.20688, + 0.1259, + 2.8336079522915245 + ], + [ + -0.25412, + 0.13848, + 2.9290906447113603 + ], + [ + -0.30234, + 0.1465, + 3.0245733371311965 + ], + [ + -0.3511, + 0.14988, + 3.1200560295510327 + ], + [ + -0.4, + 0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 35, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.04747, + 0.02076, + 2.780977846589166 + ], + [ + -0.09683, + 0.03652, + 2.8840106485893453 + ], + [ + -0.14755, + 0.04712, + 2.9870434505895243 + ], + [ + -0.19909, + 0.05245, + 3.0900762525897036 + ], + [ + -0.25091, + 0.05245, + 3.1931090545898826 + ], + [ + -0.30245, + 0.04712, + 3.296141856590062 + ], + [ + -0.35317, + 0.03652, + 3.399174658590241 + ], + [ + -0.40253, + 0.02076, + 3.5022074605904203 + ], + [ + -0.45, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 36, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.04729, + 0.0212, + 2.762244609861861 + ], + [ + -0.09619, + 0.03835, + 2.8465441751347345 + ], + [ + -0.14636, + 0.05131, + 2.9308437404076084 + ], + [ + -0.19745, + 0.06001, + 3.0151433056804824 + ], + [ + -0.24909, + 0.06437, + 3.0994428709533564 + ], + [ + -0.30091, + 0.06437, + 3.18374243622623 + ], + [ + -0.35255, + 0.06001, + 3.268042001499104 + ], + [ + -0.40364, + 0.05131, + 3.352341566771978 + ], + [ + -0.45381, + 0.03835, + 3.4366411320448518 + ], + [ + -0.50271, + 0.0212, + 3.5209406973177253 + ], + [ + -0.55, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 37, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.04409, + 0.0241, + 2.604995947937887 + ], + [ + -0.08631, + 0.05134, + 2.532046851286787 + ], + [ + -0.12643, + 0.08159, + 2.4590977546356876 + ], + [ + -0.16424, + 0.11469, + 2.3861486579845876 + ], + [ + -0.2, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 38, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + 0.025, + 2.677945044588987 + ], + [ + -0.1, + 0.05, + 2.677945044588987 + ], + [ + -0.15, + 0.075, + 2.677945044588987 + ], + [ + -0.2, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 39, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.04705, + 0.02352, + 2.677945044588987 + ], + [ + -0.09444, + 0.04634, + 2.727671917388891 + ], + [ + -0.14342, + 0.06548, + 2.8104560646290713 + ], + [ + -0.19381, + 0.0805, + 2.893240211869252 + ], + [ + -0.24528, + 0.09131, + 2.976024359109432 + ], + [ + -0.29746, + 0.09782, + 3.058808506349613 + ], + [ + -0.35, + 0.1, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 40, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.05254, + -0.00218, + 3.2243768008299734 + ], + [ + -0.10472, + -0.00869, + 3.307160948070154 + ], + [ + -0.15619, + -0.0195, + 3.3899450953103343 + ], + [ + -0.20658, + -0.03452, + 3.472729242550515 + ], + [ + -0.25556, + -0.05366, + 3.555513389790695 + ], + [ + -0.30295, + -0.07648, + 3.6052402625905993 + ], + [ + -0.35, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 41, + "start_angle_index": 8, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.04981, + -0.00236, + 3.23645807854959 + ], + [ + -0.09917, + -0.00944, + 3.331323503509387 + ], + [ + -0.14765, + -0.02115, + 3.4261889284691835 + ], + [ + -0.19479, + -0.03741, + 3.52105435342898 + ], + [ + -0.24018, + -0.05805, + 3.615919778388777 + ], + [ + -0.28341, + -0.08291, + 3.710785203348574 + ], + [ + -0.3241, + -0.11175, + 3.8056506283083706 + ], + [ + -0.36187, + -0.14431, + 3.9005160532681673 + ], + [ + -0.39638, + -0.1803, + 3.995381478227964 + ], + [ + -0.42733, + -0.2194, + 4.090246903187761 + ], + [ + -0.45444, + -0.26126, + 4.185112328147557 + ], + [ + -0.47769, + -0.30538, + 4.2487413713838835 + ], + [ + -0.5, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 42, + "start_angle_index": 8, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.04981, + 0.00236, + 3.0467272286299965 + ], + [ + -0.09917, + 0.00944, + 2.9518618036701993 + ], + [ + -0.14765, + 0.02115, + 2.8569963787104027 + ], + [ + -0.19479, + 0.03741, + 2.762130953750606 + ], + [ + -0.24018, + 0.05805, + 2.6672655287908094 + ], + [ + -0.28341, + 0.08291, + 2.5724001038310123 + ], + [ + -0.3241, + 0.11175, + 2.4775346788712156 + ], + [ + -0.36187, + 0.14431, + 2.382669253911419 + ], + [ + -0.39638, + 0.1803, + 2.2878038289516223 + ], + [ + -0.42733, + 0.2194, + 2.1929384039918256 + ], + [ + -0.45444, + 0.26126, + 2.098072979032029 + ], + [ + -0.47769, + 0.30538, + 2.0344439357957027 + ], + [ + -0.5, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 43, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.05254, + 0.00218, + 3.058808506349613 + ], + [ + -0.10472, + 0.00869, + 2.976024359109432 + ], + [ + -0.15619, + 0.0195, + 2.893240211869252 + ], + [ + -0.20658, + 0.03452, + 2.8104560646290713 + ], + [ + -0.25556, + 0.05366, + 2.727671917388891 + ], + [ + -0.30295, + 0.07648, + 2.677945044588987 + ], + [ + -0.35, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 44, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + -0.05, + 0.0, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 3.141592653589793 + ], + [ + -0.15, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 45, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + -0.025, + 3.6052402625905993 + ], + [ + -0.1, + -0.05, + 3.6052402625905993 + ], + [ + -0.15, + -0.075, + 3.6052402625905993 + ], + [ + -0.2, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 46, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.04409, + -0.0241, + 3.678189359241699 + ], + [ + -0.08631, + -0.05134, + 3.751138455892799 + ], + [ + -0.12643, + -0.08159, + 3.8240875525438986 + ], + [ + -0.16424, + -0.11469, + 3.8970366491949986 + ], + [ + -0.2, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 47, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.04747, + -0.02076, + 3.5022074605904203 + ], + [ + -0.09683, + -0.03652, + 3.399174658590241 + ], + [ + -0.14755, + -0.04712, + 3.296141856590062 + ], + [ + -0.19909, + -0.05245, + 3.1931090545898826 + ], + [ + -0.25091, + -0.05245, + 3.0900762525897036 + ], + [ + -0.30245, + -0.04712, + 2.9870434505895243 + ], + [ + -0.35317, + -0.03652, + 2.8840106485893453 + ], + [ + -0.40253, + -0.02076, + 2.780977846589166 + ], + [ + -0.45, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 48, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.04729, + -0.0212, + 3.5209406973177253 + ], + [ + -0.09619, + -0.03835, + 3.4366411320448518 + ], + [ + -0.14636, + -0.05131, + 3.352341566771978 + ], + [ + -0.19745, + -0.06001, + 3.268042001499104 + ], + [ + -0.24909, + -0.06437, + 3.18374243622623 + ], + [ + -0.30091, + -0.06437, + 3.0994428709533564 + ], + [ + -0.35255, + -0.06001, + 3.0151433056804824 + ], + [ + -0.40364, + -0.05131, + 2.9308437404076084 + ], + [ + -0.45381, + -0.03835, + 2.8465441751347345 + ], + [ + -0.50271, + -0.0212, + 2.762244609861861 + ], + [ + -0.55, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 49, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.04705, + -0.02352, + 3.6052402625905993 + ], + [ + -0.09444, + -0.04634, + 3.555513389790695 + ], + [ + -0.14342, + -0.06548, + 3.472729242550515 + ], + [ + -0.19381, + -0.0805, + 3.3899450953103343 + ], + [ + -0.24528, + -0.09131, + 3.307160948070154 + ], + [ + -0.29746, + -0.09782, + 3.2243768008299734 + ], + [ + -0.35, + -0.1, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 50, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03576, + -0.03531, + 3.8970366491949986 + ], + [ + -0.07357, + -0.06841, + 3.8240875525438986 + ], + [ + -0.11369, + -0.09866, + 3.751138455892799 + ], + [ + -0.15591, + -0.1259, + 3.678189359241699 + ], + [ + -0.2, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 51, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + -0.0375, + 3.9269908169872414 + ], + [ + -0.075, + -0.075, + 3.9269908169872414 + ], + [ + -0.1125, + -0.1125, + 3.9269908169872414 + ], + [ + -0.15, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 52, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03531, + -0.03576, + 3.956944984779484 + ], + [ + -0.06841, + -0.07357, + 4.029894081430584 + ], + [ + -0.09866, + -0.11369, + 4.102843178081684 + ], + [ + -0.1259, + -0.15591, + 4.175792274732784 + ], + [ + -0.15, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 53, + "start_angle_index": 10, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03288, + -0.03617, + 4.022473509407077 + ], + [ + -0.06215, + -0.07532, + 4.117956201826914 + ], + [ + -0.08756, + -0.11707, + 4.2134388942467496 + ], + [ + -0.10888, + -0.16106, + 4.308921586666585 + ], + [ + -0.1259, + -0.20688, + 4.404404279086421 + ], + [ + -0.13848, + -0.25412, + 4.499886971506257 + ], + [ + -0.1465, + -0.30234, + 4.595369663926093 + ], + [ + -0.14988, + -0.3511, + 4.690852356345929 + ], + [ + -0.15, + -0.4, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 54, + "start_angle_index": 10, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03617, + -0.03288, + 3.8315081245674056 + ], + [ + -0.07532, + -0.06215, + 3.7360254321475694 + ], + [ + -0.11707, + -0.08756, + 3.6405427397277337 + ], + [ + -0.16106, + -0.10888, + 3.5450600473078975 + ], + [ + -0.20688, + -0.1259, + 3.4495773548880617 + ], + [ + -0.25412, + -0.13848, + 3.354094662468226 + ], + [ + -0.30234, + -0.1465, + 3.2586119700483898 + ], + [ + -0.3511, + -0.14988, + 3.1631292776285536 + ], + [ + -0.4, + -0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 55, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.0241, + -0.04409, + 4.175792274732784 + ], + [ + -0.05134, + -0.08631, + 4.102843178081684 + ], + [ + -0.08159, + -0.12643, + 4.029894081430584 + ], + [ + -0.11469, + -0.16424, + 3.956944984779484 + ], + [ + -0.15, + -0.2, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 56, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + -0.05, + 4.2487413713838835 + ], + [ + -0.05, + -0.1, + 4.2487413713838835 + ], + [ + -0.075, + -0.15, + 4.2487413713838835 + ], + [ + -0.1, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 57, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.02352, + -0.04705, + 4.2487413713838835 + ], + [ + -0.04634, + -0.09444, + 4.298468244183788 + ], + [ + -0.06548, + -0.14342, + 4.381252391423968 + ], + [ + -0.0805, + -0.19381, + 4.464036538664148 + ], + [ + -0.09131, + -0.24528, + 4.546820685904329 + ], + [ + -0.09782, + -0.29746, + 4.629604833144509 + ], + [ + -0.1, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 58, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.02076, + -0.04747, + 4.3517741733840625 + ], + [ + -0.03652, + -0.09683, + 4.454806975384242 + ], + [ + -0.04712, + -0.14755, + 4.557839777384421 + ], + [ + -0.05245, + -0.19909, + 4.6608725793846 + ], + [ + -0.05245, + -0.25091, + 4.763905381384779 + ], + [ + -0.04712, + -0.30245, + 4.866938183384958 + ], + [ + -0.03652, + -0.35317, + 4.969970985385137 + ], + [ + -0.02076, + -0.40253, + 5.073003787385317 + ], + [ + 0.0, + -0.45, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 59, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.0212, + -0.04729, + 4.333040936656757 + ], + [ + -0.03835, + -0.09619, + 4.4173405019296315 + ], + [ + -0.05131, + -0.14636, + 4.501640067202505 + ], + [ + -0.06001, + -0.19745, + 4.5859396324753785 + ], + [ + -0.06437, + -0.24909, + 4.670239197748253 + ], + [ + -0.06437, + -0.30091, + 4.754538763021126 + ], + [ + -0.06001, + -0.35255, + 4.838838328294001 + ], + [ + -0.05131, + -0.40364, + 4.923137893566874 + ], + [ + -0.03835, + -0.45381, + 5.007437458839748 + ], + [ + -0.0212, + -0.50271, + 5.091737024112621 + ], + [ + 0.0, + -0.55, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 60, + "start_angle_index": 12, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.00236, + -0.04981, + 4.617523555424893 + ], + [ + -0.00944, + -0.09917, + 4.522658130465096 + ], + [ + -0.02115, + -0.14765, + 4.427792705505299 + ], + [ + -0.03741, + -0.19479, + 4.332927280545503 + ], + [ + -0.05805, + -0.24018, + 4.2380618555857055 + ], + [ + -0.08291, + -0.28341, + 4.143196430625909 + ], + [ + -0.11175, + -0.3241, + 4.048331005666112 + ], + [ + -0.14431, + -0.36187, + 3.9534655807063155 + ], + [ + -0.1803, + -0.39638, + 3.858600155746519 + ], + [ + -0.2194, + -0.42733, + 3.763734730786722 + ], + [ + -0.26126, + -0.45444, + 3.6688693058269255 + ], + [ + -0.30538, + -0.47769, + 3.6052402625905993 + ], + [ + -0.35, + -0.5, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 61, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.00218, + -0.05254, + 4.629604833144509 + ], + [ + -0.00869, + -0.10472, + 4.546820685904329 + ], + [ + -0.0195, + -0.15619, + 4.464036538664148 + ], + [ + -0.03452, + -0.20658, + 4.381252391423968 + ], + [ + -0.05366, + -0.25556, + 4.298468244183788 + ], + [ + -0.07648, + -0.30295, + 4.2487413713838835 + ], + [ + -0.1, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 62, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 4.71238898038469 + ], + [ + 0.0, + -0.15, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 63, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.00218, + -0.05254, + 4.79517312762487 + ], + [ + 0.00869, + -0.10472, + 4.87795727486505 + ], + [ + 0.0195, + -0.15619, + 4.960741422105231 + ], + [ + 0.03452, + -0.20658, + 5.0435255693454115 + ], + [ + 0.05366, + -0.25556, + 5.126309716585592 + ], + [ + 0.07648, + -0.30295, + 5.176036589385496 + ], + [ + 0.1, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 64, + "start_angle_index": 12, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.00236, + -0.04981, + 4.807254405344486 + ], + [ + 0.00944, + -0.09917, + 4.902119830304283 + ], + [ + 0.02115, + -0.14765, + 4.9969852552640805 + ], + [ + 0.03741, + -0.19479, + 5.091850680223876 + ], + [ + 0.05805, + -0.24018, + 5.186716105183674 + ], + [ + 0.08291, + -0.28341, + 5.2815815301434705 + ], + [ + 0.11175, + -0.3241, + 5.376446955103267 + ], + [ + 0.14431, + -0.36187, + 5.471312380063064 + ], + [ + 0.1803, + -0.39638, + 5.5661778050228605 + ], + [ + 0.2194, + -0.42733, + 5.661043229982657 + ], + [ + 0.26126, + -0.45444, + 5.755908654942454 + ], + [ + 0.30538, + -0.47769, + 5.81953769817878 + ], + [ + 0.35, + -0.5, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 65, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.02076, + -0.04747, + 5.073003787385317 + ], + [ + 0.03652, + -0.09683, + 4.969970985385137 + ], + [ + 0.04712, + -0.14755, + 4.866938183384958 + ], + [ + 0.05245, + -0.19909, + 4.763905381384779 + ], + [ + 0.05245, + -0.25091, + 4.6608725793846 + ], + [ + 0.04712, + -0.30245, + 4.557839777384421 + ], + [ + 0.03652, + -0.35317, + 4.454806975384242 + ], + [ + 0.02076, + -0.40253, + 4.3517741733840625 + ], + [ + 0.0, + -0.45, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 66, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.0212, + -0.04729, + 5.091737024112622 + ], + [ + 0.03835, + -0.09619, + 5.007437458839748 + ], + [ + 0.05131, + -0.14636, + 4.923137893566874 + ], + [ + 0.06001, + -0.19745, + 4.838838328294001 + ], + [ + 0.06437, + -0.24909, + 4.754538763021126 + ], + [ + 0.06437, + -0.30091, + 4.670239197748253 + ], + [ + 0.06001, + -0.35255, + 4.5859396324753785 + ], + [ + 0.05131, + -0.40364, + 4.501640067202505 + ], + [ + 0.03835, + -0.45381, + 4.4173405019296315 + ], + [ + 0.0212, + -0.50271, + 4.333040936656758 + ], + [ + 0.0, + -0.55, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 67, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.02352, + -0.04705, + 5.176036589385496 + ], + [ + 0.04634, + -0.09444, + 5.126309716585592 + ], + [ + 0.06548, + -0.14342, + 5.0435255693454115 + ], + [ + 0.0805, + -0.19381, + 4.960741422105231 + ], + [ + 0.09131, + -0.24528, + 4.87795727486505 + ], + [ + 0.09782, + -0.29746, + 4.79517312762487 + ], + [ + 0.1, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 68, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + -0.05, + 5.176036589385496 + ], + [ + 0.05, + -0.1, + 5.176036589385496 + ], + [ + 0.075, + -0.15, + 5.176036589385496 + ], + [ + 0.1, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 69, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.0241, + -0.04409, + 5.248985686036596 + ], + [ + 0.05134, + -0.08631, + 5.321934782687696 + ], + [ + 0.08159, + -0.12643, + 5.394883879338796 + ], + [ + 0.11469, + -0.16424, + 5.467832975989895 + ], + [ + 0.15, + -0.2, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 70, + "start_angle_index": 14, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03288, + -0.03617, + 5.402304451362302 + ], + [ + 0.06215, + -0.07532, + 5.306821758942466 + ], + [ + 0.08756, + -0.11707, + 5.21133906652263 + ], + [ + 0.10888, + -0.16106, + 5.115856374102794 + ], + [ + 0.1259, + -0.20688, + 5.020373681682958 + ], + [ + 0.13848, + -0.25412, + 4.9248909892631225 + ], + [ + 0.1465, + -0.30234, + 4.829408296843287 + ], + [ + 0.14988, + -0.3511, + 4.73392560442345 + ], + [ + 0.15, + -0.4, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 71, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03531, + -0.03576, + 5.467832975989895 + ], + [ + 0.06841, + -0.07357, + 5.394883879338796 + ], + [ + 0.09866, + -0.11369, + 5.321934782687696 + ], + [ + 0.1259, + -0.15591, + 5.248985686036596 + ], + [ + 0.15, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 72, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + -0.0375, + 5.497787143782138 + ], + [ + 0.075, + -0.075, + 5.497787143782138 + ], + [ + 0.1125, + -0.1125, + 5.497787143782138 + ], + [ + 0.15, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 73, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03576, + -0.03531, + 5.527741311574381 + ], + [ + 0.07357, + -0.06841, + 5.60069040822548 + ], + [ + 0.11369, + -0.09866, + 5.67363950487658 + ], + [ + 0.15591, + -0.1259, + 5.74658860152768 + ], + [ + 0.2, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 74, + "start_angle_index": 14, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03617, + -0.03288, + 5.593269836201974 + ], + [ + 0.07532, + -0.06215, + 5.6887525286218095 + ], + [ + 0.11707, + -0.08756, + 5.784235221041646 + ], + [ + 0.16106, + -0.10888, + 5.879717913461482 + ], + [ + 0.20688, + -0.1259, + 5.975200605881318 + ], + [ + 0.25412, + -0.13848, + 6.070683298301153 + ], + [ + 0.30234, + -0.1465, + 6.166165990720989 + ], + [ + 0.3511, + -0.14988, + 6.261648683140826 + ], + [ + 0.4, + -0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 75, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.04409, + -0.0241, + 5.74658860152768 + ], + [ + 0.08631, + -0.05134, + 5.67363950487658 + ], + [ + 0.12643, + -0.08159, + 5.60069040822548 + ], + [ + 0.16424, + -0.11469, + 5.527741311574381 + ], + [ + 0.2, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 76, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + -0.025, + 5.81953769817878 + ], + [ + 0.1, + -0.05, + 5.81953769817878 + ], + [ + 0.15, + -0.075, + 5.81953769817878 + ], + [ + 0.2, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 77, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.04705, + -0.02352, + 5.81953769817878 + ], + [ + 0.09444, + -0.04634, + 5.869264570978684 + ], + [ + 0.14342, + -0.06548, + 5.952048718218864 + ], + [ + 0.19381, + -0.0805, + 6.034832865459045 + ], + [ + 0.24528, + -0.09131, + 6.117617012699226 + ], + [ + 0.29746, + -0.09782, + 6.200401159939406 + ], + [ + 0.35, + -0.1, + 0.0 + ] + ] + }, + { + "trajectory_id": 78, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.04747, + -0.02076, + 5.922570500178959 + ], + [ + 0.09683, + -0.03652, + 6.025603302179139 + ], + [ + 0.14755, + -0.04712, + 6.128636104179318 + ], + [ + 0.19909, + -0.05245, + 6.231668906179497 + ], + [ + 0.25091, + -0.05245, + 0.05151640100008953 + ], + [ + 0.30245, + -0.04712, + 0.1545492030002687 + ], + [ + 0.35317, + -0.03652, + 0.25758200500044787 + ], + [ + 0.40253, + -0.02076, + 0.36061480700062715 + ], + [ + 0.45, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 79, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.04729, + -0.0212, + 5.903837263451654 + ], + [ + 0.09619, + -0.03835, + 5.988136828724528 + ], + [ + 0.14636, + -0.05131, + 6.0724363939974015 + ], + [ + 0.19745, + -0.06001, + 6.156735959270275 + ], + [ + 0.24909, + -0.06437, + 6.2410355245431495 + ], + [ + 0.30091, + -0.06437, + 0.04214978263643693 + ], + [ + 0.35255, + -0.06001, + 0.1264493479093109 + ], + [ + 0.40364, + -0.05131, + 0.21074891318218475 + ], + [ + 0.45381, + -0.03835, + 0.2950484784550586 + ], + [ + 0.50271, + -0.0212, + 0.37934804372793235 + ], + [ + 0.55, + 0.0, + 0.4636476090008061 + ] + ] + } + ] +} \ No newline at end of file diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/all_trajectories.png b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/all_trajectories.png new file mode 100644 index 0000000000..428f6eaffb Binary files /dev/null and b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/all_trajectories.png differ diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json new file mode 100644 index 0000000000..984bbb62d9 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json @@ -0,0 +1,4876 @@ +{ + "version": 1.0, + "date_generated": "2022-03-17", + "lattice_metadata": { + "motion_model": "diff", + "turning_radius": 0.5, + "grid_resolution": 0.05, + "stopping_threshold": 5, + "num_of_headings": 16, + "heading_angles": [ + 0.0, + 0.4636476090008061, + 0.7853981633974483, + 1.1071487177940904, + 1.5707963267948966, + 2.0344439357957027, + 2.356194490192345, + 2.677945044588987, + 3.141592653589793, + 3.6052402625905993, + 3.9269908169872414, + 4.2487413713838835, + 4.71238898038469, + 5.176036589385496, + 5.497787143782138, + 5.81953769817878 + ], + "number_of_trajectories": 112 + }, + "primitives": [ + { + "trajectory_id": 0, + "start_angle_index": 0, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.04981, + -0.00236, + 6.18831988221979 + ], + [ + 0.09917, + -0.00944, + 6.093454457259993 + ], + [ + 0.14765, + -0.02115, + 5.998589032300196 + ], + [ + 0.19479, + -0.03741, + 5.903723607340399 + ], + [ + 0.24018, + -0.05805, + 5.808858182380602 + ], + [ + 0.28341, + -0.08291, + 5.713992757420805 + ], + [ + 0.3241, + -0.11175, + 5.619127332461009 + ], + [ + 0.36187, + -0.14431, + 5.524261907501212 + ], + [ + 0.39638, + -0.1803, + 5.429396482541415 + ], + [ + 0.42733, + -0.2194, + 5.334531057581619 + ], + [ + 0.45444, + -0.26126, + 5.239665632621822 + ], + [ + 0.47769, + -0.30538, + 5.176036589385496 + ], + [ + 0.5, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 1, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.05254, + -0.00218, + 6.200401159939406 + ], + [ + 0.10472, + -0.00869, + 6.117617012699226 + ], + [ + 0.15619, + -0.0195, + 6.034832865459045 + ], + [ + 0.20658, + -0.03452, + 5.952048718218864 + ], + [ + 0.25556, + -0.05366, + 5.869264570978684 + ], + [ + 0.30295, + -0.07648, + 5.81953769817878 + ], + [ + 0.35, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 2, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 3, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.05, + 0.0, + 0.0 + ], + [ + 0.1, + 0.0, + 0.0 + ], + [ + 0.15, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 4, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.05254, + 0.00218, + 0.0827841472401804 + ], + [ + 0.10472, + 0.00869, + 0.1655682944803608 + ], + [ + 0.15619, + 0.0195, + 0.2483524417205412 + ], + [ + 0.20658, + 0.03452, + 0.3311365889607216 + ], + [ + 0.25556, + 0.05366, + 0.413920736200902 + ], + [ + 0.30295, + 0.07648, + 0.4636476090008061 + ], + [ + 0.35, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 5, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.1545492030002687 + ], + [ + 0.0, + 0.0, + 0.3090984060005374 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 6, + "start_angle_index": 0, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.04981, + 0.00236, + 0.0948654249597968 + ], + [ + 0.09917, + 0.00944, + 0.1897308499195936 + ], + [ + 0.14765, + 0.02115, + 0.2845962748793904 + ], + [ + 0.19479, + 0.03741, + 0.3794616998391872 + ], + [ + 0.24018, + 0.05805, + 0.4743271247989839 + ], + [ + 0.28341, + 0.08291, + 0.5691925497587808 + ], + [ + 0.3241, + 0.11175, + 0.6640579747185776 + ], + [ + 0.36187, + 0.14431, + 0.7589233996783744 + ], + [ + 0.39638, + 0.1803, + 0.8537888246381711 + ], + [ + 0.42733, + 0.2194, + 0.9486542495979677 + ], + [ + 0.45444, + 0.26126, + 1.0435196745577644 + ], + [ + 0.47769, + 0.30538, + 1.1071487177940904 + ], + [ + 0.5, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 7, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.04747, + 0.02076, + 0.360614807000627 + ], + [ + 0.09683, + 0.03652, + 0.2575820050004478 + ], + [ + 0.14755, + 0.04712, + 0.15454920300026875 + ], + [ + 0.19909, + 0.05245, + 0.051516401000089584 + ], + [ + 0.25091, + 0.05245, + 6.231668906179497 + ], + [ + 0.30245, + 0.04712, + 6.128636104179318 + ], + [ + 0.35317, + 0.03652, + 6.025603302179138 + ], + [ + 0.40253, + 0.02076, + 5.922570500178959 + ], + [ + 0.45, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 8, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.04729, + 0.0212, + 0.37934804372793224 + ], + [ + 0.09619, + 0.03835, + 0.29504847845505844 + ], + [ + 0.14636, + 0.05131, + 0.21074891318218458 + ], + [ + 0.19745, + 0.06001, + 0.12644934790931073 + ], + [ + 0.24909, + 0.06437, + 0.04214978263643687 + ], + [ + 0.30091, + 0.06437, + 6.2410355245431495 + ], + [ + 0.35255, + 0.06001, + 6.156735959270275 + ], + [ + 0.40364, + 0.05131, + 6.0724363939974015 + ], + [ + 0.45381, + 0.03835, + 5.988136828724528 + ], + [ + 0.50271, + 0.0212, + 5.903837263451654 + ], + [ + 0.55, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 9, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.04705, + 0.02352, + 0.4636476090008061 + ], + [ + 0.09444, + 0.04634, + 0.413920736200902 + ], + [ + 0.14342, + 0.06548, + 0.3311365889607216 + ], + [ + 0.19381, + 0.0805, + 0.24835244172054122 + ], + [ + 0.24528, + 0.09131, + 0.16556829448036087 + ], + [ + 0.29746, + 0.09782, + 0.08278414724018052 + ], + [ + 0.35, + 0.1, + 0.0 + ] + ] + }, + { + "trajectory_id": 10, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.23182380450040305 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 11, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + 0.025, + 0.4636476090008061 + ], + [ + 0.1, + 0.05, + 0.4636476090008061 + ], + [ + 0.15, + 0.075, + 0.4636476090008061 + ], + [ + 0.2, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 12, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.04409, + 0.0241, + 0.536596705651906 + ], + [ + 0.08631, + 0.05134, + 0.6095458023030058 + ], + [ + 0.12643, + 0.08159, + 0.6824948989541056 + ], + [ + 0.16424, + 0.11469, + 0.7554439956052055 + ], + [ + 0.2, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 13, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 14, + "start_angle_index": 2, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03617, + 0.03288, + 0.6899154709776123 + ], + [ + 0.07532, + 0.06215, + 0.5944327785577764 + ], + [ + 0.11707, + 0.08756, + 0.4989500861379405 + ], + [ + 0.16106, + 0.10888, + 0.4034673937181046 + ], + [ + 0.20688, + 0.1259, + 0.30798470129826866 + ], + [ + 0.25412, + 0.13848, + 0.21250200887843262 + ], + [ + 0.30234, + 0.1465, + 0.11701931645859664 + ], + [ + 0.3511, + 0.14988, + 0.021536624038760666 + ], + [ + 0.4, + 0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 15, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03576, + 0.03531, + 0.7554439956052055 + ], + [ + 0.07357, + 0.06841, + 0.6824948989541056 + ], + [ + 0.11369, + 0.09866, + 0.6095458023030058 + ], + [ + 0.15591, + 0.1259, + 0.5365967056519059 + ], + [ + 0.2, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 16, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 17, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + 0.0375, + 0.7853981633974483 + ], + [ + 0.075, + 0.075, + 0.7853981633974483 + ], + [ + 0.1125, + 0.1125, + 0.7853981633974483 + ], + [ + 0.15, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 18, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03531, + 0.03576, + 0.8153523311896911 + ], + [ + 0.06841, + 0.07357, + 0.8883014278407909 + ], + [ + 0.09866, + 0.11369, + 0.9612505244918907 + ], + [ + 0.1259, + 0.15591, + 1.0341996211429907 + ], + [ + 0.15, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 19, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.9462734405957693 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 20, + "start_angle_index": 2, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03288, + 0.03617, + 0.8808808558172843 + ], + [ + 0.06215, + 0.07532, + 0.9763635482371201 + ], + [ + 0.08756, + 0.11707, + 1.071846240656956 + ], + [ + 0.10888, + 0.16106, + 1.167328933076792 + ], + [ + 0.1259, + 0.20688, + 1.262811625496628 + ], + [ + 0.13848, + 0.25412, + 1.358294317916464 + ], + [ + 0.1465, + 0.30234, + 1.4537770103363 + ], + [ + 0.14988, + 0.3511, + 1.549259702756136 + ], + [ + 0.15, + 0.4, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 21, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.0241, + 0.04409, + 1.0341996211429905 + ], + [ + 0.05134, + 0.08631, + 0.9612505244918907 + ], + [ + 0.08159, + 0.12643, + 0.8883014278407909 + ], + [ + 0.11469, + 0.16424, + 0.8153523311896911 + ], + [ + 0.15, + 0.2, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 22, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 0.9998985329952097 + ], + [ + 0.0, + 0.0, + 0.892648348196329 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 23, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + 0.05, + 1.1071487177940904 + ], + [ + 0.05, + 0.1, + 1.1071487177940904 + ], + [ + 0.075, + 0.15, + 1.1071487177940904 + ], + [ + 0.1, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 24, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.02352, + 0.04705, + 1.1071487177940904 + ], + [ + 0.04634, + 0.09444, + 1.1568755905939945 + ], + [ + 0.06548, + 0.14342, + 1.239659737834175 + ], + [ + 0.0805, + 0.19381, + 1.3224438850743554 + ], + [ + 0.09131, + 0.24528, + 1.4052280323145356 + ], + [ + 0.09782, + 0.29746, + 1.488012179554716 + ], + [ + 0.1, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 25, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 26, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.02076, + 0.04747, + 1.2101815197942696 + ], + [ + 0.03652, + 0.09683, + 1.3132143217944487 + ], + [ + 0.04712, + 0.14755, + 1.4162471237946277 + ], + [ + 0.05245, + 0.19909, + 1.519279925794807 + ], + [ + 0.05245, + 0.25091, + 1.622312727794986 + ], + [ + 0.04712, + 0.30245, + 1.7253455297951654 + ], + [ + 0.03652, + 0.35317, + 1.8283783317953444 + ], + [ + 0.02076, + 0.40253, + 1.9314111337955238 + ], + [ + 0.0, + 0.45, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 27, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.0212, + 0.04729, + 1.1914482830669642 + ], + [ + 0.03835, + 0.09619, + 1.2757478483398381 + ], + [ + 0.05131, + 0.14636, + 1.3600474136127119 + ], + [ + 0.06001, + 0.19745, + 1.4443469788855858 + ], + [ + 0.06437, + 0.24909, + 1.5286465441584596 + ], + [ + 0.06437, + 0.30091, + 1.6129461094313333 + ], + [ + 0.06001, + 0.35255, + 1.6972456747042073 + ], + [ + 0.05131, + 0.40364, + 1.7815452399770813 + ], + [ + 0.03835, + 0.45381, + 1.8658448052499552 + ], + [ + 0.0212, + 0.50271, + 1.9501443705228287 + ], + [ + 0.0, + 0.55, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 28, + "start_angle_index": 4, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.00236, + 0.04981, + 1.4759309018350997 + ], + [ + 0.00944, + 0.09917, + 1.381065476875303 + ], + [ + 0.02115, + 0.14765, + 1.2862000519155061 + ], + [ + 0.03741, + 0.19479, + 1.1913346269557095 + ], + [ + 0.05805, + 0.24018, + 1.0964692019959126 + ], + [ + 0.08291, + 0.28341, + 1.0016037770361157 + ], + [ + 0.11175, + 0.3241, + 0.9067383520763189 + ], + [ + 0.14431, + 0.36187, + 0.8118729271165221 + ], + [ + 0.1803, + 0.39638, + 0.7170075021567255 + ], + [ + 0.2194, + 0.42733, + 0.6221420771969288 + ], + [ + 0.26126, + 0.45444, + 0.5272766522371322 + ], + [ + 0.30538, + 0.47769, + 0.4636476090008061 + ], + [ + 0.35, + 0.5, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 29, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.00218, + 0.05254, + 1.488012179554716 + ], + [ + 0.00869, + 0.10472, + 1.4052280323145356 + ], + [ + 0.0195, + 0.15619, + 1.3224438850743554 + ], + [ + 0.03452, + 0.20658, + 1.239659737834175 + ], + [ + 0.05366, + 0.25556, + 1.1568755905939945 + ], + [ + 0.07648, + 0.30295, + 1.1071487177940904 + ], + [ + 0.1, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 30, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 31, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 1.5707963267948966 + ], + [ + 0.0, + 0.15, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 32, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.00218, + 0.05254, + 1.653580474035077 + ], + [ + -0.00869, + 0.10472, + 1.7363646212752575 + ], + [ + -0.0195, + 0.15619, + 1.8191487685154377 + ], + [ + -0.03452, + 0.20658, + 1.9019329157556182 + ], + [ + -0.05366, + 0.25556, + 1.9847170629957986 + ], + [ + -0.07648, + 0.30295, + 2.0344439357957027 + ], + [ + -0.1, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 33, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.7253455297951652 + ], + [ + 0.0, + 0.0, + 1.879894732795434 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 34, + "start_angle_index": 4, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.00236, + 0.04981, + 1.6656617517546934 + ], + [ + -0.00944, + 0.09917, + 1.76052717671449 + ], + [ + -0.02115, + 0.14765, + 1.855392601674287 + ], + [ + -0.03741, + 0.19479, + 1.9502580266340837 + ], + [ + -0.05805, + 0.24018, + 2.0451234515938803 + ], + [ + -0.08291, + 0.28341, + 2.1399888765536774 + ], + [ + -0.11175, + 0.3241, + 2.234854301513474 + ], + [ + -0.14431, + 0.36187, + 2.3297197264732707 + ], + [ + -0.1803, + 0.39638, + 2.4245851514330674 + ], + [ + -0.2194, + 0.42733, + 2.519450576392864 + ], + [ + -0.26126, + 0.45444, + 2.6143160013526607 + ], + [ + -0.30538, + 0.47769, + 2.677945044588987 + ], + [ + -0.35, + 0.5, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 35, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.02076, + 0.04747, + 1.9314111337955235 + ], + [ + -0.03652, + 0.09683, + 1.8283783317953444 + ], + [ + -0.04712, + 0.14755, + 1.7253455297951654 + ], + [ + -0.05245, + 0.19909, + 1.622312727794986 + ], + [ + -0.05245, + 0.25091, + 1.519279925794807 + ], + [ + -0.04712, + 0.30245, + 1.4162471237946277 + ], + [ + -0.03652, + 0.35317, + 1.3132143217944487 + ], + [ + -0.02076, + 0.40253, + 1.2101815197942694 + ], + [ + 0.0, + 0.45, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 36, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.0212, + 0.04729, + 1.950144370522829 + ], + [ + -0.03835, + 0.09619, + 1.865844805249955 + ], + [ + -0.05131, + 0.14636, + 1.7815452399770813 + ], + [ + -0.06001, + 0.19745, + 1.6972456747042073 + ], + [ + -0.06437, + 0.24909, + 1.6129461094313335 + ], + [ + -0.06437, + 0.30091, + 1.5286465441584598 + ], + [ + -0.06001, + 0.35255, + 1.4443469788855858 + ], + [ + -0.05131, + 0.40364, + 1.3600474136127119 + ], + [ + -0.03835, + 0.45381, + 1.275747848339838 + ], + [ + -0.0212, + 0.50271, + 1.1914482830669644 + ], + [ + 0.0, + 0.55, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 37, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.02352, + 0.04705, + 2.0344439357957027 + ], + [ + -0.04634, + 0.09444, + 1.9847170629957986 + ], + [ + -0.06548, + 0.14342, + 1.9019329157556182 + ], + [ + -0.0805, + 0.19381, + 1.8191487685154377 + ], + [ + -0.09131, + 0.24528, + 1.7363646212752575 + ], + [ + -0.09782, + 0.29746, + 1.653580474035077 + ], + [ + -0.1, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 38, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 1.8026201312952996 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 39, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + 0.05, + 2.0344439357957027 + ], + [ + -0.05, + 0.1, + 2.0344439357957027 + ], + [ + -0.075, + 0.15, + 2.0344439357957027 + ], + [ + -0.1, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 40, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.0241, + 0.04409, + 2.1073930324468026 + ], + [ + -0.05134, + 0.08631, + 2.1803421290979026 + ], + [ + -0.08159, + 0.12643, + 2.253291225749002 + ], + [ + -0.11469, + 0.16424, + 2.326240322400102 + ], + [ + -0.15, + 0.2, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 41, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 42, + "start_angle_index": 6, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03288, + 0.03617, + 2.260711797772509 + ], + [ + -0.06215, + 0.07532, + 2.165229105352673 + ], + [ + -0.08756, + 0.11707, + 2.069746412932837 + ], + [ + -0.10888, + 0.16106, + 1.9742637205130011 + ], + [ + -0.1259, + 0.20688, + 1.8787810280931652 + ], + [ + -0.13848, + 0.25412, + 1.7832983356733292 + ], + [ + -0.1465, + 0.30234, + 1.6878156432534932 + ], + [ + -0.14988, + 0.3511, + 1.5923329508336572 + ], + [ + -0.15, + 0.4, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 43, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03531, + 0.03576, + 2.326240322400102 + ], + [ + -0.06841, + 0.07357, + 2.253291225749002 + ], + [ + -0.09866, + 0.11369, + 2.1803421290979026 + ], + [ + -0.1259, + 0.15591, + 2.1073930324468026 + ], + [ + -0.15, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 44, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 45, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + 0.0375, + 2.356194490192345 + ], + [ + -0.075, + 0.075, + 2.356194490192345 + ], + [ + -0.1125, + 0.1125, + 2.356194490192345 + ], + [ + -0.15, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 46, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03576, + 0.03531, + 2.3861486579845876 + ], + [ + -0.07357, + 0.06841, + 2.4590977546356876 + ], + [ + -0.11369, + 0.09866, + 2.532046851286787 + ], + [ + -0.15591, + 0.1259, + 2.604995947937887 + ], + [ + -0.2, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 47, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.517069767390666 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 48, + "start_angle_index": 6, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03617, + 0.03288, + 2.4516771826121806 + ], + [ + -0.07532, + 0.06215, + 2.547159875032017 + ], + [ + -0.11707, + 0.08756, + 2.6426425674518526 + ], + [ + -0.16106, + 0.10888, + 2.7381252598716888 + ], + [ + -0.20688, + 0.1259, + 2.8336079522915245 + ], + [ + -0.25412, + 0.13848, + 2.9290906447113603 + ], + [ + -0.30234, + 0.1465, + 3.0245733371311965 + ], + [ + -0.3511, + 0.14988, + 3.1200560295510327 + ], + [ + -0.4, + 0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 49, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.04747, + 0.02076, + 2.780977846589166 + ], + [ + -0.09683, + 0.03652, + 2.8840106485893453 + ], + [ + -0.14755, + 0.04712, + 2.9870434505895243 + ], + [ + -0.19909, + 0.05245, + 3.0900762525897036 + ], + [ + -0.25091, + 0.05245, + 3.1931090545898826 + ], + [ + -0.30245, + 0.04712, + 3.296141856590062 + ], + [ + -0.35317, + 0.03652, + 3.399174658590241 + ], + [ + -0.40253, + 0.02076, + 3.5022074605904203 + ], + [ + -0.45, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 50, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.04729, + 0.0212, + 2.762244609861861 + ], + [ + -0.09619, + 0.03835, + 2.8465441751347345 + ], + [ + -0.14636, + 0.05131, + 2.9308437404076084 + ], + [ + -0.19745, + 0.06001, + 3.0151433056804824 + ], + [ + -0.24909, + 0.06437, + 3.0994428709533564 + ], + [ + -0.30091, + 0.06437, + 3.18374243622623 + ], + [ + -0.35255, + 0.06001, + 3.268042001499104 + ], + [ + -0.40364, + 0.05131, + 3.352341566771978 + ], + [ + -0.45381, + 0.03835, + 3.4366411320448518 + ], + [ + -0.50271, + 0.0212, + 3.5209406973177253 + ], + [ + -0.55, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 51, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.04409, + 0.0241, + 2.604995947937887 + ], + [ + -0.08631, + 0.05134, + 2.532046851286787 + ], + [ + -0.12643, + 0.08159, + 2.4590977546356876 + ], + [ + -0.16424, + 0.11469, + 2.3861486579845876 + ], + [ + -0.2, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 52, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.5706948597901063 + ], + [ + 0.0, + 0.0, + 2.4634446749912255 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 53, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + 0.025, + 2.677945044588987 + ], + [ + -0.1, + 0.05, + 2.677945044588987 + ], + [ + -0.15, + 0.075, + 2.677945044588987 + ], + [ + -0.2, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 54, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.04705, + 0.02352, + 2.677945044588987 + ], + [ + -0.09444, + 0.04634, + 2.727671917388891 + ], + [ + -0.14342, + 0.06548, + 2.8104560646290713 + ], + [ + -0.19381, + 0.0805, + 2.893240211869252 + ], + [ + -0.24528, + 0.09131, + 2.976024359109432 + ], + [ + -0.29746, + 0.09782, + 3.058808506349613 + ], + [ + -0.35, + 0.1, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 55, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 56, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.05254, + -0.00218, + 3.2243768008299734 + ], + [ + -0.10472, + -0.00869, + 3.307160948070154 + ], + [ + -0.15619, + -0.0195, + 3.3899450953103343 + ], + [ + -0.20658, + -0.03452, + 3.472729242550515 + ], + [ + -0.25556, + -0.05366, + 3.555513389790695 + ], + [ + -0.30295, + -0.07648, + 3.6052402625905993 + ], + [ + -0.35, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 57, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 3.296141856590062 + ], + [ + 0.0, + 0.0, + 3.4506910595903304 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 58, + "start_angle_index": 8, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.04981, + -0.00236, + 3.23645807854959 + ], + [ + -0.09917, + -0.00944, + 3.331323503509387 + ], + [ + -0.14765, + -0.02115, + 3.4261889284691835 + ], + [ + -0.19479, + -0.03741, + 3.52105435342898 + ], + [ + -0.24018, + -0.05805, + 3.615919778388777 + ], + [ + -0.28341, + -0.08291, + 3.710785203348574 + ], + [ + -0.3241, + -0.11175, + 3.8056506283083706 + ], + [ + -0.36187, + -0.14431, + 3.9005160532681673 + ], + [ + -0.39638, + -0.1803, + 3.995381478227964 + ], + [ + -0.42733, + -0.2194, + 4.090246903187761 + ], + [ + -0.45444, + -0.26126, + 4.185112328147557 + ], + [ + -0.47769, + -0.30538, + 4.2487413713838835 + ], + [ + -0.5, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 59, + "start_angle_index": 8, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.04981, + 0.00236, + 3.0467272286299965 + ], + [ + -0.09917, + 0.00944, + 2.9518618036701993 + ], + [ + -0.14765, + 0.02115, + 2.8569963787104027 + ], + [ + -0.19479, + 0.03741, + 2.762130953750606 + ], + [ + -0.24018, + 0.05805, + 2.6672655287908094 + ], + [ + -0.28341, + 0.08291, + 2.5724001038310123 + ], + [ + -0.3241, + 0.11175, + 2.4775346788712156 + ], + [ + -0.36187, + 0.14431, + 2.382669253911419 + ], + [ + -0.39638, + 0.1803, + 2.2878038289516223 + ], + [ + -0.42733, + 0.2194, + 2.1929384039918256 + ], + [ + -0.45444, + 0.26126, + 2.098072979032029 + ], + [ + -0.47769, + 0.30538, + 2.0344439357957027 + ], + [ + -0.5, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 60, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.05254, + 0.00218, + 3.058808506349613 + ], + [ + -0.10472, + 0.00869, + 2.976024359109432 + ], + [ + -0.15619, + 0.0195, + 2.893240211869252 + ], + [ + -0.20658, + 0.03452, + 2.8104560646290713 + ], + [ + -0.25556, + 0.05366, + 2.727671917388891 + ], + [ + -0.30295, + 0.07648, + 2.677945044588987 + ], + [ + -0.35, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 61, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 62, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + -0.05, + 0.0, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 3.141592653589793 + ], + [ + -0.15, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 63, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + -0.025, + 3.6052402625905993 + ], + [ + -0.1, + -0.05, + 3.6052402625905993 + ], + [ + -0.15, + -0.075, + 3.6052402625905993 + ], + [ + -0.2, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 64, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.04409, + -0.0241, + 3.678189359241699 + ], + [ + -0.08631, + -0.05134, + 3.751138455892799 + ], + [ + -0.12643, + -0.08159, + 3.8240875525438986 + ], + [ + -0.16424, + -0.11469, + 3.8970366491949986 + ], + [ + -0.2, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 65, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 66, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.04747, + -0.02076, + 3.5022074605904203 + ], + [ + -0.09683, + -0.03652, + 3.399174658590241 + ], + [ + -0.14755, + -0.04712, + 3.296141856590062 + ], + [ + -0.19909, + -0.05245, + 3.1931090545898826 + ], + [ + -0.25091, + -0.05245, + 3.0900762525897036 + ], + [ + -0.30245, + -0.04712, + 2.9870434505895243 + ], + [ + -0.35317, + -0.03652, + 2.8840106485893453 + ], + [ + -0.40253, + -0.02076, + 2.780977846589166 + ], + [ + -0.45, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 67, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.04729, + -0.0212, + 3.5209406973177253 + ], + [ + -0.09619, + -0.03835, + 3.4366411320448518 + ], + [ + -0.14636, + -0.05131, + 3.352341566771978 + ], + [ + -0.19745, + -0.06001, + 3.268042001499104 + ], + [ + -0.24909, + -0.06437, + 3.18374243622623 + ], + [ + -0.30091, + -0.06437, + 3.0994428709533564 + ], + [ + -0.35255, + -0.06001, + 3.0151433056804824 + ], + [ + -0.40364, + -0.05131, + 2.9308437404076084 + ], + [ + -0.45381, + -0.03835, + 2.8465441751347345 + ], + [ + -0.50271, + -0.0212, + 2.762244609861861 + ], + [ + -0.55, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 68, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.04705, + -0.02352, + 3.6052402625905993 + ], + [ + -0.09444, + -0.04634, + 3.555513389790695 + ], + [ + -0.14342, + -0.06548, + 3.472729242550515 + ], + [ + -0.19381, + -0.0805, + 3.3899450953103343 + ], + [ + -0.24528, + -0.09131, + 3.307160948070154 + ], + [ + -0.29746, + -0.09782, + 3.2243768008299734 + ], + [ + -0.35, + -0.1, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 69, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.373416458090196 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 70, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03576, + -0.03531, + 3.8970366491949986 + ], + [ + -0.07357, + -0.06841, + 3.8240875525438986 + ], + [ + -0.11369, + -0.09866, + 3.751138455892799 + ], + [ + -0.15591, + -0.1259, + 3.678189359241699 + ], + [ + -0.2, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 71, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 72, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + -0.0375, + 3.9269908169872414 + ], + [ + -0.075, + -0.075, + 3.9269908169872414 + ], + [ + -0.1125, + -0.1125, + 3.9269908169872414 + ], + [ + -0.15, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 73, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03531, + -0.03576, + 3.956944984779484 + ], + [ + -0.06841, + -0.07357, + 4.029894081430584 + ], + [ + -0.09866, + -0.11369, + 4.102843178081684 + ], + [ + -0.1259, + -0.15591, + 4.175792274732784 + ], + [ + -0.15, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 74, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 4.0878660941855625 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 75, + "start_angle_index": 10, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03288, + -0.03617, + 4.022473509407077 + ], + [ + -0.06215, + -0.07532, + 4.117956201826914 + ], + [ + -0.08756, + -0.11707, + 4.2134388942467496 + ], + [ + -0.10888, + -0.16106, + 4.308921586666585 + ], + [ + -0.1259, + -0.20688, + 4.404404279086421 + ], + [ + -0.13848, + -0.25412, + 4.499886971506257 + ], + [ + -0.1465, + -0.30234, + 4.595369663926093 + ], + [ + -0.14988, + -0.3511, + 4.690852356345929 + ], + [ + -0.15, + -0.4, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 76, + "start_angle_index": 10, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03617, + -0.03288, + 3.8315081245674056 + ], + [ + -0.07532, + -0.06215, + 3.7360254321475694 + ], + [ + -0.11707, + -0.08756, + 3.6405427397277337 + ], + [ + -0.16106, + -0.10888, + 3.5450600473078975 + ], + [ + -0.20688, + -0.1259, + 3.4495773548880617 + ], + [ + -0.25412, + -0.13848, + 3.354094662468226 + ], + [ + -0.30234, + -0.1465, + 3.2586119700483898 + ], + [ + -0.3511, + -0.14988, + 3.1631292776285536 + ], + [ + -0.4, + -0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 77, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.0241, + -0.04409, + 4.175792274732784 + ], + [ + -0.05134, + -0.08631, + 4.102843178081684 + ], + [ + -0.08159, + -0.12643, + 4.029894081430584 + ], + [ + -0.11469, + -0.16424, + 3.956944984779484 + ], + [ + -0.15, + -0.2, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 78, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.141491186585003 + ], + [ + 0.0, + 0.0, + 4.034241001786122 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 79, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + -0.05, + 4.2487413713838835 + ], + [ + -0.05, + -0.1, + 4.2487413713838835 + ], + [ + -0.075, + -0.15, + 4.2487413713838835 + ], + [ + -0.1, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 80, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.02352, + -0.04705, + 4.2487413713838835 + ], + [ + -0.04634, + -0.09444, + 4.298468244183788 + ], + [ + -0.06548, + -0.14342, + 4.381252391423968 + ], + [ + -0.0805, + -0.19381, + 4.464036538664148 + ], + [ + -0.09131, + -0.24528, + 4.546820685904329 + ], + [ + -0.09782, + -0.29746, + 4.629604833144509 + ], + [ + -0.1, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 81, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 82, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.02076, + -0.04747, + 4.3517741733840625 + ], + [ + -0.03652, + -0.09683, + 4.454806975384242 + ], + [ + -0.04712, + -0.14755, + 4.557839777384421 + ], + [ + -0.05245, + -0.19909, + 4.6608725793846 + ], + [ + -0.05245, + -0.25091, + 4.763905381384779 + ], + [ + -0.04712, + -0.30245, + 4.866938183384958 + ], + [ + -0.03652, + -0.35317, + 4.969970985385137 + ], + [ + -0.02076, + -0.40253, + 5.073003787385317 + ], + [ + 0.0, + -0.45, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 83, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.0212, + -0.04729, + 4.333040936656757 + ], + [ + -0.03835, + -0.09619, + 4.4173405019296315 + ], + [ + -0.05131, + -0.14636, + 4.501640067202505 + ], + [ + -0.06001, + -0.19745, + 4.5859396324753785 + ], + [ + -0.06437, + -0.24909, + 4.670239197748253 + ], + [ + -0.06437, + -0.30091, + 4.754538763021126 + ], + [ + -0.06001, + -0.35255, + 4.838838328294001 + ], + [ + -0.05131, + -0.40364, + 4.923137893566874 + ], + [ + -0.03835, + -0.45381, + 5.007437458839748 + ], + [ + -0.0212, + -0.50271, + 5.091737024112621 + ], + [ + 0.0, + -0.55, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 84, + "start_angle_index": 12, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.00236, + -0.04981, + 4.617523555424893 + ], + [ + -0.00944, + -0.09917, + 4.522658130465096 + ], + [ + -0.02115, + -0.14765, + 4.427792705505299 + ], + [ + -0.03741, + -0.19479, + 4.332927280545503 + ], + [ + -0.05805, + -0.24018, + 4.2380618555857055 + ], + [ + -0.08291, + -0.28341, + 4.143196430625909 + ], + [ + -0.11175, + -0.3241, + 4.048331005666112 + ], + [ + -0.14431, + -0.36187, + 3.9534655807063155 + ], + [ + -0.1803, + -0.39638, + 3.858600155746519 + ], + [ + -0.2194, + -0.42733, + 3.763734730786722 + ], + [ + -0.26126, + -0.45444, + 3.6688693058269255 + ], + [ + -0.30538, + -0.47769, + 3.6052402625905993 + ], + [ + -0.35, + -0.5, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 85, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.00218, + -0.05254, + 4.629604833144509 + ], + [ + -0.00869, + -0.10472, + 4.546820685904329 + ], + [ + -0.0195, + -0.15619, + 4.464036538664148 + ], + [ + -0.03452, + -0.20658, + 4.381252391423968 + ], + [ + -0.05366, + -0.25556, + 4.298468244183788 + ], + [ + -0.07648, + -0.30295, + 4.2487413713838835 + ], + [ + -0.1, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 86, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 87, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 4.71238898038469 + ], + [ + 0.0, + -0.15, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 88, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.00218, + -0.05254, + 4.79517312762487 + ], + [ + 0.00869, + -0.10472, + 4.87795727486505 + ], + [ + 0.0195, + -0.15619, + 4.960741422105231 + ], + [ + 0.03452, + -0.20658, + 5.0435255693454115 + ], + [ + 0.05366, + -0.25556, + 5.126309716585592 + ], + [ + 0.07648, + -0.30295, + 5.176036589385496 + ], + [ + 0.1, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 89, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.866938183384958 + ], + [ + 0.0, + 0.0, + 5.021487386385227 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 90, + "start_angle_index": 12, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.00236, + -0.04981, + 4.807254405344486 + ], + [ + 0.00944, + -0.09917, + 4.902119830304283 + ], + [ + 0.02115, + -0.14765, + 4.9969852552640805 + ], + [ + 0.03741, + -0.19479, + 5.091850680223876 + ], + [ + 0.05805, + -0.24018, + 5.186716105183674 + ], + [ + 0.08291, + -0.28341, + 5.2815815301434705 + ], + [ + 0.11175, + -0.3241, + 5.376446955103267 + ], + [ + 0.14431, + -0.36187, + 5.471312380063064 + ], + [ + 0.1803, + -0.39638, + 5.5661778050228605 + ], + [ + 0.2194, + -0.42733, + 5.661043229982657 + ], + [ + 0.26126, + -0.45444, + 5.755908654942454 + ], + [ + 0.30538, + -0.47769, + 5.81953769817878 + ], + [ + 0.35, + -0.5, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 91, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.02076, + -0.04747, + 5.073003787385317 + ], + [ + 0.03652, + -0.09683, + 4.969970985385137 + ], + [ + 0.04712, + -0.14755, + 4.866938183384958 + ], + [ + 0.05245, + -0.19909, + 4.763905381384779 + ], + [ + 0.05245, + -0.25091, + 4.6608725793846 + ], + [ + 0.04712, + -0.30245, + 4.557839777384421 + ], + [ + 0.03652, + -0.35317, + 4.454806975384242 + ], + [ + 0.02076, + -0.40253, + 4.3517741733840625 + ], + [ + 0.0, + -0.45, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 92, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.0212, + -0.04729, + 5.091737024112622 + ], + [ + 0.03835, + -0.09619, + 5.007437458839748 + ], + [ + 0.05131, + -0.14636, + 4.923137893566874 + ], + [ + 0.06001, + -0.19745, + 4.838838328294001 + ], + [ + 0.06437, + -0.24909, + 4.754538763021126 + ], + [ + 0.06437, + -0.30091, + 4.670239197748253 + ], + [ + 0.06001, + -0.35255, + 4.5859396324753785 + ], + [ + 0.05131, + -0.40364, + 4.501640067202505 + ], + [ + 0.03835, + -0.45381, + 4.4173405019296315 + ], + [ + 0.0212, + -0.50271, + 4.333040936656758 + ], + [ + 0.0, + -0.55, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 93, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.02352, + -0.04705, + 5.176036589385496 + ], + [ + 0.04634, + -0.09444, + 5.126309716585592 + ], + [ + 0.06548, + -0.14342, + 5.0435255693454115 + ], + [ + 0.0805, + -0.19381, + 4.960741422105231 + ], + [ + 0.09131, + -0.24528, + 4.87795727486505 + ], + [ + 0.09782, + -0.29746, + 4.79517312762487 + ], + [ + 0.1, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 94, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 4.944212784885092 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 95, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + -0.05, + 5.176036589385496 + ], + [ + 0.05, + -0.1, + 5.176036589385496 + ], + [ + 0.075, + -0.15, + 5.176036589385496 + ], + [ + 0.1, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 96, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.0241, + -0.04409, + 5.248985686036596 + ], + [ + 0.05134, + -0.08631, + 5.321934782687696 + ], + [ + 0.08159, + -0.12643, + 5.394883879338796 + ], + [ + 0.11469, + -0.16424, + 5.467832975989895 + ], + [ + 0.15, + -0.2, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 97, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 98, + "start_angle_index": 14, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03288, + -0.03617, + 5.402304451362302 + ], + [ + 0.06215, + -0.07532, + 5.306821758942466 + ], + [ + 0.08756, + -0.11707, + 5.21133906652263 + ], + [ + 0.10888, + -0.16106, + 5.115856374102794 + ], + [ + 0.1259, + -0.20688, + 5.020373681682958 + ], + [ + 0.13848, + -0.25412, + 4.9248909892631225 + ], + [ + 0.1465, + -0.30234, + 4.829408296843287 + ], + [ + 0.14988, + -0.3511, + 4.73392560442345 + ], + [ + 0.15, + -0.4, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 99, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03531, + -0.03576, + 5.467832975989895 + ], + [ + 0.06841, + -0.07357, + 5.394883879338796 + ], + [ + 0.09866, + -0.11369, + 5.321934782687696 + ], + [ + 0.1259, + -0.15591, + 5.248985686036596 + ], + [ + 0.15, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 100, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 101, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + -0.0375, + 5.497787143782138 + ], + [ + 0.075, + -0.075, + 5.497787143782138 + ], + [ + 0.1125, + -0.1125, + 5.497787143782138 + ], + [ + 0.15, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 102, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03576, + -0.03531, + 5.527741311574381 + ], + [ + 0.07357, + -0.06841, + 5.60069040822548 + ], + [ + 0.11369, + -0.09866, + 5.67363950487658 + ], + [ + 0.15591, + -0.1259, + 5.74658860152768 + ], + [ + 0.2, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 103, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.658662420980459 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 104, + "start_angle_index": 14, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03617, + -0.03288, + 5.593269836201974 + ], + [ + 0.07532, + -0.06215, + 5.6887525286218095 + ], + [ + 0.11707, + -0.08756, + 5.784235221041646 + ], + [ + 0.16106, + -0.10888, + 5.879717913461482 + ], + [ + 0.20688, + -0.1259, + 5.975200605881318 + ], + [ + 0.25412, + -0.13848, + 6.070683298301153 + ], + [ + 0.30234, + -0.1465, + 6.166165990720989 + ], + [ + 0.3511, + -0.14988, + 6.261648683140826 + ], + [ + 0.4, + -0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 105, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.04409, + -0.0241, + 5.74658860152768 + ], + [ + 0.08631, + -0.05134, + 5.67363950487658 + ], + [ + 0.12643, + -0.08159, + 5.60069040822548 + ], + [ + 0.16424, + -0.11469, + 5.527741311574381 + ], + [ + 0.2, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 106, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.712287513379899 + ], + [ + 0.0, + 0.0, + 5.605037328581019 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 107, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + -0.025, + 5.81953769817878 + ], + [ + 0.1, + -0.05, + 5.81953769817878 + ], + [ + 0.15, + -0.075, + 5.81953769817878 + ], + [ + 0.2, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 108, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.04705, + -0.02352, + 5.81953769817878 + ], + [ + 0.09444, + -0.04634, + 5.869264570978684 + ], + [ + 0.14342, + -0.06548, + 5.952048718218864 + ], + [ + 0.19381, + -0.0805, + 6.034832865459045 + ], + [ + 0.24528, + -0.09131, + 6.117617012699226 + ], + [ + 0.29746, + -0.09782, + 6.200401159939406 + ], + [ + 0.35, + -0.1, + 0.0 + ] + ] + }, + { + "trajectory_id": 109, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 110, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.04747, + -0.02076, + 5.922570500178959 + ], + [ + 0.09683, + -0.03652, + 6.025603302179139 + ], + [ + 0.14755, + -0.04712, + 6.128636104179318 + ], + [ + 0.19909, + -0.05245, + 6.231668906179497 + ], + [ + 0.25091, + -0.05245, + 0.05151640100008953 + ], + [ + 0.30245, + -0.04712, + 0.1545492030002687 + ], + [ + 0.35317, + -0.03652, + 0.25758200500044787 + ], + [ + 0.40253, + -0.02076, + 0.36061480700062715 + ], + [ + 0.45, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 111, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.04729, + -0.0212, + 5.903837263451654 + ], + [ + 0.09619, + -0.03835, + 5.988136828724528 + ], + [ + 0.14636, + -0.05131, + 6.0724363939974015 + ], + [ + 0.19745, + -0.06001, + 6.156735959270275 + ], + [ + 0.24909, + -0.06437, + 6.2410355245431495 + ], + [ + 0.30091, + -0.06437, + 0.04214978263643693 + ], + [ + 0.35255, + -0.06001, + 0.1264493479093109 + ], + [ + 0.40364, + -0.05131, + 0.21074891318218475 + ], + [ + 0.45381, + -0.03835, + 0.2950484784550586 + ], + [ + 0.50271, + -0.0212, + 0.37934804372793235 + ], + [ + 0.55, + 0.0, + 0.4636476090008061 + ] + ] + } + ] +} \ No newline at end of file diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json new file mode 100644 index 0000000000..8ea504f6c3 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json @@ -0,0 +1,5860 @@ +{ + "version": 1.0, + "date_generated": "2022-03-17", + "lattice_metadata": { + "motion_model": "omni", + "turning_radius": 0.5, + "grid_resolution": 0.05, + "stopping_threshold": 5, + "num_of_headings": 16, + "heading_angles": [ + 0.0, + 0.4636476090008061, + 0.7853981633974483, + 1.1071487177940904, + 1.5707963267948966, + 2.0344439357957027, + 2.356194490192345, + 2.677945044588987, + 3.141592653589793, + 3.6052402625905993, + 3.9269908169872414, + 4.2487413713838835, + 4.71238898038469, + 5.176036589385496, + 5.497787143782138, + 5.81953769817878 + ], + "number_of_trajectories": 144 + }, + "primitives": [ + { + "trajectory_id": 0, + "start_angle_index": 0, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.04981, + -0.00236, + 6.18831988221979 + ], + [ + 0.09917, + -0.00944, + 6.093454457259993 + ], + [ + 0.14765, + -0.02115, + 5.998589032300196 + ], + [ + 0.19479, + -0.03741, + 5.903723607340399 + ], + [ + 0.24018, + -0.05805, + 5.808858182380602 + ], + [ + 0.28341, + -0.08291, + 5.713992757420805 + ], + [ + 0.3241, + -0.11175, + 5.619127332461009 + ], + [ + 0.36187, + -0.14431, + 5.524261907501212 + ], + [ + 0.39638, + -0.1803, + 5.429396482541415 + ], + [ + 0.42733, + -0.2194, + 5.334531057581619 + ], + [ + 0.45444, + -0.26126, + 5.239665632621822 + ], + [ + 0.47769, + -0.30538, + 5.176036589385496 + ], + [ + 0.5, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 1, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.05254, + -0.00218, + 6.200401159939406 + ], + [ + 0.10472, + -0.00869, + 6.117617012699226 + ], + [ + 0.15619, + -0.0195, + 6.034832865459045 + ], + [ + 0.20658, + -0.03452, + 5.952048718218864 + ], + [ + 0.25556, + -0.05366, + 5.869264570978684 + ], + [ + 0.30295, + -0.07648, + 5.81953769817878 + ], + [ + 0.35, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 2, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 3, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.05, + 0.0, + 0.0 + ], + [ + 0.1, + 0.0, + 0.0 + ], + [ + 0.15, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 4, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + 0.05, + 0.0 + ], + [ + 0.0, + 0.1, + 0.0 + ], + [ + 0.0, + 0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 5, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.45, + "arc_length": 0.0, + "straight_length": 0.45, + "poses": [ + [ + 0.0, + -0.05, + 0.0 + ], + [ + 0.0, + -0.1, + 0.0 + ], + [ + 0.0, + -0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 6, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.05254, + 0.00218, + 0.0827841472401804 + ], + [ + 0.10472, + 0.00869, + 0.1655682944803608 + ], + [ + 0.15619, + 0.0195, + 0.2483524417205412 + ], + [ + 0.20658, + 0.03452, + 0.3311365889607216 + ], + [ + 0.25556, + 0.05366, + 0.413920736200902 + ], + [ + 0.30295, + 0.07648, + 0.4636476090008061 + ], + [ + 0.35, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 7, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.1545492030002687 + ], + [ + 0.0, + 0.0, + 0.3090984060005374 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 8, + "start_angle_index": 0, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.04981, + 0.00236, + 0.0948654249597968 + ], + [ + 0.09917, + 0.00944, + 0.1897308499195936 + ], + [ + 0.14765, + 0.02115, + 0.2845962748793904 + ], + [ + 0.19479, + 0.03741, + 0.3794616998391872 + ], + [ + 0.24018, + 0.05805, + 0.4743271247989839 + ], + [ + 0.28341, + 0.08291, + 0.5691925497587808 + ], + [ + 0.3241, + 0.11175, + 0.6640579747185776 + ], + [ + 0.36187, + 0.14431, + 0.7589233996783744 + ], + [ + 0.39638, + 0.1803, + 0.8537888246381711 + ], + [ + 0.42733, + 0.2194, + 0.9486542495979677 + ], + [ + 0.45444, + 0.26126, + 1.0435196745577644 + ], + [ + 0.47769, + 0.30538, + 1.1071487177940904 + ], + [ + 0.5, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 9, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.04747, + 0.02076, + 0.360614807000627 + ], + [ + 0.09683, + 0.03652, + 0.2575820050004478 + ], + [ + 0.14755, + 0.04712, + 0.15454920300026875 + ], + [ + 0.19909, + 0.05245, + 0.051516401000089584 + ], + [ + 0.25091, + 0.05245, + 6.231668906179497 + ], + [ + 0.30245, + 0.04712, + 6.128636104179318 + ], + [ + 0.35317, + 0.03652, + 6.025603302179138 + ], + [ + 0.40253, + 0.02076, + 5.922570500178959 + ], + [ + 0.45, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 10, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.04729, + 0.0212, + 0.37934804372793224 + ], + [ + 0.09619, + 0.03835, + 0.29504847845505844 + ], + [ + 0.14636, + 0.05131, + 0.21074891318218458 + ], + [ + 0.19745, + 0.06001, + 0.12644934790931073 + ], + [ + 0.24909, + 0.06437, + 0.04214978263643687 + ], + [ + 0.30091, + 0.06437, + 6.2410355245431495 + ], + [ + 0.35255, + 0.06001, + 6.156735959270275 + ], + [ + 0.40364, + 0.05131, + 6.0724363939974015 + ], + [ + 0.45381, + 0.03835, + 5.988136828724528 + ], + [ + 0.50271, + 0.0212, + 5.903837263451654 + ], + [ + 0.55, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 11, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.04705, + 0.02352, + 0.4636476090008061 + ], + [ + 0.09444, + 0.04634, + 0.413920736200902 + ], + [ + 0.14342, + 0.06548, + 0.3311365889607216 + ], + [ + 0.19381, + 0.0805, + 0.24835244172054122 + ], + [ + 0.24528, + 0.09131, + 0.16556829448036087 + ], + [ + 0.29746, + 0.09782, + 0.08278414724018052 + ], + [ + 0.35, + 0.1, + 0.0 + ] + ] + }, + { + "trajectory_id": 12, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.23182380450040305 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 13, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + 0.025, + 0.4636476090008061 + ], + [ + 0.1, + 0.05, + 0.4636476090008061 + ], + [ + 0.15, + 0.075, + 0.4636476090008061 + ], + [ + 0.2, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 14, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + 0.05, + 0.4636476090008061 + ], + [ + -0.05, + 0.1, + 0.4636476090008061 + ], + [ + -0.075, + 0.15, + 0.4636476090008061 + ], + [ + -0.1, + 0.2, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 15, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.67082, + "arc_length": 0.0, + "straight_length": 0.67082, + "poses": [ + [ + 0.025, + -0.05, + 0.4636476090008061 + ], + [ + 0.05, + -0.1, + 0.4636476090008061 + ], + [ + 0.075, + -0.15, + 0.4636476090008061 + ], + [ + 0.1, + -0.2, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 16, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.04409, + 0.0241, + 0.536596705651906 + ], + [ + 0.08631, + 0.05134, + 0.6095458023030058 + ], + [ + 0.12643, + 0.08159, + 0.6824948989541056 + ], + [ + 0.16424, + 0.11469, + 0.7554439956052055 + ], + [ + 0.2, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 17, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 18, + "start_angle_index": 2, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03617, + 0.03288, + 0.6899154709776123 + ], + [ + 0.07532, + 0.06215, + 0.5944327785577764 + ], + [ + 0.11707, + 0.08756, + 0.4989500861379405 + ], + [ + 0.16106, + 0.10888, + 0.4034673937181046 + ], + [ + 0.20688, + 0.1259, + 0.30798470129826866 + ], + [ + 0.25412, + 0.13848, + 0.21250200887843262 + ], + [ + 0.30234, + 0.1465, + 0.11701931645859664 + ], + [ + 0.3511, + 0.14988, + 0.021536624038760666 + ], + [ + 0.4, + 0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 19, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03576, + 0.03531, + 0.7554439956052055 + ], + [ + 0.07357, + 0.06841, + 0.6824948989541056 + ], + [ + 0.11369, + 0.09866, + 0.6095458023030058 + ], + [ + 0.15591, + 0.1259, + 0.5365967056519059 + ], + [ + 0.2, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 20, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 21, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + 0.0375, + 0.7853981633974483 + ], + [ + 0.075, + 0.075, + 0.7853981633974483 + ], + [ + 0.1125, + 0.1125, + 0.7853981633974483 + ], + [ + 0.15, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 22, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + 0.0375, + 0.7853981633974483 + ], + [ + -0.075, + 0.075, + 0.7853981633974483 + ], + [ + -0.1125, + 0.1125, + 0.7853981633974483 + ], + [ + -0.15, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 23, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.6364, + "arc_length": 0.0, + "straight_length": 0.6364, + "poses": [ + [ + 0.0375, + -0.0375, + 0.7853981633974483 + ], + [ + 0.075, + -0.075, + 0.7853981633974483 + ], + [ + 0.1125, + -0.1125, + 0.7853981633974483 + ], + [ + 0.15, + -0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 24, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03531, + 0.03576, + 0.8153523311896911 + ], + [ + 0.06841, + 0.07357, + 0.8883014278407909 + ], + [ + 0.09866, + 0.11369, + 0.9612505244918907 + ], + [ + 0.1259, + 0.15591, + 1.0341996211429907 + ], + [ + 0.15, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 25, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.9462734405957693 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 26, + "start_angle_index": 2, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03288, + 0.03617, + 0.8808808558172843 + ], + [ + 0.06215, + 0.07532, + 0.9763635482371201 + ], + [ + 0.08756, + 0.11707, + 1.071846240656956 + ], + [ + 0.10888, + 0.16106, + 1.167328933076792 + ], + [ + 0.1259, + 0.20688, + 1.262811625496628 + ], + [ + 0.13848, + 0.25412, + 1.358294317916464 + ], + [ + 0.1465, + 0.30234, + 1.4537770103363 + ], + [ + 0.14988, + 0.3511, + 1.549259702756136 + ], + [ + 0.15, + 0.4, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 27, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.0241, + 0.04409, + 1.0341996211429905 + ], + [ + 0.05134, + 0.08631, + 0.9612505244918907 + ], + [ + 0.08159, + 0.12643, + 0.8883014278407909 + ], + [ + 0.11469, + 0.16424, + 0.8153523311896911 + ], + [ + 0.15, + 0.2, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 28, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 0.9998985329952097 + ], + [ + 0.0, + 0.0, + 0.892648348196329 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 29, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + 0.05, + 1.1071487177940904 + ], + [ + 0.05, + 0.1, + 1.1071487177940904 + ], + [ + 0.075, + 0.15, + 1.1071487177940904 + ], + [ + 0.1, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 30, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + 0.025, + 1.1071487177940904 + ], + [ + -0.1, + 0.05, + 1.1071487177940904 + ], + [ + -0.15, + 0.075, + 1.1071487177940904 + ], + [ + -0.2, + 0.1, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 31, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.67082, + "arc_length": 0.0, + "straight_length": 0.67082, + "poses": [ + [ + 0.05, + -0.025, + 1.1071487177940904 + ], + [ + 0.1, + -0.05, + 1.1071487177940904 + ], + [ + 0.15, + -0.075, + 1.1071487177940904 + ], + [ + 0.2, + -0.1, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 32, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.02352, + 0.04705, + 1.1071487177940904 + ], + [ + 0.04634, + 0.09444, + 1.1568755905939945 + ], + [ + 0.06548, + 0.14342, + 1.239659737834175 + ], + [ + 0.0805, + 0.19381, + 1.3224438850743554 + ], + [ + 0.09131, + 0.24528, + 1.4052280323145356 + ], + [ + 0.09782, + 0.29746, + 1.488012179554716 + ], + [ + 0.1, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 33, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 34, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.02076, + 0.04747, + 1.2101815197942696 + ], + [ + 0.03652, + 0.09683, + 1.3132143217944487 + ], + [ + 0.04712, + 0.14755, + 1.4162471237946277 + ], + [ + 0.05245, + 0.19909, + 1.519279925794807 + ], + [ + 0.05245, + 0.25091, + 1.622312727794986 + ], + [ + 0.04712, + 0.30245, + 1.7253455297951654 + ], + [ + 0.03652, + 0.35317, + 1.8283783317953444 + ], + [ + 0.02076, + 0.40253, + 1.9314111337955238 + ], + [ + 0.0, + 0.45, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 35, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.0212, + 0.04729, + 1.1914482830669642 + ], + [ + 0.03835, + 0.09619, + 1.2757478483398381 + ], + [ + 0.05131, + 0.14636, + 1.3600474136127119 + ], + [ + 0.06001, + 0.19745, + 1.4443469788855858 + ], + [ + 0.06437, + 0.24909, + 1.5286465441584596 + ], + [ + 0.06437, + 0.30091, + 1.6129461094313333 + ], + [ + 0.06001, + 0.35255, + 1.6972456747042073 + ], + [ + 0.05131, + 0.40364, + 1.7815452399770813 + ], + [ + 0.03835, + 0.45381, + 1.8658448052499552 + ], + [ + 0.0212, + 0.50271, + 1.9501443705228287 + ], + [ + 0.0, + 0.55, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 36, + "start_angle_index": 4, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.00236, + 0.04981, + 1.4759309018350997 + ], + [ + 0.00944, + 0.09917, + 1.381065476875303 + ], + [ + 0.02115, + 0.14765, + 1.2862000519155061 + ], + [ + 0.03741, + 0.19479, + 1.1913346269557095 + ], + [ + 0.05805, + 0.24018, + 1.0964692019959126 + ], + [ + 0.08291, + 0.28341, + 1.0016037770361157 + ], + [ + 0.11175, + 0.3241, + 0.9067383520763189 + ], + [ + 0.14431, + 0.36187, + 0.8118729271165221 + ], + [ + 0.1803, + 0.39638, + 0.7170075021567255 + ], + [ + 0.2194, + 0.42733, + 0.6221420771969288 + ], + [ + 0.26126, + 0.45444, + 0.5272766522371322 + ], + [ + 0.30538, + 0.47769, + 0.4636476090008061 + ], + [ + 0.35, + 0.5, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 37, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.00218, + 0.05254, + 1.488012179554716 + ], + [ + 0.00869, + 0.10472, + 1.4052280323145356 + ], + [ + 0.0195, + 0.15619, + 1.3224438850743554 + ], + [ + 0.03452, + 0.20658, + 1.239659737834175 + ], + [ + 0.05366, + 0.25556, + 1.1568755905939945 + ], + [ + 0.07648, + 0.30295, + 1.1071487177940904 + ], + [ + 0.1, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 38, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 39, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 1.5707963267948966 + ], + [ + 0.0, + 0.15, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 40, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + -0.05, + 0.0, + 1.5707963267948966 + ], + [ + -0.1, + 0.0, + 1.5707963267948966 + ], + [ + -0.15, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 41, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.45, + "arc_length": 0.0, + "straight_length": 0.45, + "poses": [ + [ + 0.05, + 0.0, + 1.5707963267948966 + ], + [ + 0.1, + 0.0, + 1.5707963267948966 + ], + [ + 0.15, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 42, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.00218, + 0.05254, + 1.653580474035077 + ], + [ + -0.00869, + 0.10472, + 1.7363646212752575 + ], + [ + -0.0195, + 0.15619, + 1.8191487685154377 + ], + [ + -0.03452, + 0.20658, + 1.9019329157556182 + ], + [ + -0.05366, + 0.25556, + 1.9847170629957986 + ], + [ + -0.07648, + 0.30295, + 2.0344439357957027 + ], + [ + -0.1, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 43, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.7253455297951652 + ], + [ + 0.0, + 0.0, + 1.879894732795434 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 44, + "start_angle_index": 4, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.00236, + 0.04981, + 1.6656617517546934 + ], + [ + -0.00944, + 0.09917, + 1.76052717671449 + ], + [ + -0.02115, + 0.14765, + 1.855392601674287 + ], + [ + -0.03741, + 0.19479, + 1.9502580266340837 + ], + [ + -0.05805, + 0.24018, + 2.0451234515938803 + ], + [ + -0.08291, + 0.28341, + 2.1399888765536774 + ], + [ + -0.11175, + 0.3241, + 2.234854301513474 + ], + [ + -0.14431, + 0.36187, + 2.3297197264732707 + ], + [ + -0.1803, + 0.39638, + 2.4245851514330674 + ], + [ + -0.2194, + 0.42733, + 2.519450576392864 + ], + [ + -0.26126, + 0.45444, + 2.6143160013526607 + ], + [ + -0.30538, + 0.47769, + 2.677945044588987 + ], + [ + -0.35, + 0.5, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 45, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.02076, + 0.04747, + 1.9314111337955235 + ], + [ + -0.03652, + 0.09683, + 1.8283783317953444 + ], + [ + -0.04712, + 0.14755, + 1.7253455297951654 + ], + [ + -0.05245, + 0.19909, + 1.622312727794986 + ], + [ + -0.05245, + 0.25091, + 1.519279925794807 + ], + [ + -0.04712, + 0.30245, + 1.4162471237946277 + ], + [ + -0.03652, + 0.35317, + 1.3132143217944487 + ], + [ + -0.02076, + 0.40253, + 1.2101815197942694 + ], + [ + 0.0, + 0.45, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 46, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.0212, + 0.04729, + 1.950144370522829 + ], + [ + -0.03835, + 0.09619, + 1.865844805249955 + ], + [ + -0.05131, + 0.14636, + 1.7815452399770813 + ], + [ + -0.06001, + 0.19745, + 1.6972456747042073 + ], + [ + -0.06437, + 0.24909, + 1.6129461094313335 + ], + [ + -0.06437, + 0.30091, + 1.5286465441584598 + ], + [ + -0.06001, + 0.35255, + 1.4443469788855858 + ], + [ + -0.05131, + 0.40364, + 1.3600474136127119 + ], + [ + -0.03835, + 0.45381, + 1.275747848339838 + ], + [ + -0.0212, + 0.50271, + 1.1914482830669644 + ], + [ + 0.0, + 0.55, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 47, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.02352, + 0.04705, + 2.0344439357957027 + ], + [ + -0.04634, + 0.09444, + 1.9847170629957986 + ], + [ + -0.06548, + 0.14342, + 1.9019329157556182 + ], + [ + -0.0805, + 0.19381, + 1.8191487685154377 + ], + [ + -0.09131, + 0.24528, + 1.7363646212752575 + ], + [ + -0.09782, + 0.29746, + 1.653580474035077 + ], + [ + -0.1, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 48, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 1.8026201312952996 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 49, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + 0.05, + 2.0344439357957027 + ], + [ + -0.05, + 0.1, + 2.0344439357957027 + ], + [ + -0.075, + 0.15, + 2.0344439357957027 + ], + [ + -0.1, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 50, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + -0.025, + 2.0344439357957027 + ], + [ + -0.1, + -0.05, + 2.0344439357957027 + ], + [ + -0.15, + -0.075, + 2.0344439357957027 + ], + [ + -0.2, + -0.1, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 51, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.67082, + "arc_length": 0.0, + "straight_length": 0.67082, + "poses": [ + [ + 0.05, + 0.025, + 2.0344439357957027 + ], + [ + 0.1, + 0.05, + 2.0344439357957027 + ], + [ + 0.15, + 0.075, + 2.0344439357957027 + ], + [ + 0.2, + 0.1, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 52, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.0241, + 0.04409, + 2.1073930324468026 + ], + [ + -0.05134, + 0.08631, + 2.1803421290979026 + ], + [ + -0.08159, + 0.12643, + 2.253291225749002 + ], + [ + -0.11469, + 0.16424, + 2.326240322400102 + ], + [ + -0.15, + 0.2, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 53, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 54, + "start_angle_index": 6, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03288, + 0.03617, + 2.260711797772509 + ], + [ + -0.06215, + 0.07532, + 2.165229105352673 + ], + [ + -0.08756, + 0.11707, + 2.069746412932837 + ], + [ + -0.10888, + 0.16106, + 1.9742637205130011 + ], + [ + -0.1259, + 0.20688, + 1.8787810280931652 + ], + [ + -0.13848, + 0.25412, + 1.7832983356733292 + ], + [ + -0.1465, + 0.30234, + 1.6878156432534932 + ], + [ + -0.14988, + 0.3511, + 1.5923329508336572 + ], + [ + -0.15, + 0.4, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 55, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03531, + 0.03576, + 2.326240322400102 + ], + [ + -0.06841, + 0.07357, + 2.253291225749002 + ], + [ + -0.09866, + 0.11369, + 2.1803421290979026 + ], + [ + -0.1259, + 0.15591, + 2.1073930324468026 + ], + [ + -0.15, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 56, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 57, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + 0.0375, + 2.356194490192345 + ], + [ + -0.075, + 0.075, + 2.356194490192345 + ], + [ + -0.1125, + 0.1125, + 2.356194490192345 + ], + [ + -0.15, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 58, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + -0.0375, + 2.356194490192345 + ], + [ + -0.075, + -0.075, + 2.356194490192345 + ], + [ + -0.1125, + -0.1125, + 2.356194490192345 + ], + [ + -0.15, + -0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 59, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.6364, + "arc_length": 0.0, + "straight_length": 0.6364, + "poses": [ + [ + 0.0375, + 0.0375, + 2.356194490192345 + ], + [ + 0.075, + 0.075, + 2.356194490192345 + ], + [ + 0.1125, + 0.1125, + 2.356194490192345 + ], + [ + 0.15, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 60, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03576, + 0.03531, + 2.3861486579845876 + ], + [ + -0.07357, + 0.06841, + 2.4590977546356876 + ], + [ + -0.11369, + 0.09866, + 2.532046851286787 + ], + [ + -0.15591, + 0.1259, + 2.604995947937887 + ], + [ + -0.2, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 61, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.517069767390666 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 62, + "start_angle_index": 6, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03617, + 0.03288, + 2.4516771826121806 + ], + [ + -0.07532, + 0.06215, + 2.547159875032017 + ], + [ + -0.11707, + 0.08756, + 2.6426425674518526 + ], + [ + -0.16106, + 0.10888, + 2.7381252598716888 + ], + [ + -0.20688, + 0.1259, + 2.8336079522915245 + ], + [ + -0.25412, + 0.13848, + 2.9290906447113603 + ], + [ + -0.30234, + 0.1465, + 3.0245733371311965 + ], + [ + -0.3511, + 0.14988, + 3.1200560295510327 + ], + [ + -0.4, + 0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 63, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.04747, + 0.02076, + 2.780977846589166 + ], + [ + -0.09683, + 0.03652, + 2.8840106485893453 + ], + [ + -0.14755, + 0.04712, + 2.9870434505895243 + ], + [ + -0.19909, + 0.05245, + 3.0900762525897036 + ], + [ + -0.25091, + 0.05245, + 3.1931090545898826 + ], + [ + -0.30245, + 0.04712, + 3.296141856590062 + ], + [ + -0.35317, + 0.03652, + 3.399174658590241 + ], + [ + -0.40253, + 0.02076, + 3.5022074605904203 + ], + [ + -0.45, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 64, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.04729, + 0.0212, + 2.762244609861861 + ], + [ + -0.09619, + 0.03835, + 2.8465441751347345 + ], + [ + -0.14636, + 0.05131, + 2.9308437404076084 + ], + [ + -0.19745, + 0.06001, + 3.0151433056804824 + ], + [ + -0.24909, + 0.06437, + 3.0994428709533564 + ], + [ + -0.30091, + 0.06437, + 3.18374243622623 + ], + [ + -0.35255, + 0.06001, + 3.268042001499104 + ], + [ + -0.40364, + 0.05131, + 3.352341566771978 + ], + [ + -0.45381, + 0.03835, + 3.4366411320448518 + ], + [ + -0.50271, + 0.0212, + 3.5209406973177253 + ], + [ + -0.55, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 65, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.04409, + 0.0241, + 2.604995947937887 + ], + [ + -0.08631, + 0.05134, + 2.532046851286787 + ], + [ + -0.12643, + 0.08159, + 2.4590977546356876 + ], + [ + -0.16424, + 0.11469, + 2.3861486579845876 + ], + [ + -0.2, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 66, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.5706948597901063 + ], + [ + 0.0, + 0.0, + 2.4634446749912255 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 67, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + 0.025, + 2.677945044588987 + ], + [ + -0.1, + 0.05, + 2.677945044588987 + ], + [ + -0.15, + 0.075, + 2.677945044588987 + ], + [ + -0.2, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 68, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + -0.05, + 2.677945044588987 + ], + [ + -0.05, + -0.1, + 2.677945044588987 + ], + [ + -0.075, + -0.15, + 2.677945044588987 + ], + [ + -0.1, + -0.2, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 69, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.67082, + "arc_length": 0.0, + "straight_length": 0.67082, + "poses": [ + [ + 0.025, + 0.05, + 2.677945044588987 + ], + [ + 0.05, + 0.1, + 2.677945044588987 + ], + [ + 0.075, + 0.15, + 2.677945044588987 + ], + [ + 0.1, + 0.2, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 70, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.04705, + 0.02352, + 2.677945044588987 + ], + [ + -0.09444, + 0.04634, + 2.727671917388891 + ], + [ + -0.14342, + 0.06548, + 2.8104560646290713 + ], + [ + -0.19381, + 0.0805, + 2.893240211869252 + ], + [ + -0.24528, + 0.09131, + 2.976024359109432 + ], + [ + -0.29746, + 0.09782, + 3.058808506349613 + ], + [ + -0.35, + 0.1, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 71, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 72, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.05254, + -0.00218, + 3.2243768008299734 + ], + [ + -0.10472, + -0.00869, + 3.307160948070154 + ], + [ + -0.15619, + -0.0195, + 3.3899450953103343 + ], + [ + -0.20658, + -0.03452, + 3.472729242550515 + ], + [ + -0.25556, + -0.05366, + 3.555513389790695 + ], + [ + -0.30295, + -0.07648, + 3.6052402625905993 + ], + [ + -0.35, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 73, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 3.296141856590062 + ], + [ + 0.0, + 0.0, + 3.4506910595903304 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 74, + "start_angle_index": 8, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.04981, + -0.00236, + 3.23645807854959 + ], + [ + -0.09917, + -0.00944, + 3.331323503509387 + ], + [ + -0.14765, + -0.02115, + 3.4261889284691835 + ], + [ + -0.19479, + -0.03741, + 3.52105435342898 + ], + [ + -0.24018, + -0.05805, + 3.615919778388777 + ], + [ + -0.28341, + -0.08291, + 3.710785203348574 + ], + [ + -0.3241, + -0.11175, + 3.8056506283083706 + ], + [ + -0.36187, + -0.14431, + 3.9005160532681673 + ], + [ + -0.39638, + -0.1803, + 3.995381478227964 + ], + [ + -0.42733, + -0.2194, + 4.090246903187761 + ], + [ + -0.45444, + -0.26126, + 4.185112328147557 + ], + [ + -0.47769, + -0.30538, + 4.2487413713838835 + ], + [ + -0.5, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 75, + "start_angle_index": 8, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.04981, + 0.00236, + 3.0467272286299965 + ], + [ + -0.09917, + 0.00944, + 2.9518618036701993 + ], + [ + -0.14765, + 0.02115, + 2.8569963787104027 + ], + [ + -0.19479, + 0.03741, + 2.762130953750606 + ], + [ + -0.24018, + 0.05805, + 2.6672655287908094 + ], + [ + -0.28341, + 0.08291, + 2.5724001038310123 + ], + [ + -0.3241, + 0.11175, + 2.4775346788712156 + ], + [ + -0.36187, + 0.14431, + 2.382669253911419 + ], + [ + -0.39638, + 0.1803, + 2.2878038289516223 + ], + [ + -0.42733, + 0.2194, + 2.1929384039918256 + ], + [ + -0.45444, + 0.26126, + 2.098072979032029 + ], + [ + -0.47769, + 0.30538, + 2.0344439357957027 + ], + [ + -0.5, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 76, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.05254, + 0.00218, + 3.058808506349613 + ], + [ + -0.10472, + 0.00869, + 2.976024359109432 + ], + [ + -0.15619, + 0.0195, + 2.893240211869252 + ], + [ + -0.20658, + 0.03452, + 2.8104560646290713 + ], + [ + -0.25556, + 0.05366, + 2.727671917388891 + ], + [ + -0.30295, + 0.07648, + 2.677945044588987 + ], + [ + -0.35, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 77, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 78, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + -0.05, + 0.0, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 3.141592653589793 + ], + [ + -0.15, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 79, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + -0.05, + 3.141592653589793 + ], + [ + 0.0, + -0.1, + 3.141592653589793 + ], + [ + 0.0, + -0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 80, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.45, + "arc_length": 0.0, + "straight_length": 0.45, + "poses": [ + [ + 0.0, + 0.05, + 3.141592653589793 + ], + [ + 0.0, + 0.1, + 3.141592653589793 + ], + [ + 0.0, + 0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 81, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + -0.025, + 3.6052402625905993 + ], + [ + -0.1, + -0.05, + 3.6052402625905993 + ], + [ + -0.15, + -0.075, + 3.6052402625905993 + ], + [ + -0.2, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 82, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + -0.05, + 3.6052402625905993 + ], + [ + 0.05, + -0.1, + 3.6052402625905993 + ], + [ + 0.075, + -0.15, + 3.6052402625905993 + ], + [ + 0.1, + -0.2, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 83, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.67082, + "arc_length": 0.0, + "straight_length": 0.67082, + "poses": [ + [ + -0.025, + 0.05, + 3.6052402625905993 + ], + [ + -0.05, + 0.1, + 3.6052402625905993 + ], + [ + -0.075, + 0.15, + 3.6052402625905993 + ], + [ + -0.1, + 0.2, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 84, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.04409, + -0.0241, + 3.678189359241699 + ], + [ + -0.08631, + -0.05134, + 3.751138455892799 + ], + [ + -0.12643, + -0.08159, + 3.8240875525438986 + ], + [ + -0.16424, + -0.11469, + 3.8970366491949986 + ], + [ + -0.2, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 85, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 86, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.04747, + -0.02076, + 3.5022074605904203 + ], + [ + -0.09683, + -0.03652, + 3.399174658590241 + ], + [ + -0.14755, + -0.04712, + 3.296141856590062 + ], + [ + -0.19909, + -0.05245, + 3.1931090545898826 + ], + [ + -0.25091, + -0.05245, + 3.0900762525897036 + ], + [ + -0.30245, + -0.04712, + 2.9870434505895243 + ], + [ + -0.35317, + -0.03652, + 2.8840106485893453 + ], + [ + -0.40253, + -0.02076, + 2.780977846589166 + ], + [ + -0.45, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 87, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.04729, + -0.0212, + 3.5209406973177253 + ], + [ + -0.09619, + -0.03835, + 3.4366411320448518 + ], + [ + -0.14636, + -0.05131, + 3.352341566771978 + ], + [ + -0.19745, + -0.06001, + 3.268042001499104 + ], + [ + -0.24909, + -0.06437, + 3.18374243622623 + ], + [ + -0.30091, + -0.06437, + 3.0994428709533564 + ], + [ + -0.35255, + -0.06001, + 3.0151433056804824 + ], + [ + -0.40364, + -0.05131, + 2.9308437404076084 + ], + [ + -0.45381, + -0.03835, + 2.8465441751347345 + ], + [ + -0.50271, + -0.0212, + 2.762244609861861 + ], + [ + -0.55, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 88, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.04705, + -0.02352, + 3.6052402625905993 + ], + [ + -0.09444, + -0.04634, + 3.555513389790695 + ], + [ + -0.14342, + -0.06548, + 3.472729242550515 + ], + [ + -0.19381, + -0.0805, + 3.3899450953103343 + ], + [ + -0.24528, + -0.09131, + 3.307160948070154 + ], + [ + -0.29746, + -0.09782, + 3.2243768008299734 + ], + [ + -0.35, + -0.1, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 89, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.373416458090196 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 90, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03576, + -0.03531, + 3.8970366491949986 + ], + [ + -0.07357, + -0.06841, + 3.8240875525438986 + ], + [ + -0.11369, + -0.09866, + 3.751138455892799 + ], + [ + -0.15591, + -0.1259, + 3.678189359241699 + ], + [ + -0.2, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 91, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 92, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + -0.0375, + 3.9269908169872414 + ], + [ + -0.075, + -0.075, + 3.9269908169872414 + ], + [ + -0.1125, + -0.1125, + 3.9269908169872414 + ], + [ + -0.15, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 93, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + -0.0375, + 3.9269908169872414 + ], + [ + 0.075, + -0.075, + 3.9269908169872414 + ], + [ + 0.1125, + -0.1125, + 3.9269908169872414 + ], + [ + 0.15, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 94, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.6364, + "arc_length": 0.0, + "straight_length": 0.6364, + "poses": [ + [ + -0.0375, + 0.0375, + 3.9269908169872414 + ], + [ + -0.075, + 0.075, + 3.9269908169872414 + ], + [ + -0.1125, + 0.1125, + 3.9269908169872414 + ], + [ + -0.15, + 0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 95, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03531, + -0.03576, + 3.956944984779484 + ], + [ + -0.06841, + -0.07357, + 4.029894081430584 + ], + [ + -0.09866, + -0.11369, + 4.102843178081684 + ], + [ + -0.1259, + -0.15591, + 4.175792274732784 + ], + [ + -0.15, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 96, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 4.0878660941855625 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 97, + "start_angle_index": 10, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03288, + -0.03617, + 4.022473509407077 + ], + [ + -0.06215, + -0.07532, + 4.117956201826914 + ], + [ + -0.08756, + -0.11707, + 4.2134388942467496 + ], + [ + -0.10888, + -0.16106, + 4.308921586666585 + ], + [ + -0.1259, + -0.20688, + 4.404404279086421 + ], + [ + -0.13848, + -0.25412, + 4.499886971506257 + ], + [ + -0.1465, + -0.30234, + 4.595369663926093 + ], + [ + -0.14988, + -0.3511, + 4.690852356345929 + ], + [ + -0.15, + -0.4, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 98, + "start_angle_index": 10, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03617, + -0.03288, + 3.8315081245674056 + ], + [ + -0.07532, + -0.06215, + 3.7360254321475694 + ], + [ + -0.11707, + -0.08756, + 3.6405427397277337 + ], + [ + -0.16106, + -0.10888, + 3.5450600473078975 + ], + [ + -0.20688, + -0.1259, + 3.4495773548880617 + ], + [ + -0.25412, + -0.13848, + 3.354094662468226 + ], + [ + -0.30234, + -0.1465, + 3.2586119700483898 + ], + [ + -0.3511, + -0.14988, + 3.1631292776285536 + ], + [ + -0.4, + -0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 99, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.0241, + -0.04409, + 4.175792274732784 + ], + [ + -0.05134, + -0.08631, + 4.102843178081684 + ], + [ + -0.08159, + -0.12643, + 4.029894081430584 + ], + [ + -0.11469, + -0.16424, + 3.956944984779484 + ], + [ + -0.15, + -0.2, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 100, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.141491186585003 + ], + [ + 0.0, + 0.0, + 4.034241001786122 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 101, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + -0.05, + 4.2487413713838835 + ], + [ + -0.05, + -0.1, + 4.2487413713838835 + ], + [ + -0.075, + -0.15, + 4.2487413713838835 + ], + [ + -0.1, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 102, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + -0.025, + 4.2487413713838835 + ], + [ + 0.1, + -0.05, + 4.2487413713838835 + ], + [ + 0.15, + -0.075, + 4.2487413713838835 + ], + [ + 0.2, + -0.1, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 103, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.67082, + "arc_length": 0.0, + "straight_length": 0.67082, + "poses": [ + [ + -0.05, + 0.025, + 4.2487413713838835 + ], + [ + -0.1, + 0.05, + 4.2487413713838835 + ], + [ + -0.15, + 0.075, + 4.2487413713838835 + ], + [ + -0.2, + 0.1, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 104, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.02352, + -0.04705, + 4.2487413713838835 + ], + [ + -0.04634, + -0.09444, + 4.298468244183788 + ], + [ + -0.06548, + -0.14342, + 4.381252391423968 + ], + [ + -0.0805, + -0.19381, + 4.464036538664148 + ], + [ + -0.09131, + -0.24528, + 4.546820685904329 + ], + [ + -0.09782, + -0.29746, + 4.629604833144509 + ], + [ + -0.1, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 105, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 106, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.02076, + -0.04747, + 4.3517741733840625 + ], + [ + -0.03652, + -0.09683, + 4.454806975384242 + ], + [ + -0.04712, + -0.14755, + 4.557839777384421 + ], + [ + -0.05245, + -0.19909, + 4.6608725793846 + ], + [ + -0.05245, + -0.25091, + 4.763905381384779 + ], + [ + -0.04712, + -0.30245, + 4.866938183384958 + ], + [ + -0.03652, + -0.35317, + 4.969970985385137 + ], + [ + -0.02076, + -0.40253, + 5.073003787385317 + ], + [ + 0.0, + -0.45, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 107, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.0212, + -0.04729, + 4.333040936656757 + ], + [ + -0.03835, + -0.09619, + 4.4173405019296315 + ], + [ + -0.05131, + -0.14636, + 4.501640067202505 + ], + [ + -0.06001, + -0.19745, + 4.5859396324753785 + ], + [ + -0.06437, + -0.24909, + 4.670239197748253 + ], + [ + -0.06437, + -0.30091, + 4.754538763021126 + ], + [ + -0.06001, + -0.35255, + 4.838838328294001 + ], + [ + -0.05131, + -0.40364, + 4.923137893566874 + ], + [ + -0.03835, + -0.45381, + 5.007437458839748 + ], + [ + -0.0212, + -0.50271, + 5.091737024112621 + ], + [ + 0.0, + -0.55, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 108, + "start_angle_index": 12, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.00236, + -0.04981, + 4.617523555424893 + ], + [ + -0.00944, + -0.09917, + 4.522658130465096 + ], + [ + -0.02115, + -0.14765, + 4.427792705505299 + ], + [ + -0.03741, + -0.19479, + 4.332927280545503 + ], + [ + -0.05805, + -0.24018, + 4.2380618555857055 + ], + [ + -0.08291, + -0.28341, + 4.143196430625909 + ], + [ + -0.11175, + -0.3241, + 4.048331005666112 + ], + [ + -0.14431, + -0.36187, + 3.9534655807063155 + ], + [ + -0.1803, + -0.39638, + 3.858600155746519 + ], + [ + -0.2194, + -0.42733, + 3.763734730786722 + ], + [ + -0.26126, + -0.45444, + 3.6688693058269255 + ], + [ + -0.30538, + -0.47769, + 3.6052402625905993 + ], + [ + -0.35, + -0.5, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 109, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.00218, + -0.05254, + 4.629604833144509 + ], + [ + -0.00869, + -0.10472, + 4.546820685904329 + ], + [ + -0.0195, + -0.15619, + 4.464036538664148 + ], + [ + -0.03452, + -0.20658, + 4.381252391423968 + ], + [ + -0.05366, + -0.25556, + 4.298468244183788 + ], + [ + -0.07648, + -0.30295, + 4.2487413713838835 + ], + [ + -0.1, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 110, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 111, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 4.71238898038469 + ], + [ + 0.0, + -0.15, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 112, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.05, + 0.0, + 4.71238898038469 + ], + [ + 0.1, + 0.0, + 4.71238898038469 + ], + [ + 0.15, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 113, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.45, + "arc_length": 0.0, + "straight_length": 0.45, + "poses": [ + [ + -0.05, + 0.0, + 4.71238898038469 + ], + [ + -0.1, + 0.0, + 4.71238898038469 + ], + [ + -0.15, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 114, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.00218, + -0.05254, + 4.79517312762487 + ], + [ + 0.00869, + -0.10472, + 4.87795727486505 + ], + [ + 0.0195, + -0.15619, + 4.960741422105231 + ], + [ + 0.03452, + -0.20658, + 5.0435255693454115 + ], + [ + 0.05366, + -0.25556, + 5.126309716585592 + ], + [ + 0.07648, + -0.30295, + 5.176036589385496 + ], + [ + 0.1, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 115, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.866938183384958 + ], + [ + 0.0, + 0.0, + 5.021487386385227 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 116, + "start_angle_index": 12, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.00236, + -0.04981, + 4.807254405344486 + ], + [ + 0.00944, + -0.09917, + 4.902119830304283 + ], + [ + 0.02115, + -0.14765, + 4.9969852552640805 + ], + [ + 0.03741, + -0.19479, + 5.091850680223876 + ], + [ + 0.05805, + -0.24018, + 5.186716105183674 + ], + [ + 0.08291, + -0.28341, + 5.2815815301434705 + ], + [ + 0.11175, + -0.3241, + 5.376446955103267 + ], + [ + 0.14431, + -0.36187, + 5.471312380063064 + ], + [ + 0.1803, + -0.39638, + 5.5661778050228605 + ], + [ + 0.2194, + -0.42733, + 5.661043229982657 + ], + [ + 0.26126, + -0.45444, + 5.755908654942454 + ], + [ + 0.30538, + -0.47769, + 5.81953769817878 + ], + [ + 0.35, + -0.5, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 117, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.02076, + -0.04747, + 5.073003787385317 + ], + [ + 0.03652, + -0.09683, + 4.969970985385137 + ], + [ + 0.04712, + -0.14755, + 4.866938183384958 + ], + [ + 0.05245, + -0.19909, + 4.763905381384779 + ], + [ + 0.05245, + -0.25091, + 4.6608725793846 + ], + [ + 0.04712, + -0.30245, + 4.557839777384421 + ], + [ + 0.03652, + -0.35317, + 4.454806975384242 + ], + [ + 0.02076, + -0.40253, + 4.3517741733840625 + ], + [ + 0.0, + -0.45, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 118, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.0212, + -0.04729, + 5.091737024112622 + ], + [ + 0.03835, + -0.09619, + 5.007437458839748 + ], + [ + 0.05131, + -0.14636, + 4.923137893566874 + ], + [ + 0.06001, + -0.19745, + 4.838838328294001 + ], + [ + 0.06437, + -0.24909, + 4.754538763021126 + ], + [ + 0.06437, + -0.30091, + 4.670239197748253 + ], + [ + 0.06001, + -0.35255, + 4.5859396324753785 + ], + [ + 0.05131, + -0.40364, + 4.501640067202505 + ], + [ + 0.03835, + -0.45381, + 4.4173405019296315 + ], + [ + 0.0212, + -0.50271, + 4.333040936656758 + ], + [ + 0.0, + -0.55, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 119, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.02352, + -0.04705, + 5.176036589385496 + ], + [ + 0.04634, + -0.09444, + 5.126309716585592 + ], + [ + 0.06548, + -0.14342, + 5.0435255693454115 + ], + [ + 0.0805, + -0.19381, + 4.960741422105231 + ], + [ + 0.09131, + -0.24528, + 4.87795727486505 + ], + [ + 0.09782, + -0.29746, + 4.79517312762487 + ], + [ + 0.1, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 120, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 4.944212784885092 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 121, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + -0.05, + 5.176036589385496 + ], + [ + 0.05, + -0.1, + 5.176036589385496 + ], + [ + 0.075, + -0.15, + 5.176036589385496 + ], + [ + 0.1, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 122, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + 0.025, + 5.176036589385496 + ], + [ + 0.1, + 0.05, + 5.176036589385496 + ], + [ + 0.15, + 0.075, + 5.176036589385496 + ], + [ + 0.2, + 0.1, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 123, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.67082, + "arc_length": 0.0, + "straight_length": 0.67082, + "poses": [ + [ + -0.05, + -0.025, + 5.176036589385496 + ], + [ + -0.1, + -0.05, + 5.176036589385496 + ], + [ + -0.15, + -0.075, + 5.176036589385496 + ], + [ + -0.2, + -0.1, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 124, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.0241, + -0.04409, + 5.248985686036596 + ], + [ + 0.05134, + -0.08631, + 5.321934782687696 + ], + [ + 0.08159, + -0.12643, + 5.394883879338796 + ], + [ + 0.11469, + -0.16424, + 5.467832975989895 + ], + [ + 0.15, + -0.2, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 125, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 126, + "start_angle_index": 14, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03288, + -0.03617, + 5.402304451362302 + ], + [ + 0.06215, + -0.07532, + 5.306821758942466 + ], + [ + 0.08756, + -0.11707, + 5.21133906652263 + ], + [ + 0.10888, + -0.16106, + 5.115856374102794 + ], + [ + 0.1259, + -0.20688, + 5.020373681682958 + ], + [ + 0.13848, + -0.25412, + 4.9248909892631225 + ], + [ + 0.1465, + -0.30234, + 4.829408296843287 + ], + [ + 0.14988, + -0.3511, + 4.73392560442345 + ], + [ + 0.15, + -0.4, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 127, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03531, + -0.03576, + 5.467832975989895 + ], + [ + 0.06841, + -0.07357, + 5.394883879338796 + ], + [ + 0.09866, + -0.11369, + 5.321934782687696 + ], + [ + 0.1259, + -0.15591, + 5.248985686036596 + ], + [ + 0.15, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 128, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 129, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + -0.0375, + 5.497787143782138 + ], + [ + 0.075, + -0.075, + 5.497787143782138 + ], + [ + 0.1125, + -0.1125, + 5.497787143782138 + ], + [ + 0.15, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 130, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + 0.0375, + 5.497787143782138 + ], + [ + 0.075, + 0.075, + 5.497787143782138 + ], + [ + 0.1125, + 0.1125, + 5.497787143782138 + ], + [ + 0.15, + 0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 131, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.6364, + "arc_length": 0.0, + "straight_length": 0.6364, + "poses": [ + [ + -0.0375, + -0.0375, + 5.497787143782138 + ], + [ + -0.075, + -0.075, + 5.497787143782138 + ], + [ + -0.1125, + -0.1125, + 5.497787143782138 + ], + [ + -0.15, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 132, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03576, + -0.03531, + 5.527741311574381 + ], + [ + 0.07357, + -0.06841, + 5.60069040822548 + ], + [ + 0.11369, + -0.09866, + 5.67363950487658 + ], + [ + 0.15591, + -0.1259, + 5.74658860152768 + ], + [ + 0.2, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 133, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.658662420980459 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 134, + "start_angle_index": 14, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03617, + -0.03288, + 5.593269836201974 + ], + [ + 0.07532, + -0.06215, + 5.6887525286218095 + ], + [ + 0.11707, + -0.08756, + 5.784235221041646 + ], + [ + 0.16106, + -0.10888, + 5.879717913461482 + ], + [ + 0.20688, + -0.1259, + 5.975200605881318 + ], + [ + 0.25412, + -0.13848, + 6.070683298301153 + ], + [ + 0.30234, + -0.1465, + 6.166165990720989 + ], + [ + 0.3511, + -0.14988, + 6.261648683140826 + ], + [ + 0.4, + -0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 135, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.04409, + -0.0241, + 5.74658860152768 + ], + [ + 0.08631, + -0.05134, + 5.67363950487658 + ], + [ + 0.12643, + -0.08159, + 5.60069040822548 + ], + [ + 0.16424, + -0.11469, + 5.527741311574381 + ], + [ + 0.2, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 136, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.712287513379899 + ], + [ + 0.0, + 0.0, + 5.605037328581019 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 137, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + -0.025, + 5.81953769817878 + ], + [ + 0.1, + -0.05, + 5.81953769817878 + ], + [ + 0.15, + -0.075, + 5.81953769817878 + ], + [ + 0.2, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 138, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + 0.05, + 5.81953769817878 + ], + [ + 0.05, + 0.1, + 5.81953769817878 + ], + [ + 0.075, + 0.15, + 5.81953769817878 + ], + [ + 0.1, + 0.2, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 139, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.67082, + "arc_length": 0.0, + "straight_length": 0.67082, + "poses": [ + [ + -0.025, + -0.05, + 5.81953769817878 + ], + [ + -0.05, + -0.1, + 5.81953769817878 + ], + [ + -0.075, + -0.15, + 5.81953769817878 + ], + [ + -0.1, + -0.2, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 140, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.04705, + -0.02352, + 5.81953769817878 + ], + [ + 0.09444, + -0.04634, + 5.869264570978684 + ], + [ + 0.14342, + -0.06548, + 5.952048718218864 + ], + [ + 0.19381, + -0.0805, + 6.034832865459045 + ], + [ + 0.24528, + -0.09131, + 6.117617012699226 + ], + [ + 0.29746, + -0.09782, + 6.200401159939406 + ], + [ + 0.35, + -0.1, + 0.0 + ] + ] + }, + { + "trajectory_id": 141, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 142, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.04747, + -0.02076, + 5.922570500178959 + ], + [ + 0.09683, + -0.03652, + 6.025603302179139 + ], + [ + 0.14755, + -0.04712, + 6.128636104179318 + ], + [ + 0.19909, + -0.05245, + 6.231668906179497 + ], + [ + 0.25091, + -0.05245, + 0.05151640100008953 + ], + [ + 0.30245, + -0.04712, + 0.1545492030002687 + ], + [ + 0.35317, + -0.03652, + 0.25758200500044787 + ], + [ + 0.40253, + -0.02076, + 0.36061480700062715 + ], + [ + 0.45, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 143, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.04729, + -0.0212, + 5.903837263451654 + ], + [ + 0.09619, + -0.03835, + 5.988136828724528 + ], + [ + 0.14636, + -0.05131, + 6.0724363939974015 + ], + [ + 0.19745, + -0.06001, + 6.156735959270275 + ], + [ + 0.24909, + -0.06437, + 6.2410355245431495 + ], + [ + 0.30091, + -0.06437, + 0.04214978263643693 + ], + [ + 0.35255, + -0.06001, + 0.1264493479093109 + ], + [ + 0.40364, + -0.05131, + 0.21074891318218475 + ], + [ + 0.45381, + -0.03835, + 0.2950484784550586 + ], + [ + 0.50271, + -0.0212, + 0.37934804372793235 + ], + [ + 0.55, + 0.0, + 0.4636476090008061 + ] + ] + } + ] +} \ No newline at end of file diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json new file mode 100644 index 0000000000..646513c0aa --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json @@ -0,0 +1,4000 @@ +{ + "version": 1.0, + "date_generated": "2022-03-17", + "lattice_metadata": { + "motion_model": "ackermann", + "turning_radius": 1, + "grid_resolution": 0.05, + "stopping_threshold": 5, + "num_of_headings": 16, + "heading_angles": [ + 0.0, + 0.4636476090008061, + 0.7853981633974483, + 1.1071487177940904, + 1.5707963267948966, + 2.0344439357957027, + 2.356194490192345, + 2.677945044588987, + 3.141592653589793, + 3.6052402625905993, + 3.9269908169872414, + 4.2487413713838835, + 4.71238898038469, + 5.176036589385496, + 5.497787143782138, + 5.81953769817878 + ], + "number_of_trajectories": 64 + }, + "primitives": [ + { + "trajectory_id": 0, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04802, + -0.00109, + 6.237827154712447 + ], + [ + 0.09594, + -0.00435, + 6.192469002245307 + ], + [ + 0.14366, + -0.00979, + 6.1471108497781675 + ], + [ + 0.19109, + -0.01738, + 6.101752697311028 + ], + [ + 0.23812, + -0.02712, + 6.056394544843888 + ], + [ + 0.28467, + -0.03898, + 6.011036392376749 + ], + [ + 0.33063, + -0.05293, + 5.965678239909609 + ], + [ + 0.3759, + -0.06896, + 5.9203200874424695 + ], + [ + 0.42041, + -0.08702, + 5.87496193497533 + ], + [ + 0.46405, + -0.10708, + 5.82960378250819 + ], + [ + 0.50704, + -0.12852, + 5.81953769817878 + ], + [ + 0.55, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 1, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.05179, + -0.00049, + 6.256998923161878 + ], + [ + 0.10353, + -0.00279, + 6.220543821079953 + ], + [ + 0.15516, + -0.00697, + 6.184088718998028 + ], + [ + 0.2066, + -0.01303, + 6.147633616916103 + ], + [ + 0.25778, + -0.02097, + 6.111178514834179 + ], + [ + 0.30864, + -0.03076, + 6.074723412752253 + ], + [ + 0.3591, + -0.0424, + 6.038268310670329 + ], + [ + 0.40911, + -0.05587, + 6.001813208588404 + ], + [ + 0.4586, + -0.07116, + 5.965358106506479 + ], + [ + 0.5075, + -0.08824, + 5.928903004424555 + ], + [ + 0.55574, + -0.10709, + 5.89244790234263 + ], + [ + 0.60326, + -0.12769, + 5.855992800260705 + ], + [ + 0.65, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 2, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.05, + 0.0, + 0.0 + ], + [ + 0.1, + 0.0, + 0.0 + ], + [ + 0.15, + 0.0, + 0.0 + ], + [ + 0.2, + 0.0, + 0.0 + ], + [ + 0.25, + 0.0, + 0.0 + ], + [ + 0.3, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 3, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04802, + 0.00109, + 0.045358152467139604 + ], + [ + 0.09594, + 0.00435, + 0.09071630493427921 + ], + [ + 0.14366, + 0.00979, + 0.13607445740141882 + ], + [ + 0.19109, + 0.01738, + 0.18143260986855841 + ], + [ + 0.23812, + 0.02712, + 0.226790762335698 + ], + [ + 0.28467, + 0.03898, + 0.2721489148028376 + ], + [ + 0.33063, + 0.05293, + 0.3175070672699772 + ], + [ + 0.3759, + 0.06896, + 0.36286521973711683 + ], + [ + 0.42041, + 0.08702, + 0.4082233722042565 + ], + [ + 0.46405, + 0.10708, + 0.45358152467139606 + ], + [ + 0.50704, + 0.12852, + 0.4636476090008061 + ], + [ + 0.55, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 4, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.05179, + 0.00049, + 0.026186384017708383 + ], + [ + 0.10353, + 0.00279, + 0.0626414860996332 + ], + [ + 0.15516, + 0.00697, + 0.09909658818155803 + ], + [ + 0.2066, + 0.01303, + 0.13555169026348282 + ], + [ + 0.25778, + 0.02097, + 0.17200679234540764 + ], + [ + 0.30864, + 0.03076, + 0.20846189442733246 + ], + [ + 0.3591, + 0.0424, + 0.24491699650925727 + ], + [ + 0.40911, + 0.05587, + 0.28137209859118206 + ], + [ + 0.4586, + 0.07116, + 0.3178272006731068 + ], + [ + 0.5075, + 0.08824, + 0.35428230275503164 + ], + [ + 0.55574, + 0.10709, + 0.3907374048369565 + ], + [ + 0.60326, + 0.12769, + 0.4271925069188813 + ], + [ + 0.65, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 5, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04296, + 0.02148, + 0.4636476090008061 + ], + [ + 0.08595, + 0.04292, + 0.453581524671396 + ], + [ + 0.12959, + 0.06298, + 0.4082233722042564 + ], + [ + 0.1741, + 0.08104, + 0.36286521973711683 + ], + [ + 0.21937, + 0.09707, + 0.31750706726997724 + ], + [ + 0.26533, + 0.11102, + 0.27214891480283765 + ], + [ + 0.31188, + 0.12288, + 0.22679076233569806 + ], + [ + 0.35891, + 0.13262, + 0.18143260986855847 + ], + [ + 0.40634, + 0.14021, + 0.13607445740141882 + ], + [ + 0.45406, + 0.14565, + 0.09071630493427918 + ], + [ + 0.50198, + 0.14891, + 0.04535815246713959 + ], + [ + 0.55, + 0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 6, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.04286, + 0.02143, + 0.4636476090008061 + ], + [ + 0.08571, + 0.04286, + 0.4636476090008061 + ], + [ + 0.12857, + 0.06429, + 0.4636476090008061 + ], + [ + 0.17143, + 0.08571, + 0.4636476090008061 + ], + [ + 0.21429, + 0.10714, + 0.4636476090008061 + ], + [ + 0.25714, + 0.12857, + 0.4636476090008061 + ], + [ + 0.3, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 7, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.0427, + 0.02193, + 0.4915935702646175 + ], + [ + 0.0846, + 0.04536, + 0.5283191444062214 + ], + [ + 0.12561, + 0.07032, + 0.5650447185478252 + ], + [ + 0.16568, + 0.09676, + 0.601770292689429 + ], + [ + 0.20474, + 0.12466, + 0.6384958668310329 + ], + [ + 0.24276, + 0.15397, + 0.6752214409726367 + ], + [ + 0.27967, + 0.18466, + 0.7119470151142406 + ], + [ + 0.31543, + 0.21669, + 0.7486725892558445 + ], + [ + 0.35, + 0.25, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 8, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03457, + 0.03331, + 0.7486725892558445 + ], + [ + 0.07033, + 0.06534, + 0.7119470151142406 + ], + [ + 0.10724, + 0.09603, + 0.6752214409726367 + ], + [ + 0.14526, + 0.12534, + 0.6384958668310329 + ], + [ + 0.18432, + 0.15324, + 0.601770292689429 + ], + [ + 0.22439, + 0.17968, + 0.5650447185478252 + ], + [ + 0.2654, + 0.20464, + 0.5283191444062213 + ], + [ + 0.3073, + 0.22807, + 0.4915935702646175 + ], + [ + 0.35, + 0.25, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 9, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + 0.03685, + 0.7853981633974483 + ], + [ + 0.07371, + 0.07371, + 0.7853981633974483 + ], + [ + 0.11073, + 0.11039, + 0.7662317560971083 + ], + [ + 0.14896, + 0.14581, + 0.7284087377100704 + ], + [ + 0.1885, + 0.17976, + 0.6905857193230327 + ], + [ + 0.22929, + 0.21219, + 0.6527627009359949 + ], + [ + 0.27128, + 0.24305, + 0.614939682548957 + ], + [ + 0.31441, + 0.27231, + 0.5771166641619194 + ], + [ + 0.35862, + 0.29991, + 0.5392936457748816 + ], + [ + 0.40383, + 0.32582, + 0.5014706273878439 + ], + [ + 0.45, + 0.35, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 10, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + 0.0375, + 0.0375, + 0.7853981633974483 + ], + [ + 0.075, + 0.075, + 0.7853981633974483 + ], + [ + 0.1125, + 0.1125, + 0.7853981633974483 + ], + [ + 0.15, + 0.15, + 0.7853981633974483 + ], + [ + 0.1875, + 0.1875, + 0.7853981633974483 + ], + [ + 0.225, + 0.225, + 0.7853981633974483 + ], + [ + 0.2625, + 0.2625, + 0.7853981633974483 + ], + [ + 0.3, + 0.3, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 11, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03331, + 0.03457, + 0.8221237375390521 + ], + [ + 0.06534, + 0.07033, + 0.8588493116806559 + ], + [ + 0.09603, + 0.10724, + 0.8955748858222599 + ], + [ + 0.12534, + 0.14526, + 0.9323004599638637 + ], + [ + 0.15324, + 0.18432, + 0.9690260341054675 + ], + [ + 0.17968, + 0.22439, + 1.0057516082470714 + ], + [ + 0.20464, + 0.2654, + 1.0424771823886751 + ], + [ + 0.22807, + 0.3073, + 1.079202756530279 + ], + [ + 0.25, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 12, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + 0.03685, + 0.7853981633974483 + ], + [ + 0.07371, + 0.07371, + 0.7853981633974483 + ], + [ + 0.11039, + 0.11073, + 0.8045645706977883 + ], + [ + 0.14581, + 0.14896, + 0.8423875890848261 + ], + [ + 0.17976, + 0.1885, + 0.8802106074718639 + ], + [ + 0.21219, + 0.22929, + 0.9180336258589017 + ], + [ + 0.24305, + 0.27128, + 0.9558566442459394 + ], + [ + 0.27231, + 0.31441, + 0.9936796626329771 + ], + [ + 0.29991, + 0.35862, + 1.031502681020015 + ], + [ + 0.32582, + 0.40383, + 1.0693256994070528 + ], + [ + 0.35, + 0.45, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 13, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.02193, + 0.0427, + 1.079202756530279 + ], + [ + 0.04536, + 0.0846, + 1.0424771823886751 + ], + [ + 0.07032, + 0.12561, + 1.0057516082470712 + ], + [ + 0.09676, + 0.16568, + 0.9690260341054675 + ], + [ + 0.12466, + 0.20474, + 0.9323004599638636 + ], + [ + 0.15397, + 0.24276, + 0.8955748858222597 + ], + [ + 0.18466, + 0.27967, + 0.8588493116806559 + ], + [ + 0.21669, + 0.31543, + 0.8221237375390521 + ], + [ + 0.25, + 0.35, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 14, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.02143, + 0.04286, + 1.1071487177940904 + ], + [ + 0.04286, + 0.08571, + 1.1071487177940904 + ], + [ + 0.06429, + 0.12857, + 1.1071487177940904 + ], + [ + 0.08571, + 0.17143, + 1.1071487177940904 + ], + [ + 0.10714, + 0.21429, + 1.1071487177940904 + ], + [ + 0.12857, + 0.25714, + 1.1071487177940904 + ], + [ + 0.15, + 0.3, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 15, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.02148, + 0.04296, + 1.1071487177940904 + ], + [ + 0.04292, + 0.08595, + 1.1172148021235004 + ], + [ + 0.06298, + 0.12959, + 1.16257295459064 + ], + [ + 0.08104, + 0.1741, + 1.2079311070577796 + ], + [ + 0.09707, + 0.21937, + 1.2532892595249192 + ], + [ + 0.11102, + 0.26533, + 1.298647411992059 + ], + [ + 0.12288, + 0.31188, + 1.3440055644591986 + ], + [ + 0.13262, + 0.35891, + 1.3893637169263382 + ], + [ + 0.14021, + 0.40634, + 1.4347218693934778 + ], + [ + 0.14565, + 0.45406, + 1.4800800218606174 + ], + [ + 0.14891, + 0.50198, + 1.525438174327757 + ], + [ + 0.15, + 0.55, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 16, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.00109, + 0.04802, + 1.525438174327757 + ], + [ + 0.00435, + 0.09594, + 1.4800800218606174 + ], + [ + 0.00979, + 0.14366, + 1.4347218693934778 + ], + [ + 0.01738, + 0.19109, + 1.3893637169263382 + ], + [ + 0.02712, + 0.23812, + 1.3440055644591986 + ], + [ + 0.03898, + 0.28467, + 1.298647411992059 + ], + [ + 0.05293, + 0.33063, + 1.2532892595249194 + ], + [ + 0.06896, + 0.3759, + 1.2079311070577798 + ], + [ + 0.08702, + 0.42041, + 1.16257295459064 + ], + [ + 0.10708, + 0.46405, + 1.1172148021235004 + ], + [ + 0.12852, + 0.50704, + 1.1071487177940904 + ], + [ + 0.15, + 0.55, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 17, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.00049, + 0.05179, + 1.5446099427771882 + ], + [ + 0.00279, + 0.10353, + 1.5081548406952634 + ], + [ + 0.00697, + 0.15516, + 1.4716997386133386 + ], + [ + 0.01303, + 0.2066, + 1.4352446365314138 + ], + [ + 0.02097, + 0.25778, + 1.398789534449489 + ], + [ + 0.03076, + 0.30864, + 1.3623344323675641 + ], + [ + 0.0424, + 0.3591, + 1.3258793302856393 + ], + [ + 0.05587, + 0.40911, + 1.2894242282037145 + ], + [ + 0.07116, + 0.4586, + 1.2529691261217897 + ], + [ + 0.08824, + 0.5075, + 1.2165140240398649 + ], + [ + 0.10709, + 0.55574, + 1.18005892195794 + ], + [ + 0.12769, + 0.60326, + 1.1436038198760152 + ], + [ + 0.15, + 0.65, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 18, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.0, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 1.5707963267948966 + ], + [ + 0.0, + 0.15, + 1.5707963267948966 + ], + [ + 0.0, + 0.2, + 1.5707963267948966 + ], + [ + 0.0, + 0.25, + 1.5707963267948966 + ], + [ + 0.0, + 0.3, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 19, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.00109, + 0.04802, + 1.6161544792620361 + ], + [ + -0.00435, + 0.09594, + 1.6615126317291757 + ], + [ + -0.00979, + 0.14366, + 1.7068707841963153 + ], + [ + -0.01738, + 0.19109, + 1.752228936663455 + ], + [ + -0.02712, + 0.23812, + 1.7975870891305945 + ], + [ + -0.03898, + 0.28467, + 1.842945241597734 + ], + [ + -0.05293, + 0.33063, + 1.8883033940648737 + ], + [ + -0.06896, + 0.3759, + 1.9336615465320133 + ], + [ + -0.08702, + 0.42041, + 1.979019698999153 + ], + [ + -0.10708, + 0.46405, + 2.0243778514662925 + ], + [ + -0.12852, + 0.50704, + 2.0344439357957027 + ], + [ + -0.15, + 0.55, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 20, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.00049, + 0.05179, + 1.596982710812605 + ], + [ + -0.00279, + 0.10353, + 1.6334378128945297 + ], + [ + -0.00697, + 0.15516, + 1.6698929149764545 + ], + [ + -0.01303, + 0.2066, + 1.7063480170583794 + ], + [ + -0.02097, + 0.25778, + 1.7428031191403042 + ], + [ + -0.03076, + 0.30864, + 1.779258221222229 + ], + [ + -0.0424, + 0.3591, + 1.8157133233041538 + ], + [ + -0.05587, + 0.40911, + 1.8521684253860786 + ], + [ + -0.07116, + 0.4586, + 1.8886235274680034 + ], + [ + -0.08824, + 0.5075, + 1.9250786295499283 + ], + [ + -0.10709, + 0.55574, + 1.961533731631853 + ], + [ + -0.12769, + 0.60326, + 1.997988833713778 + ], + [ + -0.15, + 0.65, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 21, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.02148, + 0.04296, + 2.0344439357957027 + ], + [ + -0.04292, + 0.08595, + 2.0243778514662925 + ], + [ + -0.06298, + 0.12959, + 1.979019698999153 + ], + [ + -0.08104, + 0.1741, + 1.9336615465320135 + ], + [ + -0.09707, + 0.21937, + 1.888303394064874 + ], + [ + -0.11102, + 0.26533, + 1.8429452415977343 + ], + [ + -0.12288, + 0.31188, + 1.7975870891305947 + ], + [ + -0.13262, + 0.35891, + 1.752228936663455 + ], + [ + -0.14021, + 0.40634, + 1.7068707841963153 + ], + [ + -0.14565, + 0.45406, + 1.6615126317291757 + ], + [ + -0.14891, + 0.50198, + 1.6161544792620361 + ], + [ + -0.15, + 0.55, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 22, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.02143, + 0.04286, + 2.0344439357957027 + ], + [ + -0.04286, + 0.08571, + 2.0344439357957027 + ], + [ + -0.06429, + 0.12857, + 2.0344439357957027 + ], + [ + -0.08571, + 0.17143, + 2.0344439357957027 + ], + [ + -0.10714, + 0.21429, + 2.0344439357957027 + ], + [ + -0.12857, + 0.25714, + 2.0344439357957027 + ], + [ + -0.15, + 0.3, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 23, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.02193, + 0.0427, + 2.0623898970595143 + ], + [ + -0.04536, + 0.0846, + 2.099115471201118 + ], + [ + -0.07032, + 0.12561, + 2.1358410453427217 + ], + [ + -0.09676, + 0.16568, + 2.172566619484326 + ], + [ + -0.12466, + 0.20474, + 2.2092921936259295 + ], + [ + -0.15397, + 0.24276, + 2.2460177677675333 + ], + [ + -0.18466, + 0.27967, + 2.2827433419091374 + ], + [ + -0.21669, + 0.31543, + 2.319468916050741 + ], + [ + -0.25, + 0.35, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 24, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03331, + 0.03457, + 2.319468916050741 + ], + [ + -0.06534, + 0.07033, + 2.282743341909137 + ], + [ + -0.09603, + 0.10724, + 2.2460177677675333 + ], + [ + -0.12534, + 0.14526, + 2.2092921936259295 + ], + [ + -0.15324, + 0.18432, + 2.172566619484326 + ], + [ + -0.17968, + 0.22439, + 2.1358410453427217 + ], + [ + -0.20464, + 0.2654, + 2.099115471201118 + ], + [ + -0.22807, + 0.3073, + 2.0623898970595143 + ], + [ + -0.25, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 25, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + 0.03685, + 2.356194490192345 + ], + [ + -0.07371, + 0.07371, + 2.356194490192345 + ], + [ + -0.11039, + 0.11073, + 2.337028082892005 + ], + [ + -0.14581, + 0.14896, + 2.299205064504967 + ], + [ + -0.17976, + 0.1885, + 2.2613820461179293 + ], + [ + -0.21219, + 0.22929, + 2.2235590277308916 + ], + [ + -0.24305, + 0.27128, + 2.1857360093438536 + ], + [ + -0.27231, + 0.31441, + 2.147912990956816 + ], + [ + -0.29991, + 0.35862, + 2.110089972569778 + ], + [ + -0.32582, + 0.40383, + 2.0722669541827403 + ], + [ + -0.35, + 0.45, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 26, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + -0.0375, + 0.0375, + 2.356194490192345 + ], + [ + -0.075, + 0.075, + 2.356194490192345 + ], + [ + -0.1125, + 0.1125, + 2.356194490192345 + ], + [ + -0.15, + 0.15, + 2.356194490192345 + ], + [ + -0.1875, + 0.1875, + 2.356194490192345 + ], + [ + -0.225, + 0.225, + 2.356194490192345 + ], + [ + -0.2625, + 0.2625, + 2.356194490192345 + ], + [ + -0.3, + 0.3, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 27, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03457, + 0.03331, + 2.3929200643339485 + ], + [ + -0.07033, + 0.06534, + 2.4296456384755527 + ], + [ + -0.10724, + 0.09603, + 2.4663712126171564 + ], + [ + -0.14526, + 0.12534, + 2.50309678675876 + ], + [ + -0.18432, + 0.15324, + 2.539822360900364 + ], + [ + -0.22439, + 0.17968, + 2.576547935041968 + ], + [ + -0.2654, + 0.20464, + 2.6132735091835717 + ], + [ + -0.3073, + 0.22807, + 2.6499990833251754 + ], + [ + -0.35, + 0.25, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 28, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + 0.03685, + 2.356194490192345 + ], + [ + -0.07371, + 0.07371, + 2.356194490192345 + ], + [ + -0.11073, + 0.11039, + 2.3753608974926848 + ], + [ + -0.14896, + 0.14581, + 2.413183915879723 + ], + [ + -0.1885, + 0.17976, + 2.4510069342667604 + ], + [ + -0.22929, + 0.21219, + 2.488829952653798 + ], + [ + -0.27128, + 0.24305, + 2.526652971040836 + ], + [ + -0.31441, + 0.27231, + 2.5644759894278737 + ], + [ + -0.35862, + 0.29991, + 2.6022990078149117 + ], + [ + -0.40383, + 0.32582, + 2.6401220262019494 + ], + [ + -0.45, + 0.35, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 29, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.0427, + 0.02193, + 2.6499990833251754 + ], + [ + -0.0846, + 0.04536, + 2.6132735091835717 + ], + [ + -0.12561, + 0.07032, + 2.576547935041968 + ], + [ + -0.16568, + 0.09676, + 2.539822360900364 + ], + [ + -0.20474, + 0.12466, + 2.50309678675876 + ], + [ + -0.24276, + 0.15397, + 2.4663712126171564 + ], + [ + -0.27967, + 0.18466, + 2.4296456384755523 + ], + [ + -0.31543, + 0.21669, + 2.3929200643339485 + ], + [ + -0.35, + 0.25, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 30, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.04286, + 0.02143, + 2.677945044588987 + ], + [ + -0.08571, + 0.04286, + 2.677945044588987 + ], + [ + -0.12857, + 0.06429, + 2.677945044588987 + ], + [ + -0.17143, + 0.08571, + 2.677945044588987 + ], + [ + -0.21429, + 0.10714, + 2.677945044588987 + ], + [ + -0.25714, + 0.12857, + 2.677945044588987 + ], + [ + -0.3, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 31, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04296, + 0.02148, + 2.677945044588987 + ], + [ + -0.08595, + 0.04292, + 2.688011128918397 + ], + [ + -0.12959, + 0.06298, + 2.733369281385537 + ], + [ + -0.1741, + 0.08104, + 2.7787274338526764 + ], + [ + -0.21937, + 0.09707, + 2.824085586319816 + ], + [ + -0.26533, + 0.11102, + 2.8694437387869556 + ], + [ + -0.31188, + 0.12288, + 2.914801891254095 + ], + [ + -0.35891, + 0.13262, + 2.9601600437212348 + ], + [ + -0.40634, + 0.14021, + 3.0055181961883743 + ], + [ + -0.45406, + 0.14565, + 3.050876348655514 + ], + [ + -0.50198, + 0.14891, + 3.0962345011226535 + ], + [ + -0.55, + 0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 32, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04802, + -0.00109, + 3.1869508060569327 + ], + [ + -0.09594, + -0.00435, + 3.2323089585240723 + ], + [ + -0.14366, + -0.00979, + 3.277667110991212 + ], + [ + -0.19109, + -0.01738, + 3.3230252634583515 + ], + [ + -0.23812, + -0.02712, + 3.368383415925491 + ], + [ + -0.28467, + -0.03898, + 3.4137415683926307 + ], + [ + -0.33063, + -0.05293, + 3.4590997208597702 + ], + [ + -0.3759, + -0.06896, + 3.50445787332691 + ], + [ + -0.42041, + -0.08702, + 3.5498160257940494 + ], + [ + -0.46405, + -0.10708, + 3.595174178261189 + ], + [ + -0.50704, + -0.12852, + 3.6052402625905993 + ], + [ + -0.55, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 33, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.05179, + -0.00049, + 3.1677790376075015 + ], + [ + -0.10353, + -0.00279, + 3.2042341396894263 + ], + [ + -0.15516, + -0.00697, + 3.240689241771351 + ], + [ + -0.2066, + -0.01303, + 3.277144343853276 + ], + [ + -0.25778, + -0.02097, + 3.3135994459352007 + ], + [ + -0.30864, + -0.03076, + 3.3500545480171255 + ], + [ + -0.3591, + -0.0424, + 3.3865096500990504 + ], + [ + -0.40911, + -0.05587, + 3.422964752180975 + ], + [ + -0.4586, + -0.07116, + 3.4594198542629 + ], + [ + -0.5075, + -0.08824, + 3.495874956344825 + ], + [ + -0.55574, + -0.10709, + 3.5323300584267496 + ], + [ + -0.60326, + -0.12769, + 3.5687851605086744 + ], + [ + -0.65, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 34, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04802, + 0.00109, + 3.0962345011226535 + ], + [ + -0.09594, + 0.00435, + 3.050876348655514 + ], + [ + -0.14366, + 0.00979, + 3.0055181961883743 + ], + [ + -0.19109, + 0.01738, + 2.9601600437212348 + ], + [ + -0.23812, + 0.02712, + 2.914801891254095 + ], + [ + -0.28467, + 0.03898, + 2.8694437387869556 + ], + [ + -0.33063, + 0.05293, + 2.824085586319816 + ], + [ + -0.3759, + 0.06896, + 2.7787274338526764 + ], + [ + -0.42041, + 0.08702, + 2.733369281385537 + ], + [ + -0.46405, + 0.10708, + 2.688011128918397 + ], + [ + -0.50704, + 0.12852, + 2.677945044588987 + ], + [ + -0.55, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 35, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.05179, + 0.00049, + 3.1154062695720848 + ], + [ + -0.10353, + 0.00279, + 3.07895116749016 + ], + [ + -0.15516, + 0.00697, + 3.042496065408235 + ], + [ + -0.2066, + 0.01303, + 3.0060409633263103 + ], + [ + -0.25778, + 0.02097, + 2.9695858612443855 + ], + [ + -0.30864, + 0.03076, + 2.9331307591624607 + ], + [ + -0.3591, + 0.0424, + 2.896675657080536 + ], + [ + -0.40911, + 0.05587, + 2.860220554998611 + ], + [ + -0.4586, + 0.07116, + 2.8237654529166862 + ], + [ + -0.5075, + 0.08824, + 2.7873103508347614 + ], + [ + -0.55574, + 0.10709, + 2.7508552487528366 + ], + [ + -0.60326, + 0.12769, + 2.714400146670912 + ], + [ + -0.65, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 36, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + -0.05, + 0.0, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 3.141592653589793 + ], + [ + -0.15, + 0.0, + 3.141592653589793 + ], + [ + -0.2, + 0.0, + 3.141592653589793 + ], + [ + -0.25, + 0.0, + 3.141592653589793 + ], + [ + -0.3, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 37, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.04286, + -0.02143, + 3.6052402625905993 + ], + [ + -0.08571, + -0.04286, + 3.6052402625905993 + ], + [ + -0.12857, + -0.06429, + 3.6052402625905993 + ], + [ + -0.17143, + -0.08571, + 3.6052402625905993 + ], + [ + -0.21429, + -0.10714, + 3.6052402625905993 + ], + [ + -0.25714, + -0.12857, + 3.6052402625905993 + ], + [ + -0.3, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 38, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.0427, + -0.02193, + 3.633186223854411 + ], + [ + -0.0846, + -0.04536, + 3.6699117979960145 + ], + [ + -0.12561, + -0.07032, + 3.7066373721376182 + ], + [ + -0.16568, + -0.09676, + 3.7433629462792224 + ], + [ + -0.20474, + -0.12466, + 3.780088520420826 + ], + [ + -0.24276, + -0.15397, + 3.81681409456243 + ], + [ + -0.27967, + -0.18466, + 3.853539668704034 + ], + [ + -0.31543, + -0.21669, + 3.8902652428456377 + ], + [ + -0.35, + -0.25, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 39, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04296, + -0.02148, + 3.6052402625905993 + ], + [ + -0.08595, + -0.04292, + 3.595174178261189 + ], + [ + -0.12959, + -0.06298, + 3.5498160257940494 + ], + [ + -0.1741, + -0.08104, + 3.50445787332691 + ], + [ + -0.21937, + -0.09707, + 3.4590997208597702 + ], + [ + -0.26533, + -0.11102, + 3.4137415683926307 + ], + [ + -0.31188, + -0.12288, + 3.368383415925491 + ], + [ + -0.35891, + -0.13262, + 3.3230252634583515 + ], + [ + -0.40634, + -0.14021, + 3.277667110991212 + ], + [ + -0.45406, + -0.14565, + 3.2323089585240723 + ], + [ + -0.50198, + -0.14891, + 3.1869508060569327 + ], + [ + -0.55, + -0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 40, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03457, + -0.03331, + 3.8902652428456377 + ], + [ + -0.07033, + -0.06534, + 3.8535396687040335 + ], + [ + -0.10724, + -0.09603, + 3.81681409456243 + ], + [ + -0.14526, + -0.12534, + 3.780088520420826 + ], + [ + -0.18432, + -0.15324, + 3.7433629462792224 + ], + [ + -0.22439, + -0.17968, + 3.7066373721376182 + ], + [ + -0.2654, + -0.20464, + 3.6699117979960145 + ], + [ + -0.3073, + -0.22807, + 3.633186223854411 + ], + [ + -0.35, + -0.25, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 41, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + -0.03685, + 3.9269908169872414 + ], + [ + -0.07371, + -0.07371, + 3.9269908169872414 + ], + [ + -0.11073, + -0.11039, + 3.9078244096869015 + ], + [ + -0.14896, + -0.14581, + 3.8700013912998634 + ], + [ + -0.1885, + -0.17976, + 3.832178372912826 + ], + [ + -0.22929, + -0.21219, + 3.794355354525788 + ], + [ + -0.27128, + -0.24305, + 3.75653233613875 + ], + [ + -0.31441, + -0.27231, + 3.7187093177517125 + ], + [ + -0.35862, + -0.29991, + 3.6808862993646745 + ], + [ + -0.40383, + -0.32582, + 3.643063280977637 + ], + [ + -0.45, + -0.35, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 42, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + -0.0375, + -0.0375, + 3.9269908169872414 + ], + [ + -0.075, + -0.075, + 3.9269908169872414 + ], + [ + -0.1125, + -0.1125, + 3.9269908169872414 + ], + [ + -0.15, + -0.15, + 3.9269908169872414 + ], + [ + -0.1875, + -0.1875, + 3.9269908169872414 + ], + [ + -0.225, + -0.225, + 3.9269908169872414 + ], + [ + -0.2625, + -0.2625, + 3.9269908169872414 + ], + [ + -0.3, + -0.3, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 43, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03331, + -0.03457, + 3.963716391128845 + ], + [ + -0.06534, + -0.07033, + 4.000441965270449 + ], + [ + -0.09603, + -0.10724, + 4.037167539412053 + ], + [ + -0.12534, + -0.14526, + 4.073893113553657 + ], + [ + -0.15324, + -0.18432, + 4.11061868769526 + ], + [ + -0.17968, + -0.22439, + 4.1473442618368646 + ], + [ + -0.20464, + -0.2654, + 4.184069835978468 + ], + [ + -0.22807, + -0.3073, + 4.220795410120072 + ], + [ + -0.25, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 44, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + -0.03685, + 3.9269908169872414 + ], + [ + -0.07371, + -0.07371, + 3.9269908169872414 + ], + [ + -0.11039, + -0.11073, + 3.9461572242875813 + ], + [ + -0.14581, + -0.14896, + 3.9839802426746194 + ], + [ + -0.17976, + -0.1885, + 4.021803261061657 + ], + [ + -0.21219, + -0.22929, + 4.059626279448695 + ], + [ + -0.24305, + -0.27128, + 4.097449297835732 + ], + [ + -0.27231, + -0.31441, + 4.13527231622277 + ], + [ + -0.29991, + -0.35862, + 4.173095334609808 + ], + [ + -0.32582, + -0.40383, + 4.2109183529968455 + ], + [ + -0.35, + -0.45, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 45, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.02193, + -0.0427, + 4.220795410120072 + ], + [ + -0.04536, + -0.0846, + 4.184069835978468 + ], + [ + -0.07032, + -0.12561, + 4.1473442618368646 + ], + [ + -0.09676, + -0.16568, + 4.11061868769526 + ], + [ + -0.12466, + -0.20474, + 4.073893113553657 + ], + [ + -0.15397, + -0.24276, + 4.037167539412053 + ], + [ + -0.18466, + -0.27967, + 4.000441965270449 + ], + [ + -0.21669, + -0.31543, + 3.963716391128845 + ], + [ + -0.25, + -0.35, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 46, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.02143, + -0.04286, + 4.2487413713838835 + ], + [ + -0.04286, + -0.08571, + 4.2487413713838835 + ], + [ + -0.06429, + -0.12857, + 4.2487413713838835 + ], + [ + -0.08571, + -0.17143, + 4.2487413713838835 + ], + [ + -0.10714, + -0.21429, + 4.2487413713838835 + ], + [ + -0.12857, + -0.25714, + 4.2487413713838835 + ], + [ + -0.15, + -0.3, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 47, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.02148, + -0.04296, + 4.2487413713838835 + ], + [ + -0.04292, + -0.08595, + 4.258807455713294 + ], + [ + -0.06298, + -0.12959, + 4.304165608180433 + ], + [ + -0.08104, + -0.1741, + 4.349523760647573 + ], + [ + -0.09707, + -0.21937, + 4.3948819131147125 + ], + [ + -0.11102, + -0.26533, + 4.440240065581852 + ], + [ + -0.12288, + -0.31188, + 4.485598218048992 + ], + [ + -0.13262, + -0.35891, + 4.530956370516131 + ], + [ + -0.14021, + -0.40634, + 4.576314522983271 + ], + [ + -0.14565, + -0.45406, + 4.6216726754504105 + ], + [ + -0.14891, + -0.50198, + 4.66703082791755 + ], + [ + -0.15, + -0.55, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 48, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.00109, + -0.04802, + 4.66703082791755 + ], + [ + -0.00435, + -0.09594, + 4.6216726754504105 + ], + [ + -0.00979, + -0.14366, + 4.576314522983271 + ], + [ + -0.01738, + -0.19109, + 4.530956370516131 + ], + [ + -0.02712, + -0.23812, + 4.485598218048992 + ], + [ + -0.03898, + -0.28467, + 4.440240065581852 + ], + [ + -0.05293, + -0.33063, + 4.3948819131147125 + ], + [ + -0.06896, + -0.3759, + 4.349523760647573 + ], + [ + -0.08702, + -0.42041, + 4.304165608180433 + ], + [ + -0.10708, + -0.46405, + 4.258807455713294 + ], + [ + -0.12852, + -0.50704, + 4.2487413713838835 + ], + [ + -0.15, + -0.55, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 49, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.00049, + -0.05179, + 4.686202596366981 + ], + [ + -0.00279, + -0.10353, + 4.649747494285057 + ], + [ + -0.00697, + -0.15516, + 4.613292392203132 + ], + [ + -0.01303, + -0.2066, + 4.576837290121206 + ], + [ + -0.02097, + -0.25778, + 4.540382188039282 + ], + [ + -0.03076, + -0.30864, + 4.503927085957358 + ], + [ + -0.0424, + -0.3591, + 4.467471983875432 + ], + [ + -0.05587, + -0.40911, + 4.431016881793507 + ], + [ + -0.07116, + -0.4586, + 4.394561779711583 + ], + [ + -0.08824, + -0.5075, + 4.358106677629658 + ], + [ + -0.10709, + -0.55574, + 4.321651575547733 + ], + [ + -0.12769, + -0.60326, + 4.285196473465808 + ], + [ + -0.15, + -0.65, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 50, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.0, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 4.71238898038469 + ], + [ + 0.0, + -0.15, + 4.71238898038469 + ], + [ + 0.0, + -0.2, + 4.71238898038469 + ], + [ + 0.0, + -0.25, + 4.71238898038469 + ], + [ + 0.0, + -0.3, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 51, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.00109, + -0.04802, + 4.757747132851829 + ], + [ + 0.00435, + -0.09594, + 4.803105285318969 + ], + [ + 0.00979, + -0.14366, + 4.848463437786108 + ], + [ + 0.01738, + -0.19109, + 4.893821590253248 + ], + [ + 0.02712, + -0.23812, + 4.939179742720388 + ], + [ + 0.03898, + -0.28467, + 4.984537895187527 + ], + [ + 0.05293, + -0.33063, + 5.029896047654667 + ], + [ + 0.06896, + -0.3759, + 5.075254200121806 + ], + [ + 0.08702, + -0.42041, + 5.120612352588946 + ], + [ + 0.10708, + -0.46405, + 5.165970505056086 + ], + [ + 0.12852, + -0.50704, + 5.176036589385496 + ], + [ + 0.15, + -0.55, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 52, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.00049, + -0.05179, + 4.738575364402398 + ], + [ + 0.00279, + -0.10353, + 4.775030466484322 + ], + [ + 0.00697, + -0.15516, + 4.811485568566248 + ], + [ + 0.01303, + -0.2066, + 4.847940670648173 + ], + [ + 0.02097, + -0.25778, + 4.884395772730097 + ], + [ + 0.03076, + -0.30864, + 4.920850874812022 + ], + [ + 0.0424, + -0.3591, + 4.957305976893947 + ], + [ + 0.05587, + -0.40911, + 4.993761078975872 + ], + [ + 0.07116, + -0.4586, + 5.0302161810577966 + ], + [ + 0.08824, + -0.5075, + 5.066671283139721 + ], + [ + 0.10709, + -0.55574, + 5.103126385221646 + ], + [ + 0.12769, + -0.60326, + 5.1395814873035714 + ], + [ + 0.15, + -0.65, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 53, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.02148, + -0.04296, + 5.176036589385496 + ], + [ + 0.04292, + -0.08595, + 5.165970505056086 + ], + [ + 0.06298, + -0.12959, + 5.120612352588946 + ], + [ + 0.08104, + -0.1741, + 5.075254200121806 + ], + [ + 0.09707, + -0.21937, + 5.029896047654667 + ], + [ + 0.11102, + -0.26533, + 4.984537895187527 + ], + [ + 0.12288, + -0.31188, + 4.939179742720388 + ], + [ + 0.13262, + -0.35891, + 4.893821590253248 + ], + [ + 0.14021, + -0.40634, + 4.848463437786108 + ], + [ + 0.14565, + -0.45406, + 4.803105285318969 + ], + [ + 0.14891, + -0.50198, + 4.757747132851829 + ], + [ + 0.15, + -0.55, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 54, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.02143, + -0.04286, + 5.176036589385496 + ], + [ + 0.04286, + -0.08571, + 5.176036589385496 + ], + [ + 0.06429, + -0.12857, + 5.176036589385496 + ], + [ + 0.08571, + -0.17143, + 5.176036589385496 + ], + [ + 0.10714, + -0.21429, + 5.176036589385496 + ], + [ + 0.12857, + -0.25714, + 5.176036589385496 + ], + [ + 0.15, + -0.3, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 55, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.02193, + -0.0427, + 5.203982550649307 + ], + [ + 0.04536, + -0.0846, + 5.2407081247909115 + ], + [ + 0.07032, + -0.12561, + 5.277433698932515 + ], + [ + 0.09676, + -0.16568, + 5.314159273074119 + ], + [ + 0.12466, + -0.20474, + 5.350884847215722 + ], + [ + 0.15397, + -0.24276, + 5.387610421357326 + ], + [ + 0.18466, + -0.27967, + 5.4243359954989305 + ], + [ + 0.21669, + -0.31543, + 5.461061569640534 + ], + [ + 0.25, + -0.35, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 56, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03331, + -0.03457, + 5.461061569640534 + ], + [ + 0.06534, + -0.07033, + 5.4243359954989305 + ], + [ + 0.09603, + -0.10724, + 5.387610421357326 + ], + [ + 0.12534, + -0.14526, + 5.350884847215722 + ], + [ + 0.15324, + -0.18432, + 5.314159273074119 + ], + [ + 0.17968, + -0.22439, + 5.277433698932515 + ], + [ + 0.20464, + -0.2654, + 5.2407081247909115 + ], + [ + 0.22807, + -0.3073, + 5.203982550649307 + ], + [ + 0.25, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 57, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + -0.03685, + 5.497787143782138 + ], + [ + 0.07371, + -0.07371, + 5.497787143782138 + ], + [ + 0.11039, + -0.11073, + 5.478620736481798 + ], + [ + 0.14581, + -0.14896, + 5.44079771809476 + ], + [ + 0.17976, + -0.1885, + 5.402974699707722 + ], + [ + 0.21219, + -0.22929, + 5.365151681320684 + ], + [ + 0.24305, + -0.27128, + 5.327328662933647 + ], + [ + 0.27231, + -0.31441, + 5.289505644546609 + ], + [ + 0.29991, + -0.35862, + 5.251682626159571 + ], + [ + 0.32582, + -0.40383, + 5.213859607772534 + ], + [ + 0.35, + -0.45, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 58, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + 0.0375, + -0.0375, + 5.497787143782138 + ], + [ + 0.075, + -0.075, + 5.497787143782138 + ], + [ + 0.1125, + -0.1125, + 5.497787143782138 + ], + [ + 0.15, + -0.15, + 5.497787143782138 + ], + [ + 0.1875, + -0.1875, + 5.497787143782138 + ], + [ + 0.225, + -0.225, + 5.497787143782138 + ], + [ + 0.2625, + -0.2625, + 5.497787143782138 + ], + [ + 0.3, + -0.3, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 59, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03457, + -0.03331, + 5.534512717923742 + ], + [ + 0.07033, + -0.06534, + 5.571238292065345 + ], + [ + 0.10724, + -0.09603, + 5.6079638662069495 + ], + [ + 0.14526, + -0.12534, + 5.644689440348554 + ], + [ + 0.18432, + -0.15324, + 5.681415014490157 + ], + [ + 0.22439, + -0.17968, + 5.718140588631761 + ], + [ + 0.2654, + -0.20464, + 5.754866162773365 + ], + [ + 0.3073, + -0.22807, + 5.7915917369149685 + ], + [ + 0.35, + -0.25, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 60, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + -0.03685, + 5.497787143782138 + ], + [ + 0.07371, + -0.07371, + 5.497787143782138 + ], + [ + 0.11073, + -0.11039, + 5.516953551082478 + ], + [ + 0.14896, + -0.14581, + 5.5547765694695155 + ], + [ + 0.1885, + -0.17976, + 5.5925995878565535 + ], + [ + 0.22929, + -0.21219, + 5.630422606243592 + ], + [ + 0.27128, + -0.24305, + 5.668245624630629 + ], + [ + 0.31441, + -0.27231, + 5.706068643017667 + ], + [ + 0.35862, + -0.29991, + 5.743891661404705 + ], + [ + 0.40383, + -0.32582, + 5.781714679791742 + ], + [ + 0.45, + -0.35, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 61, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.0427, + -0.02193, + 5.7915917369149685 + ], + [ + 0.0846, + -0.04536, + 5.754866162773364 + ], + [ + 0.12561, + -0.07032, + 5.718140588631761 + ], + [ + 0.16568, + -0.09676, + 5.681415014490157 + ], + [ + 0.20474, + -0.12466, + 5.644689440348554 + ], + [ + 0.24276, + -0.15397, + 5.6079638662069495 + ], + [ + 0.27967, + -0.18466, + 5.571238292065345 + ], + [ + 0.31543, + -0.21669, + 5.534512717923742 + ], + [ + 0.35, + -0.25, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 62, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.04286, + -0.02143, + 5.81953769817878 + ], + [ + 0.08571, + -0.04286, + 5.81953769817878 + ], + [ + 0.12857, + -0.06429, + 5.81953769817878 + ], + [ + 0.17143, + -0.08571, + 5.81953769817878 + ], + [ + 0.21429, + -0.10714, + 5.81953769817878 + ], + [ + 0.25714, + -0.12857, + 5.81953769817878 + ], + [ + 0.3, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 63, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04296, + -0.02148, + 5.81953769817878 + ], + [ + 0.08595, + -0.04292, + 5.82960378250819 + ], + [ + 0.12959, + -0.06298, + 5.87496193497533 + ], + [ + 0.1741, + -0.08104, + 5.9203200874424695 + ], + [ + 0.21937, + -0.09707, + 5.965678239909609 + ], + [ + 0.26533, + -0.11102, + 6.011036392376749 + ], + [ + 0.31188, + -0.12288, + 6.056394544843888 + ], + [ + 0.35891, + -0.13262, + 6.101752697311028 + ], + [ + 0.40634, + -0.14021, + 6.1471108497781675 + ], + [ + 0.45406, + -0.14565, + 6.192469002245307 + ], + [ + 0.50198, + -0.14891, + 6.237827154712447 + ], + [ + 0.55, + -0.15, + 0.0 + ] + ] + } + ] +} \ No newline at end of file diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/all_trajectories.png b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/all_trajectories.png new file mode 100644 index 0000000000..5e6ac0ead7 Binary files /dev/null and b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/all_trajectories.png differ diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json new file mode 100644 index 0000000000..9146355201 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json @@ -0,0 +1,4944 @@ +{ + "version": 1.0, + "date_generated": "2022-03-17", + "lattice_metadata": { + "motion_model": "diff", + "turning_radius": 1, + "grid_resolution": 0.05, + "stopping_threshold": 5, + "num_of_headings": 16, + "heading_angles": [ + 0.0, + 0.4636476090008061, + 0.7853981633974483, + 1.1071487177940904, + 1.5707963267948966, + 2.0344439357957027, + 2.356194490192345, + 2.677945044588987, + 3.141592653589793, + 3.6052402625905993, + 3.9269908169872414, + 4.2487413713838835, + 4.71238898038469, + 5.176036589385496, + 5.497787143782138, + 5.81953769817878 + ], + "number_of_trajectories": 96 + }, + "primitives": [ + { + "trajectory_id": 0, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04802, + -0.00109, + 6.237827154712447 + ], + [ + 0.09594, + -0.00435, + 6.192469002245307 + ], + [ + 0.14366, + -0.00979, + 6.1471108497781675 + ], + [ + 0.19109, + -0.01738, + 6.101752697311028 + ], + [ + 0.23812, + -0.02712, + 6.056394544843888 + ], + [ + 0.28467, + -0.03898, + 6.011036392376749 + ], + [ + 0.33063, + -0.05293, + 5.965678239909609 + ], + [ + 0.3759, + -0.06896, + 5.9203200874424695 + ], + [ + 0.42041, + -0.08702, + 5.87496193497533 + ], + [ + 0.46405, + -0.10708, + 5.82960378250819 + ], + [ + 0.50704, + -0.12852, + 5.81953769817878 + ], + [ + 0.55, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 1, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.05179, + -0.00049, + 6.256998923161878 + ], + [ + 0.10353, + -0.00279, + 6.220543821079953 + ], + [ + 0.15516, + -0.00697, + 6.184088718998028 + ], + [ + 0.2066, + -0.01303, + 6.147633616916103 + ], + [ + 0.25778, + -0.02097, + 6.111178514834179 + ], + [ + 0.30864, + -0.03076, + 6.074723412752253 + ], + [ + 0.3591, + -0.0424, + 6.038268310670329 + ], + [ + 0.40911, + -0.05587, + 6.001813208588404 + ], + [ + 0.4586, + -0.07116, + 5.965358106506479 + ], + [ + 0.5075, + -0.08824, + 5.928903004424555 + ], + [ + 0.55574, + -0.10709, + 5.89244790234263 + ], + [ + 0.60326, + -0.12769, + 5.855992800260705 + ], + [ + 0.65, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 2, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 3, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.05, + 0.0, + 0.0 + ], + [ + 0.1, + 0.0, + 0.0 + ], + [ + 0.15, + 0.0, + 0.0 + ], + [ + 0.2, + 0.0, + 0.0 + ], + [ + 0.25, + 0.0, + 0.0 + ], + [ + 0.3, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 4, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04802, + 0.00109, + 0.045358152467139604 + ], + [ + 0.09594, + 0.00435, + 0.09071630493427921 + ], + [ + 0.14366, + 0.00979, + 0.13607445740141882 + ], + [ + 0.19109, + 0.01738, + 0.18143260986855841 + ], + [ + 0.23812, + 0.02712, + 0.226790762335698 + ], + [ + 0.28467, + 0.03898, + 0.2721489148028376 + ], + [ + 0.33063, + 0.05293, + 0.3175070672699772 + ], + [ + 0.3759, + 0.06896, + 0.36286521973711683 + ], + [ + 0.42041, + 0.08702, + 0.4082233722042565 + ], + [ + 0.46405, + 0.10708, + 0.45358152467139606 + ], + [ + 0.50704, + 0.12852, + 0.4636476090008061 + ], + [ + 0.55, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 5, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.05179, + 0.00049, + 0.026186384017708383 + ], + [ + 0.10353, + 0.00279, + 0.0626414860996332 + ], + [ + 0.15516, + 0.00697, + 0.09909658818155803 + ], + [ + 0.2066, + 0.01303, + 0.13555169026348282 + ], + [ + 0.25778, + 0.02097, + 0.17200679234540764 + ], + [ + 0.30864, + 0.03076, + 0.20846189442733246 + ], + [ + 0.3591, + 0.0424, + 0.24491699650925727 + ], + [ + 0.40911, + 0.05587, + 0.28137209859118206 + ], + [ + 0.4586, + 0.07116, + 0.3178272006731068 + ], + [ + 0.5075, + 0.08824, + 0.35428230275503164 + ], + [ + 0.55574, + 0.10709, + 0.3907374048369565 + ], + [ + 0.60326, + 0.12769, + 0.4271925069188813 + ], + [ + 0.65, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 6, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.1545492030002687 + ], + [ + 0.0, + 0.0, + 0.3090984060005374 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 7, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04296, + 0.02148, + 0.4636476090008061 + ], + [ + 0.08595, + 0.04292, + 0.453581524671396 + ], + [ + 0.12959, + 0.06298, + 0.4082233722042564 + ], + [ + 0.1741, + 0.08104, + 0.36286521973711683 + ], + [ + 0.21937, + 0.09707, + 0.31750706726997724 + ], + [ + 0.26533, + 0.11102, + 0.27214891480283765 + ], + [ + 0.31188, + 0.12288, + 0.22679076233569806 + ], + [ + 0.35891, + 0.13262, + 0.18143260986855847 + ], + [ + 0.40634, + 0.14021, + 0.13607445740141882 + ], + [ + 0.45406, + 0.14565, + 0.09071630493427918 + ], + [ + 0.50198, + 0.14891, + 0.04535815246713959 + ], + [ + 0.55, + 0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 8, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.23182380450040305 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 9, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.04286, + 0.02143, + 0.4636476090008061 + ], + [ + 0.08571, + 0.04286, + 0.4636476090008061 + ], + [ + 0.12857, + 0.06429, + 0.4636476090008061 + ], + [ + 0.17143, + 0.08571, + 0.4636476090008061 + ], + [ + 0.21429, + 0.10714, + 0.4636476090008061 + ], + [ + 0.25714, + 0.12857, + 0.4636476090008061 + ], + [ + 0.3, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 10, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.0427, + 0.02193, + 0.4915935702646175 + ], + [ + 0.0846, + 0.04536, + 0.5283191444062214 + ], + [ + 0.12561, + 0.07032, + 0.5650447185478252 + ], + [ + 0.16568, + 0.09676, + 0.601770292689429 + ], + [ + 0.20474, + 0.12466, + 0.6384958668310329 + ], + [ + 0.24276, + 0.15397, + 0.6752214409726367 + ], + [ + 0.27967, + 0.18466, + 0.7119470151142406 + ], + [ + 0.31543, + 0.21669, + 0.7486725892558445 + ], + [ + 0.35, + 0.25, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 11, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 12, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03457, + 0.03331, + 0.7486725892558445 + ], + [ + 0.07033, + 0.06534, + 0.7119470151142406 + ], + [ + 0.10724, + 0.09603, + 0.6752214409726367 + ], + [ + 0.14526, + 0.12534, + 0.6384958668310329 + ], + [ + 0.18432, + 0.15324, + 0.601770292689429 + ], + [ + 0.22439, + 0.17968, + 0.5650447185478252 + ], + [ + 0.2654, + 0.20464, + 0.5283191444062213 + ], + [ + 0.3073, + 0.22807, + 0.4915935702646175 + ], + [ + 0.35, + 0.25, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 13, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + 0.03685, + 0.7853981633974483 + ], + [ + 0.07371, + 0.07371, + 0.7853981633974483 + ], + [ + 0.11073, + 0.11039, + 0.7662317560971083 + ], + [ + 0.14896, + 0.14581, + 0.7284087377100704 + ], + [ + 0.1885, + 0.17976, + 0.6905857193230327 + ], + [ + 0.22929, + 0.21219, + 0.6527627009359949 + ], + [ + 0.27128, + 0.24305, + 0.614939682548957 + ], + [ + 0.31441, + 0.27231, + 0.5771166641619194 + ], + [ + 0.35862, + 0.29991, + 0.5392936457748816 + ], + [ + 0.40383, + 0.32582, + 0.5014706273878439 + ], + [ + 0.45, + 0.35, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 14, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 15, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + 0.0375, + 0.0375, + 0.7853981633974483 + ], + [ + 0.075, + 0.075, + 0.7853981633974483 + ], + [ + 0.1125, + 0.1125, + 0.7853981633974483 + ], + [ + 0.15, + 0.15, + 0.7853981633974483 + ], + [ + 0.1875, + 0.1875, + 0.7853981633974483 + ], + [ + 0.225, + 0.225, + 0.7853981633974483 + ], + [ + 0.2625, + 0.2625, + 0.7853981633974483 + ], + [ + 0.3, + 0.3, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 16, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03331, + 0.03457, + 0.8221237375390521 + ], + [ + 0.06534, + 0.07033, + 0.8588493116806559 + ], + [ + 0.09603, + 0.10724, + 0.8955748858222599 + ], + [ + 0.12534, + 0.14526, + 0.9323004599638637 + ], + [ + 0.15324, + 0.18432, + 0.9690260341054675 + ], + [ + 0.17968, + 0.22439, + 1.0057516082470714 + ], + [ + 0.20464, + 0.2654, + 1.0424771823886751 + ], + [ + 0.22807, + 0.3073, + 1.079202756530279 + ], + [ + 0.25, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 17, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + 0.03685, + 0.7853981633974483 + ], + [ + 0.07371, + 0.07371, + 0.7853981633974483 + ], + [ + 0.11039, + 0.11073, + 0.8045645706977883 + ], + [ + 0.14581, + 0.14896, + 0.8423875890848261 + ], + [ + 0.17976, + 0.1885, + 0.8802106074718639 + ], + [ + 0.21219, + 0.22929, + 0.9180336258589017 + ], + [ + 0.24305, + 0.27128, + 0.9558566442459394 + ], + [ + 0.27231, + 0.31441, + 0.9936796626329771 + ], + [ + 0.29991, + 0.35862, + 1.031502681020015 + ], + [ + 0.32582, + 0.40383, + 1.0693256994070528 + ], + [ + 0.35, + 0.45, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 18, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.9462734405957693 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 19, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.02193, + 0.0427, + 1.079202756530279 + ], + [ + 0.04536, + 0.0846, + 1.0424771823886751 + ], + [ + 0.07032, + 0.12561, + 1.0057516082470712 + ], + [ + 0.09676, + 0.16568, + 0.9690260341054675 + ], + [ + 0.12466, + 0.20474, + 0.9323004599638636 + ], + [ + 0.15397, + 0.24276, + 0.8955748858222597 + ], + [ + 0.18466, + 0.27967, + 0.8588493116806559 + ], + [ + 0.21669, + 0.31543, + 0.8221237375390521 + ], + [ + 0.25, + 0.35, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 20, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 0.9998985329952097 + ], + [ + 0.0, + 0.0, + 0.892648348196329 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 21, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.02143, + 0.04286, + 1.1071487177940904 + ], + [ + 0.04286, + 0.08571, + 1.1071487177940904 + ], + [ + 0.06429, + 0.12857, + 1.1071487177940904 + ], + [ + 0.08571, + 0.17143, + 1.1071487177940904 + ], + [ + 0.10714, + 0.21429, + 1.1071487177940904 + ], + [ + 0.12857, + 0.25714, + 1.1071487177940904 + ], + [ + 0.15, + 0.3, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 22, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.02148, + 0.04296, + 1.1071487177940904 + ], + [ + 0.04292, + 0.08595, + 1.1172148021235004 + ], + [ + 0.06298, + 0.12959, + 1.16257295459064 + ], + [ + 0.08104, + 0.1741, + 1.2079311070577796 + ], + [ + 0.09707, + 0.21937, + 1.2532892595249192 + ], + [ + 0.11102, + 0.26533, + 1.298647411992059 + ], + [ + 0.12288, + 0.31188, + 1.3440055644591986 + ], + [ + 0.13262, + 0.35891, + 1.3893637169263382 + ], + [ + 0.14021, + 0.40634, + 1.4347218693934778 + ], + [ + 0.14565, + 0.45406, + 1.4800800218606174 + ], + [ + 0.14891, + 0.50198, + 1.525438174327757 + ], + [ + 0.15, + 0.55, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 23, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 24, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.00109, + 0.04802, + 1.525438174327757 + ], + [ + 0.00435, + 0.09594, + 1.4800800218606174 + ], + [ + 0.00979, + 0.14366, + 1.4347218693934778 + ], + [ + 0.01738, + 0.19109, + 1.3893637169263382 + ], + [ + 0.02712, + 0.23812, + 1.3440055644591986 + ], + [ + 0.03898, + 0.28467, + 1.298647411992059 + ], + [ + 0.05293, + 0.33063, + 1.2532892595249194 + ], + [ + 0.06896, + 0.3759, + 1.2079311070577798 + ], + [ + 0.08702, + 0.42041, + 1.16257295459064 + ], + [ + 0.10708, + 0.46405, + 1.1172148021235004 + ], + [ + 0.12852, + 0.50704, + 1.1071487177940904 + ], + [ + 0.15, + 0.55, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 25, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.00049, + 0.05179, + 1.5446099427771882 + ], + [ + 0.00279, + 0.10353, + 1.5081548406952634 + ], + [ + 0.00697, + 0.15516, + 1.4716997386133386 + ], + [ + 0.01303, + 0.2066, + 1.4352446365314138 + ], + [ + 0.02097, + 0.25778, + 1.398789534449489 + ], + [ + 0.03076, + 0.30864, + 1.3623344323675641 + ], + [ + 0.0424, + 0.3591, + 1.3258793302856393 + ], + [ + 0.05587, + 0.40911, + 1.2894242282037145 + ], + [ + 0.07116, + 0.4586, + 1.2529691261217897 + ], + [ + 0.08824, + 0.5075, + 1.2165140240398649 + ], + [ + 0.10709, + 0.55574, + 1.18005892195794 + ], + [ + 0.12769, + 0.60326, + 1.1436038198760152 + ], + [ + 0.15, + 0.65, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 26, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 27, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.0, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 1.5707963267948966 + ], + [ + 0.0, + 0.15, + 1.5707963267948966 + ], + [ + 0.0, + 0.2, + 1.5707963267948966 + ], + [ + 0.0, + 0.25, + 1.5707963267948966 + ], + [ + 0.0, + 0.3, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 28, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.00109, + 0.04802, + 1.6161544792620361 + ], + [ + -0.00435, + 0.09594, + 1.6615126317291757 + ], + [ + -0.00979, + 0.14366, + 1.7068707841963153 + ], + [ + -0.01738, + 0.19109, + 1.752228936663455 + ], + [ + -0.02712, + 0.23812, + 1.7975870891305945 + ], + [ + -0.03898, + 0.28467, + 1.842945241597734 + ], + [ + -0.05293, + 0.33063, + 1.8883033940648737 + ], + [ + -0.06896, + 0.3759, + 1.9336615465320133 + ], + [ + -0.08702, + 0.42041, + 1.979019698999153 + ], + [ + -0.10708, + 0.46405, + 2.0243778514662925 + ], + [ + -0.12852, + 0.50704, + 2.0344439357957027 + ], + [ + -0.15, + 0.55, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 29, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.00049, + 0.05179, + 1.596982710812605 + ], + [ + -0.00279, + 0.10353, + 1.6334378128945297 + ], + [ + -0.00697, + 0.15516, + 1.6698929149764545 + ], + [ + -0.01303, + 0.2066, + 1.7063480170583794 + ], + [ + -0.02097, + 0.25778, + 1.7428031191403042 + ], + [ + -0.03076, + 0.30864, + 1.779258221222229 + ], + [ + -0.0424, + 0.3591, + 1.8157133233041538 + ], + [ + -0.05587, + 0.40911, + 1.8521684253860786 + ], + [ + -0.07116, + 0.4586, + 1.8886235274680034 + ], + [ + -0.08824, + 0.5075, + 1.9250786295499283 + ], + [ + -0.10709, + 0.55574, + 1.961533731631853 + ], + [ + -0.12769, + 0.60326, + 1.997988833713778 + ], + [ + -0.15, + 0.65, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 30, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.7253455297951652 + ], + [ + 0.0, + 0.0, + 1.879894732795434 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 31, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.02148, + 0.04296, + 2.0344439357957027 + ], + [ + -0.04292, + 0.08595, + 2.0243778514662925 + ], + [ + -0.06298, + 0.12959, + 1.979019698999153 + ], + [ + -0.08104, + 0.1741, + 1.9336615465320135 + ], + [ + -0.09707, + 0.21937, + 1.888303394064874 + ], + [ + -0.11102, + 0.26533, + 1.8429452415977343 + ], + [ + -0.12288, + 0.31188, + 1.7975870891305947 + ], + [ + -0.13262, + 0.35891, + 1.752228936663455 + ], + [ + -0.14021, + 0.40634, + 1.7068707841963153 + ], + [ + -0.14565, + 0.45406, + 1.6615126317291757 + ], + [ + -0.14891, + 0.50198, + 1.6161544792620361 + ], + [ + -0.15, + 0.55, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 32, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 1.8026201312952996 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 33, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.02143, + 0.04286, + 2.0344439357957027 + ], + [ + -0.04286, + 0.08571, + 2.0344439357957027 + ], + [ + -0.06429, + 0.12857, + 2.0344439357957027 + ], + [ + -0.08571, + 0.17143, + 2.0344439357957027 + ], + [ + -0.10714, + 0.21429, + 2.0344439357957027 + ], + [ + -0.12857, + 0.25714, + 2.0344439357957027 + ], + [ + -0.15, + 0.3, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 34, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.02193, + 0.0427, + 2.0623898970595143 + ], + [ + -0.04536, + 0.0846, + 2.099115471201118 + ], + [ + -0.07032, + 0.12561, + 2.1358410453427217 + ], + [ + -0.09676, + 0.16568, + 2.172566619484326 + ], + [ + -0.12466, + 0.20474, + 2.2092921936259295 + ], + [ + -0.15397, + 0.24276, + 2.2460177677675333 + ], + [ + -0.18466, + 0.27967, + 2.2827433419091374 + ], + [ + -0.21669, + 0.31543, + 2.319468916050741 + ], + [ + -0.25, + 0.35, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 35, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 36, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03331, + 0.03457, + 2.319468916050741 + ], + [ + -0.06534, + 0.07033, + 2.282743341909137 + ], + [ + -0.09603, + 0.10724, + 2.2460177677675333 + ], + [ + -0.12534, + 0.14526, + 2.2092921936259295 + ], + [ + -0.15324, + 0.18432, + 2.172566619484326 + ], + [ + -0.17968, + 0.22439, + 2.1358410453427217 + ], + [ + -0.20464, + 0.2654, + 2.099115471201118 + ], + [ + -0.22807, + 0.3073, + 2.0623898970595143 + ], + [ + -0.25, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 37, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + 0.03685, + 2.356194490192345 + ], + [ + -0.07371, + 0.07371, + 2.356194490192345 + ], + [ + -0.11039, + 0.11073, + 2.337028082892005 + ], + [ + -0.14581, + 0.14896, + 2.299205064504967 + ], + [ + -0.17976, + 0.1885, + 2.2613820461179293 + ], + [ + -0.21219, + 0.22929, + 2.2235590277308916 + ], + [ + -0.24305, + 0.27128, + 2.1857360093438536 + ], + [ + -0.27231, + 0.31441, + 2.147912990956816 + ], + [ + -0.29991, + 0.35862, + 2.110089972569778 + ], + [ + -0.32582, + 0.40383, + 2.0722669541827403 + ], + [ + -0.35, + 0.45, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 38, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 39, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + -0.0375, + 0.0375, + 2.356194490192345 + ], + [ + -0.075, + 0.075, + 2.356194490192345 + ], + [ + -0.1125, + 0.1125, + 2.356194490192345 + ], + [ + -0.15, + 0.15, + 2.356194490192345 + ], + [ + -0.1875, + 0.1875, + 2.356194490192345 + ], + [ + -0.225, + 0.225, + 2.356194490192345 + ], + [ + -0.2625, + 0.2625, + 2.356194490192345 + ], + [ + -0.3, + 0.3, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 40, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03457, + 0.03331, + 2.3929200643339485 + ], + [ + -0.07033, + 0.06534, + 2.4296456384755527 + ], + [ + -0.10724, + 0.09603, + 2.4663712126171564 + ], + [ + -0.14526, + 0.12534, + 2.50309678675876 + ], + [ + -0.18432, + 0.15324, + 2.539822360900364 + ], + [ + -0.22439, + 0.17968, + 2.576547935041968 + ], + [ + -0.2654, + 0.20464, + 2.6132735091835717 + ], + [ + -0.3073, + 0.22807, + 2.6499990833251754 + ], + [ + -0.35, + 0.25, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 41, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + 0.03685, + 2.356194490192345 + ], + [ + -0.07371, + 0.07371, + 2.356194490192345 + ], + [ + -0.11073, + 0.11039, + 2.3753608974926848 + ], + [ + -0.14896, + 0.14581, + 2.413183915879723 + ], + [ + -0.1885, + 0.17976, + 2.4510069342667604 + ], + [ + -0.22929, + 0.21219, + 2.488829952653798 + ], + [ + -0.27128, + 0.24305, + 2.526652971040836 + ], + [ + -0.31441, + 0.27231, + 2.5644759894278737 + ], + [ + -0.35862, + 0.29991, + 2.6022990078149117 + ], + [ + -0.40383, + 0.32582, + 2.6401220262019494 + ], + [ + -0.45, + 0.35, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 42, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.517069767390666 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 43, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.0427, + 0.02193, + 2.6499990833251754 + ], + [ + -0.0846, + 0.04536, + 2.6132735091835717 + ], + [ + -0.12561, + 0.07032, + 2.576547935041968 + ], + [ + -0.16568, + 0.09676, + 2.539822360900364 + ], + [ + -0.20474, + 0.12466, + 2.50309678675876 + ], + [ + -0.24276, + 0.15397, + 2.4663712126171564 + ], + [ + -0.27967, + 0.18466, + 2.4296456384755523 + ], + [ + -0.31543, + 0.21669, + 2.3929200643339485 + ], + [ + -0.35, + 0.25, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 44, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.5706948597901063 + ], + [ + 0.0, + 0.0, + 2.4634446749912255 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 45, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.04286, + 0.02143, + 2.677945044588987 + ], + [ + -0.08571, + 0.04286, + 2.677945044588987 + ], + [ + -0.12857, + 0.06429, + 2.677945044588987 + ], + [ + -0.17143, + 0.08571, + 2.677945044588987 + ], + [ + -0.21429, + 0.10714, + 2.677945044588987 + ], + [ + -0.25714, + 0.12857, + 2.677945044588987 + ], + [ + -0.3, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 46, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04296, + 0.02148, + 2.677945044588987 + ], + [ + -0.08595, + 0.04292, + 2.688011128918397 + ], + [ + -0.12959, + 0.06298, + 2.733369281385537 + ], + [ + -0.1741, + 0.08104, + 2.7787274338526764 + ], + [ + -0.21937, + 0.09707, + 2.824085586319816 + ], + [ + -0.26533, + 0.11102, + 2.8694437387869556 + ], + [ + -0.31188, + 0.12288, + 2.914801891254095 + ], + [ + -0.35891, + 0.13262, + 2.9601600437212348 + ], + [ + -0.40634, + 0.14021, + 3.0055181961883743 + ], + [ + -0.45406, + 0.14565, + 3.050876348655514 + ], + [ + -0.50198, + 0.14891, + 3.0962345011226535 + ], + [ + -0.55, + 0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 47, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 48, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04802, + -0.00109, + 3.1869508060569327 + ], + [ + -0.09594, + -0.00435, + 3.2323089585240723 + ], + [ + -0.14366, + -0.00979, + 3.277667110991212 + ], + [ + -0.19109, + -0.01738, + 3.3230252634583515 + ], + [ + -0.23812, + -0.02712, + 3.368383415925491 + ], + [ + -0.28467, + -0.03898, + 3.4137415683926307 + ], + [ + -0.33063, + -0.05293, + 3.4590997208597702 + ], + [ + -0.3759, + -0.06896, + 3.50445787332691 + ], + [ + -0.42041, + -0.08702, + 3.5498160257940494 + ], + [ + -0.46405, + -0.10708, + 3.595174178261189 + ], + [ + -0.50704, + -0.12852, + 3.6052402625905993 + ], + [ + -0.55, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 49, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.05179, + -0.00049, + 3.1677790376075015 + ], + [ + -0.10353, + -0.00279, + 3.2042341396894263 + ], + [ + -0.15516, + -0.00697, + 3.240689241771351 + ], + [ + -0.2066, + -0.01303, + 3.277144343853276 + ], + [ + -0.25778, + -0.02097, + 3.3135994459352007 + ], + [ + -0.30864, + -0.03076, + 3.3500545480171255 + ], + [ + -0.3591, + -0.0424, + 3.3865096500990504 + ], + [ + -0.40911, + -0.05587, + 3.422964752180975 + ], + [ + -0.4586, + -0.07116, + 3.4594198542629 + ], + [ + -0.5075, + -0.08824, + 3.495874956344825 + ], + [ + -0.55574, + -0.10709, + 3.5323300584267496 + ], + [ + -0.60326, + -0.12769, + 3.5687851605086744 + ], + [ + -0.65, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 50, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 3.296141856590062 + ], + [ + 0.0, + 0.0, + 3.4506910595903304 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 51, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04802, + 0.00109, + 3.0962345011226535 + ], + [ + -0.09594, + 0.00435, + 3.050876348655514 + ], + [ + -0.14366, + 0.00979, + 3.0055181961883743 + ], + [ + -0.19109, + 0.01738, + 2.9601600437212348 + ], + [ + -0.23812, + 0.02712, + 2.914801891254095 + ], + [ + -0.28467, + 0.03898, + 2.8694437387869556 + ], + [ + -0.33063, + 0.05293, + 2.824085586319816 + ], + [ + -0.3759, + 0.06896, + 2.7787274338526764 + ], + [ + -0.42041, + 0.08702, + 2.733369281385537 + ], + [ + -0.46405, + 0.10708, + 2.688011128918397 + ], + [ + -0.50704, + 0.12852, + 2.677945044588987 + ], + [ + -0.55, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 52, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.05179, + 0.00049, + 3.1154062695720848 + ], + [ + -0.10353, + 0.00279, + 3.07895116749016 + ], + [ + -0.15516, + 0.00697, + 3.042496065408235 + ], + [ + -0.2066, + 0.01303, + 3.0060409633263103 + ], + [ + -0.25778, + 0.02097, + 2.9695858612443855 + ], + [ + -0.30864, + 0.03076, + 2.9331307591624607 + ], + [ + -0.3591, + 0.0424, + 2.896675657080536 + ], + [ + -0.40911, + 0.05587, + 2.860220554998611 + ], + [ + -0.4586, + 0.07116, + 2.8237654529166862 + ], + [ + -0.5075, + 0.08824, + 2.7873103508347614 + ], + [ + -0.55574, + 0.10709, + 2.7508552487528366 + ], + [ + -0.60326, + 0.12769, + 2.714400146670912 + ], + [ + -0.65, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 53, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 54, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + -0.05, + 0.0, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 3.141592653589793 + ], + [ + -0.15, + 0.0, + 3.141592653589793 + ], + [ + -0.2, + 0.0, + 3.141592653589793 + ], + [ + -0.25, + 0.0, + 3.141592653589793 + ], + [ + -0.3, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 55, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.04286, + -0.02143, + 3.6052402625905993 + ], + [ + -0.08571, + -0.04286, + 3.6052402625905993 + ], + [ + -0.12857, + -0.06429, + 3.6052402625905993 + ], + [ + -0.17143, + -0.08571, + 3.6052402625905993 + ], + [ + -0.21429, + -0.10714, + 3.6052402625905993 + ], + [ + -0.25714, + -0.12857, + 3.6052402625905993 + ], + [ + -0.3, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 56, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.0427, + -0.02193, + 3.633186223854411 + ], + [ + -0.0846, + -0.04536, + 3.6699117979960145 + ], + [ + -0.12561, + -0.07032, + 3.7066373721376182 + ], + [ + -0.16568, + -0.09676, + 3.7433629462792224 + ], + [ + -0.20474, + -0.12466, + 3.780088520420826 + ], + [ + -0.24276, + -0.15397, + 3.81681409456243 + ], + [ + -0.27967, + -0.18466, + 3.853539668704034 + ], + [ + -0.31543, + -0.21669, + 3.8902652428456377 + ], + [ + -0.35, + -0.25, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 57, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 58, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04296, + -0.02148, + 3.6052402625905993 + ], + [ + -0.08595, + -0.04292, + 3.595174178261189 + ], + [ + -0.12959, + -0.06298, + 3.5498160257940494 + ], + [ + -0.1741, + -0.08104, + 3.50445787332691 + ], + [ + -0.21937, + -0.09707, + 3.4590997208597702 + ], + [ + -0.26533, + -0.11102, + 3.4137415683926307 + ], + [ + -0.31188, + -0.12288, + 3.368383415925491 + ], + [ + -0.35891, + -0.13262, + 3.3230252634583515 + ], + [ + -0.40634, + -0.14021, + 3.277667110991212 + ], + [ + -0.45406, + -0.14565, + 3.2323089585240723 + ], + [ + -0.50198, + -0.14891, + 3.1869508060569327 + ], + [ + -0.55, + -0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 59, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.373416458090196 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 60, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03457, + -0.03331, + 3.8902652428456377 + ], + [ + -0.07033, + -0.06534, + 3.8535396687040335 + ], + [ + -0.10724, + -0.09603, + 3.81681409456243 + ], + [ + -0.14526, + -0.12534, + 3.780088520420826 + ], + [ + -0.18432, + -0.15324, + 3.7433629462792224 + ], + [ + -0.22439, + -0.17968, + 3.7066373721376182 + ], + [ + -0.2654, + -0.20464, + 3.6699117979960145 + ], + [ + -0.3073, + -0.22807, + 3.633186223854411 + ], + [ + -0.35, + -0.25, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 61, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + -0.03685, + 3.9269908169872414 + ], + [ + -0.07371, + -0.07371, + 3.9269908169872414 + ], + [ + -0.11073, + -0.11039, + 3.9078244096869015 + ], + [ + -0.14896, + -0.14581, + 3.8700013912998634 + ], + [ + -0.1885, + -0.17976, + 3.832178372912826 + ], + [ + -0.22929, + -0.21219, + 3.794355354525788 + ], + [ + -0.27128, + -0.24305, + 3.75653233613875 + ], + [ + -0.31441, + -0.27231, + 3.7187093177517125 + ], + [ + -0.35862, + -0.29991, + 3.6808862993646745 + ], + [ + -0.40383, + -0.32582, + 3.643063280977637 + ], + [ + -0.45, + -0.35, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 62, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 63, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + -0.0375, + -0.0375, + 3.9269908169872414 + ], + [ + -0.075, + -0.075, + 3.9269908169872414 + ], + [ + -0.1125, + -0.1125, + 3.9269908169872414 + ], + [ + -0.15, + -0.15, + 3.9269908169872414 + ], + [ + -0.1875, + -0.1875, + 3.9269908169872414 + ], + [ + -0.225, + -0.225, + 3.9269908169872414 + ], + [ + -0.2625, + -0.2625, + 3.9269908169872414 + ], + [ + -0.3, + -0.3, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 64, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03331, + -0.03457, + 3.963716391128845 + ], + [ + -0.06534, + -0.07033, + 4.000441965270449 + ], + [ + -0.09603, + -0.10724, + 4.037167539412053 + ], + [ + -0.12534, + -0.14526, + 4.073893113553657 + ], + [ + -0.15324, + -0.18432, + 4.11061868769526 + ], + [ + -0.17968, + -0.22439, + 4.1473442618368646 + ], + [ + -0.20464, + -0.2654, + 4.184069835978468 + ], + [ + -0.22807, + -0.3073, + 4.220795410120072 + ], + [ + -0.25, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 65, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + -0.03685, + 3.9269908169872414 + ], + [ + -0.07371, + -0.07371, + 3.9269908169872414 + ], + [ + -0.11039, + -0.11073, + 3.9461572242875813 + ], + [ + -0.14581, + -0.14896, + 3.9839802426746194 + ], + [ + -0.17976, + -0.1885, + 4.021803261061657 + ], + [ + -0.21219, + -0.22929, + 4.059626279448695 + ], + [ + -0.24305, + -0.27128, + 4.097449297835732 + ], + [ + -0.27231, + -0.31441, + 4.13527231622277 + ], + [ + -0.29991, + -0.35862, + 4.173095334609808 + ], + [ + -0.32582, + -0.40383, + 4.2109183529968455 + ], + [ + -0.35, + -0.45, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 66, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 4.0878660941855625 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 67, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.02193, + -0.0427, + 4.220795410120072 + ], + [ + -0.04536, + -0.0846, + 4.184069835978468 + ], + [ + -0.07032, + -0.12561, + 4.1473442618368646 + ], + [ + -0.09676, + -0.16568, + 4.11061868769526 + ], + [ + -0.12466, + -0.20474, + 4.073893113553657 + ], + [ + -0.15397, + -0.24276, + 4.037167539412053 + ], + [ + -0.18466, + -0.27967, + 4.000441965270449 + ], + [ + -0.21669, + -0.31543, + 3.963716391128845 + ], + [ + -0.25, + -0.35, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 68, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.141491186585003 + ], + [ + 0.0, + 0.0, + 4.034241001786122 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 69, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.02143, + -0.04286, + 4.2487413713838835 + ], + [ + -0.04286, + -0.08571, + 4.2487413713838835 + ], + [ + -0.06429, + -0.12857, + 4.2487413713838835 + ], + [ + -0.08571, + -0.17143, + 4.2487413713838835 + ], + [ + -0.10714, + -0.21429, + 4.2487413713838835 + ], + [ + -0.12857, + -0.25714, + 4.2487413713838835 + ], + [ + -0.15, + -0.3, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 70, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.02148, + -0.04296, + 4.2487413713838835 + ], + [ + -0.04292, + -0.08595, + 4.258807455713294 + ], + [ + -0.06298, + -0.12959, + 4.304165608180433 + ], + [ + -0.08104, + -0.1741, + 4.349523760647573 + ], + [ + -0.09707, + -0.21937, + 4.3948819131147125 + ], + [ + -0.11102, + -0.26533, + 4.440240065581852 + ], + [ + -0.12288, + -0.31188, + 4.485598218048992 + ], + [ + -0.13262, + -0.35891, + 4.530956370516131 + ], + [ + -0.14021, + -0.40634, + 4.576314522983271 + ], + [ + -0.14565, + -0.45406, + 4.6216726754504105 + ], + [ + -0.14891, + -0.50198, + 4.66703082791755 + ], + [ + -0.15, + -0.55, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 71, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 72, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.00109, + -0.04802, + 4.66703082791755 + ], + [ + -0.00435, + -0.09594, + 4.6216726754504105 + ], + [ + -0.00979, + -0.14366, + 4.576314522983271 + ], + [ + -0.01738, + -0.19109, + 4.530956370516131 + ], + [ + -0.02712, + -0.23812, + 4.485598218048992 + ], + [ + -0.03898, + -0.28467, + 4.440240065581852 + ], + [ + -0.05293, + -0.33063, + 4.3948819131147125 + ], + [ + -0.06896, + -0.3759, + 4.349523760647573 + ], + [ + -0.08702, + -0.42041, + 4.304165608180433 + ], + [ + -0.10708, + -0.46405, + 4.258807455713294 + ], + [ + -0.12852, + -0.50704, + 4.2487413713838835 + ], + [ + -0.15, + -0.55, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 73, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.00049, + -0.05179, + 4.686202596366981 + ], + [ + -0.00279, + -0.10353, + 4.649747494285057 + ], + [ + -0.00697, + -0.15516, + 4.613292392203132 + ], + [ + -0.01303, + -0.2066, + 4.576837290121206 + ], + [ + -0.02097, + -0.25778, + 4.540382188039282 + ], + [ + -0.03076, + -0.30864, + 4.503927085957358 + ], + [ + -0.0424, + -0.3591, + 4.467471983875432 + ], + [ + -0.05587, + -0.40911, + 4.431016881793507 + ], + [ + -0.07116, + -0.4586, + 4.394561779711583 + ], + [ + -0.08824, + -0.5075, + 4.358106677629658 + ], + [ + -0.10709, + -0.55574, + 4.321651575547733 + ], + [ + -0.12769, + -0.60326, + 4.285196473465808 + ], + [ + -0.15, + -0.65, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 74, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 75, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.0, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 4.71238898038469 + ], + [ + 0.0, + -0.15, + 4.71238898038469 + ], + [ + 0.0, + -0.2, + 4.71238898038469 + ], + [ + 0.0, + -0.25, + 4.71238898038469 + ], + [ + 0.0, + -0.3, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 76, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.00109, + -0.04802, + 4.757747132851829 + ], + [ + 0.00435, + -0.09594, + 4.803105285318969 + ], + [ + 0.00979, + -0.14366, + 4.848463437786108 + ], + [ + 0.01738, + -0.19109, + 4.893821590253248 + ], + [ + 0.02712, + -0.23812, + 4.939179742720388 + ], + [ + 0.03898, + -0.28467, + 4.984537895187527 + ], + [ + 0.05293, + -0.33063, + 5.029896047654667 + ], + [ + 0.06896, + -0.3759, + 5.075254200121806 + ], + [ + 0.08702, + -0.42041, + 5.120612352588946 + ], + [ + 0.10708, + -0.46405, + 5.165970505056086 + ], + [ + 0.12852, + -0.50704, + 5.176036589385496 + ], + [ + 0.15, + -0.55, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 77, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.00049, + -0.05179, + 4.738575364402398 + ], + [ + 0.00279, + -0.10353, + 4.775030466484322 + ], + [ + 0.00697, + -0.15516, + 4.811485568566248 + ], + [ + 0.01303, + -0.2066, + 4.847940670648173 + ], + [ + 0.02097, + -0.25778, + 4.884395772730097 + ], + [ + 0.03076, + -0.30864, + 4.920850874812022 + ], + [ + 0.0424, + -0.3591, + 4.957305976893947 + ], + [ + 0.05587, + -0.40911, + 4.993761078975872 + ], + [ + 0.07116, + -0.4586, + 5.0302161810577966 + ], + [ + 0.08824, + -0.5075, + 5.066671283139721 + ], + [ + 0.10709, + -0.55574, + 5.103126385221646 + ], + [ + 0.12769, + -0.60326, + 5.1395814873035714 + ], + [ + 0.15, + -0.65, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 78, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.866938183384958 + ], + [ + 0.0, + 0.0, + 5.021487386385227 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 79, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.02148, + -0.04296, + 5.176036589385496 + ], + [ + 0.04292, + -0.08595, + 5.165970505056086 + ], + [ + 0.06298, + -0.12959, + 5.120612352588946 + ], + [ + 0.08104, + -0.1741, + 5.075254200121806 + ], + [ + 0.09707, + -0.21937, + 5.029896047654667 + ], + [ + 0.11102, + -0.26533, + 4.984537895187527 + ], + [ + 0.12288, + -0.31188, + 4.939179742720388 + ], + [ + 0.13262, + -0.35891, + 4.893821590253248 + ], + [ + 0.14021, + -0.40634, + 4.848463437786108 + ], + [ + 0.14565, + -0.45406, + 4.803105285318969 + ], + [ + 0.14891, + -0.50198, + 4.757747132851829 + ], + [ + 0.15, + -0.55, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 80, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 4.944212784885092 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 81, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.02143, + -0.04286, + 5.176036589385496 + ], + [ + 0.04286, + -0.08571, + 5.176036589385496 + ], + [ + 0.06429, + -0.12857, + 5.176036589385496 + ], + [ + 0.08571, + -0.17143, + 5.176036589385496 + ], + [ + 0.10714, + -0.21429, + 5.176036589385496 + ], + [ + 0.12857, + -0.25714, + 5.176036589385496 + ], + [ + 0.15, + -0.3, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 82, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.02193, + -0.0427, + 5.203982550649307 + ], + [ + 0.04536, + -0.0846, + 5.2407081247909115 + ], + [ + 0.07032, + -0.12561, + 5.277433698932515 + ], + [ + 0.09676, + -0.16568, + 5.314159273074119 + ], + [ + 0.12466, + -0.20474, + 5.350884847215722 + ], + [ + 0.15397, + -0.24276, + 5.387610421357326 + ], + [ + 0.18466, + -0.27967, + 5.4243359954989305 + ], + [ + 0.21669, + -0.31543, + 5.461061569640534 + ], + [ + 0.25, + -0.35, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 83, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 84, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03331, + -0.03457, + 5.461061569640534 + ], + [ + 0.06534, + -0.07033, + 5.4243359954989305 + ], + [ + 0.09603, + -0.10724, + 5.387610421357326 + ], + [ + 0.12534, + -0.14526, + 5.350884847215722 + ], + [ + 0.15324, + -0.18432, + 5.314159273074119 + ], + [ + 0.17968, + -0.22439, + 5.277433698932515 + ], + [ + 0.20464, + -0.2654, + 5.2407081247909115 + ], + [ + 0.22807, + -0.3073, + 5.203982550649307 + ], + [ + 0.25, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 85, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + -0.03685, + 5.497787143782138 + ], + [ + 0.07371, + -0.07371, + 5.497787143782138 + ], + [ + 0.11039, + -0.11073, + 5.478620736481798 + ], + [ + 0.14581, + -0.14896, + 5.44079771809476 + ], + [ + 0.17976, + -0.1885, + 5.402974699707722 + ], + [ + 0.21219, + -0.22929, + 5.365151681320684 + ], + [ + 0.24305, + -0.27128, + 5.327328662933647 + ], + [ + 0.27231, + -0.31441, + 5.289505644546609 + ], + [ + 0.29991, + -0.35862, + 5.251682626159571 + ], + [ + 0.32582, + -0.40383, + 5.213859607772534 + ], + [ + 0.35, + -0.45, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 86, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 87, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + 0.0375, + -0.0375, + 5.497787143782138 + ], + [ + 0.075, + -0.075, + 5.497787143782138 + ], + [ + 0.1125, + -0.1125, + 5.497787143782138 + ], + [ + 0.15, + -0.15, + 5.497787143782138 + ], + [ + 0.1875, + -0.1875, + 5.497787143782138 + ], + [ + 0.225, + -0.225, + 5.497787143782138 + ], + [ + 0.2625, + -0.2625, + 5.497787143782138 + ], + [ + 0.3, + -0.3, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 88, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03457, + -0.03331, + 5.534512717923742 + ], + [ + 0.07033, + -0.06534, + 5.571238292065345 + ], + [ + 0.10724, + -0.09603, + 5.6079638662069495 + ], + [ + 0.14526, + -0.12534, + 5.644689440348554 + ], + [ + 0.18432, + -0.15324, + 5.681415014490157 + ], + [ + 0.22439, + -0.17968, + 5.718140588631761 + ], + [ + 0.2654, + -0.20464, + 5.754866162773365 + ], + [ + 0.3073, + -0.22807, + 5.7915917369149685 + ], + [ + 0.35, + -0.25, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 89, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + -0.03685, + 5.497787143782138 + ], + [ + 0.07371, + -0.07371, + 5.497787143782138 + ], + [ + 0.11073, + -0.11039, + 5.516953551082478 + ], + [ + 0.14896, + -0.14581, + 5.5547765694695155 + ], + [ + 0.1885, + -0.17976, + 5.5925995878565535 + ], + [ + 0.22929, + -0.21219, + 5.630422606243592 + ], + [ + 0.27128, + -0.24305, + 5.668245624630629 + ], + [ + 0.31441, + -0.27231, + 5.706068643017667 + ], + [ + 0.35862, + -0.29991, + 5.743891661404705 + ], + [ + 0.40383, + -0.32582, + 5.781714679791742 + ], + [ + 0.45, + -0.35, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 90, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.658662420980459 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 91, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.0427, + -0.02193, + 5.7915917369149685 + ], + [ + 0.0846, + -0.04536, + 5.754866162773364 + ], + [ + 0.12561, + -0.07032, + 5.718140588631761 + ], + [ + 0.16568, + -0.09676, + 5.681415014490157 + ], + [ + 0.20474, + -0.12466, + 5.644689440348554 + ], + [ + 0.24276, + -0.15397, + 5.6079638662069495 + ], + [ + 0.27967, + -0.18466, + 5.571238292065345 + ], + [ + 0.31543, + -0.21669, + 5.534512717923742 + ], + [ + 0.35, + -0.25, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 92, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.712287513379899 + ], + [ + 0.0, + 0.0, + 5.605037328581019 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 93, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.04286, + -0.02143, + 5.81953769817878 + ], + [ + 0.08571, + -0.04286, + 5.81953769817878 + ], + [ + 0.12857, + -0.06429, + 5.81953769817878 + ], + [ + 0.17143, + -0.08571, + 5.81953769817878 + ], + [ + 0.21429, + -0.10714, + 5.81953769817878 + ], + [ + 0.25714, + -0.12857, + 5.81953769817878 + ], + [ + 0.3, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 94, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04296, + -0.02148, + 5.81953769817878 + ], + [ + 0.08595, + -0.04292, + 5.82960378250819 + ], + [ + 0.12959, + -0.06298, + 5.87496193497533 + ], + [ + 0.1741, + -0.08104, + 5.9203200874424695 + ], + [ + 0.21937, + -0.09707, + 5.965678239909609 + ], + [ + 0.26533, + -0.11102, + 6.011036392376749 + ], + [ + 0.31188, + -0.12288, + 6.056394544843888 + ], + [ + 0.35891, + -0.13262, + 6.101752697311028 + ], + [ + 0.40634, + -0.14021, + 6.1471108497781675 + ], + [ + 0.45406, + -0.14565, + 6.192469002245307 + ], + [ + 0.50198, + -0.14891, + 6.237827154712447 + ], + [ + 0.55, + -0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 95, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + } + ] +} \ No newline at end of file diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json new file mode 100644 index 0000000000..95f8dc2d85 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json @@ -0,0 +1,6448 @@ +{ + "version": 1.0, + "date_generated": "2022-03-17", + "lattice_metadata": { + "motion_model": "omni", + "turning_radius": 1, + "grid_resolution": 0.05, + "stopping_threshold": 5, + "num_of_headings": 16, + "heading_angles": [ + 0.0, + 0.4636476090008061, + 0.7853981633974483, + 1.1071487177940904, + 1.5707963267948966, + 2.0344439357957027, + 2.356194490192345, + 2.677945044588987, + 3.141592653589793, + 3.6052402625905993, + 3.9269908169872414, + 4.2487413713838835, + 4.71238898038469, + 5.176036589385496, + 5.497787143782138, + 5.81953769817878 + ], + "number_of_trajectories": 128 + }, + "primitives": [ + { + "trajectory_id": 0, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04802, + -0.00109, + 6.237827154712447 + ], + [ + 0.09594, + -0.00435, + 6.192469002245307 + ], + [ + 0.14366, + -0.00979, + 6.1471108497781675 + ], + [ + 0.19109, + -0.01738, + 6.101752697311028 + ], + [ + 0.23812, + -0.02712, + 6.056394544843888 + ], + [ + 0.28467, + -0.03898, + 6.011036392376749 + ], + [ + 0.33063, + -0.05293, + 5.965678239909609 + ], + [ + 0.3759, + -0.06896, + 5.9203200874424695 + ], + [ + 0.42041, + -0.08702, + 5.87496193497533 + ], + [ + 0.46405, + -0.10708, + 5.82960378250819 + ], + [ + 0.50704, + -0.12852, + 5.81953769817878 + ], + [ + 0.55, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 1, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.05179, + -0.00049, + 6.256998923161878 + ], + [ + 0.10353, + -0.00279, + 6.220543821079953 + ], + [ + 0.15516, + -0.00697, + 6.184088718998028 + ], + [ + 0.2066, + -0.01303, + 6.147633616916103 + ], + [ + 0.25778, + -0.02097, + 6.111178514834179 + ], + [ + 0.30864, + -0.03076, + 6.074723412752253 + ], + [ + 0.3591, + -0.0424, + 6.038268310670329 + ], + [ + 0.40911, + -0.05587, + 6.001813208588404 + ], + [ + 0.4586, + -0.07116, + 5.965358106506479 + ], + [ + 0.5075, + -0.08824, + 5.928903004424555 + ], + [ + 0.55574, + -0.10709, + 5.89244790234263 + ], + [ + 0.60326, + -0.12769, + 5.855992800260705 + ], + [ + 0.65, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 2, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 3, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.05, + 0.0, + 0.0 + ], + [ + 0.1, + 0.0, + 0.0 + ], + [ + 0.15, + 0.0, + 0.0 + ], + [ + 0.2, + 0.0, + 0.0 + ], + [ + 0.25, + 0.0, + 0.0 + ], + [ + 0.3, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 4, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.0, + 0.05, + 0.0 + ], + [ + 0.0, + 0.1, + 0.0 + ], + [ + 0.0, + 0.15, + 0.0 + ], + [ + 0.0, + 0.2, + 0.0 + ], + [ + 0.0, + 0.25, + 0.0 + ], + [ + 0.0, + 0.3, + 0.0 + ] + ] + }, + { + "trajectory_id": 5, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.9, + "arc_length": 0.0, + "straight_length": 0.9, + "poses": [ + [ + 0.0, + -0.05, + 0.0 + ], + [ + 0.0, + -0.1, + 0.0 + ], + [ + 0.0, + -0.15, + 0.0 + ], + [ + 0.0, + -0.2, + 0.0 + ], + [ + 0.0, + -0.25, + 0.0 + ], + [ + 0.0, + -0.3, + 0.0 + ] + ] + }, + { + "trajectory_id": 6, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04802, + 0.00109, + 0.045358152467139604 + ], + [ + 0.09594, + 0.00435, + 0.09071630493427921 + ], + [ + 0.14366, + 0.00979, + 0.13607445740141882 + ], + [ + 0.19109, + 0.01738, + 0.18143260986855841 + ], + [ + 0.23812, + 0.02712, + 0.226790762335698 + ], + [ + 0.28467, + 0.03898, + 0.2721489148028376 + ], + [ + 0.33063, + 0.05293, + 0.3175070672699772 + ], + [ + 0.3759, + 0.06896, + 0.36286521973711683 + ], + [ + 0.42041, + 0.08702, + 0.4082233722042565 + ], + [ + 0.46405, + 0.10708, + 0.45358152467139606 + ], + [ + 0.50704, + 0.12852, + 0.4636476090008061 + ], + [ + 0.55, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 7, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.05179, + 0.00049, + 0.026186384017708383 + ], + [ + 0.10353, + 0.00279, + 0.0626414860996332 + ], + [ + 0.15516, + 0.00697, + 0.09909658818155803 + ], + [ + 0.2066, + 0.01303, + 0.13555169026348282 + ], + [ + 0.25778, + 0.02097, + 0.17200679234540764 + ], + [ + 0.30864, + 0.03076, + 0.20846189442733246 + ], + [ + 0.3591, + 0.0424, + 0.24491699650925727 + ], + [ + 0.40911, + 0.05587, + 0.28137209859118206 + ], + [ + 0.4586, + 0.07116, + 0.3178272006731068 + ], + [ + 0.5075, + 0.08824, + 0.35428230275503164 + ], + [ + 0.55574, + 0.10709, + 0.3907374048369565 + ], + [ + 0.60326, + 0.12769, + 0.4271925069188813 + ], + [ + 0.65, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 8, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.1545492030002687 + ], + [ + 0.0, + 0.0, + 0.3090984060005374 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 9, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04296, + 0.02148, + 0.4636476090008061 + ], + [ + 0.08595, + 0.04292, + 0.453581524671396 + ], + [ + 0.12959, + 0.06298, + 0.4082233722042564 + ], + [ + 0.1741, + 0.08104, + 0.36286521973711683 + ], + [ + 0.21937, + 0.09707, + 0.31750706726997724 + ], + [ + 0.26533, + 0.11102, + 0.27214891480283765 + ], + [ + 0.31188, + 0.12288, + 0.22679076233569806 + ], + [ + 0.35891, + 0.13262, + 0.18143260986855847 + ], + [ + 0.40634, + 0.14021, + 0.13607445740141882 + ], + [ + 0.45406, + 0.14565, + 0.09071630493427918 + ], + [ + 0.50198, + 0.14891, + 0.04535815246713959 + ], + [ + 0.55, + 0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 10, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.23182380450040305 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 11, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.04286, + 0.02143, + 0.4636476090008061 + ], + [ + 0.08571, + 0.04286, + 0.4636476090008061 + ], + [ + 0.12857, + 0.06429, + 0.4636476090008061 + ], + [ + 0.17143, + 0.08571, + 0.4636476090008061 + ], + [ + 0.21429, + 0.10714, + 0.4636476090008061 + ], + [ + 0.25714, + 0.12857, + 0.4636476090008061 + ], + [ + 0.3, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 12, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.02143, + 0.04286, + 0.4636476090008061 + ], + [ + -0.04286, + 0.08571, + 0.4636476090008061 + ], + [ + -0.06429, + 0.12857, + 0.4636476090008061 + ], + [ + -0.08571, + 0.17143, + 0.4636476090008061 + ], + [ + -0.10714, + 0.21429, + 0.4636476090008061 + ], + [ + -0.12857, + 0.25714, + 0.4636476090008061 + ], + [ + -0.15, + 0.3, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 13, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.00623, + "arc_length": 0.0, + "straight_length": 1.00623, + "poses": [ + [ + 0.02143, + -0.04286, + 0.4636476090008061 + ], + [ + 0.04286, + -0.08571, + 0.4636476090008061 + ], + [ + 0.06429, + -0.12857, + 0.4636476090008061 + ], + [ + 0.08571, + -0.17143, + 0.4636476090008061 + ], + [ + 0.10714, + -0.21429, + 0.4636476090008061 + ], + [ + 0.12857, + -0.25714, + 0.4636476090008061 + ], + [ + 0.15, + -0.3, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 14, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.0427, + 0.02193, + 0.4915935702646175 + ], + [ + 0.0846, + 0.04536, + 0.5283191444062214 + ], + [ + 0.12561, + 0.07032, + 0.5650447185478252 + ], + [ + 0.16568, + 0.09676, + 0.601770292689429 + ], + [ + 0.20474, + 0.12466, + 0.6384958668310329 + ], + [ + 0.24276, + 0.15397, + 0.6752214409726367 + ], + [ + 0.27967, + 0.18466, + 0.7119470151142406 + ], + [ + 0.31543, + 0.21669, + 0.7486725892558445 + ], + [ + 0.35, + 0.25, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 15, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 16, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03457, + 0.03331, + 0.7486725892558445 + ], + [ + 0.07033, + 0.06534, + 0.7119470151142406 + ], + [ + 0.10724, + 0.09603, + 0.6752214409726367 + ], + [ + 0.14526, + 0.12534, + 0.6384958668310329 + ], + [ + 0.18432, + 0.15324, + 0.601770292689429 + ], + [ + 0.22439, + 0.17968, + 0.5650447185478252 + ], + [ + 0.2654, + 0.20464, + 0.5283191444062213 + ], + [ + 0.3073, + 0.22807, + 0.4915935702646175 + ], + [ + 0.35, + 0.25, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 17, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + 0.03685, + 0.7853981633974483 + ], + [ + 0.07371, + 0.07371, + 0.7853981633974483 + ], + [ + 0.11073, + 0.11039, + 0.7662317560971083 + ], + [ + 0.14896, + 0.14581, + 0.7284087377100704 + ], + [ + 0.1885, + 0.17976, + 0.6905857193230327 + ], + [ + 0.22929, + 0.21219, + 0.6527627009359949 + ], + [ + 0.27128, + 0.24305, + 0.614939682548957 + ], + [ + 0.31441, + 0.27231, + 0.5771166641619194 + ], + [ + 0.35862, + 0.29991, + 0.5392936457748816 + ], + [ + 0.40383, + 0.32582, + 0.5014706273878439 + ], + [ + 0.45, + 0.35, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 18, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 19, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + 0.0375, + 0.0375, + 0.7853981633974483 + ], + [ + 0.075, + 0.075, + 0.7853981633974483 + ], + [ + 0.1125, + 0.1125, + 0.7853981633974483 + ], + [ + 0.15, + 0.15, + 0.7853981633974483 + ], + [ + 0.1875, + 0.1875, + 0.7853981633974483 + ], + [ + 0.225, + 0.225, + 0.7853981633974483 + ], + [ + 0.2625, + 0.2625, + 0.7853981633974483 + ], + [ + 0.3, + 0.3, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 20, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + -0.0375, + 0.0375, + 0.7853981633974483 + ], + [ + -0.075, + 0.075, + 0.7853981633974483 + ], + [ + -0.1125, + 0.1125, + 0.7853981633974483 + ], + [ + -0.15, + 0.15, + 0.7853981633974483 + ], + [ + -0.1875, + 0.1875, + 0.7853981633974483 + ], + [ + -0.225, + 0.225, + 0.7853981633974483 + ], + [ + -0.2625, + 0.2625, + 0.7853981633974483 + ], + [ + -0.3, + 0.3, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 21, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.27279, + "arc_length": 0.0, + "straight_length": 1.27279, + "poses": [ + [ + 0.0375, + -0.0375, + 0.7853981633974483 + ], + [ + 0.075, + -0.075, + 0.7853981633974483 + ], + [ + 0.1125, + -0.1125, + 0.7853981633974483 + ], + [ + 0.15, + -0.15, + 0.7853981633974483 + ], + [ + 0.1875, + -0.1875, + 0.7853981633974483 + ], + [ + 0.225, + -0.225, + 0.7853981633974483 + ], + [ + 0.2625, + -0.2625, + 0.7853981633974483 + ], + [ + 0.3, + -0.3, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 22, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03331, + 0.03457, + 0.8221237375390521 + ], + [ + 0.06534, + 0.07033, + 0.8588493116806559 + ], + [ + 0.09603, + 0.10724, + 0.8955748858222599 + ], + [ + 0.12534, + 0.14526, + 0.9323004599638637 + ], + [ + 0.15324, + 0.18432, + 0.9690260341054675 + ], + [ + 0.17968, + 0.22439, + 1.0057516082470714 + ], + [ + 0.20464, + 0.2654, + 1.0424771823886751 + ], + [ + 0.22807, + 0.3073, + 1.079202756530279 + ], + [ + 0.25, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 23, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + 0.03685, + 0.7853981633974483 + ], + [ + 0.07371, + 0.07371, + 0.7853981633974483 + ], + [ + 0.11039, + 0.11073, + 0.8045645706977883 + ], + [ + 0.14581, + 0.14896, + 0.8423875890848261 + ], + [ + 0.17976, + 0.1885, + 0.8802106074718639 + ], + [ + 0.21219, + 0.22929, + 0.9180336258589017 + ], + [ + 0.24305, + 0.27128, + 0.9558566442459394 + ], + [ + 0.27231, + 0.31441, + 0.9936796626329771 + ], + [ + 0.29991, + 0.35862, + 1.031502681020015 + ], + [ + 0.32582, + 0.40383, + 1.0693256994070528 + ], + [ + 0.35, + 0.45, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 24, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.9462734405957693 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 25, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.02193, + 0.0427, + 1.079202756530279 + ], + [ + 0.04536, + 0.0846, + 1.0424771823886751 + ], + [ + 0.07032, + 0.12561, + 1.0057516082470712 + ], + [ + 0.09676, + 0.16568, + 0.9690260341054675 + ], + [ + 0.12466, + 0.20474, + 0.9323004599638636 + ], + [ + 0.15397, + 0.24276, + 0.8955748858222597 + ], + [ + 0.18466, + 0.27967, + 0.8588493116806559 + ], + [ + 0.21669, + 0.31543, + 0.8221237375390521 + ], + [ + 0.25, + 0.35, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 26, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 0.9998985329952097 + ], + [ + 0.0, + 0.0, + 0.892648348196329 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 27, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.02143, + 0.04286, + 1.1071487177940904 + ], + [ + 0.04286, + 0.08571, + 1.1071487177940904 + ], + [ + 0.06429, + 0.12857, + 1.1071487177940904 + ], + [ + 0.08571, + 0.17143, + 1.1071487177940904 + ], + [ + 0.10714, + 0.21429, + 1.1071487177940904 + ], + [ + 0.12857, + 0.25714, + 1.1071487177940904 + ], + [ + 0.15, + 0.3, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 28, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.04286, + 0.02143, + 1.1071487177940904 + ], + [ + -0.08571, + 0.04286, + 1.1071487177940904 + ], + [ + -0.12857, + 0.06429, + 1.1071487177940904 + ], + [ + -0.17143, + 0.08571, + 1.1071487177940904 + ], + [ + -0.21429, + 0.10714, + 1.1071487177940904 + ], + [ + -0.25714, + 0.12857, + 1.1071487177940904 + ], + [ + -0.3, + 0.15, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 29, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.00623, + "arc_length": 0.0, + "straight_length": 1.00623, + "poses": [ + [ + 0.04286, + -0.02143, + 1.1071487177940904 + ], + [ + 0.08571, + -0.04286, + 1.1071487177940904 + ], + [ + 0.12857, + -0.06429, + 1.1071487177940904 + ], + [ + 0.17143, + -0.08571, + 1.1071487177940904 + ], + [ + 0.21429, + -0.10714, + 1.1071487177940904 + ], + [ + 0.25714, + -0.12857, + 1.1071487177940904 + ], + [ + 0.3, + -0.15, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 30, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.02148, + 0.04296, + 1.1071487177940904 + ], + [ + 0.04292, + 0.08595, + 1.1172148021235004 + ], + [ + 0.06298, + 0.12959, + 1.16257295459064 + ], + [ + 0.08104, + 0.1741, + 1.2079311070577796 + ], + [ + 0.09707, + 0.21937, + 1.2532892595249192 + ], + [ + 0.11102, + 0.26533, + 1.298647411992059 + ], + [ + 0.12288, + 0.31188, + 1.3440055644591986 + ], + [ + 0.13262, + 0.35891, + 1.3893637169263382 + ], + [ + 0.14021, + 0.40634, + 1.4347218693934778 + ], + [ + 0.14565, + 0.45406, + 1.4800800218606174 + ], + [ + 0.14891, + 0.50198, + 1.525438174327757 + ], + [ + 0.15, + 0.55, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 31, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 32, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.00109, + 0.04802, + 1.525438174327757 + ], + [ + 0.00435, + 0.09594, + 1.4800800218606174 + ], + [ + 0.00979, + 0.14366, + 1.4347218693934778 + ], + [ + 0.01738, + 0.19109, + 1.3893637169263382 + ], + [ + 0.02712, + 0.23812, + 1.3440055644591986 + ], + [ + 0.03898, + 0.28467, + 1.298647411992059 + ], + [ + 0.05293, + 0.33063, + 1.2532892595249194 + ], + [ + 0.06896, + 0.3759, + 1.2079311070577798 + ], + [ + 0.08702, + 0.42041, + 1.16257295459064 + ], + [ + 0.10708, + 0.46405, + 1.1172148021235004 + ], + [ + 0.12852, + 0.50704, + 1.1071487177940904 + ], + [ + 0.15, + 0.55, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 33, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.00049, + 0.05179, + 1.5446099427771882 + ], + [ + 0.00279, + 0.10353, + 1.5081548406952634 + ], + [ + 0.00697, + 0.15516, + 1.4716997386133386 + ], + [ + 0.01303, + 0.2066, + 1.4352446365314138 + ], + [ + 0.02097, + 0.25778, + 1.398789534449489 + ], + [ + 0.03076, + 0.30864, + 1.3623344323675641 + ], + [ + 0.0424, + 0.3591, + 1.3258793302856393 + ], + [ + 0.05587, + 0.40911, + 1.2894242282037145 + ], + [ + 0.07116, + 0.4586, + 1.2529691261217897 + ], + [ + 0.08824, + 0.5075, + 1.2165140240398649 + ], + [ + 0.10709, + 0.55574, + 1.18005892195794 + ], + [ + 0.12769, + 0.60326, + 1.1436038198760152 + ], + [ + 0.15, + 0.65, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 34, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 35, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.0, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 1.5707963267948966 + ], + [ + 0.0, + 0.15, + 1.5707963267948966 + ], + [ + 0.0, + 0.2, + 1.5707963267948966 + ], + [ + 0.0, + 0.25, + 1.5707963267948966 + ], + [ + 0.0, + 0.3, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 36, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + -0.05, + 0.0, + 1.5707963267948966 + ], + [ + -0.1, + 0.0, + 1.5707963267948966 + ], + [ + -0.15, + 0.0, + 1.5707963267948966 + ], + [ + -0.2, + 0.0, + 1.5707963267948966 + ], + [ + -0.25, + 0.0, + 1.5707963267948966 + ], + [ + -0.3, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 37, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.9, + "arc_length": 0.0, + "straight_length": 0.9, + "poses": [ + [ + 0.05, + 0.0, + 1.5707963267948966 + ], + [ + 0.1, + 0.0, + 1.5707963267948966 + ], + [ + 0.15, + 0.0, + 1.5707963267948966 + ], + [ + 0.2, + 0.0, + 1.5707963267948966 + ], + [ + 0.25, + 0.0, + 1.5707963267948966 + ], + [ + 0.3, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 38, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.00109, + 0.04802, + 1.6161544792620361 + ], + [ + -0.00435, + 0.09594, + 1.6615126317291757 + ], + [ + -0.00979, + 0.14366, + 1.7068707841963153 + ], + [ + -0.01738, + 0.19109, + 1.752228936663455 + ], + [ + -0.02712, + 0.23812, + 1.7975870891305945 + ], + [ + -0.03898, + 0.28467, + 1.842945241597734 + ], + [ + -0.05293, + 0.33063, + 1.8883033940648737 + ], + [ + -0.06896, + 0.3759, + 1.9336615465320133 + ], + [ + -0.08702, + 0.42041, + 1.979019698999153 + ], + [ + -0.10708, + 0.46405, + 2.0243778514662925 + ], + [ + -0.12852, + 0.50704, + 2.0344439357957027 + ], + [ + -0.15, + 0.55, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 39, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.00049, + 0.05179, + 1.596982710812605 + ], + [ + -0.00279, + 0.10353, + 1.6334378128945297 + ], + [ + -0.00697, + 0.15516, + 1.6698929149764545 + ], + [ + -0.01303, + 0.2066, + 1.7063480170583794 + ], + [ + -0.02097, + 0.25778, + 1.7428031191403042 + ], + [ + -0.03076, + 0.30864, + 1.779258221222229 + ], + [ + -0.0424, + 0.3591, + 1.8157133233041538 + ], + [ + -0.05587, + 0.40911, + 1.8521684253860786 + ], + [ + -0.07116, + 0.4586, + 1.8886235274680034 + ], + [ + -0.08824, + 0.5075, + 1.9250786295499283 + ], + [ + -0.10709, + 0.55574, + 1.961533731631853 + ], + [ + -0.12769, + 0.60326, + 1.997988833713778 + ], + [ + -0.15, + 0.65, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 40, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.7253455297951652 + ], + [ + 0.0, + 0.0, + 1.879894732795434 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 41, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.02148, + 0.04296, + 2.0344439357957027 + ], + [ + -0.04292, + 0.08595, + 2.0243778514662925 + ], + [ + -0.06298, + 0.12959, + 1.979019698999153 + ], + [ + -0.08104, + 0.1741, + 1.9336615465320135 + ], + [ + -0.09707, + 0.21937, + 1.888303394064874 + ], + [ + -0.11102, + 0.26533, + 1.8429452415977343 + ], + [ + -0.12288, + 0.31188, + 1.7975870891305947 + ], + [ + -0.13262, + 0.35891, + 1.752228936663455 + ], + [ + -0.14021, + 0.40634, + 1.7068707841963153 + ], + [ + -0.14565, + 0.45406, + 1.6615126317291757 + ], + [ + -0.14891, + 0.50198, + 1.6161544792620361 + ], + [ + -0.15, + 0.55, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 42, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 1.8026201312952996 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 43, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.02143, + 0.04286, + 2.0344439357957027 + ], + [ + -0.04286, + 0.08571, + 2.0344439357957027 + ], + [ + -0.06429, + 0.12857, + 2.0344439357957027 + ], + [ + -0.08571, + 0.17143, + 2.0344439357957027 + ], + [ + -0.10714, + 0.21429, + 2.0344439357957027 + ], + [ + -0.12857, + 0.25714, + 2.0344439357957027 + ], + [ + -0.15, + 0.3, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 44, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.04286, + -0.02143, + 2.0344439357957027 + ], + [ + -0.08571, + -0.04286, + 2.0344439357957027 + ], + [ + -0.12857, + -0.06429, + 2.0344439357957027 + ], + [ + -0.17143, + -0.08571, + 2.0344439357957027 + ], + [ + -0.21429, + -0.10714, + 2.0344439357957027 + ], + [ + -0.25714, + -0.12857, + 2.0344439357957027 + ], + [ + -0.3, + -0.15, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 45, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.00623, + "arc_length": 0.0, + "straight_length": 1.00623, + "poses": [ + [ + 0.04286, + 0.02143, + 2.0344439357957027 + ], + [ + 0.08571, + 0.04286, + 2.0344439357957027 + ], + [ + 0.12857, + 0.06429, + 2.0344439357957027 + ], + [ + 0.17143, + 0.08571, + 2.0344439357957027 + ], + [ + 0.21429, + 0.10714, + 2.0344439357957027 + ], + [ + 0.25714, + 0.12857, + 2.0344439357957027 + ], + [ + 0.3, + 0.15, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 46, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.02193, + 0.0427, + 2.0623898970595143 + ], + [ + -0.04536, + 0.0846, + 2.099115471201118 + ], + [ + -0.07032, + 0.12561, + 2.1358410453427217 + ], + [ + -0.09676, + 0.16568, + 2.172566619484326 + ], + [ + -0.12466, + 0.20474, + 2.2092921936259295 + ], + [ + -0.15397, + 0.24276, + 2.2460177677675333 + ], + [ + -0.18466, + 0.27967, + 2.2827433419091374 + ], + [ + -0.21669, + 0.31543, + 2.319468916050741 + ], + [ + -0.25, + 0.35, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 47, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 48, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03331, + 0.03457, + 2.319468916050741 + ], + [ + -0.06534, + 0.07033, + 2.282743341909137 + ], + [ + -0.09603, + 0.10724, + 2.2460177677675333 + ], + [ + -0.12534, + 0.14526, + 2.2092921936259295 + ], + [ + -0.15324, + 0.18432, + 2.172566619484326 + ], + [ + -0.17968, + 0.22439, + 2.1358410453427217 + ], + [ + -0.20464, + 0.2654, + 2.099115471201118 + ], + [ + -0.22807, + 0.3073, + 2.0623898970595143 + ], + [ + -0.25, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 49, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + 0.03685, + 2.356194490192345 + ], + [ + -0.07371, + 0.07371, + 2.356194490192345 + ], + [ + -0.11039, + 0.11073, + 2.337028082892005 + ], + [ + -0.14581, + 0.14896, + 2.299205064504967 + ], + [ + -0.17976, + 0.1885, + 2.2613820461179293 + ], + [ + -0.21219, + 0.22929, + 2.2235590277308916 + ], + [ + -0.24305, + 0.27128, + 2.1857360093438536 + ], + [ + -0.27231, + 0.31441, + 2.147912990956816 + ], + [ + -0.29991, + 0.35862, + 2.110089972569778 + ], + [ + -0.32582, + 0.40383, + 2.0722669541827403 + ], + [ + -0.35, + 0.45, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 50, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 51, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + -0.0375, + 0.0375, + 2.356194490192345 + ], + [ + -0.075, + 0.075, + 2.356194490192345 + ], + [ + -0.1125, + 0.1125, + 2.356194490192345 + ], + [ + -0.15, + 0.15, + 2.356194490192345 + ], + [ + -0.1875, + 0.1875, + 2.356194490192345 + ], + [ + -0.225, + 0.225, + 2.356194490192345 + ], + [ + -0.2625, + 0.2625, + 2.356194490192345 + ], + [ + -0.3, + 0.3, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 52, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + -0.0375, + -0.0375, + 2.356194490192345 + ], + [ + -0.075, + -0.075, + 2.356194490192345 + ], + [ + -0.1125, + -0.1125, + 2.356194490192345 + ], + [ + -0.15, + -0.15, + 2.356194490192345 + ], + [ + -0.1875, + -0.1875, + 2.356194490192345 + ], + [ + -0.225, + -0.225, + 2.356194490192345 + ], + [ + -0.2625, + -0.2625, + 2.356194490192345 + ], + [ + -0.3, + -0.3, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 53, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.27279, + "arc_length": 0.0, + "straight_length": 1.27279, + "poses": [ + [ + 0.0375, + 0.0375, + 2.356194490192345 + ], + [ + 0.075, + 0.075, + 2.356194490192345 + ], + [ + 0.1125, + 0.1125, + 2.356194490192345 + ], + [ + 0.15, + 0.15, + 2.356194490192345 + ], + [ + 0.1875, + 0.1875, + 2.356194490192345 + ], + [ + 0.225, + 0.225, + 2.356194490192345 + ], + [ + 0.2625, + 0.2625, + 2.356194490192345 + ], + [ + 0.3, + 0.3, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 54, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03457, + 0.03331, + 2.3929200643339485 + ], + [ + -0.07033, + 0.06534, + 2.4296456384755527 + ], + [ + -0.10724, + 0.09603, + 2.4663712126171564 + ], + [ + -0.14526, + 0.12534, + 2.50309678675876 + ], + [ + -0.18432, + 0.15324, + 2.539822360900364 + ], + [ + -0.22439, + 0.17968, + 2.576547935041968 + ], + [ + -0.2654, + 0.20464, + 2.6132735091835717 + ], + [ + -0.3073, + 0.22807, + 2.6499990833251754 + ], + [ + -0.35, + 0.25, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 55, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + 0.03685, + 2.356194490192345 + ], + [ + -0.07371, + 0.07371, + 2.356194490192345 + ], + [ + -0.11073, + 0.11039, + 2.3753608974926848 + ], + [ + -0.14896, + 0.14581, + 2.413183915879723 + ], + [ + -0.1885, + 0.17976, + 2.4510069342667604 + ], + [ + -0.22929, + 0.21219, + 2.488829952653798 + ], + [ + -0.27128, + 0.24305, + 2.526652971040836 + ], + [ + -0.31441, + 0.27231, + 2.5644759894278737 + ], + [ + -0.35862, + 0.29991, + 2.6022990078149117 + ], + [ + -0.40383, + 0.32582, + 2.6401220262019494 + ], + [ + -0.45, + 0.35, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 56, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.517069767390666 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 57, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.0427, + 0.02193, + 2.6499990833251754 + ], + [ + -0.0846, + 0.04536, + 2.6132735091835717 + ], + [ + -0.12561, + 0.07032, + 2.576547935041968 + ], + [ + -0.16568, + 0.09676, + 2.539822360900364 + ], + [ + -0.20474, + 0.12466, + 2.50309678675876 + ], + [ + -0.24276, + 0.15397, + 2.4663712126171564 + ], + [ + -0.27967, + 0.18466, + 2.4296456384755523 + ], + [ + -0.31543, + 0.21669, + 2.3929200643339485 + ], + [ + -0.35, + 0.25, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 58, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.5706948597901063 + ], + [ + 0.0, + 0.0, + 2.4634446749912255 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 59, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.04286, + 0.02143, + 2.677945044588987 + ], + [ + -0.08571, + 0.04286, + 2.677945044588987 + ], + [ + -0.12857, + 0.06429, + 2.677945044588987 + ], + [ + -0.17143, + 0.08571, + 2.677945044588987 + ], + [ + -0.21429, + 0.10714, + 2.677945044588987 + ], + [ + -0.25714, + 0.12857, + 2.677945044588987 + ], + [ + -0.3, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 60, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.02143, + -0.04286, + 2.677945044588987 + ], + [ + -0.04286, + -0.08571, + 2.677945044588987 + ], + [ + -0.06429, + -0.12857, + 2.677945044588987 + ], + [ + -0.08571, + -0.17143, + 2.677945044588987 + ], + [ + -0.10714, + -0.21429, + 2.677945044588987 + ], + [ + -0.12857, + -0.25714, + 2.677945044588987 + ], + [ + -0.15, + -0.3, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 61, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.00623, + "arc_length": 0.0, + "straight_length": 1.00623, + "poses": [ + [ + 0.02143, + 0.04286, + 2.677945044588987 + ], + [ + 0.04286, + 0.08571, + 2.677945044588987 + ], + [ + 0.06429, + 0.12857, + 2.677945044588987 + ], + [ + 0.08571, + 0.17143, + 2.677945044588987 + ], + [ + 0.10714, + 0.21429, + 2.677945044588987 + ], + [ + 0.12857, + 0.25714, + 2.677945044588987 + ], + [ + 0.15, + 0.3, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 62, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04296, + 0.02148, + 2.677945044588987 + ], + [ + -0.08595, + 0.04292, + 2.688011128918397 + ], + [ + -0.12959, + 0.06298, + 2.733369281385537 + ], + [ + -0.1741, + 0.08104, + 2.7787274338526764 + ], + [ + -0.21937, + 0.09707, + 2.824085586319816 + ], + [ + -0.26533, + 0.11102, + 2.8694437387869556 + ], + [ + -0.31188, + 0.12288, + 2.914801891254095 + ], + [ + -0.35891, + 0.13262, + 2.9601600437212348 + ], + [ + -0.40634, + 0.14021, + 3.0055181961883743 + ], + [ + -0.45406, + 0.14565, + 3.050876348655514 + ], + [ + -0.50198, + 0.14891, + 3.0962345011226535 + ], + [ + -0.55, + 0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 63, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 64, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04802, + -0.00109, + 3.1869508060569327 + ], + [ + -0.09594, + -0.00435, + 3.2323089585240723 + ], + [ + -0.14366, + -0.00979, + 3.277667110991212 + ], + [ + -0.19109, + -0.01738, + 3.3230252634583515 + ], + [ + -0.23812, + -0.02712, + 3.368383415925491 + ], + [ + -0.28467, + -0.03898, + 3.4137415683926307 + ], + [ + -0.33063, + -0.05293, + 3.4590997208597702 + ], + [ + -0.3759, + -0.06896, + 3.50445787332691 + ], + [ + -0.42041, + -0.08702, + 3.5498160257940494 + ], + [ + -0.46405, + -0.10708, + 3.595174178261189 + ], + [ + -0.50704, + -0.12852, + 3.6052402625905993 + ], + [ + -0.55, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 65, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.05179, + -0.00049, + 3.1677790376075015 + ], + [ + -0.10353, + -0.00279, + 3.2042341396894263 + ], + [ + -0.15516, + -0.00697, + 3.240689241771351 + ], + [ + -0.2066, + -0.01303, + 3.277144343853276 + ], + [ + -0.25778, + -0.02097, + 3.3135994459352007 + ], + [ + -0.30864, + -0.03076, + 3.3500545480171255 + ], + [ + -0.3591, + -0.0424, + 3.3865096500990504 + ], + [ + -0.40911, + -0.05587, + 3.422964752180975 + ], + [ + -0.4586, + -0.07116, + 3.4594198542629 + ], + [ + -0.5075, + -0.08824, + 3.495874956344825 + ], + [ + -0.55574, + -0.10709, + 3.5323300584267496 + ], + [ + -0.60326, + -0.12769, + 3.5687851605086744 + ], + [ + -0.65, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 66, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 3.296141856590062 + ], + [ + 0.0, + 0.0, + 3.4506910595903304 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 67, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04802, + 0.00109, + 3.0962345011226535 + ], + [ + -0.09594, + 0.00435, + 3.050876348655514 + ], + [ + -0.14366, + 0.00979, + 3.0055181961883743 + ], + [ + -0.19109, + 0.01738, + 2.9601600437212348 + ], + [ + -0.23812, + 0.02712, + 2.914801891254095 + ], + [ + -0.28467, + 0.03898, + 2.8694437387869556 + ], + [ + -0.33063, + 0.05293, + 2.824085586319816 + ], + [ + -0.3759, + 0.06896, + 2.7787274338526764 + ], + [ + -0.42041, + 0.08702, + 2.733369281385537 + ], + [ + -0.46405, + 0.10708, + 2.688011128918397 + ], + [ + -0.50704, + 0.12852, + 2.677945044588987 + ], + [ + -0.55, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 68, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.05179, + 0.00049, + 3.1154062695720848 + ], + [ + -0.10353, + 0.00279, + 3.07895116749016 + ], + [ + -0.15516, + 0.00697, + 3.042496065408235 + ], + [ + -0.2066, + 0.01303, + 3.0060409633263103 + ], + [ + -0.25778, + 0.02097, + 2.9695858612443855 + ], + [ + -0.30864, + 0.03076, + 2.9331307591624607 + ], + [ + -0.3591, + 0.0424, + 2.896675657080536 + ], + [ + -0.40911, + 0.05587, + 2.860220554998611 + ], + [ + -0.4586, + 0.07116, + 2.8237654529166862 + ], + [ + -0.5075, + 0.08824, + 2.7873103508347614 + ], + [ + -0.55574, + 0.10709, + 2.7508552487528366 + ], + [ + -0.60326, + 0.12769, + 2.714400146670912 + ], + [ + -0.65, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 69, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 70, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + -0.05, + 0.0, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 3.141592653589793 + ], + [ + -0.15, + 0.0, + 3.141592653589793 + ], + [ + -0.2, + 0.0, + 3.141592653589793 + ], + [ + -0.25, + 0.0, + 3.141592653589793 + ], + [ + -0.3, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 71, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.0, + -0.05, + 3.141592653589793 + ], + [ + 0.0, + -0.1, + 3.141592653589793 + ], + [ + 0.0, + -0.15, + 3.141592653589793 + ], + [ + 0.0, + -0.2, + 3.141592653589793 + ], + [ + 0.0, + -0.25, + 3.141592653589793 + ], + [ + 0.0, + -0.3, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 72, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.9, + "arc_length": 0.0, + "straight_length": 0.9, + "poses": [ + [ + 0.0, + 0.05, + 3.141592653589793 + ], + [ + 0.0, + 0.1, + 3.141592653589793 + ], + [ + 0.0, + 0.15, + 3.141592653589793 + ], + [ + 0.0, + 0.2, + 3.141592653589793 + ], + [ + 0.0, + 0.25, + 3.141592653589793 + ], + [ + 0.0, + 0.3, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 73, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.04286, + -0.02143, + 3.6052402625905993 + ], + [ + -0.08571, + -0.04286, + 3.6052402625905993 + ], + [ + -0.12857, + -0.06429, + 3.6052402625905993 + ], + [ + -0.17143, + -0.08571, + 3.6052402625905993 + ], + [ + -0.21429, + -0.10714, + 3.6052402625905993 + ], + [ + -0.25714, + -0.12857, + 3.6052402625905993 + ], + [ + -0.3, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 74, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.02143, + -0.04286, + 3.6052402625905993 + ], + [ + 0.04286, + -0.08571, + 3.6052402625905993 + ], + [ + 0.06429, + -0.12857, + 3.6052402625905993 + ], + [ + 0.08571, + -0.17143, + 3.6052402625905993 + ], + [ + 0.10714, + -0.21429, + 3.6052402625905993 + ], + [ + 0.12857, + -0.25714, + 3.6052402625905993 + ], + [ + 0.15, + -0.3, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 75, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.00623, + "arc_length": 0.0, + "straight_length": 1.00623, + "poses": [ + [ + -0.02143, + 0.04286, + 3.6052402625905993 + ], + [ + -0.04286, + 0.08571, + 3.6052402625905993 + ], + [ + -0.06429, + 0.12857, + 3.6052402625905993 + ], + [ + -0.08571, + 0.17143, + 3.6052402625905993 + ], + [ + -0.10714, + 0.21429, + 3.6052402625905993 + ], + [ + -0.12857, + 0.25714, + 3.6052402625905993 + ], + [ + -0.15, + 0.3, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 76, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.0427, + -0.02193, + 3.633186223854411 + ], + [ + -0.0846, + -0.04536, + 3.6699117979960145 + ], + [ + -0.12561, + -0.07032, + 3.7066373721376182 + ], + [ + -0.16568, + -0.09676, + 3.7433629462792224 + ], + [ + -0.20474, + -0.12466, + 3.780088520420826 + ], + [ + -0.24276, + -0.15397, + 3.81681409456243 + ], + [ + -0.27967, + -0.18466, + 3.853539668704034 + ], + [ + -0.31543, + -0.21669, + 3.8902652428456377 + ], + [ + -0.35, + -0.25, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 77, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 78, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.04296, + -0.02148, + 3.6052402625905993 + ], + [ + -0.08595, + -0.04292, + 3.595174178261189 + ], + [ + -0.12959, + -0.06298, + 3.5498160257940494 + ], + [ + -0.1741, + -0.08104, + 3.50445787332691 + ], + [ + -0.21937, + -0.09707, + 3.4590997208597702 + ], + [ + -0.26533, + -0.11102, + 3.4137415683926307 + ], + [ + -0.31188, + -0.12288, + 3.368383415925491 + ], + [ + -0.35891, + -0.13262, + 3.3230252634583515 + ], + [ + -0.40634, + -0.14021, + 3.277667110991212 + ], + [ + -0.45406, + -0.14565, + 3.2323089585240723 + ], + [ + -0.50198, + -0.14891, + 3.1869508060569327 + ], + [ + -0.55, + -0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 79, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.373416458090196 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 80, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03457, + -0.03331, + 3.8902652428456377 + ], + [ + -0.07033, + -0.06534, + 3.8535396687040335 + ], + [ + -0.10724, + -0.09603, + 3.81681409456243 + ], + [ + -0.14526, + -0.12534, + 3.780088520420826 + ], + [ + -0.18432, + -0.15324, + 3.7433629462792224 + ], + [ + -0.22439, + -0.17968, + 3.7066373721376182 + ], + [ + -0.2654, + -0.20464, + 3.6699117979960145 + ], + [ + -0.3073, + -0.22807, + 3.633186223854411 + ], + [ + -0.35, + -0.25, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 81, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + -0.03685, + 3.9269908169872414 + ], + [ + -0.07371, + -0.07371, + 3.9269908169872414 + ], + [ + -0.11073, + -0.11039, + 3.9078244096869015 + ], + [ + -0.14896, + -0.14581, + 3.8700013912998634 + ], + [ + -0.1885, + -0.17976, + 3.832178372912826 + ], + [ + -0.22929, + -0.21219, + 3.794355354525788 + ], + [ + -0.27128, + -0.24305, + 3.75653233613875 + ], + [ + -0.31441, + -0.27231, + 3.7187093177517125 + ], + [ + -0.35862, + -0.29991, + 3.6808862993646745 + ], + [ + -0.40383, + -0.32582, + 3.643063280977637 + ], + [ + -0.45, + -0.35, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 82, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 83, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + -0.0375, + -0.0375, + 3.9269908169872414 + ], + [ + -0.075, + -0.075, + 3.9269908169872414 + ], + [ + -0.1125, + -0.1125, + 3.9269908169872414 + ], + [ + -0.15, + -0.15, + 3.9269908169872414 + ], + [ + -0.1875, + -0.1875, + 3.9269908169872414 + ], + [ + -0.225, + -0.225, + 3.9269908169872414 + ], + [ + -0.2625, + -0.2625, + 3.9269908169872414 + ], + [ + -0.3, + -0.3, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 84, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + 0.0375, + -0.0375, + 3.9269908169872414 + ], + [ + 0.075, + -0.075, + 3.9269908169872414 + ], + [ + 0.1125, + -0.1125, + 3.9269908169872414 + ], + [ + 0.15, + -0.15, + 3.9269908169872414 + ], + [ + 0.1875, + -0.1875, + 3.9269908169872414 + ], + [ + 0.225, + -0.225, + 3.9269908169872414 + ], + [ + 0.2625, + -0.2625, + 3.9269908169872414 + ], + [ + 0.3, + -0.3, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 85, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.27279, + "arc_length": 0.0, + "straight_length": 1.27279, + "poses": [ + [ + -0.0375, + 0.0375, + 3.9269908169872414 + ], + [ + -0.075, + 0.075, + 3.9269908169872414 + ], + [ + -0.1125, + 0.1125, + 3.9269908169872414 + ], + [ + -0.15, + 0.15, + 3.9269908169872414 + ], + [ + -0.1875, + 0.1875, + 3.9269908169872414 + ], + [ + -0.225, + 0.225, + 3.9269908169872414 + ], + [ + -0.2625, + 0.2625, + 3.9269908169872414 + ], + [ + -0.3, + 0.3, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 86, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.03331, + -0.03457, + 3.963716391128845 + ], + [ + -0.06534, + -0.07033, + 4.000441965270449 + ], + [ + -0.09603, + -0.10724, + 4.037167539412053 + ], + [ + -0.12534, + -0.14526, + 4.073893113553657 + ], + [ + -0.15324, + -0.18432, + 4.11061868769526 + ], + [ + -0.17968, + -0.22439, + 4.1473442618368646 + ], + [ + -0.20464, + -0.2654, + 4.184069835978468 + ], + [ + -0.22807, + -0.3073, + 4.220795410120072 + ], + [ + -0.25, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 87, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + -0.03685, + -0.03685, + 3.9269908169872414 + ], + [ + -0.07371, + -0.07371, + 3.9269908169872414 + ], + [ + -0.11039, + -0.11073, + 3.9461572242875813 + ], + [ + -0.14581, + -0.14896, + 3.9839802426746194 + ], + [ + -0.17976, + -0.1885, + 4.021803261061657 + ], + [ + -0.21219, + -0.22929, + 4.059626279448695 + ], + [ + -0.24305, + -0.27128, + 4.097449297835732 + ], + [ + -0.27231, + -0.31441, + 4.13527231622277 + ], + [ + -0.29991, + -0.35862, + 4.173095334609808 + ], + [ + -0.32582, + -0.40383, + 4.2109183529968455 + ], + [ + -0.35, + -0.45, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 88, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 4.0878660941855625 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 89, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + -0.02193, + -0.0427, + 4.220795410120072 + ], + [ + -0.04536, + -0.0846, + 4.184069835978468 + ], + [ + -0.07032, + -0.12561, + 4.1473442618368646 + ], + [ + -0.09676, + -0.16568, + 4.11061868769526 + ], + [ + -0.12466, + -0.20474, + 4.073893113553657 + ], + [ + -0.15397, + -0.24276, + 4.037167539412053 + ], + [ + -0.18466, + -0.27967, + 4.000441965270449 + ], + [ + -0.21669, + -0.31543, + 3.963716391128845 + ], + [ + -0.25, + -0.35, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 90, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.141491186585003 + ], + [ + 0.0, + 0.0, + 4.034241001786122 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 91, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + -0.02143, + -0.04286, + 4.2487413713838835 + ], + [ + -0.04286, + -0.08571, + 4.2487413713838835 + ], + [ + -0.06429, + -0.12857, + 4.2487413713838835 + ], + [ + -0.08571, + -0.17143, + 4.2487413713838835 + ], + [ + -0.10714, + -0.21429, + 4.2487413713838835 + ], + [ + -0.12857, + -0.25714, + 4.2487413713838835 + ], + [ + -0.15, + -0.3, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 92, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.04286, + -0.02143, + 4.2487413713838835 + ], + [ + 0.08571, + -0.04286, + 4.2487413713838835 + ], + [ + 0.12857, + -0.06429, + 4.2487413713838835 + ], + [ + 0.17143, + -0.08571, + 4.2487413713838835 + ], + [ + 0.21429, + -0.10714, + 4.2487413713838835 + ], + [ + 0.25714, + -0.12857, + 4.2487413713838835 + ], + [ + 0.3, + -0.15, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 93, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.00623, + "arc_length": 0.0, + "straight_length": 1.00623, + "poses": [ + [ + -0.04286, + 0.02143, + 4.2487413713838835 + ], + [ + -0.08571, + 0.04286, + 4.2487413713838835 + ], + [ + -0.12857, + 0.06429, + 4.2487413713838835 + ], + [ + -0.17143, + 0.08571, + 4.2487413713838835 + ], + [ + -0.21429, + 0.10714, + 4.2487413713838835 + ], + [ + -0.25714, + 0.12857, + 4.2487413713838835 + ], + [ + -0.3, + 0.15, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 94, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.02148, + -0.04296, + 4.2487413713838835 + ], + [ + -0.04292, + -0.08595, + 4.258807455713294 + ], + [ + -0.06298, + -0.12959, + 4.304165608180433 + ], + [ + -0.08104, + -0.1741, + 4.349523760647573 + ], + [ + -0.09707, + -0.21937, + 4.3948819131147125 + ], + [ + -0.11102, + -0.26533, + 4.440240065581852 + ], + [ + -0.12288, + -0.31188, + 4.485598218048992 + ], + [ + -0.13262, + -0.35891, + 4.530956370516131 + ], + [ + -0.14021, + -0.40634, + 4.576314522983271 + ], + [ + -0.14565, + -0.45406, + 4.6216726754504105 + ], + [ + -0.14891, + -0.50198, + 4.66703082791755 + ], + [ + -0.15, + -0.55, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 95, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 96, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + -0.00109, + -0.04802, + 4.66703082791755 + ], + [ + -0.00435, + -0.09594, + 4.6216726754504105 + ], + [ + -0.00979, + -0.14366, + 4.576314522983271 + ], + [ + -0.01738, + -0.19109, + 4.530956370516131 + ], + [ + -0.02712, + -0.23812, + 4.485598218048992 + ], + [ + -0.03898, + -0.28467, + 4.440240065581852 + ], + [ + -0.05293, + -0.33063, + 4.3948819131147125 + ], + [ + -0.06896, + -0.3759, + 4.349523760647573 + ], + [ + -0.08702, + -0.42041, + 4.304165608180433 + ], + [ + -0.10708, + -0.46405, + 4.258807455713294 + ], + [ + -0.12852, + -0.50704, + 4.2487413713838835 + ], + [ + -0.15, + -0.55, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 97, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + -0.00049, + -0.05179, + 4.686202596366981 + ], + [ + -0.00279, + -0.10353, + 4.649747494285057 + ], + [ + -0.00697, + -0.15516, + 4.613292392203132 + ], + [ + -0.01303, + -0.2066, + 4.576837290121206 + ], + [ + -0.02097, + -0.25778, + 4.540382188039282 + ], + [ + -0.03076, + -0.30864, + 4.503927085957358 + ], + [ + -0.0424, + -0.3591, + 4.467471983875432 + ], + [ + -0.05587, + -0.40911, + 4.431016881793507 + ], + [ + -0.07116, + -0.4586, + 4.394561779711583 + ], + [ + -0.08824, + -0.5075, + 4.358106677629658 + ], + [ + -0.10709, + -0.55574, + 4.321651575547733 + ], + [ + -0.12769, + -0.60326, + 4.285196473465808 + ], + [ + -0.15, + -0.65, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 98, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 99, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.0, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 4.71238898038469 + ], + [ + 0.0, + -0.15, + 4.71238898038469 + ], + [ + 0.0, + -0.2, + 4.71238898038469 + ], + [ + 0.0, + -0.25, + 4.71238898038469 + ], + [ + 0.0, + -0.3, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 100, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.3, + "arc_length": 0.0, + "straight_length": 0.3, + "poses": [ + [ + 0.05, + 0.0, + 4.71238898038469 + ], + [ + 0.1, + 0.0, + 4.71238898038469 + ], + [ + 0.15, + 0.0, + 4.71238898038469 + ], + [ + 0.2, + 0.0, + 4.71238898038469 + ], + [ + 0.25, + 0.0, + 4.71238898038469 + ], + [ + 0.3, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 101, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.9, + "arc_length": 0.0, + "straight_length": 0.9, + "poses": [ + [ + -0.05, + 0.0, + 4.71238898038469 + ], + [ + -0.1, + 0.0, + 4.71238898038469 + ], + [ + -0.15, + 0.0, + 4.71238898038469 + ], + [ + -0.2, + 0.0, + 4.71238898038469 + ], + [ + -0.25, + 0.0, + 4.71238898038469 + ], + [ + -0.3, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 102, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.00109, + -0.04802, + 4.757747132851829 + ], + [ + 0.00435, + -0.09594, + 4.803105285318969 + ], + [ + 0.00979, + -0.14366, + 4.848463437786108 + ], + [ + 0.01738, + -0.19109, + 4.893821590253248 + ], + [ + 0.02712, + -0.23812, + 4.939179742720388 + ], + [ + 0.03898, + -0.28467, + 4.984537895187527 + ], + [ + 0.05293, + -0.33063, + 5.029896047654667 + ], + [ + 0.06896, + -0.3759, + 5.075254200121806 + ], + [ + 0.08702, + -0.42041, + 5.120612352588946 + ], + [ + 0.10708, + -0.46405, + 5.165970505056086 + ], + [ + 0.12852, + -0.50704, + 5.176036589385496 + ], + [ + 0.15, + -0.55, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 103, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 1.42082, + "trajectory_length": 0.67335, + "arc_length": 0.65876, + "straight_length": 0.01459, + "poses": [ + [ + 0.00049, + -0.05179, + 4.738575364402398 + ], + [ + 0.00279, + -0.10353, + 4.775030466484322 + ], + [ + 0.00697, + -0.15516, + 4.811485568566248 + ], + [ + 0.01303, + -0.2066, + 4.847940670648173 + ], + [ + 0.02097, + -0.25778, + 4.884395772730097 + ], + [ + 0.03076, + -0.30864, + 4.920850874812022 + ], + [ + 0.0424, + -0.3591, + 4.957305976893947 + ], + [ + 0.05587, + -0.40911, + 4.993761078975872 + ], + [ + 0.07116, + -0.4586, + 5.0302161810577966 + ], + [ + 0.08824, + -0.5075, + 5.066671283139721 + ], + [ + 0.10709, + -0.55574, + 5.103126385221646 + ], + [ + 0.12769, + -0.60326, + 5.1395814873035714 + ], + [ + 0.15, + -0.65, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 104, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.866938183384958 + ], + [ + 0.0, + 0.0, + 5.021487386385227 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 105, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.02148, + -0.04296, + 5.176036589385496 + ], + [ + 0.04292, + -0.08595, + 5.165970505056086 + ], + [ + 0.06298, + -0.12959, + 5.120612352588946 + ], + [ + 0.08104, + -0.1741, + 5.075254200121806 + ], + [ + 0.09707, + -0.21937, + 5.029896047654667 + ], + [ + 0.11102, + -0.26533, + 4.984537895187527 + ], + [ + 0.12288, + -0.31188, + 4.939179742720388 + ], + [ + 0.13262, + -0.35891, + 4.893821590253248 + ], + [ + 0.14021, + -0.40634, + 4.848463437786108 + ], + [ + 0.14565, + -0.45406, + 4.803105285318969 + ], + [ + 0.14891, + -0.50198, + 4.757747132851829 + ], + [ + 0.15, + -0.55, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 106, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 4.944212784885092 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 107, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.02143, + -0.04286, + 5.176036589385496 + ], + [ + 0.04286, + -0.08571, + 5.176036589385496 + ], + [ + 0.06429, + -0.12857, + 5.176036589385496 + ], + [ + 0.08571, + -0.17143, + 5.176036589385496 + ], + [ + 0.10714, + -0.21429, + 5.176036589385496 + ], + [ + 0.12857, + -0.25714, + 5.176036589385496 + ], + [ + 0.15, + -0.3, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 108, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.04286, + 0.02143, + 5.176036589385496 + ], + [ + 0.08571, + 0.04286, + 5.176036589385496 + ], + [ + 0.12857, + 0.06429, + 5.176036589385496 + ], + [ + 0.17143, + 0.08571, + 5.176036589385496 + ], + [ + 0.21429, + 0.10714, + 5.176036589385496 + ], + [ + 0.25714, + 0.12857, + 5.176036589385496 + ], + [ + 0.3, + 0.15, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 109, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.00623, + "arc_length": 0.0, + "straight_length": 1.00623, + "poses": [ + [ + -0.04286, + -0.02143, + 5.176036589385496 + ], + [ + -0.08571, + -0.04286, + 5.176036589385496 + ], + [ + -0.12857, + -0.06429, + 5.176036589385496 + ], + [ + -0.17143, + -0.08571, + 5.176036589385496 + ], + [ + -0.21429, + -0.10714, + 5.176036589385496 + ], + [ + -0.25714, + -0.12857, + 5.176036589385496 + ], + [ + -0.3, + -0.15, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 110, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.02193, + -0.0427, + 5.203982550649307 + ], + [ + 0.04536, + -0.0846, + 5.2407081247909115 + ], + [ + 0.07032, + -0.12561, + 5.277433698932515 + ], + [ + 0.09676, + -0.16568, + 5.314159273074119 + ], + [ + 0.12466, + -0.20474, + 5.350884847215722 + ], + [ + 0.15397, + -0.24276, + 5.387610421357326 + ], + [ + 0.18466, + -0.27967, + 5.4243359954989305 + ], + [ + 0.21669, + -0.31543, + 5.461061569640534 + ], + [ + 0.25, + -0.35, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 111, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 112, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03331, + -0.03457, + 5.461061569640534 + ], + [ + 0.06534, + -0.07033, + 5.4243359954989305 + ], + [ + 0.09603, + -0.10724, + 5.387610421357326 + ], + [ + 0.12534, + -0.14526, + 5.350884847215722 + ], + [ + 0.15324, + -0.18432, + 5.314159273074119 + ], + [ + 0.17968, + -0.22439, + 5.277433698932515 + ], + [ + 0.20464, + -0.2654, + 5.2407081247909115 + ], + [ + 0.22807, + -0.3073, + 5.203982550649307 + ], + [ + 0.25, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 113, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + -0.03685, + 5.497787143782138 + ], + [ + 0.07371, + -0.07371, + 5.497787143782138 + ], + [ + 0.11039, + -0.11073, + 5.478620736481798 + ], + [ + 0.14581, + -0.14896, + 5.44079771809476 + ], + [ + 0.17976, + -0.1885, + 5.402974699707722 + ], + [ + 0.21219, + -0.22929, + 5.365151681320684 + ], + [ + 0.24305, + -0.27128, + 5.327328662933647 + ], + [ + 0.27231, + -0.31441, + 5.289505644546609 + ], + [ + 0.29991, + -0.35862, + 5.251682626159571 + ], + [ + 0.32582, + -0.40383, + 5.213859607772534 + ], + [ + 0.35, + -0.45, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 114, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 115, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + 0.0375, + -0.0375, + 5.497787143782138 + ], + [ + 0.075, + -0.075, + 5.497787143782138 + ], + [ + 0.1125, + -0.1125, + 5.497787143782138 + ], + [ + 0.15, + -0.15, + 5.497787143782138 + ], + [ + 0.1875, + -0.1875, + 5.497787143782138 + ], + [ + 0.225, + -0.225, + 5.497787143782138 + ], + [ + 0.2625, + -0.2625, + 5.497787143782138 + ], + [ + 0.3, + -0.3, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 116, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.42426, + "arc_length": 0.0, + "straight_length": 0.42426, + "poses": [ + [ + 0.0375, + 0.0375, + 5.497787143782138 + ], + [ + 0.075, + 0.075, + 5.497787143782138 + ], + [ + 0.1125, + 0.1125, + 5.497787143782138 + ], + [ + 0.15, + 0.15, + 5.497787143782138 + ], + [ + 0.1875, + 0.1875, + 5.497787143782138 + ], + [ + 0.225, + 0.225, + 5.497787143782138 + ], + [ + 0.2625, + 0.2625, + 5.497787143782138 + ], + [ + 0.3, + 0.3, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 117, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.27279, + "arc_length": 0.0, + "straight_length": 1.27279, + "poses": [ + [ + -0.0375, + -0.0375, + 5.497787143782138 + ], + [ + -0.075, + -0.075, + 5.497787143782138 + ], + [ + -0.1125, + -0.1125, + 5.497787143782138 + ], + [ + -0.15, + -0.15, + 5.497787143782138 + ], + [ + -0.1875, + -0.1875, + 5.497787143782138 + ], + [ + -0.225, + -0.225, + 5.497787143782138 + ], + [ + -0.2625, + -0.2625, + 5.497787143782138 + ], + [ + -0.3, + -0.3, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 118, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.03457, + -0.03331, + 5.534512717923742 + ], + [ + 0.07033, + -0.06534, + 5.571238292065345 + ], + [ + 0.10724, + -0.09603, + 5.6079638662069495 + ], + [ + 0.14526, + -0.12534, + 5.644689440348554 + ], + [ + 0.18432, + -0.15324, + 5.681415014490157 + ], + [ + 0.22439, + -0.17968, + 5.718140588631761 + ], + [ + 0.2654, + -0.20464, + 5.754866162773365 + ], + [ + 0.3073, + -0.22807, + 5.7915917369149685 + ], + [ + 0.35, + -0.25, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 119, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 1.37794, + "trajectory_length": 0.5733, + "arc_length": 0.44335, + "straight_length": 0.12994, + "poses": [ + [ + 0.03685, + -0.03685, + 5.497787143782138 + ], + [ + 0.07371, + -0.07371, + 5.497787143782138 + ], + [ + 0.11073, + -0.11039, + 5.516953551082478 + ], + [ + 0.14896, + -0.14581, + 5.5547765694695155 + ], + [ + 0.1885, + -0.17976, + 5.5925995878565535 + ], + [ + 0.22929, + -0.21219, + 5.630422606243592 + ], + [ + 0.27128, + -0.24305, + 5.668245624630629 + ], + [ + 0.31441, + -0.27231, + 5.706068643017667 + ], + [ + 0.35862, + -0.29991, + 5.743891661404705 + ], + [ + 0.40383, + -0.32582, + 5.781714679791742 + ], + [ + 0.45, + -0.35, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 120, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.658662420980459 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 121, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 1.30721, + "trajectory_length": 0.43207, + "arc_length": 0.4206, + "straight_length": 0.01148, + "poses": [ + [ + 0.0427, + -0.02193, + 5.7915917369149685 + ], + [ + 0.0846, + -0.04536, + 5.754866162773364 + ], + [ + 0.12561, + -0.07032, + 5.718140588631761 + ], + [ + 0.16568, + -0.09676, + 5.681415014490157 + ], + [ + 0.20474, + -0.12466, + 5.644689440348554 + ], + [ + 0.24276, + -0.15397, + 5.6079638662069495 + ], + [ + 0.27967, + -0.18466, + 5.571238292065345 + ], + [ + 0.31543, + -0.21669, + 5.534512717923742 + ], + [ + 0.35, + -0.25, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 122, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.712287513379899 + ], + [ + 0.0, + 0.0, + 5.605037328581019 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 123, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.04286, + -0.02143, + 5.81953769817878 + ], + [ + 0.08571, + -0.04286, + 5.81953769817878 + ], + [ + 0.12857, + -0.06429, + 5.81953769817878 + ], + [ + 0.17143, + -0.08571, + 5.81953769817878 + ], + [ + 0.21429, + -0.10714, + 5.81953769817878 + ], + [ + 0.25714, + -0.12857, + 5.81953769817878 + ], + [ + 0.3, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 124, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.33541, + "arc_length": 0.0, + "straight_length": 0.33541, + "poses": [ + [ + 0.02143, + 0.04286, + 5.81953769817878 + ], + [ + 0.04286, + 0.08571, + 5.81953769817878 + ], + [ + 0.06429, + 0.12857, + 5.81953769817878 + ], + [ + 0.08571, + 0.17143, + 5.81953769817878 + ], + [ + 0.10714, + 0.21429, + 5.81953769817878 + ], + [ + 0.12857, + 0.25714, + 5.81953769817878 + ], + [ + 0.15, + 0.3, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 125, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 1.00623, + "arc_length": 0.0, + "straight_length": 1.00623, + "poses": [ + [ + -0.02143, + -0.04286, + 5.81953769817878 + ], + [ + -0.04286, + -0.08571, + 5.81953769817878 + ], + [ + -0.06429, + -0.12857, + 5.81953769817878 + ], + [ + -0.08571, + -0.17143, + 5.81953769817878 + ], + [ + -0.10714, + -0.21429, + 5.81953769817878 + ], + [ + -0.12857, + -0.25714, + 5.81953769817878 + ], + [ + -0.15, + -0.3, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 126, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 1.05902, + "trajectory_length": 0.57642, + "arc_length": 0.49101, + "straight_length": 0.08541, + "poses": [ + [ + 0.04296, + -0.02148, + 5.81953769817878 + ], + [ + 0.08595, + -0.04292, + 5.82960378250819 + ], + [ + 0.12959, + -0.06298, + 5.87496193497533 + ], + [ + 0.1741, + -0.08104, + 5.9203200874424695 + ], + [ + 0.21937, + -0.09707, + 5.965678239909609 + ], + [ + 0.26533, + -0.11102, + 6.011036392376749 + ], + [ + 0.31188, + -0.12288, + 6.056394544843888 + ], + [ + 0.35891, + -0.13262, + 6.101752697311028 + ], + [ + 0.40634, + -0.14021, + 6.1471108497781675 + ], + [ + 0.45406, + -0.14565, + 6.192469002245307 + ], + [ + 0.50198, + -0.14891, + 6.237827154712447 + ], + [ + 0.55, + -0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 127, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + } + ] +} \ No newline at end of file diff --git a/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py new file mode 100644 index 0000000000..01476decd0 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py @@ -0,0 +1,90 @@ +# Copyright (c) 2021, Matthew Booker +# +# 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. + +import unittest + +from lattice_generator import LatticeGenerator +import numpy as np + +MOTION_MODEL = 'ackermann' +TURNING_RADIUS = 0.5 +GRID_RESOLUTION = 0.05 +STOPPING_THRESHOLD = 5 +NUM_OF_HEADINGS = 16 + + +class TestLatticeGenerator(unittest.TestCase): + """Contains the unit tests for the TrajectoryGenerator.""" + + def setUp(self) -> None: + config = {'motion_model': MOTION_MODEL, + 'turning_radius': TURNING_RADIUS, + 'grid_resolution': GRID_RESOLUTION, + 'stopping_threshold': STOPPING_THRESHOLD, + 'num_of_headings': NUM_OF_HEADINGS} + + lattice_gen = LatticeGenerator(config) + + self.minimal_set = lattice_gen.run() + + def test_minimal_set_lengths_are_positive(self): + # Test that lengths are all positive + + for start_angle in self.minimal_set.keys(): + for trajectory in self.minimal_set[start_angle]: + + self.assertGreaterEqual(trajectory.parameters.arc_length, 0) + self.assertGreaterEqual(trajectory.parameters.start_straight_length, 0) + self.assertGreaterEqual(trajectory.parameters.end_straight_length, 0) + self.assertGreaterEqual(trajectory.parameters.total_length, 0) + + def test_minimal_set_end_points_lie_on_grid(self): + # Test that end points lie on the grid resolution + + for start_angle in self.minimal_set.keys(): + for trajectory in self.minimal_set[start_angle]: + + end_point_x = trajectory.path.xs[-1] + end_point_y = trajectory.path.ys[-1] + + div_x = end_point_x / GRID_RESOLUTION + div_y = end_point_y / GRID_RESOLUTION + + self.assertAlmostEqual(div_x, np.round(div_x), delta=0.00001) + self.assertAlmostEqual(div_y, np.round(div_y), delta=0.00001) + + def test_minimal_set_end_angle_is_correct(self): + # Test that end angle agrees with the end angle parameter + + for start_angle in self.minimal_set.keys(): + for trajectory in self.minimal_set[start_angle]: + + end_point_angle = trajectory.path.yaws[-1] + + self.assertEqual(end_point_angle, trajectory.parameters.end_angle) + + def test_output_angles_in_correct_range(self): + # Test that the outputted angles always lie within 0 to 2*pi + + for start_angle in self.minimal_set.keys(): + for trajectory in self.minimal_set[start_angle]: + output = trajectory.path.to_output_format() + + for x, y, angle in output: + self.assertGreaterEqual(angle, 0) + self.assertLessEqual(angle, 2*np.pi) + + +if __name__ == '__main__': + unittest.main() diff --git a/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py new file mode 100644 index 0000000000..0a994cac49 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py @@ -0,0 +1,226 @@ +# Copyright (c) 2021, Matthew Booker +# +# 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. + +import unittest + +import numpy as np +from trajectory_generator import TrajectoryGenerator + +TURNING_RADIUS = 1 +STEP_DISTANCE = 0.1 + + +class TestTrajectoryGenerator(unittest.TestCase): + """Contains the unit tests for the TrajectoryGenerator.""" + + def setUp(self) -> None: + config = {'turning_radius': TURNING_RADIUS} + self.trajectory_generator = TrajectoryGenerator(config) + + def test_generate_trajectory_only_arc(self): + # Quadrant 1 + end_point = np.array([1, 1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 2 + end_point = np.array([-1, 1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 3 + end_point = np.array([-1, -1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 4 + end_point = np.array([1, -1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + def test_generate_trajectory_only_line(self): + # Quadrant 1 + end_point = np.array([1, 1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(45), np.deg2rad(45), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 2 + end_point = np.array([-1, 1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(135), np.deg2rad(135), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 3 + end_point = np.array([-1, -1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(135), -np.deg2rad(135), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 4 + end_point = np.array([1, -1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(45), -np.deg2rad(45), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + def test_generate_trajectory_line_to_arc(self): + # Quadrant 1 + end_point = np.array([2, 1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 2 + end_point = np.array([-2, 1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 3 + end_point = np.array([-2, -1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 1 + end_point = np.array([2, -1]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + def test_generate_trajectory_line_to_end(self): + # Quadrant 1 + end_point = np.array([1, 2]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 2 + end_point = np.array([-1, 2]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 3 + end_point = np.array([-1, -2]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 4 + end_point = np.array([1, -2]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + def test_generate_trajectory_radius_too_small(self): + # Quadrant 1 + end_point = np.array([.9, .9]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(trajectory, None) + + # Quadrant 2 + end_point = np.array([-.9, -.9]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(trajectory, None) + + # Quadrant 3 + end_point = np.array([-.9, -.9]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(trajectory, None) + + # Quadrant 4 + end_point = np.array([.9, -.9]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE) + + self.assertEqual(trajectory, None) + + def test_generate_trajectory_parallel_lines_coincident(self): + # Quadrant 1 + end_point = np.array([5, 0]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + # Quadrant 2 + end_point = np.array([-5, 0]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE) + + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) + self.assertGreater(len(trajectory.path.xs), 0) + + def test_generate_trajectory_parallel_lines_not_coincident(self): + # Quadrant 1 + end_point = np.array([0, 3]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE) + + self.assertEqual(trajectory, None) + + # Quadrant 2 + end_point = np.array([0, 3]) + trajectory = self.trajectory_generator.generate_trajectory( + end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE) + + self.assertEqual(trajectory, None) + + +if __name__ == '__main__': + unittest.main() diff --git a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py new file mode 100644 index 0000000000..ef1ce0f472 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py @@ -0,0 +1,98 @@ +# Copyright (c) 2021, Matthew Booker +# +# 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. + +""" +This script is used visualize each trajectory individually. + +This helps to better understand how a single trajectory looks and +to ensure that the x, y, and yaw values are correct. This is mainly +used for debugging when making changes to parts of the code. +However, if you would like to see how each trajectory in your +ouput file looks then you can run this script. +""" + +import json +from pathlib import Path + +import matplotlib.pyplot as plt + +import numpy as np + + +def plot_arrow(x, y, yaw, length=1.0, fc='r', ec='k'): + """Plot arrow.""" + plt.arrow(x, y, length * np.cos(yaw), length * + np.sin(yaw), width=0.05*length, length_includes_head=True) + plt.plot(x, y) + plt.plot(0, 0) + + +def read_trajectories_data(file_path): + + with open(file_path) as data_file: + trajectory_data = json.load(data_file) + + return trajectory_data + + +cur_file_path = Path(__file__) +trajectory_file_path = cur_file_path.parent.parent / 'output.json' + + +trajectory_data = read_trajectories_data(trajectory_file_path) +min_x = min([min([pose[0] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives']]) +max_x = max([max([pose[0] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives']]) + +min_y = min([min([pose[1] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives']]) +max_y = max([max([pose[1] for pose in primitive['poses']]) + for primitive in trajectory_data['primitives']]) + +heading_angles = trajectory_data['lattice_metadata']['heading_angles'] + +for primitive in trajectory_data['primitives']: + arrow_length = (primitive['arc_length'] + + primitive['straight_length']) / len(primitive['poses']) + + if arrow_length == 0: + arrow_length = max_x / len(primitive['poses']) + + xs = np.array([pose[0] for pose in primitive['poses']]) + ys = np.array([pose[1] for pose in primitive['poses']]) + + lengths = np.sqrt((xs[1:] - xs[:-1])**2 + (ys[1:] - ys[:-1])**2) + print('Distances between points: ', lengths) + + for x, y, yaw in primitive['poses']: + plot_arrow(x, y, yaw, length=arrow_length) + + plt.scatter(xs, ys) + plt.grid(True) + plt.axis('square') + + left_x, right_x = plt.xlim() + left_y, right_y = plt.ylim() + plt.xlim(1.2*min_x, 1.2*max_x) + plt.ylim(1.2*min_y, 1.2*max_y) + + start_angle = np.rad2deg(heading_angles[primitive['start_angle_index']]) + end_angle = np.rad2deg(heading_angles[primitive['end_angle_index']]) + + plt.title(f"Trajectory ID: {primitive['trajectory_id']}") + plt.figtext( + 0.7, 0.9, f'Start: {start_angle}\nEnd: {end_angle}') + + plt.show() diff --git a/nav2_smac_planner/lattice_primitives/trajectory.py b/nav2_smac_planner/lattice_primitives/trajectory.py new file mode 100644 index 0000000000..520ded328d --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/trajectory.py @@ -0,0 +1,148 @@ +# Copyright (c) 2021, Matthew Booker +# +# 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. + +from dataclasses import dataclass + +from helper import angle_difference, normalize_angle + +import numpy as np + + +@dataclass(frozen=True) +class TrajectoryParameters: + """ + A dataclass that holds the data needed to create the path for a trajectory. + + turning_radius: The radius of the circle used to generate + the arc of the path + x_offset: The x coordinate of the circle used to generate + the arc of the path + y_offset: They y coordinate of the circle used to generate + the arc of the path + end_point: The end coordinate of the path + start_angle: The starting angle of the path + - given in radians from -pi to pi where 0 radians is along + the positive x axis + end_angle: The end angle of the path + - given in radians from -pi to pi where 0 radians is along + the positive x axis + left_turn: Whether the arc in the path turns to the left + arc_start_point: Coordinates of the starting position of the arc + arc_end_point: Coordinates of the ending position of the arc + """ + + turning_radius: float + x_offset: float + y_offset: float + end_point: np.array + start_angle: float + end_angle: float + left_turn: bool + + arc_start_point: float + arc_end_point: float + + @property + def arc_length(self): + """Arc length of the trajectory.""" + return self.turning_radius * angle_difference( + self.start_angle, self.end_angle, self.left_turn + ) + + @property + def start_straight_length(self): + """Length of the straight line from start to arc.""" + return np.linalg.norm(self.arc_start_point) + + @property + def end_straight_length(self): + """Length of the straight line from arc to end.""" + return np.linalg.norm(self.end_point - self.arc_end_point) + + @property + def total_length(self): + """Total length of trajectory.""" + return self.arc_length + self.start_straight_length + \ + self.end_straight_length + + @staticmethod + def no_arc(end_point, start_angle, end_angle): + """Create the parameters for a trajectory with no arc.""" + return TrajectoryParameters( + turning_radius=0.0, + x_offset=0.0, + y_offset=0.0, + end_point=end_point, + start_angle=start_angle, + end_angle=end_angle, + left_turn=True, + arc_start_point=end_point, + arc_end_point=end_point, + ) + + +@dataclass(frozen=True) +class Path: + """ + A dataclass that holds the generated poses for a given trajectory. + + xs: X coordinates of poses along trajectory + ys: Y coordinates of poses along trajectory + yaws: Yaws of poses along trajectory + """ + + xs: np.array + ys: np.array + yaws: np.array + + def __add__(self, rhs): + """Add two paths together by concatenating them.""" + if self.xs is None: + return rhs + + xs = np.concatenate((self.xs, rhs.xs)) + ys = np.concatenate((self.ys, rhs.ys)) + yaws = np.concatenate((self.yaws, rhs.yaws)) + + return Path(xs, ys, yaws) + + def to_output_format(self): + """Return the path data in a format suitable for outputting.""" + output_xs = self.xs.round(5) + output_ys = self.ys.round(5) + + # A bit of a hack but it removes any -0.0 + output_xs = output_xs + 0.0 + output_ys = output_ys + 0.0 + output_yaws = self.yaws + 0.0 + + vectorized_normalize_angle = np.vectorize(normalize_angle) + output_yaws = vectorized_normalize_angle(output_yaws) + + stacked = np.vstack([output_xs, output_ys, output_yaws]).transpose() + + return stacked.tolist() + + +@dataclass(frozen=True) +class Trajectory: + """ + A dataclass that holds the path and parameters for a trajectory. + + path: The Path that represents the trajectory + parameters: The TrajectoryParameters that represent the trajectory + """ + + path: Path + parameters: TrajectoryParameters diff --git a/nav2_smac_planner/lattice_primitives/trajectory_generator.py b/nav2_smac_planner/lattice_primitives/trajectory_generator.py new file mode 100644 index 0000000000..f2f51baf01 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py @@ -0,0 +1,568 @@ +# Copyright (c) 2021, Matthew Booker +# +# 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. + +import logging +from typing import Tuple, Union + +import numpy as np + +from trajectory import Path, Trajectory, TrajectoryParameters + +logger = logging.getLogger(__name__) + + +class TrajectoryGenerator: + """Handles all the logic for generating trajectories.""" + + def __init__(self, config: dict): + """Init TrajectoryGenerator using the user supplied config.""" + self.turning_radius = config['turning_radius'] + + def _get_arc_point( + self, trajectory_params: TrajectoryParameters, t: float + ) -> Tuple[float, float, float]: + """ + Get point on the arc trajectory using the following parameterization. + + r(t) = + + R = radius + a = x offset + b = y offset + + Args + ---- + trajectory_params: TrajectoryParameters + The parameters that describe the arc to create + t: float + A value between 0 - 1 that denotes where along the arc + to calculate the point + + Returns + ------- + x: float + x coordinate of generated point + y: float + y coordinate of generated point + yaw: float + angle of tangent line to arc at point (x,y) + + """ + start_angle = trajectory_params.start_angle + + arc_dist = t * trajectory_params.arc_length + angle_step = arc_dist / trajectory_params.turning_radius + + if trajectory_params.left_turn: + # Calculate points using + # r(t) = + t = start_angle + angle_step + x = ( + trajectory_params.turning_radius * np.cos(t - np.pi / 2) + + trajectory_params.x_offset + ) + y = ( + trajectory_params.turning_radius * np.sin(t - np.pi / 2) + + trajectory_params.y_offset + ) + + yaw = t + + else: + # Right turns go the opposite way across the arc, so we + # need to invert the angles and adjust the parametrization + start_angle = -start_angle + + # Calculate points using + # r(t) = + t = start_angle + angle_step + x = ( + trajectory_params.turning_radius * -np.cos(t + np.pi / 2) + + trajectory_params.x_offset + ) + y = ( + trajectory_params.turning_radius * np.sin(t + np.pi / 2) + + trajectory_params.y_offset + ) + + yaw = -t + + return x, y, yaw + + def _get_line_point( + self, start_point: np.array, end_point: np.array, t: float + ) -> Tuple[float, float]: + """ + Get point on a line segment using the following parameterization. + + r(t) = p + t * (q - p) + + p = start point + q = end point + + Args + ---- + start_point: np.array(2,) + Starting point of the line segment + end_point: np.array(2,) + End point of the line segment + t: float + A value between 0 - 1 that denotes where along the segment + to calculate the point + + Returns + ------- + x: float + x coordinate of generated point + y: float + y coordinate of generated point + + """ + return start_point + t * (end_point - start_point) + + def _create_path( + self, trajectory_params: TrajectoryParameters, primitive_resolution: float + ) -> Path: + """ + Create the full trajectory path from the given trajectory parameters. + + Args + ---- + trajectory_params: TrajectoryParameters + The parameters that describe the trajectory to create + primitive_resolution: float + The desired distance between sampled points along the line. + This value is not strictly adhered to as the path may not + be neatly divisible, however the spacing will be as close + as possible. + + Returns + ------- + TrajectoryPath + The trajectory path described by the trajectory parameters + + """ + number_of_steps = np.round( + trajectory_params.total_length / primitive_resolution + ).astype(int) + t_step = 1 / number_of_steps + + start_to_arc_dist = np.linalg.norm(trajectory_params.arc_start_point) + + transition_points = [ + start_to_arc_dist / trajectory_params.total_length, + (start_to_arc_dist + trajectory_params.arc_length) + / trajectory_params.total_length, + ] + + cur_t = t_step + + xs = [] + ys = [] + yaws = [] + + for i in range(1, number_of_steps + 1): + + # This prevents cur_t going over 1 due to rounding issues in t_step + cur_t = min(cur_t, 1) + + # Handle the initial straight line segment + if cur_t <= transition_points[0]: + line_t = cur_t / transition_points[0] + x, y = self._get_line_point( + np.array([0, 0]), trajectory_params.arc_start_point, line_t + ) + yaw = trajectory_params.start_angle + + # Handle the arc + elif cur_t <= transition_points[1]: + arc_t = (cur_t - transition_points[0]) / ( + transition_points[1] - transition_points[0] + ) + x, y, yaw = self._get_arc_point(trajectory_params, arc_t) + + # Handle the end straight line segment + else: + line_t = (cur_t - transition_points[1]) / (1 - transition_points[1]) + x, y = self._get_line_point( + trajectory_params.arc_end_point, trajectory_params.end_point, line_t + ) + yaw = trajectory_params.end_angle + + xs.append(x) + ys.append(y) + yaws.append(yaw) + + cur_t += t_step + + # Convert to numpy arrays + xs = np.array(xs) + ys = np.array(ys) + yaws = np.array(yaws) + + # The last point may be slightly off due to rounding issues + # so we correct the last point to be exactly the end point + xs[-1], ys[-1] = trajectory_params.end_point + yaws[-1] = trajectory_params.end_angle + + return Path(xs, ys, yaws) + + def _get_intersection_point( + self, m1: float, c1: float, m2: float, c2: float + ) -> np.array: + """ + Get the intersection point of two lines. + + The two lines are described by m1 * x + c1 and m2 * x + c2. + + Args + ---- + m1: float + Gradient of line 1 + c1: float + y-intercept of line 1 + m2: float + Gradient of line 2 + c2: float + y-intercept of line2 + + Returns + ------- + np.array (2,) + The intersection point of line 1 and 2 + + """ + def line1(x): + return m1 * x + c1 + + x_point = (c2 - c1) / (m1 - m2) + + return np.array([x_point, line1(x_point)]) + + def _is_left_turn(self, intersection_point: np.array, end_point: np.array) -> bool: + """ + Determine if a trajectory will be a left turn. + + Uses the determinant to determine whether the arc formed by the + intersection and end point turns left or right. + + Args + ---- + intersection_point: np.array(2,) + The intersection point of the lines formed from the start + and end angles + end_point: np.array(2,) + The chosen end point of the trajectory + + Returns + ------- + bool + True if curve turns left, false otherwise + + """ + matrix = np.vstack([intersection_point, end_point]) + det = np.linalg.det(matrix) + + return det >= 0 + + def _is_dir_vec_correct( + self, point1: np.array, point2: np.array, line_angle: float + ) -> bool: + """ + Check that the direction vector agrees with the line angle. + + The direction vector is defined as the vector from point 1 to + point 2. + + Args + ---- + point1: np.array(2,) + The start point of the vector + point2: np.array(2,) + The end point of the vector + line_angle: float + The angle of a line to compare against the vector + + Returns + ------- + bool + True if both line and vector point in same direction + + """ + # Need to round to prevent very small values for 0 + m = abs(np.tan(line_angle).round(5)) + + if line_angle < 0: + m *= -1 + + direction_vec_from_points = point2 - point1 + + direction_vec_from_gradient = np.array([1, m]) + + # Handle when line angle is in quadrant 2 or 3 and when angle is 90 + if abs(line_angle) > np.pi / 2: + direction_vec_from_gradient = np.array([-1, m]) + elif abs(line_angle) == np.pi / 2: + direction_vec_from_gradient = np.array([0, m]) + + direction_vec_from_gradient = direction_vec_from_gradient.round(5) + direction_vec_from_points = direction_vec_from_points.round(5) + + if np.all( + np.sign(direction_vec_from_points) == np.sign(direction_vec_from_gradient) + ): + return True + else: + return False + + def _calculate_trajectory_params( + self, end_point: np.array, start_angle: float, end_angle: float + ) -> Union[TrajectoryParameters, None]: + """ + Calculate the parameters for a trajectory with the desired constraints. + + The trajectory may consist of an arc and at most two line segments. + A straight trajectory will consist of a single line segment. Similarly, + a purely curving trajectory will only consist of an arc. + + Idea: + 1. Extend a line from (0,0) with angle of start_angle + 2. Extend a line from end_point with angle of end_angle + 3. Compute their intersection point, I + 4. Check that the intersection point leads to a + valid trajectory + - If I is too close to (0,0) or the end point then + no arc greater than the turning radius will reach + from (0,0) to end point + + If two segments from the same exterior point are tangent to + a circle then they are congruent + + Args + ---- + end_point: np.array(2,) + The desired end point of the trajectory + start_angle: float + The start angle of the trajectory in radians + end_angle: float + The end angle of the trajectory in radians + + Returns + ------- + TrajectoryParameters or None + If a valid trajectory exists then the Trajectory parameters + are returned, otherwise None + + """ + x2, y2 = end_point + arc_start_point = np.array([0, 0]) + arc_end_point = end_point + + # Find gradient of line 1 passing through (0,0) that makes an angle + # of start_angle with x_axis + m1 = np.tan(start_angle).round(5) + + # Find gradient of line 2 passing through end point that makes an angle + # of end_angle with x-axis + m2 = np.tan(end_angle).round(5) + + # Deal with lines that are parallel + if m1 == m2: + # If they are coincident (i.e. y-intercept is same) then simply + # return a circle with infinite radius + if round(-m2 * x2 + y2, 5) == 0: + return TrajectoryParameters.no_arc( + end_point=end_point, start_angle=start_angle, end_angle=end_angle + ) + + # Deal with edge case of 90 + elif ( + abs(start_angle) == np.pi / 2 and arc_end_point[0] == arc_start_point[0] + ): + return TrajectoryParameters.no_arc( + end_point=end_point, + start_angle=start_angle, + end_angle=end_angle, + ) + + else: + logger.debug( + 'No trajectory possible for equivalent start and ' + + f'end angles that also passes through p = {x2, y2}' + ) + return None + + # Find intersection point of lines 1 and 2 + intersection_point = self._get_intersection_point(m1, 0, m2, -m2 * x2 + y2) + + # Check that the vector from (0,0) to intersection point agrees + # with the angle of line 1 + if not self._is_dir_vec_correct( + arc_start_point, intersection_point, start_angle + ): + logger.debug( + 'No trajectory possible since intersection point occurs ' + + 'before start point on line 1' + ) + return None + + # Check that the vector from intersection point to arc start point agrees with + # the angle of line 2 + if not self._is_dir_vec_correct(intersection_point, arc_end_point, end_angle): + logger.debug( + 'No trajectory possible since intersection point occurs ' + + 'after end point on line 2' + ) + return None + + # Calculate distance between arc start point and intersection point + dist_a = round(np.linalg.norm(arc_start_point - intersection_point), 5) + + # Calculate distance between arc end point and intersection point + dist_b = round(np.linalg.norm(arc_end_point - intersection_point), 5) + + # Calculate the angle between start angle and end angle lines + angle_between_lines = np.pi - abs(end_angle - start_angle) + + # The closer the arc start and end points are to the intersection point the + # smaller the turning radius will be. However, we have a constraint on how + # small this turning radius can get. To calculate the minimum allowed + # distance we draw a right triangle with height equal to the constrained + # radius and angle opposite the height leg as half the angle between the + # start and end angle lines. The minimum valid distance is then the + # base of this triangle. + min_valid_distance = round( + self.turning_radius / np.tan(angle_between_lines / 2), 5 + ) + + # Both the distance of p along line 2 and intersection point along + # line 1 must be greater than the minimum valid distance + if dist_a < min_valid_distance or dist_b < min_valid_distance: + logger.debug( + 'No trajectory possible where radius is larger than ' + + 'minimum turning radius' + ) + return None + + if dist_a < dist_b: + # Find new point on line 2 that is equidistant away from + # intersection point as arc start point is on line 1 + vec_line2 = arc_end_point - intersection_point + vec_line2 /= np.linalg.norm(vec_line2) + arc_end_point = intersection_point + dist_a * vec_line2 + + elif dist_a > dist_b: + # Find new point on line 1 that is equidistant away from + # intersection point as arc end point is on line 2 + vec_line1 = arc_start_point - intersection_point + vec_line1 /= np.linalg.norm(vec_line1) + + arc_start_point = intersection_point + dist_b * vec_line1 + + x1, y1 = arc_start_point + x2, y2 = arc_end_point + + # Find intersection point of the perpindicular lines of line 1 and 2 + # that pass through arc start and arc end point respectively + if m1 == 0: + # If line 1 has gradient 0 then it is the x-axis. + + def perp_line2(x): + return -1 / m2 * (x - x2) + y2 + + circle_center = np.array([x1, perp_line2(x1)]) + elif m2 == 0: + + def perp_line1(x): + return -1 / m1 * (x - x1) + y1 + + circle_center = np.array([x2, perp_line1(x2)]) + else: + perp_m1 = -1 / m1 if m1 != 0 else 0 + perp_m2 = -1 / m2 if m2 != 0 else 0 + + circle_center = self._get_intersection_point( + perp_m1, -perp_m1 * x1 + y1, perp_m2, -perp_m2 * x2 + y2 + ) + + # The circles radius is the length from the center to arc start/end point + # (both distances are the same) + radius = np.linalg.norm(circle_center - arc_end_point).round(5) + x_offset = circle_center[0].round(5) + y_offset = circle_center[1].round(5) + + if radius < self.turning_radius: + logger.debug( + 'Calculated circle radius is smaller than allowed turning ' + + f'radius: r = {radius}, min_radius = {self.turning_radius}' + ) + return None + + left_turn = self._is_left_turn(intersection_point, end_point) + + return TrajectoryParameters( + radius, + x_offset, + y_offset, + end_point, + start_angle, + end_angle, + left_turn, + arc_start_point, + arc_end_point, + ) + + def generate_trajectory( + self, + end_point: np.array, + start_angle: float, + end_angle: float, + primitive_resolution: float, + ) -> Union[Trajectory, None]: + """ + Create a trajectory from (0,0, start_angle) to (end_point, end_angle). + + The trajectory will consist of a path that contains discrete points + that are spaced primitive_resolution apart. + + Args + ---- + end_point: np.array(2,) + The desired end point of the trajectory + start_angle: float + The start angle of the trajectory in radians + end_angle: float + The end angle of the trajectory in radians + primitive_resolution: float + The spacing between points along the trajectory + + Returns + ------- + Trajectory or None + If a valid trajectory exists then the Trajectory is returned, + otherwise None + + """ + trajectory_params = self._calculate_trajectory_params( + end_point, start_angle, end_angle + ) + + if trajectory_params is None: + return None + + logger.debug('Trajectory found') + + trajectory_path = self._create_path(trajectory_params, primitive_resolution) + + return Trajectory(trajectory_path, trajectory_params) diff --git a/nav2_smac_planner/media/A.png b/nav2_smac_planner/media/A.png new file mode 100644 index 0000000000..8e04208118 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 0000000000..e9098ed5bf 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 new file mode 100644 index 0000000000..ad91608779 --- /dev/null +++ b/nav2_smac_planner/package.xml @@ -0,0 +1,43 @@ + + + + nav2_smac_planner + 1.1.0 + Smac global planning plugin: A*, Hybrid-A*, State Lattice + Steve Macenski + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_lifecycle + visualization_msgs + nav2_util + nav2_msgs + nav_msgs + geometry_msgs + builtin_interfaces + nav2_common + tf2_ros + nav2_costmap_2d + nav2_core + pluginlib + eigen3_cmake_module + eigen + ompl + nlohmann-json-dev + angles + + ament_lint_common + ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest + + + ament_cmake + + + + + diff --git a/nav2_smac_planner/smac_plugin_2d.xml b/nav2_smac_planner/smac_plugin_2d.xml new file mode 100644 index 0000000000..5bae687472 --- /dev/null +++ b/nav2_smac_planner/smac_plugin_2d.xml @@ -0,0 +1,5 @@ + + + 2D A* SMAC Planner + + diff --git a/nav2_smac_planner/smac_plugin_hybrid.xml b/nav2_smac_planner/smac_plugin_hybrid.xml new file mode 100644 index 0000000000..7b52bb24d7 --- /dev/null +++ b/nav2_smac_planner/smac_plugin_hybrid.xml @@ -0,0 +1,5 @@ + + + Hybrid-A* SMAC planner + + diff --git a/nav2_smac_planner/smac_plugin_lattice.xml b/nav2_smac_planner/smac_plugin_lattice.xml new file mode 100644 index 0000000000..34149230f4 --- /dev/null +++ b/nav2_smac_planner/smac_plugin_lattice.xml @@ -0,0 +1,5 @@ + + + State Lattice SMAC planner + + diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp new file mode 100644 index 0000000000..deaa597b92 --- /dev/null +++ b/nav2_smac_planner/src/a_star.cpp @@ -0,0 +1,435 @@ +// 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 +#include +#include + +#include "nav2_smac_planner/a_star.hpp" +using namespace std::chrono; // NOLINT + +namespace nav2_smac_planner +{ + +template +AStarAlgorithm::AStarAlgorithm( + const MotionModel & motion_model, + const SearchInfo & search_info) +: _traverse_unknown(true), + _max_iterations(0), + _max_planning_time(0), + _x_size(0), + _y_size(0), + _search_info(search_info), + _goal_coordinates(Coordinates()), + _start(nullptr), + _goal(nullptr), + _motion_model(motion_model) +{ + _graph.reserve(100000); +} + +template +AStarAlgorithm::~AStarAlgorithm() +{ +} + +template +void AStarAlgorithm::initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations, + const double & max_planning_time, + 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; + _max_planning_time = max_planning_time; + NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + _dim3_size = dim_3_size; + _expander = std::make_unique>( + _motion_model, _search_info, _traverse_unknown, _dim3_size); +} + +template<> +void AStarAlgorithm::initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations, + const double & max_planning_time, + 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; + _max_planning_time = max_planning_time; + + if (dim_3_size != 1) { + throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization."); + } + _dim3_size = dim_3_size; + _expander = std::make_unique>( + _motion_model, _search_info, _traverse_unknown, _dim3_size); +} + +template +void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision_checker) +{ + _collision_checker = collision_checker; + _costmap = collision_checker->getCostmap(); + unsigned int x_size = _costmap->getSizeInCellsX(); + unsigned int y_size = _costmap->getSizeInCellsY(); + + clearGraph(); + + if (getSizeX() != x_size || getSizeY() != y_size) { + _x_size = x_size; + _y_size = y_size; + NodeT::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info); + } + _expander->setCollisionChecker(collision_checker); +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( + const unsigned int & index) +{ + // 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<> +void AStarAlgorithm::setStart( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + if (dim_3 != 0) { + throw std::runtime_error("Node type Node2D cannot be given non-zero starting dim 3."); + } + _start = addToGraph(Node2D::getIndex(mx, my, getSizeX())); +} + +template +void AStarAlgorithm::setStart( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + _start = addToGraph(NodeT::getIndex(mx, my, dim_3)); + _start->setPose( + Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); +} + +template<> +void AStarAlgorithm::setGoal( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + if (dim_3 != 0) { + throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); + } + + _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX())); + _goal_coordinates = Node2D::Coordinates(mx, my); +} + +template +void AStarAlgorithm::setGoal( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + _goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); + + typename NodeT::Coordinates goal_coords( + static_cast(mx), + static_cast(my), + static_cast(dim_3)); + + if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { + if (!_start) { + throw std::runtime_error("Start must be set before goal."); + } + + NodeT::resetObstacleHeuristic(_costmap, _start->pose.x, _start->pose.y, mx, my); + } + + _goal_coordinates = goal_coords; + _goal->setPose(_goal_coordinates); +} + +template +bool AStarAlgorithm::areInputsValid() +{ + // Check if graph was filled in + if (_graph.empty()) { + throw std::runtime_error("Failed to compute path, no costmap given."); + } + + // Check if points were filled in + if (!_start || !_goal) { + throw std::runtime_error("Failed to compute path, no valid start or goal given."); + } + + // Check if ending point is valid + if (getToleranceHeuristic() < 0.001 && + !_goal->isNodeValid(_traverse_unknown, _collision_checker)) + { + throw std::runtime_error("Failed to compute path, goal is occupied with no tolerance."); + } + + // Check if starting point is valid + if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { + throw std::runtime_error("Starting point in lethal space! Cannot create feasible plan."); + } + + return true; +} + +template +bool AStarAlgorithm::createPath( + CoordinateVector & path, int & iterations, + const float & tolerance) +{ + steady_clock::time_point start_time = steady_clock::now(); + _tolerance = tolerance; + _best_heuristic_node = {std::numeric_limits::max(), 0}; + clearQueue(); + + if (!areInputsValid()) { + return false; + } + + // 0) Add starting point to the open set + addNode(0.0, getStart()); + getStart()->setAccumulatedCost(0.0); + + // 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; + NeighborIterator neighbor_iterator; + int analytic_iterations = 0; + int closest_distance = std::numeric_limits::max(); + + // Given an index, return a node ptr reference if its collision-free and valid + const unsigned int max_index = getSizeX() * getSizeY() * getSizeDim3(); + NodeGetter neighborGetter = + [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool + { + if (index >= max_index) { + return false; + } + + neighbor_rtn = addToGraph(index); + return true; + }; + + while (iterations < getMaxIterations() && !_queue.empty()) { + // Check for planning timeout only on every Nth iteration + if (iterations % _timing_interval == 0) { + std::chrono::duration planning_duration = + std::chrono::duration_cast>(steady_clock::now() - start_time); + if (static_cast(planning_duration.count()) >= _max_planning_time) { + return false; + } + } + + // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue + current_node = getNextNode(); + + // We allow for nodes to be queued multiple times in case + // shorter paths result in it, but we can visit only once + if (current_node->wasVisited()) { + continue; + } + + iterations++; + + // 2) Mark Nbest as visited + current_node->visited(); + + // 2.1) Use an analytic expansion (if available) to generate a path + expansion_result = nullptr; + expansion_result = _expander->tryAnalyticExpansion( + current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance); + if (expansion_result != nullptr) { + current_node = expansion_result; + } + + // 3) Check if we're at the goal, backtrace if required + if (isGoal(current_node)) { + return current_node->backtracePath(path); + } else if (_best_heuristic_node.first < getToleranceHeuristic()) { + // Optimization: Let us find when in tolerance and refine within reason + approach_iterations++; + if (approach_iterations >= getOnApproachMaxIterations()) { + return _graph.at(_best_heuristic_node.second).backtracePath(path); + } + } + + // 4) Expand neighbors of Nbest not visited + neighbors.clear(); + current_node->getNeighbors(neighborGetter, _collision_checker, _traverse_unknown, neighbors); + + for (neighbor_iterator = neighbors.begin(); + neighbor_iterator != neighbors.end(); ++neighbor_iterator) + { + neighbor = *neighbor_iterator; + + // 4.1) Compute the cost to go to this node + 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 < neighbor->getAccumulatedCost()) { + neighbor->setAccumulatedCost(g_cost); + neighbor->parent = current_node; + + // 4.3) Add to queue with heuristic cost + addNode(g_cost + getHeuristicCost(neighbor), neighbor); + } + } + } + + return false; +} + +template +bool AStarAlgorithm::isGoal(NodePtr & node) +{ + return node == getGoal(); +} + +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() +{ + return _start; +} + +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() +{ + return _goal; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +{ + NodeBasic node = _queue.top().second; + _queue.pop(); + node.processSearchNode(); + return node.graph_node_ptr; +} + +template +void AStarAlgorithm::addNode(const float & cost, NodePtr & node) +{ + NodeBasic queued_node(node->getIndex()); + queued_node.populateSearchNode(node); + _queue.emplace(cost, queued_node); +} + +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, _costmap); + + if (heuristic < _best_heuristic_node.first) { + _best_heuristic_node = {heuristic, node->getIndex()}; + } + + return heuristic; +} + +template +void AStarAlgorithm::clearQueue() +{ + NodeQueue q; + std::swap(_queue, q); +} + +template +void AStarAlgorithm::clearGraph() +{ + Graph g; + std::swap(_graph, g); + _graph.reserve(100000); +} + +template +int & AStarAlgorithm::getMaxIterations() +{ + return _max_iterations; +} + +template +int & AStarAlgorithm::getOnApproachMaxIterations() +{ + return _max_on_approach_iterations; +} + +template +float & AStarAlgorithm::getToleranceHeuristic() +{ + return _tolerance; +} + +template +unsigned int & AStarAlgorithm::getSizeX() +{ + return _x_size; +} + +template +unsigned int & AStarAlgorithm::getSizeY() +{ + return _y_size; +} + +template +unsigned int & AStarAlgorithm::getSizeDim3() +{ + return _dim3_size; +} + +// 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/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp new file mode 100644 index 0000000000..831bccbd9e --- /dev/null +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -0,0 +1,286 @@ +// 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 "nav2_smac_planner/analytic_expansion.hpp" + +namespace nav2_smac_planner +{ + +template +AnalyticExpansion::AnalyticExpansion( + const MotionModel & motion_model, + const SearchInfo & search_info, + const bool & traverse_unknown, + const unsigned int & dim_3_size) +: _motion_model(motion_model), + _search_info(search_info), + _traverse_unknown(traverse_unknown), + _dim_3_size(dim_3_size), + _collision_checker(nullptr) +{ +} + +template +void AnalyticExpansion::setCollisionChecker( + GridCollisionChecker * collision_checker) +{ + _collision_checker = collision_checker; +} + +template +typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( + const NodePtr & current_node, const NodePtr & goal_node, + const NodeGetter & getter, int & analytic_iterations, + int & closest_distance) +{ + // This must be a valid motion model for analytic expansion to be attempted + 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 + auto costmap = _collision_checker->getCostmap(); + const Coordinates node_coords = + NodeT::getCoords(current_node->getIndex(), costmap->getSizeInCellsX(), _dim_3_size); + closest_distance = std::min( + closest_distance, + static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose, 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))); + + // If we are closer now, we should update the target number of iterations to go + analytic_iterations = + std::min(analytic_iterations, desired_iterations); + + // 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 + analytic_iterations = desired_iterations; + AnalyticExpansionNodes analytic_nodes = getAnalyticPath(current_node, goal_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, goal_node, getter); + if (refined_analytic_nodes.empty()) { + break; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { + break; + } + } + + return setAnalyticPath(node, goal_node, analytic_nodes); + } + } + + analytic_iterations--; + } + + // No valid motion model - return nullptr + return NodePtr(nullptr); +} + +template +typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( + const NodePtr & node, + const NodePtr & goal, + 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->motion_table.getAngleFromBin(node->pose.theta); + to[0] = goal->pose.x; + to[1] = goal->pose.y; + to[2] = node->motion_table.getAngleFromBin(goal->pose.theta); + + float d = node->motion_table.state_space->distance(from(), to()); + + // If the length is too far, exit. This prevents unsafe shortcutting of paths + // into higher cost areas far out from the goal itself, let search to the work of getting + // close before the analytic expansion brings it home. This should never be smaller than + // 4-5x the minimum turning radius being used, or planning times will begin to spike. + if (d > _search_info.analytic_expansion_max_length) { + return AnalyticExpansionNodes(); + } + + // 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; + // When "from" and "to" are zero or one cell away, + // num_intervals == 0 + possible_nodes.reserve(num_intervals); // We won't store this node or the goal + std::vector reals; + double theta; + + // 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(); + // Make sure in range [0, 2PI) + theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; + theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; + angle = node->motion_table.getClosestAngularBin(theta); + + // 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 AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( + const NodePtr & node, + const NodePtr & goal_node, + const AnalyticExpansionNodes & expanded_nodes) +{ + _detached_nodes.clear(); + // Legitimate final path - set the parent relationships, states, and poses + NodePtr prev = node; + for (const auto & node_pose : expanded_nodes) { + auto n = node_pose.node; + cleanNode(n); + if (n->getIndex() != goal_node->getIndex()) { + if (n->wasVisited()) { + _detached_nodes.push_back(std::make_unique(-1)); + n = _detached_nodes.back().get(); + } + n->parent = prev; + n->pose = node_pose.proposed_coords; + n->visited(); + prev = n; + } + } + if (goal_node != prev) { + goal_node->parent = prev; + cleanNode(goal_node); + goal_node->visited(); + } + return goal_node; +} + +template<> +void AnalyticExpansion::cleanNode(const NodePtr & node) +{ + node->setMotionPrimitive(nullptr); +} + +template +void AnalyticExpansion::cleanNode(const NodePtr & /*expanded_nodes*/) +{ +} + +template<> +typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion:: +getAnalyticPath( + const NodePtr & node, + const NodePtr & goal, + const NodeGetter & node_getter) +{ + return AnalyticExpansionNodes(); +} + +template<> +typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( + const NodePtr & node, + const NodePtr & goal_node, + const AnalyticExpansionNodes & expanded_nodes) +{ + return NodePtr(nullptr); +} + +template<> +typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( + const NodePtr & current_node, const NodePtr & goal_node, + const NodeGetter & getter, int & analytic_iterations, + int & closest_distance) +{ + return NodePtr(nullptr); +} + +template class AnalyticExpansion; +template class AnalyticExpansion; +template class AnalyticExpansion; + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp new file mode 100644 index 0000000000..c0cbfb98fa --- /dev/null +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -0,0 +1,179 @@ +// 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 "nav2_smac_planner/collision_checker.hpp" + +namespace nav2_smac_planner +{ + +GridCollisionChecker::GridCollisionChecker( + nav2_costmap_2d::Costmap2D * costmap, + unsigned int num_quantizations) +: FootprintCollisionChecker(costmap) +{ + // Convert number of regular bins into angles + float bin_size = 2 * M_PI / static_cast(num_quantizations); + angles_.reserve(num_quantizations); + for (unsigned int i = 0; i != num_quantizations; i++) { + angles_.push_back(bin_size * i); + } +} + +// GridCollisionChecker::GridCollisionChecker( +// nav2_costmap_2d::Costmap2D * costmap, +// std::vector & angles) +// : FootprintCollisionChecker(costmap), +// angles_(angles) +// { +// } + +void GridCollisionChecker::setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const double & possible_inscribed_cost) +{ + 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; + } + + oriented_footprints_.reserve(angles_.size()); + 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 != angles_.size(); i++) { + sin_th = sin(angles_[i]); + cos_th = cos(angles_[i]); + 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; +} + +bool GridCollisionChecker::inCollision( + const float & x, + const float & y, + const float & angle_bin, + const bool & traverse_unknown) +{ + // Check to make sure cell is inside the map + if (outsideRange(costmap_->getSizeInCellsX(), x) || + outsideRange(costmap_->getSizeInCellsY(), y)) + { + return false; + } + + // Assumes setFootprint already set + double wx, wy; + costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); + + if (!footprint_is_radius_) { + // 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 + 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; + } else { + // if radius, then we can check the center of the cost assuming inflation is used + footprint_cost_ = costmap_->getCost( + static_cast(x), static_cast(y)); + + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return static_cast(footprint_cost_) >= INSCRIBED; + } +} + +bool GridCollisionChecker::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; +} + +float GridCollisionChecker::getCost() +{ + // Assumes inCollision called prior + return static_cast(footprint_cost_); +} + +bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value) +{ + return value < 0.0f || value > max; +} + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/costmap_downsampler.cpp b/nav2_smac_planner/src/costmap_downsampler.cpp new file mode 100644 index 0000000000..d0581daaf8 --- /dev/null +++ b/nav2_smac_planner/src/costmap_downsampler.cpp @@ -0,0 +1,154 @@ +// Copyright (c) 2020, Carlos Luis +// 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 "nav2_smac_planner/costmap_downsampler.hpp" + +#include +#include +#include + +namespace nav2_smac_planner +{ + +CostmapDownsampler::CostmapDownsampler() +: _costmap(nullptr), + _downsampled_costmap(nullptr), + _downsampled_costmap_pub(nullptr) +{ +} + +CostmapDownsampler::~CostmapDownsampler() +{ +} + +void CostmapDownsampler::on_configure( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & global_frame, + const std::string & topic_name, + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor, + const bool & use_min_cost_neighbor) +{ + _costmap = costmap; + _downsampling_factor = downsampling_factor; + _use_min_cost_neighbor = use_min_cost_neighbor; + updateCostmapSize(); + + _downsampled_costmap = std::make_unique( + _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, + _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN); + + if (!node.expired()) { + _downsampled_costmap_pub = std::make_unique( + node, _downsampled_costmap.get(), global_frame, topic_name, false); + } +} + +void CostmapDownsampler::on_activate() +{ + if (_downsampled_costmap_pub) { + _downsampled_costmap_pub->on_activate(); + } +} + +void CostmapDownsampler::on_deactivate() +{ + if (_downsampled_costmap_pub) { + _downsampled_costmap_pub->on_deactivate(); + } +} + +void CostmapDownsampler::on_cleanup() +{ + _costmap = nullptr; + _downsampled_costmap.reset(); + _downsampled_costmap_pub.reset(); +} + +nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( + const unsigned int & downsampling_factor) +{ + _downsampling_factor = downsampling_factor; + updateCostmapSize(); + + // Adjust costmap size if needed + if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x || + _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y || + _downsampled_costmap->getResolution() != _downsampled_resolution) + { + resizeCostmap(); + } + + // Assign costs + for (unsigned int i = 0; i < _downsampled_size_x; ++i) { + for (unsigned int j = 0; j < _downsampled_size_y; ++j) { + setCostOfCell(i, j); + } + } + + if (_downsampled_costmap_pub) { + _downsampled_costmap_pub->publishCostmap(); + } + return _downsampled_costmap.get(); +} + +void CostmapDownsampler::updateCostmapSize() +{ + _size_x = _costmap->getSizeInCellsX(); + _size_y = _costmap->getSizeInCellsY(); + _downsampled_size_x = ceil(static_cast(_size_x) / _downsampling_factor); + _downsampled_size_y = ceil(static_cast(_size_y) / _downsampling_factor); + _downsampled_resolution = _downsampling_factor * _costmap->getResolution(); +} + +void CostmapDownsampler::resizeCostmap() +{ + _downsampled_costmap->resizeMap( + _downsampled_size_x, + _downsampled_size_y, + _downsampled_resolution, + _costmap->getOriginX(), + _costmap->getOriginY()); +} + +void CostmapDownsampler::setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my) +{ + unsigned int mx, my; + unsigned char cost = _use_min_cost_neighbor ? 255 : 0; + unsigned int x_offset = new_mx * _downsampling_factor; + unsigned int y_offset = new_my * _downsampling_factor; + + for (unsigned int i = 0; i < _downsampling_factor; ++i) { + mx = x_offset + i; + if (mx >= _size_x) { + continue; + } + for (unsigned int j = 0; j < _downsampling_factor; ++j) { + my = y_offset + j; + if (my >= _size_y) { + continue; + } + cost = _use_min_cost_neighbor ? + std::min(cost, _costmap->getCost(mx, my)) : + std::max(cost, _costmap->getCost(mx, my)); + } + } + + _downsampled_costmap->setCost(new_mx, new_my, cost); +} + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp new file mode 100644 index 0000000000..7917d062f0 --- /dev/null +++ b/nav2_smac_planner/src/node_2d.cpp @@ -0,0 +1,175 @@ +// 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 "nav2_smac_planner/node_2d.hpp" + +#include +#include + +namespace nav2_smac_planner +{ + +// defining static member for all instance to share +std::vector Node2D::_neighbors_grid_offsets; +float Node2D::cost_travel_multiplier = 2.0; + +Node2D::Node2D(const unsigned int index) +: parent(nullptr), + _cell_cost(std::numeric_limits::quiet_NaN()), + _accumulated_cost(std::numeric_limits::max()), + _index(index), + _was_visited(false), + _is_queued(false) +{ +} + +Node2D::~Node2D() +{ + parent = nullptr; +} + +void Node2D::reset() +{ + parent = nullptr; + _cell_cost = std::numeric_limits::quiet_NaN(); + _accumulated_cost = std::numeric_limits::max(); + _was_visited = false; + _is_queued = false; +} + +bool Node2D::isNodeValid( + const bool & traverse_unknown, + GridCollisionChecker * collision_checker) +{ + if (collision_checker->inCollision(this->getIndex(), traverse_unknown)) { + return false; + } + + _cell_cost = collision_checker->getCost(); + return true; +} + +float Node2D::getTraversalCost(const NodePtr & child) +{ + 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; + } + + return 1.0 + cost_travel_multiplier * normalized_cost; +} + +float Node2D::getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coordinates, + const nav2_costmap_2d::Costmap2D * /*costmap*/) +{ + // 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::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."); + case MotionModel::VON_NEUMANN: + _neighbors_grid_offsets = {-1, +1, -x_size, +x_size}; + break; + case MotionModel::MOORE: + _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1, + -x_size + 1, +x_size - 1, +x_size + 1}; + break; + default: + throw std::runtime_error( + "Invalid neighborhood type selected. " + "Von-Neumann and Moore are valid for Node2D."); + } +} + +void Node2D::getNeighbors( + std::function & NeighborGetter, + 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 + // 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. + // 100 50 0 + // 100 50 50 + // 100 100 100 where lower-middle '100' is visited with same cost by both bottom '50' nodes + // Therefore, it is valuable to have some low-potential across the entire map + // rather than a small inflation around the obstacles + int index; + NodePtr neighbor; + int node_i = this->getIndex(); + const Coordinates parent = getCoords(this->getIndex()); + Coordinates child; + + for (unsigned int i = 0; i != _neighbors_grid_offsets.size(); ++i) { + index = node_i + _neighbors_grid_offsets[i]; + + // Check for wrap around conditions + child = getCoords(index); + if (fabs(parent.x - child.x) > 1 || fabs(parent.y - child.y) > 1) { + continue; + } + + if (NeighborGetter(index, neighbor)) { + if (neighbor->isNodeValid(traverse_unknown, collision_checker) && !neighbor->wasVisited()) { + neighbors.push_back(neighbor); + } + } + } +} + +bool Node2D::backtracePath(CoordinateVector & path) +{ + if (!this->parent) { + return false; + } + + NodePtr current_node = this; + + while (current_node->parent) { + path.push_back( + Node2D::getCoords(current_node->getIndex())); + current_node = current_node->parent; + } + + return path.size() > 0; +} + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_basic.cpp b/nav2_smac_planner/src/node_basic.cpp new file mode 100644 index 0000000000..7c116d64d6 --- /dev/null +++ b/nav2_smac_planner/src/node_basic.cpp @@ -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. Reserved. + +#include "nav2_smac_planner/node_basic.hpp" + +namespace nav2_smac_planner +{ + +template +void NodeBasic::processSearchNode() +{ +} + +template<> +void NodeBasic::processSearchNode() +{ + // 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 (!this->graph_node_ptr->wasVisited()) { + this->graph_node_ptr->pose = this->pose; + this->graph_node_ptr->setMotionPrimitiveIndex(this->motion_index); + } +} + +template<> +void NodeBasic::processSearchNode() +{ + // We only want to override the node's pose/primitive 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 (!this->graph_node_ptr->wasVisited()) { + this->graph_node_ptr->pose = this->pose; + this->graph_node_ptr->setMotionPrimitive(this->prim_ptr); + this->graph_node_ptr->backwards(this->backward); + } +} + +template<> +void NodeBasic::populateSearchNode(Node2D * & node) +{ + this->graph_node_ptr = node; +} + +template<> +void NodeBasic::populateSearchNode(NodeHybrid * & node) +{ + this->pose = node->pose; + this->graph_node_ptr = node; + this->motion_index = node->getMotionPrimitiveIndex(); +} + +template<> +void NodeBasic::populateSearchNode(NodeLattice * & node) +{ + this->pose = node->pose; + this->graph_node_ptr = node; + this->prim_ptr = node->getMotionPrimitive(); + this->backward = node->isBackward(); +} + +template class NodeBasic; +template class NodeBasic; +template class NodeBasic; + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp new file mode 100644 index 0000000000..d75c11ffd3 --- /dev/null +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -0,0 +1,718 @@ +// 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; +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; +ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue; + +// 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; + travel_distance_reward = 1.0f - search_info.retrospective_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 && + motion_model == MotionModel::DUBIN) + { + 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; + motion_model = MotionModel::DUBIN; + + // 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 + 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; + travel_distance_reward = 1.0f - search_info.retrospective_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 && + motion_model == MotionModel::REEDS_SHEPP) + { + 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; + motion_model = MotionModel::REEDS_SHEPP; + + 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 + 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 < 0.0) { + new_heading += num_angle_quantization_float; + } + + if (new_heading >= num_angle_quantization_float) { + 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; +} + +unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) +{ + return static_cast(floor(theta / bin_size)); +} + +float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) +{ + return bin_idx * bin_size; +} + +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 /*bin number*/, 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; + } + + float travel_cost = 0.0; + float travel_cost_raw = + NodeHybrid::travel_distance_cost * + (motion_table.travel_distance_reward + motion_table.cost_penalty * normalized_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 (child->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, motion_table.cost_penalty); + 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; +} + +inline float distanceHeuristic2D( + const unsigned int idx, const unsigned int size_x, + const unsigned int target_x, const unsigned int target_y) +{ + return std::hypotf( + static_cast(idx % size_x) - static_cast(target_x), + static_cast(idx / size_x) - static_cast(target_y)); +} + +void NodeHybrid::resetObstacleHeuristic( + 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) +{ + // 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, true); + 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); + } + + obstacle_heuristic_queue.clear(); + obstacle_heuristic_queue.reserve( + sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY()); + + // Set initial goal point to queue from. Divided by 2 due to downsampled costmap. + const unsigned int size_x = sampled_costmap->getSizeInCellsX(); + const unsigned int goal_index = floor(goal_y / 2.0) * size_x + floor(goal_x / 2.0); + obstacle_heuristic_queue.emplace_back( + distanceHeuristic2D(goal_index, size_x, start_x, start_y), goal_index); + + // initialize goal cell with a very small value to differentiate it from 0.0 (~uninitialized) + // the negative value means the cell is in the open set + obstacle_heuristic_lookup_table[goal_index] = -0.00001f; +} + +float NodeHybrid::getObstacleHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const double & cost_penalty) +{ + // If already expanded, return the cost + const unsigned int size_x = sampled_costmap->getSizeInCellsX(); + // Divided by 2 due to downsampled costmap. + const unsigned int start_y = floor(node_coords.y / 2.0); + const unsigned int start_x = floor(node_coords.x / 2.0); + const unsigned int start_index = start_y * size_x + start_x; + const float & requested_node_cost = obstacle_heuristic_lookup_table[start_index]; + if (requested_node_cost > 0.0f) { + // costs are doubled due to downsampling + return 2.0 * requested_node_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. + + // start_x and start_y have changed since last call + // we need to recompute 2D distance heuristic and reprioritize queue + for (auto & n : obstacle_heuristic_queue) { + n.first = -obstacle_heuristic_lookup_table[n.second] + + distanceHeuristic2D(n.second, size_x, start_x, start_y); + } + std::make_heap( + obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(), + ObstacleHeuristicComparator{}); + + const int size_x_int = static_cast(size_x); + const unsigned int size_y = sampled_costmap->getSizeInCellsY(); + const float sqrt_2 = sqrt(2); + float c_cost, cost, travel_cost, new_cost, existing_cost; + unsigned int idx, mx, my, mx_idx, my_idx; + unsigned int new_idx = 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()) { + idx = obstacle_heuristic_queue.front().second; + std::pop_heap( + obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(), + ObstacleHeuristicComparator{}); + obstacle_heuristic_queue.pop_back(); + c_cost = obstacle_heuristic_lookup_table[idx]; + if (c_cost > 0.0f) { + // cell has been processed and closed, no further cost improvements + // are mathematically possible thanks to euclidean distance heuristic consistency + continue; + } + c_cost = -c_cost; + obstacle_heuristic_lookup_table[idx] = c_cost; // set a positive value to close the cell + + 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]); + + // if neighbor path is better and non-lethal, set new cost and add to queue + if (new_idx < size_x * size_y) { + cost = static_cast(sampled_costmap->getCost(new_idx)); + if (cost >= INSCRIBED) { + continue; + } + + 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.0f) { + travel_cost = + ((i <= 3) ? 1.0f : sqrt_2) * (1.0f + (cost_penalty * cost / 252.0f)); + new_cost = c_cost + travel_cost; + if (existing_cost == 0.0f || -existing_cost > new_cost) { + // the negative value means the cell is in the open set + obstacle_heuristic_lookup_table[new_idx] = -new_cost; + obstacle_heuristic_queue.emplace_back( + new_cost + distanceHeuristic2D(new_idx, size_x, start_x, start_y), new_idx); + std::push_heap( + obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(), + ObstacleHeuristicComparator{}); + } + } + } + } + + if (idx == start_index) { + break; + } + } + + // return requested_node_cost which has been updated by the search + // costs are doubled due to downsampling + return 2.0 * requested_node_cost; +} + +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 < 0) { + dtheta_bin += motion_table.num_angle_quantization; + } + if (dtheta_bin > motion_table.num_angle_quantization) { + 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); + } + } + } +} + +bool NodeHybrid::backtracePath(CoordinateVector & path) +{ + if (!this->parent) { + return false; + } + + NodePtr current_node = this; + + while (current_node->parent) { + path.push_back(current_node->pose); + // Convert angle to radians + path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta); + current_node = current_node->parent; + } + + return path.size() > 0; +} + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp new file mode 100644 index 0000000000..7e0a917f48 --- /dev/null +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -0,0 +1,584 @@ +// 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 +#include +#include + +#include "ompl/base/ScopedState.h" +#include "ompl/base/spaces/DubinsStateSpace.h" +#include "ompl/base/spaces/ReedsSheppStateSpace.h" + +#include "nav2_smac_planner/node_lattice.hpp" + +using namespace std::chrono; // NOLINT + +namespace nav2_smac_planner +{ + +// defining static member for all instance to share +LatticeMotionTable NodeLattice::motion_table; +float NodeLattice::size_lookup = 25; +LookupTable NodeLattice::dist_heuristic_lookup_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. +void LatticeMotionTable::initMotionModel( + unsigned int & size_x_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + + if (current_lattice_filepath == search_info.lattice_filepath) { + return; + } + + 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; + travel_distance_reward = 1.0f - search_info.retrospective_penalty; + current_lattice_filepath = search_info.lattice_filepath; + allow_reverse_expansion = search_info.allow_reverse_expansion; + rotation_penalty = search_info.rotation_penalty; + + // Get the metadata about this minimum control set + lattice_metadata = getLatticeMetadata(current_lattice_filepath); + std::ifstream latticeFile(current_lattice_filepath); + if (!latticeFile.is_open()) { + throw std::runtime_error("Could not open lattice file"); + } + nlohmann::json json; + latticeFile >> json; + num_angle_quantization = lattice_metadata.number_of_headings; + + if (!state_space) { + if (!allow_reverse_expansion) { + state_space = std::make_unique( + lattice_metadata.min_turning_radius); + } else { + state_space = std::make_unique( + lattice_metadata.min_turning_radius); + } + } + + // Populate the motion primitives at each heading angle + float prev_start_angle = 0.0; + std::vector primitives; + nlohmann::json json_primitives = json["primitives"]; + for (unsigned int i = 0; i < json_primitives.size(); ++i) { + MotionPrimitive new_primitive; + fromJsonToMotionPrimitive(json_primitives[i], new_primitive); + + if (prev_start_angle != new_primitive.start_angle) { + motion_primitives.push_back(primitives); + primitives.clear(); + prev_start_angle = new_primitive.start_angle; + } + primitives.push_back(new_primitive); + } + motion_primitives.push_back(primitives); + + // Populate useful precomputed values to be leveraged + trig_values.reserve(lattice_metadata.number_of_headings); + for (unsigned int i = 0; i < lattice_metadata.heading_angles.size(); ++i) { + trig_values.emplace_back( + cos(lattice_metadata.heading_angles[i]), + sin(lattice_metadata.heading_angles[i])); + } +} + +MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(const NodeLattice * node) +{ + MotionPrimitives & prims_at_heading = motion_primitives[node->pose.theta]; + MotionPrimitivePtrs primitive_projection_list; + for (unsigned int i = 0; i != prims_at_heading.size(); i++) { + primitive_projection_list.push_back(&prims_at_heading[i]); + } + + if (allow_reverse_expansion) { + // Find normalized heading bin of the reverse expansion + double reserve_heading = node->pose.theta - (num_angle_quantization / 2); + if (reserve_heading < 0) { + reserve_heading += num_angle_quantization; + } + if (reserve_heading > num_angle_quantization) { + reserve_heading -= num_angle_quantization; + } + + MotionPrimitives & prims_at_reverse_heading = motion_primitives[reserve_heading]; + for (unsigned int i = 0; i != prims_at_reverse_heading.size(); i++) { + primitive_projection_list.push_back(&prims_at_reverse_heading[i]); + } + } + + return primitive_projection_list; +} + +LatticeMetadata LatticeMotionTable::getLatticeMetadata(const std::string & lattice_filepath) +{ + std::ifstream lattice_file(lattice_filepath); + if (!lattice_file.is_open()) { + throw std::runtime_error("Could not open lattice file!"); + } + + nlohmann::json j; + lattice_file >> j; + LatticeMetadata metadata; + fromJsonToMetaData(j["lattice_metadata"], metadata); + return metadata; +} + +unsigned int LatticeMotionTable::getClosestAngularBin(const double & theta) +{ + float min_dist = std::numeric_limits::max(); + unsigned int closest_idx = 0; + float dist = 0.0; + for (unsigned int i = 0; i != lattice_metadata.heading_angles.size(); i++) { + dist = fabs(angles::shortest_angular_distance(theta, lattice_metadata.heading_angles[i])); + if (dist < min_dist) { + min_dist = dist; + closest_idx = i; + } + } + return closest_idx; +} + +float & LatticeMotionTable::getAngleFromBin(const unsigned int & bin_idx) +{ + return lattice_metadata.heading_angles[bin_idx]; +} + +NodeLattice::NodeLattice(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(nullptr), + _backwards(false) +{ +} + +NodeLattice::~NodeLattice() +{ + parent = nullptr; +} + +void NodeLattice::reset() +{ + parent = nullptr; + _cell_cost = std::numeric_limits::quiet_NaN(); + _accumulated_cost = std::numeric_limits::max(); + _was_visited = false; + pose.x = 0.0f; + pose.y = 0.0f; + pose.theta = 0.0f; + _motion_primitive = nullptr; + _backwards = false; +} + +bool NodeLattice::isNodeValid( + const bool & traverse_unknown, + GridCollisionChecker * collision_checker, + MotionPrimitive * motion_primitive, + bool is_backwards) +{ + // Check primitive end pose + // Convert grid quantization of primitives to radians, then collision checker quantization + static const double bin_size = 2.0 * M_PI / collision_checker->getPrecomputedAngles().size(); + const double & angle = motion_table.getAngleFromBin(this->pose.theta) / bin_size; + if (collision_checker->inCollision( + this->pose.x, this->pose.y, angle /*bin in collision checker*/, traverse_unknown)) + { + return false; + } + + // Set the cost of a node to the highest cost across the primitive + float max_cell_cost = collision_checker->getCost(); + + // If valid motion primitives are set, check intermediary poses > 1 cell apart + if (motion_primitive) { + const float pi_2 = 2.0 * M_PI; + const float & grid_resolution = motion_table.lattice_metadata.grid_resolution; + const float & resolution_diag_sq = 2.0 * grid_resolution * grid_resolution; + MotionPose last_pose(1e9, 1e9, 1e9), pose_dist(0.0, 0.0, 0.0); + + // Back out the initial node starting point to move motion primitive relative to + MotionPose initial_pose, prim_pose; + initial_pose._x = this->pose.x - (motion_primitive->poses.back()._x / grid_resolution); + initial_pose._y = this->pose.y - (motion_primitive->poses.back()._y / grid_resolution); + initial_pose._theta = motion_table.getAngleFromBin(motion_primitive->start_angle); + + for (auto it = motion_primitive->poses.begin(); it != motion_primitive->poses.end(); ++it) { + // poses are in metric coordinates from (0, 0), not grid space yet + pose_dist = *it - last_pose; + // Avoid square roots by (hypot(x, y) > res) == (x*x+y*y > diag*diag) + if (pose_dist._x * pose_dist._x + pose_dist._y * pose_dist._y > resolution_diag_sq) { + last_pose = *it; + // Convert primitive pose into grid space if it should be checked + prim_pose._x = initial_pose._x + (it->_x / grid_resolution); + prim_pose._y = initial_pose._y + (it->_y / grid_resolution); + // If reversing, invert the angle because the robot is backing into the primitive + // not driving forward with it + if (is_backwards) { + prim_pose._theta = std::fmod(it->_theta + M_PI, pi_2); + } else { + prim_pose._theta = it->_theta; + } + if (collision_checker->inCollision( + prim_pose._x, + prim_pose._y, + prim_pose._theta / bin_size /*bin in collision checker*/, + traverse_unknown)) + { + return false; + } + max_cell_cost = std::max(max_cell_cost, collision_checker->getCost()); + } + } + } + + _cell_cost = max_cell_cost; + return true; +} + +float NodeLattice::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 collision cost!"); + } + + // this is the first node + MotionPrimitive * prim = this->getMotionPrimitive(); + MotionPrimitive * transition_prim = child->getMotionPrimitive(); + const float prim_length = + transition_prim->trajectory_length / motion_table.lattice_metadata.grid_resolution; + if (prim == nullptr) { + return prim_length; + } + + // Pure rotation in place 1 angular bin in either direction + if (transition_prim->trajectory_length < 1e-4) { + return motion_table.rotation_penalty * (1.0 + motion_table.cost_penalty * normalized_cost); + } + + float travel_cost = 0.0; + float travel_cost_raw = prim_length * + (motion_table.travel_distance_reward + motion_table.cost_penalty * normalized_cost); + + if (transition_prim->arc_length < 0.001) { + // New motion is a straight motion, no additional costs to be applied + travel_cost = travel_cost_raw; + } else { + if (prim->left_turn == transition_prim->left_turn) { + // Turning motion but keeps in same general direction: encourages to commit to actions + travel_cost = travel_cost_raw * motion_table.non_straight_penalty; + } else { + // Turning motion and velocity directions: penalizes wiggling. + travel_cost = travel_cost_raw * + (motion_table.non_straight_penalty + motion_table.change_penalty); + } + } + + // If backwards flag is set, this primitive is moving in reverse + if (child->isBackward()) { + // reverse direction + travel_cost *= motion_table.reverse_penalty; + } + + return travel_cost; +} + +float NodeLattice::getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const nav2_costmap_2d::Costmap2D * costmap) +{ + // get obstacle heuristic value + const float obstacle_heuristic = getObstacleHeuristic( + node_coords, goal_coords, motion_table.cost_penalty); + const float distance_heuristic = + getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); + return std::max(obstacle_heuristic, distance_heuristic); +} + +void NodeLattice::initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & /*size_y*/, + unsigned int & /*num_angle_quantization*/, + SearchInfo & search_info) +{ + if (motion_model != MotionModel::STATE_LATTICE) { + throw std::runtime_error( + "Invalid motion model for Lattice node. Please select" + " STATE_LATTICE and provide a valid lattice file."); + } + + motion_table.initMotionModel(size_x, search_info); +} + +float NodeLattice::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 length) + + // 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 < 0) { + dtheta_bin += motion_table.num_angle_quantization; + } + if (dtheta_bin > motion_table.num_angle_quantization) { + 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) { + 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] = motion_table.getAngleFromBin(goal_coords.theta); + from[0] = node_coords.x; + from[1] = node_coords.y; + from[2] = motion_table.getAngleFromBin(node_coords.theta); + motion_heuristic = motion_table.state_space->distance(from(), to()); + } + + return motion_heuristic; +} + +void NodeLattice::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 (!search_info.allow_reverse_expansion) { + motion_table.state_space = std::make_unique( + search_info.minimum_turning_radius); + } else { + motion_table.state_space = std::make_unique( + search_info.minimum_turning_radius); + } + motion_table.lattice_metadata = + LatticeMotionTable::getLatticeMetadata(search_info.lattice_filepath); + + 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); + + // 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] = motion_table.getAngleFromBin(heading); + motion_heuristic = motion_table.state_space->distance(from(), to()); + dist_heuristic_lookup_table[index] = motion_heuristic; + index++; + } + } + } +} + +void NodeLattice::getNeighbors( + std::function & NeighborGetter, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors) +{ + unsigned int index = 0; + float angle; + bool backwards = false; + NodePtr neighbor = nullptr; + Coordinates initial_node_coords, motion_projection; + MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives(this); + const float & grid_resolution = motion_table.lattice_metadata.grid_resolution; + + unsigned int direction_change_idx = 1e9; + for (unsigned int i = 0; i != motion_primitives.size(); i++) { + if (motion_primitives[0]->start_angle != motion_primitives[i]->start_angle) { + direction_change_idx = i; + break; + } + } + + for (unsigned int i = 0; i != motion_primitives.size(); i++) { + const MotionPose & end_pose = motion_primitives[i]->poses.back(); + motion_projection.x = this->pose.x + (end_pose._x / grid_resolution); + motion_projection.y = this->pose.y + (end_pose._y / grid_resolution); + motion_projection.theta = motion_primitives[i]->end_angle /*this is the ending angular bin*/; + + index = NodeLattice::getIndex( + static_cast(motion_projection.x), + static_cast(motion_projection.y), + static_cast(motion_projection.theta)); + + 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; + // if i >= idx, then we're in a reversing primitive. In that situation, + // the orientation of the robot is mirrored from what it would otherwise + // appear to be from the motion primitives file. We want to take this into + // account in case the robot base footprint is asymmetric. + backwards = false; + angle = motion_projection.theta; + if (i >= direction_change_idx) { + backwards = true; + angle = motion_projection.theta - (motion_table.num_angle_quantization / 2); + if (angle < 0) { + angle += motion_table.num_angle_quantization; + } + if (angle > motion_table.num_angle_quantization) { + angle -= motion_table.num_angle_quantization; + } + } + + neighbor->setPose( + Coordinates( + motion_projection.x, + motion_projection.y, + angle)); + + // Using a special isNodeValid API here, giving the motion primitive to use to + // validity check the transition of the current node to the new node over + if (neighbor->isNodeValid( + traverse_unknown, collision_checker, motion_primitives[i], backwards)) + { + neighbor->setMotionPrimitive(motion_primitives[i]); + // Marking if this search was obtained in the reverse direction + neighbor->backwards(backwards); + neighbors.push_back(neighbor); + } else { + neighbor->setPose(initial_node_coords); + } + } + } +} + +bool NodeLattice::backtracePath(CoordinateVector & path) +{ + if (!this->parent) { + return false; + } + + Coordinates initial_pose, prim_pose; + NodePtr current_node = this; + MotionPrimitive * prim = nullptr; + const float & grid_resolution = NodeLattice::motion_table.lattice_metadata.grid_resolution; + const float pi_2 = 2.0 * M_PI; + + while (current_node->parent) { + prim = current_node->getMotionPrimitive(); + // if motion primitive is valid, then was searched (rather than analytically expanded), + // include dense path of subpoints making up the primitive at grid resolution + if (prim) { + initial_pose.x = current_node->pose.x - (prim->poses.back()._x / grid_resolution); + initial_pose.y = current_node->pose.y - (prim->poses.back()._y / grid_resolution); + initial_pose.theta = NodeLattice::motion_table.getAngleFromBin(prim->start_angle); + + for (auto it = prim->poses.crbegin(); it != prim->poses.crend(); ++it) { + // Convert primitive pose into grid space if it should be checked + prim_pose.x = initial_pose.x + (it->_x / grid_resolution); + prim_pose.y = initial_pose.y + (it->_y / grid_resolution); + // If reversing, invert the angle because the robot is backing into the primitive + // not driving forward with it + if (current_node->isBackward()) { + prim_pose.theta = std::fmod(it->_theta + M_PI, pi_2); + } else { + prim_pose.theta = it->_theta; + } + path.push_back(prim_pose); + } + } else { + // For analytic expansion nodes where there is no valid motion primitive + path.push_back(current_node->pose); + path.back().theta = NodeLattice::motion_table.getAngleFromBin(path.back().theta); + } + + current_node = current_node->parent; + } + + return path.size() > 0; +} + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp new file mode 100644 index 0000000000..cf8657118f --- /dev/null +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -0,0 +1,442 @@ +// 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 + +#include "nav2_smac_planner/smac_planner_2d.hpp" +#include "nav2_util/geometry_utils.hpp" + +// #define BENCHMARK_TESTING + +namespace nav2_smac_planner +{ +using namespace std::chrono; // NOLINT +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +SmacPlanner2D::SmacPlanner2D() +: _a_star(nullptr), + _collision_checker(nullptr, 1), + _smoother(nullptr), + _costmap(nullptr), + _costmap_downsampler(nullptr) +{ +} + +SmacPlanner2D::~SmacPlanner2D() +{ + RCLCPP_INFO( + _logger, "Destroying plugin %s of type SmacPlanner2D", + _name.c_str()); +} + +void SmacPlanner2D::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr costmap_ros) +{ + _node = parent; + auto node = parent.lock(); + _logger = node->get_logger(); + _clock = node->get_clock(); + _costmap = costmap_ros->getCostmap(); + _name = name; + _global_frame = costmap_ros->getGlobalFrameID(); + + RCLCPP_INFO(_logger, "Configuring %s of type SmacPlanner2D", name.c_str()); + + // 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(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 + ".cost_travel_multiplier", rclcpp::ParameterValue(1.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(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 + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".use_final_approach_orientation", _use_final_approach_orientation); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".max_planning_time", rclcpp::ParameterValue(2.0)); + node->get_parameter(name + ".max_planning_time", _max_planning_time); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE"))); + node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search); + _motion_model = fromString(_motion_model_for_search); + if (_motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + _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, " + "disabling maximum iterations."); + _max_iterations = std::numeric_limits::max(); + } + + // 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, + _max_planning_time, + 0.0 /*unused for 2D*/, + 1.0 /*unused for 2D*/); + + // Initialize path smoother + SmootherParams params; + params.get(node, name); + params.holonomic_ = true; // So smoother will treat this as a grid search + _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(); + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); + } + + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + + RCLCPP_INFO( + _logger, "Configured plugin %s of type SmacPlanner2D 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, + _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + toString(_motion_model).c_str()); +} + +void SmacPlanner2D::activate() +{ + RCLCPP_INFO( + _logger, "Activating plugin %s of type SmacPlanner2D", + _name.c_str()); + _raw_plan_publisher->on_activate(); + if (_costmap_downsampler) { + _costmap_downsampler->on_activate(); + } + auto node = _node.lock(); + // Add callback for dynamic parameters + _dyn_params_handler = node->add_on_set_parameters_callback( + std::bind(&SmacPlanner2D::dynamicParametersCallback, this, _1)); +} + +void SmacPlanner2D::deactivate() +{ + RCLCPP_INFO( + _logger, "Deactivating plugin %s of type SmacPlanner2D", + _name.c_str()); + _raw_plan_publisher->on_deactivate(); + if (_costmap_downsampler) { + _costmap_downsampler->on_deactivate(); + } + _dyn_params_handler.reset(); +} + +void SmacPlanner2D::cleanup() +{ + RCLCPP_INFO( + _logger, "Cleaning up plugin %s of type SmacPlanner2D", + _name.c_str()); + _a_star.reset(); + _smoother.reset(); + if (_costmap_downsampler) { + _costmap_downsampler->on_cleanup(); + _costmap_downsampler.reset(); + } + _raw_plan_publisher.reset(); +} + +nav_msgs::msg::Path SmacPlanner2D::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + std::lock_guard lock_reinit(_mutex); + steady_clock::time_point a = steady_clock::now(); + + std::unique_lock lock(*(_costmap->getMutex())); + + // Downsample costmap, if required + nav2_costmap_2d::Costmap2D * costmap = _costmap; + if (_costmap_downsampler) { + costmap = _costmap_downsampler->downsample(_downsampling_factor); + _collision_checker.setCostmap(costmap); + } + + // Set collision checker and costmap information + _a_star->setCollisionChecker(&_collision_checker); + + // Set starting point + unsigned int mx_start, my_start, mx_goal, my_goal; + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); + _a_star->setStart(mx_start, my_start, 0); + + // Set goal point + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); + _a_star->setGoal(mx_goal, my_goal, 0); + + // Setup message + nav_msgs::msg::Path plan; + plan.header.stamp = _clock->now(); + plan.header.frame_id = _global_frame; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + 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; + + // Corner case of start and goal beeing on the same cell + if (mx_start == mx_goal && my_start == my_goal) { + if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { + RCLCPP_WARN(_logger, "Failed to create a unique pose path because of obstacles"); + return plan; + } + pose.pose = start.pose; + // if we have a different start and goal orientation, set the unique path pose to the goal + // orientation, unless use_final_approach_orientation=true where we need it to be the start + // orientation to avoid movement from the local planner + if (start.pose.orientation != goal.pose.orientation && !_use_final_approach_orientation) { + pose.pose.orientation = goal.pose.orientation; + } + plan.poses.push_back(pose); + return plan; + } + + // Compute plan + Node2D::CoordinateVector path; + int num_iterations = 0; + std::string error; + try { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) + { + if (num_iterations < _a_star->getMaxIterations()) { + error = std::string("no valid path found"); + } else { + error = std::string("exceeded maximum iterations"); + } + } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _logger, + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + return plan; + } + + // Convert to world coordinates + plan.poses.reserve(path.size()); + for (int i = path.size() - 1; i >= 0; --i) { + pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + _raw_plan_publisher->publish(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()); + +#ifdef BENCHMARK_TESTING + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif + + // Smooth plan + _smoother->smooth(plan, costmap, time_remaining); + + // If use_final_approach_orientation=true, interpolate the last pose orientation from the + // previous pose to set the orientation to the 'final approach' orientation of the robot so + // it does not rotate. + // And deal with corner case of plan of length 1 + // If use_final_approach_orientation=false (default), override last pose orientation to match goal + size_t plan_size = plan.poses.size(); + if (_use_final_approach_orientation) { + if (plan_size == 1) { + plan.poses.back().pose.orientation = start.pose.orientation; + } else if (plan_size > 1) { + double dx, dy, theta; + auto last_pose = plan.poses.back().pose.position; + auto approach_pose = plan.poses[plan_size - 2].pose.position; + dx = last_pose.x - approach_pose.x; + dy = last_pose.y - approach_pose.y; + theta = atan2(dy, dx); + plan.poses.back().pose.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(theta); + } + } else if (plan_size > 0) { + plan.poses.back().pose.orientation = goal.pose.orientation; + } + + return plan; +} + +rcl_interfaces::msg::SetParametersResult +SmacPlanner2D::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(_mutex); + + bool reinit_a_star = false; + bool reinit_downsampler = false; + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == _name + ".tolerance") { + _tolerance = static_cast(parameter.as_double()); + } else if (name == _name + ".cost_travel_multiplier") { + reinit_a_star = true; + _search_info.cost_penalty = parameter.as_double(); + } else if (name == _name + ".max_planning_time") { + reinit_a_star = true; + _max_planning_time = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == _name + ".downsample_costmap") { + reinit_downsampler = true; + _downsample_costmap = parameter.as_bool(); + } else if (name == _name + ".allow_unknown") { + reinit_a_star = true; + _allow_unknown = parameter.as_bool(); + } else if (name == _name + ".use_final_approach_orientation") { + _use_final_approach_orientation = parameter.as_bool(); + } + } else if (type == ParameterType::PARAMETER_INTEGER) { + if (name == _name + ".downsampling_factor") { + reinit_downsampler = true; + _downsampling_factor = parameter.as_int(); + } else if (name == _name + ".max_iterations") { + reinit_a_star = true; + _max_iterations = parameter.as_int(); + if (_max_iterations <= 0) { + RCLCPP_INFO( + _logger, "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + _max_iterations = std::numeric_limits::max(); + } + } else if (name == _name + ".max_on_approach_iterations") { + reinit_a_star = true; + _max_on_approach_iterations = parameter.as_int(); + 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(); + } + } + } else if (type == ParameterType::PARAMETER_STRING) { + if (name == _name + ".motion_model_for_search") { + reinit_a_star = true; + _motion_model = fromString(parameter.as_string()); + if (_motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + _motion_model_for_search.c_str()); + } + } + } + } + + // Re-init if needed with mutex lock (to avoid re-init while creating a plan) + if (reinit_a_star || reinit_downsampler) { + // Re-Initialize A* template + if (reinit_a_star) { + _a_star = std::make_unique>(_motion_model, _search_info); + _a_star->initialize( + _allow_unknown, + _max_iterations, + _max_on_approach_iterations, + _max_planning_time, + 0.0 /*unused for 2D*/, + 1.0 /*unused for 2D*/); + } + + // Re-Initialize costmap downsampler + if (reinit_downsampler) { + if (_downsample_costmap && _downsampling_factor > 1) { + auto node = _node.lock(); + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); + } + } + } + result.successful = true; + return result; +} + +} // namespace nav2_smac_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlanner2D, nav2_core::GlobalPlanner) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp new file mode 100644 index 0000000000..279af1804c --- /dev/null +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -0,0 +1,549 @@ +// 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 + +#include "Eigen/Core" +#include "nav2_smac_planner/smac_planner_hybrid.hpp" + +// #define BENCHMARK_TESTING + +namespace nav2_smac_planner +{ + +using namespace std::chrono; // NOLINT +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +SmacPlannerHybrid::SmacPlannerHybrid() +: _a_star(nullptr), + _collision_checker(nullptr, 1), + _smoother(nullptr), + _costmap(nullptr), + _costmap_downsampler(nullptr) +{ +} + +SmacPlannerHybrid::~SmacPlannerHybrid() +{ + RCLCPP_INFO( + _logger, "Destroying plugin %s of type SmacPlannerHybrid", + _name.c_str()); +} + +void SmacPlannerHybrid::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr costmap_ros) +{ + _node = parent; + auto node = parent.lock(); + _logger = node->get_logger(); + _clock = node->get_clock(); + _costmap = costmap_ros->getCostmap(); + _costmap_ros = costmap_ros; + _name = name; + _global_frame = costmap_ros->getGlobalFrameID(); + + RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerHybrid", name.c_str()); + + int angle_quantizations; + double analytic_expansion_max_length_m; + bool smooth_path; + + // General planner params + nav2_util::declare_parameter_if_not_declared( + 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(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); + + 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(1000000)); + node->get_parameter(name + ".max_iterations", _max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".smooth_path", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".smooth_path", smooth_path); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4)); + node->get_parameter(name + ".minimum_turning_radius", _minimum_turning_radius_global_coords); + 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.0)); + 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.2)); + 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(2.0)); + node->get_parameter(name + ".cost_penalty", _search_info.cost_penalty); + nav2_util::declare_parameter_if_not_declared( + node, name + ".retrospective_penalty", rclcpp::ParameterValue(0.015)); + node->get_parameter(name + ".retrospective_penalty", _search_info.retrospective_penalty); + nav2_util::declare_parameter_if_not_declared( + 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 + ".analytic_expansion_max_length", rclcpp::ParameterValue(3.0)); + node->get_parameter(name + ".analytic_expansion_max_length", analytic_expansion_max_length_m); + _search_info.analytic_expansion_max_length = + analytic_expansion_max_length_m / _costmap->getResolution(); + + 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"))); + node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search); + _motion_model = fromString(_motion_model_for_search); + if (_motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.", + _motion_model_for_search.c_str()); + } + + if (_max_iterations <= 0) { + RCLCPP_INFO( + _logger, "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + _max_iterations = std::numeric_limits::max(); + } + + // convert to grid coordinates + if (!_downsample_costmap) { + _downsampling_factor = 1; + } + _search_info.minimum_turning_radius = + _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor); + _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)); + + // 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, + std::numeric_limits::max(), + _max_planning_time, + _lookup_table_dim, + _angle_quantizations); + + // Initialize path smoother + if (smooth_path) { + 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) { + _costmap_downsampler = std::make_unique(); + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); + } + + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + + RCLCPP_INFO( + _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 SmacPlannerHybrid::activate() +{ + RCLCPP_INFO( + _logger, "Activating plugin %s of type SmacPlannerHybrid", + _name.c_str()); + _raw_plan_publisher->on_activate(); + if (_costmap_downsampler) { + _costmap_downsampler->on_activate(); + } + auto node = _node.lock(); + // Add callback for dynamic parameters + _dyn_params_handler = node->add_on_set_parameters_callback( + std::bind(&SmacPlannerHybrid::dynamicParametersCallback, this, _1)); +} + +void SmacPlannerHybrid::deactivate() +{ + RCLCPP_INFO( + _logger, "Deactivating plugin %s of type SmacPlannerHybrid", + _name.c_str()); + _raw_plan_publisher->on_deactivate(); + if (_costmap_downsampler) { + _costmap_downsampler->on_deactivate(); + } + _dyn_params_handler.reset(); +} + +void SmacPlannerHybrid::cleanup() +{ + RCLCPP_INFO( + _logger, "Cleaning up plugin %s of type SmacPlannerHybrid", + _name.c_str()); + _a_star.reset(); + _smoother.reset(); + if (_costmap_downsampler) { + _costmap_downsampler->on_cleanup(); + _costmap_downsampler.reset(); + } + _raw_plan_publisher.reset(); +} + +nav_msgs::msg::Path SmacPlannerHybrid::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + std::lock_guard lock_reinit(_mutex); + steady_clock::time_point a = steady_clock::now(); + + std::unique_lock lock(*(_costmap->getMutex())); + + // Downsample costmap, if required + nav2_costmap_2d::Costmap2D * costmap = _costmap; + if (_costmap_downsampler) { + costmap = _costmap_downsampler->downsample(_downsampling_factor); + _collision_checker.setCostmap(costmap); + } + + // Set collision checker and costmap information + _a_star->setCollisionChecker(&_collision_checker); + + // Set starting point, in A* bin search coordinates + unsigned int mx, my; + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; + while (orientation_bin < 0.0) { + orientation_bin += static_cast(_angle_quantizations); + } + // This is needed to handle precision issues + if (orientation_bin >= static_cast(_angle_quantizations)) { + orientation_bin -= static_cast(_angle_quantizations); + } + unsigned int orientation_bin_id = static_cast(floor(orientation_bin)); + _a_star->setStart(mx, my, orientation_bin_id); + + // Set goal point, in A* bin search coordinates + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; + while (orientation_bin < 0.0) { + orientation_bin += static_cast(_angle_quantizations); + } + // This is needed to handle precision issues + if (orientation_bin >= static_cast(_angle_quantizations)) { + orientation_bin -= static_cast(_angle_quantizations); + } + orientation_bin_id = static_cast(floor(orientation_bin)); + _a_star->setGoal(mx, my, orientation_bin_id); + + // Setup message + nav_msgs::msg::Path plan; + plan.header.stamp = _clock->now(); + plan.header.frame_id = _global_frame; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + 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; + + // Compute plan + NodeHybrid::CoordinateVector path; + int num_iterations = 0; + std::string error; + try { + if (!_a_star->createPath(path, num_iterations, 0.0)) { + if (num_iterations < _a_star->getMaxIterations()) { + error = std::string("no valid path found"); + } else { + error = std::string("exceeded maximum iterations"); + } + } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _logger, + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + return plan; + } + + // Convert to world coordinates + plan.poses.reserve(path.size()); + for (int i = path.size() - 1; i >= 0; --i) { + pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); + pose.pose.orientation = getWorldOrientation(path[i].theta); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + _raw_plan_publisher->publish(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()); + +#ifdef BENCHMARK_TESTING + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif + + // Smooth plan + if (_smoother && num_iterations > 1) { + _smoother->smooth(plan, costmap, time_remaining); + } + +#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; +} + +rcl_interfaces::msg::SetParametersResult +SmacPlannerHybrid::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(_mutex); + + bool reinit_collision_checker = false; + bool reinit_a_star = false; + bool reinit_downsampler = false; + bool reinit_smoother = false; + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == _name + ".max_planning_time") { + reinit_a_star = true; + _max_planning_time = parameter.as_double(); + } else if (name == _name + ".lookup_table_size") { + reinit_a_star = true; + _lookup_table_size = parameter.as_double(); + } else if (name == _name + ".minimum_turning_radius") { + reinit_a_star = true; + if (_smoother) { + reinit_smoother = true; + } + _minimum_turning_radius_global_coords = static_cast(parameter.as_double()); + } else if (name == _name + ".reverse_penalty") { + reinit_a_star = true; + _search_info.reverse_penalty = static_cast(parameter.as_double()); + } else if (name == _name + ".change_penalty") { + reinit_a_star = true; + _search_info.change_penalty = static_cast(parameter.as_double()); + } else if (name == _name + ".non_straight_penalty") { + reinit_a_star = true; + _search_info.non_straight_penalty = static_cast(parameter.as_double()); + } else if (name == _name + ".cost_penalty") { + reinit_a_star = true; + _search_info.cost_penalty = static_cast(parameter.as_double()); + } else if (name == _name + ".analytic_expansion_ratio") { + reinit_a_star = true; + _search_info.analytic_expansion_ratio = static_cast(parameter.as_double()); + } else if (name == _name + ".analytic_expansion_max_length") { + reinit_a_star = true; + _search_info.analytic_expansion_max_length = + static_cast(parameter.as_double()) / _costmap->getResolution(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == _name + ".downsample_costmap") { + reinit_downsampler = true; + _downsample_costmap = parameter.as_bool(); + } else if (name == _name + ".allow_unknown") { + reinit_a_star = true; + _allow_unknown = parameter.as_bool(); + } else if (name == _name + ".cache_obstacle_heuristic") { + reinit_a_star = true; + _search_info.cache_obstacle_heuristic = parameter.as_bool(); + } else if (name == _name + ".smooth_path") { + if (parameter.as_bool()) { + reinit_smoother = true; + } else { + _smoother.reset(); + } + } + } else if (type == ParameterType::PARAMETER_INTEGER) { + if (name == _name + ".downsampling_factor") { + reinit_a_star = true; + reinit_downsampler = true; + _downsampling_factor = parameter.as_int(); + } else if (name == _name + ".max_iterations") { + reinit_a_star = true; + _max_iterations = parameter.as_int(); + if (_max_iterations <= 0) { + RCLCPP_INFO( + _logger, "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + _max_iterations = std::numeric_limits::max(); + } + } else if (name == _name + ".angle_quantization_bins") { + reinit_collision_checker = true; + reinit_a_star = true; + int angle_quantizations = parameter.as_int(); + _angle_bin_size = 2.0 * M_PI / angle_quantizations; + _angle_quantizations = static_cast(angle_quantizations); + } + } else if (type == ParameterType::PARAMETER_STRING) { + if (name == _name + ".motion_model_for_search") { + reinit_a_star = true; + _motion_model = fromString(parameter.as_string()); + if (_motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + _motion_model_for_search.c_str()); + } + } + } + } + + // Re-init if needed with mutex lock (to avoid re-init while creating a plan) + if (reinit_a_star || reinit_downsampler || reinit_collision_checker || reinit_smoother) { + // convert to grid coordinates + if (!_downsample_costmap) { + _downsampling_factor = 1; + } + _search_info.minimum_turning_radius = + _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor); + _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)); + + // 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; + } + + // Re-Initialize A* template + if (reinit_a_star) { + _a_star = std::make_unique>(_motion_model, _search_info); + _a_star->initialize( + _allow_unknown, + _max_iterations, + std::numeric_limits::max(), + _max_planning_time, + _lookup_table_dim, + _angle_quantizations); + } + + // Re-Initialize costmap downsampler + if (reinit_downsampler) { + if (_downsample_costmap && _downsampling_factor > 1) { + auto node = _node.lock(); + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); + } + } + + // Re-Initialize collision checker + if (reinit_collision_checker) { + _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); + _collision_checker.setFootprint( + _costmap_ros->getRobotFootprint(), + _costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); + } + + // Re-Initialize smoother + if (reinit_smoother) { + auto node = _node.lock(); + SmootherParams params; + params.get(node, _name); + _smoother = std::make_unique(params); + _smoother->initialize(_minimum_turning_radius_global_coords); + } + } + result.successful = true; + return result; +} + +} // namespace nav2_smac_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlannerHybrid, nav2_core::GlobalPlanner) diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp new file mode 100644 index 0000000000..09a3591928 --- /dev/null +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -0,0 +1,470 @@ +// 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 "Eigen/Core" +#include "nav2_smac_planner/smac_planner_lattice.hpp" + +// #define BENCHMARK_TESTING + +namespace nav2_smac_planner +{ + +using namespace std::chrono; // NOLINT +using rcl_interfaces::msg::ParameterType; + +SmacPlannerLattice::SmacPlannerLattice() +: _a_star(nullptr), + _collision_checker(nullptr, 1), + _smoother(nullptr), + _costmap(nullptr) +{ +} + +SmacPlannerLattice::~SmacPlannerLattice() +{ + RCLCPP_INFO( + _logger, "Destroying plugin %s of type SmacPlannerLattice", + _name.c_str()); +} + +void SmacPlannerLattice::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr costmap_ros) +{ + _node = parent; + auto node = parent.lock(); + _logger = node->get_logger(); + _clock = node->get_clock(); + _costmap = costmap_ros->getCostmap(); + _costmap_ros = costmap_ros; + _name = name; + _global_frame = costmap_ros->getGlobalFrameID(); + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + + RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerLattice", name.c_str()); + + // General planner params + double analytic_expansion_max_length_m; + bool smooth_path; + + 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(1000000)); + node->get_parameter(name + ".max_iterations", _max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".smooth_path", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".smooth_path", smooth_path); + + // Default to a well rounded model: 16 bin, 0.4m turning radius, ackermann model + nav2_util::declare_parameter_if_not_declared( + node, name + ".lattice_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_smac_planner") + + "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json")); + node->get_parameter(name + ".lattice_filepath", _search_info.lattice_filepath); + 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.05)); + 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->get_parameter(name + ".non_straight_penalty", _search_info.non_straight_penalty); + nav2_util::declare_parameter_if_not_declared( + node, name + ".cost_penalty", rclcpp::ParameterValue(2.0)); + node->get_parameter(name + ".cost_penalty", _search_info.cost_penalty); + nav2_util::declare_parameter_if_not_declared( + node, name + ".retrospective_penalty", rclcpp::ParameterValue(0.015)); + node->get_parameter(name + ".retrospective_penalty", _search_info.retrospective_penalty); + nav2_util::declare_parameter_if_not_declared( + node, name + ".rotation_penalty", rclcpp::ParameterValue(5.0)); + node->get_parameter(name + ".rotation_penalty", _search_info.rotation_penalty); + nav2_util::declare_parameter_if_not_declared( + 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 + ".analytic_expansion_max_length", rclcpp::ParameterValue(3.0)); + node->get_parameter(name + ".analytic_expansion_max_length", analytic_expansion_max_length_m); + _search_info.analytic_expansion_max_length = + analytic_expansion_max_length_m / _costmap->getResolution(); + + 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 + ".allow_reverse_expansion", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".allow_reverse_expansion", _search_info.allow_reverse_expansion); + + _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); + _search_info.minimum_turning_radius = + _metadata.min_turning_radius / (_costmap->getResolution()); + _motion_model = MotionModel::STATE_LATTICE; + + if (_max_iterations <= 0) { + RCLCPP_INFO( + _logger, "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + _max_iterations = std::numeric_limits::max(); + } + + float lookup_table_dim = + static_cast(_lookup_table_size) / + static_cast(_costmap->getResolution()); + + // Make sure its a whole number + lookup_table_dim = static_cast(static_cast(lookup_table_dim)); + + // 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 using 72 evenly sized bins instead of the lattice + // heading angles. This is done so that we have precomputed angles every 5 degrees. + // If we used the sparse lattice headings (usually 16), then when we attempt to collision + // check for intermediary points of the primitives, we're forced to round to one of the 16 + // increments causing "wobbly" checks that could cause larger robots to virtually show collisions + // in valid configurations. This approximation helps to bound orientation error for all checks + // in exchange for slight inaccuracies in the collision headings in terminal search states. + _collision_checker = GridCollisionChecker(_costmap, 72u); + _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, + std::numeric_limits::max(), + _max_planning_time, + lookup_table_dim, + _metadata.number_of_headings); + + // Initialize path smoother + if (smooth_path) { + SmootherParams params; + params.get(node, name); + _smoother = std::make_unique(params); + _smoother->initialize(_metadata.min_turning_radius); + } + + RCLCPP_INFO( + _logger, "Configured plugin %s of type SmacPlannerLattice with " + "maximum iterations %i, " + "and %s. Using motion model: %s. State lattice file: %s.", + _name.c_str(), _max_iterations, + _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + toString(_motion_model).c_str(), _search_info.lattice_filepath.c_str()); +} + +void SmacPlannerLattice::activate() +{ + RCLCPP_INFO( + _logger, "Activating plugin %s of type SmacPlannerLattice", + _name.c_str()); + _raw_plan_publisher->on_activate(); + auto node = _node.lock(); + // Add callback for dynamic parameters + _dyn_params_handler = node->add_on_set_parameters_callback( + std::bind(&SmacPlannerLattice::dynamicParametersCallback, this, std::placeholders::_1)); +} + +void SmacPlannerLattice::deactivate() +{ + RCLCPP_INFO( + _logger, "Deactivating plugin %s of type SmacPlannerLattice", + _name.c_str()); + _raw_plan_publisher->on_deactivate(); + _dyn_params_handler.reset(); +} + +void SmacPlannerLattice::cleanup() +{ + RCLCPP_INFO( + _logger, "Cleaning up plugin %s of type SmacPlannerLattice", + _name.c_str()); + _a_star.reset(); + _smoother.reset(); + _raw_plan_publisher.reset(); +} + +nav_msgs::msg::Path SmacPlannerLattice::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + std::lock_guard lock_reinit(_mutex); + steady_clock::time_point a = steady_clock::now(); + + std::unique_lock lock(*(_costmap->getMutex())); + + // Set collision checker and costmap information + _a_star->setCollisionChecker(&_collision_checker); + + // Set starting point, in A* bin search coordinates + unsigned int mx, my; + _costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + _a_star->setStart( + mx, my, + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation))); + + // Set goal point, in A* bin search coordinates + _costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + _a_star->setGoal( + mx, my, + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); + + // Setup message + nav_msgs::msg::Path plan; + plan.header.stamp = _clock->now(); + plan.header.frame_id = _global_frame; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + 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; + + // Compute plan + NodeLattice::CoordinateVector path; + int num_iterations = 0; + std::string error; + try { + if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { + if (num_iterations < _a_star->getMaxIterations()) { + error = std::string("no valid path found"); + } else { + error = std::string("exceeded maximum iterations"); + } + } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _logger, + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + return plan; + } + + // Convert to world coordinates + plan.poses.reserve(path.size()); + geometry_msgs::msg::PoseStamped last_pose = pose; + for (int i = path.size() - 1; i >= 0; --i) { + pose.pose = getWorldCoords(path[i].x, path[i].y, _costmap); + pose.pose.orientation = getWorldOrientation(path[i].theta); + if (fabs(pose.pose.position.x - last_pose.pose.position.x) < 1e-4 && + fabs(pose.pose.position.y - last_pose.pose.position.y) < 1e-4 && + fabs(tf2::getYaw(pose.pose.orientation) - tf2::getYaw(last_pose.pose.orientation)) < 1e-4) + { + RCLCPP_DEBUG( + _logger, + "Removed a path from the path due to replication. " + "Make sure your minimum control set does not contain duplicate values!"); + continue; + } + last_pose = pose; + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + _raw_plan_publisher->publish(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()); + +#ifdef BENCHMARK_TESTING + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif + + // Smooth plan + if (_smoother && num_iterations > 1) { + _smoother->smooth(plan, _costmap, time_remaining); + } + +#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; +} + +rcl_interfaces::msg::SetParametersResult +SmacPlannerLattice::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(_mutex); + + bool reinit_a_star = false; + bool reinit_smoother = false; + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == _name + ".max_planning_time") { + reinit_a_star = true; + _max_planning_time = parameter.as_double(); + } else if (name == _name + ".lookup_table_size") { + reinit_a_star = true; + _lookup_table_size = parameter.as_double(); + } else if (name == _name + ".reverse_penalty") { + reinit_a_star = true; + _search_info.reverse_penalty = static_cast(parameter.as_double()); + } else if (name == _name + ".change_penalty") { + reinit_a_star = true; + _search_info.change_penalty = static_cast(parameter.as_double()); + } else if (name == _name + ".non_straight_penalty") { + reinit_a_star = true; + _search_info.non_straight_penalty = static_cast(parameter.as_double()); + } else if (name == _name + ".cost_penalty") { + reinit_a_star = true; + _search_info.cost_penalty = static_cast(parameter.as_double()); + } else if (name == _name + ".rotation_penalty") { + reinit_a_star = true; + _search_info.rotation_penalty = static_cast(parameter.as_double()); + } else if (name == _name + ".analytic_expansion_ratio") { + reinit_a_star = true; + _search_info.analytic_expansion_ratio = static_cast(parameter.as_double()); + } else if (name == _name + ".analytic_expansion_max_length") { + reinit_a_star = true; + _search_info.analytic_expansion_max_length = + static_cast(parameter.as_double()) / _costmap->getResolution(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == _name + ".allow_unknown") { + reinit_a_star = true; + _allow_unknown = parameter.as_bool(); + } else if (name == _name + ".cache_obstacle_heuristic") { + reinit_a_star = true; + _search_info.cache_obstacle_heuristic = parameter.as_bool(); + } else if (name == _name + ".allow_reverse_expansion") { + reinit_a_star = true; + _search_info.allow_reverse_expansion = parameter.as_bool(); + } else if (name == _name + ".smooth_path") { + if (parameter.as_bool()) { + reinit_smoother = true; + } else { + _smoother.reset(); + } + } + } else if (type == ParameterType::PARAMETER_INTEGER) { + if (name == _name + ".max_iterations") { + reinit_a_star = true; + _max_iterations = parameter.as_int(); + if (_max_iterations <= 0) { + RCLCPP_INFO( + _logger, "maximum iteration selected as <= 0, " + "disabling maximum iterations."); + _max_iterations = std::numeric_limits::max(); + } + } + } else if (type == ParameterType::PARAMETER_STRING) { + if (name == _name + ".lattice_filepath") { + reinit_a_star = true; + if (_smoother) { + reinit_smoother = true; + } + _search_info.lattice_filepath = parameter.as_string(); + _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); + _search_info.minimum_turning_radius = + _metadata.min_turning_radius / (_costmap->getResolution()); + } + } + } + + // Re-init if needed with mutex lock (to avoid re-init while creating a plan) + if (reinit_a_star || reinit_smoother) { + // convert to grid coordinates + _search_info.minimum_turning_radius = + _metadata.min_turning_radius / (_costmap->getResolution()); + float lookup_table_dim = + static_cast(_lookup_table_size) / + static_cast(_costmap->getResolution()); + + // Make sure its a whole number + lookup_table_dim = static_cast(static_cast(lookup_table_dim)); + + // 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; + } + + // Re-Initialize smoother + if (reinit_smoother) { + auto node = _node.lock(); + SmootherParams params; + params.get(node, _name); + _smoother = std::make_unique(params); + _smoother->initialize(_metadata.min_turning_radius); + } + + // Re-Initialize A* template + if (reinit_a_star) { + _a_star = std::make_unique>(_motion_model, _search_info); + _a_star->initialize( + _allow_unknown, + _max_iterations, + std::numeric_limits::max(), + _max_planning_time, + lookup_table_dim, + _metadata.number_of_headings); + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_smac_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlannerLattice, nav2_core::GlobalPlanner) diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp new file mode 100644 index 0000000000..fd0ac34be2 --- /dev/null +++ b/nav2_smac_planner/src/smoother.cpp @@ -0,0 +1,512 @@ +// 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 "nav2_smac_planner/smoother.hpp" + +namespace nav2_smac_planner +{ +using namespace nav2_util::geometry_utils; // NOLINT +using namespace std::chrono; // NOLINT + +Smoother::Smoother(const SmootherParams & params) +{ + tolerance_ = params.tolerance_; + max_its_ = params.max_its_; + data_w_ = params.w_data_; + smooth_w_ = params.w_smooth_; + is_holonomic_ = params.holonomic_; + do_refinement_ = params.do_refinement_; +} + +void Smoother::initialize(const double & min_turning_radius) +{ + min_turning_rad_ = min_turning_radius; + state_space_ = std::make_unique(min_turning_rad_); +} + +bool Smoother::smooth( + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time) +{ + // by-pass path orientations approximation when skipping smac smoother + if (max_its_ == 0) { + return false; + } + + refinement_ctr_ = 0; + steady_clock::time_point start = steady_clock::now(); + double time_remaining = max_time; + bool success = true, reversing_segment; + nav_msgs::msg::Path curr_path_segment; + curr_path_segment.header = path.header; + std::vector path_segments = findDirectionalPathSegments(path); + + for (unsigned int i = 0; i != path_segments.size(); i++) { + if (path_segments[i].end - path_segments[i].start > 10) { + // Populate path segment + curr_path_segment.poses.clear(); + std::copy( + path.poses.begin() + path_segments[i].start, + path.poses.begin() + path_segments[i].end + 1, + std::back_inserter(curr_path_segment.poses)); + + // Make sure we're still able to smooth with time remaining + steady_clock::time_point now = steady_clock::now(); + time_remaining = max_time - duration_cast>(now - start).count(); + + // Smooth path segment naively + const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose; + const geometry_msgs::msg::Pose goal_pose = curr_path_segment.poses.back().pose; + bool local_success = + smoothImpl(curr_path_segment, reversing_segment, costmap, time_remaining); + success = success && local_success; + + // Enforce boundary conditions + if (!is_holonomic_ && local_success) { + enforceStartBoundaryConditions(start_pose, curr_path_segment, costmap, reversing_segment); + enforceEndBoundaryConditions(goal_pose, curr_path_segment, costmap, reversing_segment); + } + + // Assemble the path changes to the main path + std::copy( + curr_path_segment.poses.begin(), + curr_path_segment.poses.end(), + path.poses.begin() + path_segments[i].start); + } + } + + return success; +} + +bool Smoother::smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time) +{ + 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_i_org; + unsigned int mx, my; + + 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, reversing_segment); + 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, reversing_segment); + return false; + } + + for (unsigned int i = 1; i != path_size - 1; 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; + + // Smooth based on local 3 point neighborhood and original data locations + 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 && cost != UNKNOWN) { + 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, reversing_segment); + return false; + } + } + + last_path = new_path; + } + + // 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_ && refinement_ctr_ < 4) { + refinement_ctr_++; + smoothImpl(new_path, reversing_segment, costmap, max_time); + } + + updateApproximatePathOrientations(new_path, reversing_segment); + path = new_path; + return true; +} + +double Smoother::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; + } +} + +void Smoother::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; + } +} + +std::vector Smoother::findDirectionalPathSegments(const nav_msgs::msg::Path & path) +{ + std::vector segments; + PathSegment curr_segment; + curr_segment.start = 0; + + // If holonomic, no directional changes and + // may have abrupt angular changes from naive grid search + if (is_holonomic_) { + curr_segment.end = path.poses.size() - 1; + segments.push_back(curr_segment); + return segments; + } + + // Iterating through the path to determine the position of the cusp + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + double oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + double ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + double ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existance of cusp, in the path, using the dot product. + double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + + // Checking for the existance of a differential rotation in place. + double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); + double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); + double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); + if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + } + + curr_segment.end = path.poses.size() - 1; + segments.push_back(curr_segment); + return segments; +} + +void Smoother::updateApproximatePathOrientations( + nav_msgs::msg::Path & path, + bool & reversing_segment) +{ + double dx, dy, theta, pt_yaw; + reversing_segment = false; + + // Find if this path segment is in reverse + dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; + dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; + theta = atan2(dy, dx); + pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); + if (!is_holonomic_ && fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { + reversing_segment = true; + } + + // Find the angle relative the path position vectors + 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); + + // If points are overlapping, pass + if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { + continue; + } + + // Flip the angle if this path segment is in reverse + if (reversing_segment) { + theta += M_PI; // orientationAroundZAxis will normalize + } + + path.poses[i].pose.orientation = orientationAroundZAxis(theta); + } +} + +unsigned int Smoother::findShortestBoundaryExpansionIdx( + const BoundaryExpansions & boundary_expansions) +{ + // Check which is valid with the minimum integrated length such that + // shorter end-points away that are infeasible to achieve without + // a loop-de-loop are punished + double min_length = 1e9; + int shortest_boundary_expansion_idx = 1e9; + for (unsigned int idx = 0; idx != boundary_expansions.size(); idx++) { + if (boundary_expansions[idx].expansion_path_length0.0 && + boundary_expansions[idx].expansion_path_length > 0.0) + { + min_length = boundary_expansions[idx].expansion_path_length; + shortest_boundary_expansion_idx = idx; + } + } + + return shortest_boundary_expansion_idx; +} + +void Smoother::findBoundaryExpansion( + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & end, + BoundaryExpansion & expansion, + const nav2_costmap_2d::Costmap2D * costmap) +{ + static ompl::base::ScopedState<> from(state_space_), to(state_space_), s(state_space_); + + from[0] = start.position.x; + from[1] = start.position.y; + from[2] = tf2::getYaw(start.orientation); + to[0] = end.position.x; + to[1] = end.position.y; + to[2] = tf2::getYaw(end.orientation); + + double d = state_space_->distance(from(), to()); + // If this path is too long compared to the original, then this is probably + // a loop-de-loop, treat as invalid as to not deviate too far from the original path. + // 2.0 selected from prinicipled choice of boundary test points + // r, 2 * r, r * PI, and 2 * PI * r. If there is a loop, it will be + // approximately 2 * PI * r, which is 2 * PI > r, PI > 2 * r, and 2 > r * PI. + // For all but the last backup test point, a loop would be approximately + // 2x greater than any of the selections. + if (d > 2.0 * expansion.original_path_length) { + return; + } + + std::vector reals; + double theta(0.0), x(0.0), y(0.0); + double x_m = start.position.x; + double y_m = start.position.y; + + // Get intermediary poses + for (double i = 0; i <= expansion.path_end_idx; i++) { + state_space_->interpolate(from(), to(), i / expansion.path_end_idx, s()); + reals = s.reals(); + // Make sure in range [0, 2PI) + theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2]; + theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta; + x = reals[0]; + y = reals[1]; + + // Check for collision + unsigned int mx, my; + costmap->worldToMap(x, y, mx, my); + if (static_cast(costmap->getCost(mx, my)) >= INSCRIBED) { + expansion.in_collision = true; + } + + // Integrate path length + expansion.expansion_path_length += hypot(x - x_m, y - y_m); + x_m = x; + y_m = y; + + // Store point + expansion.pts.emplace_back(x, y, theta); + } +} + +template +BoundaryExpansions Smoother::generateBoundaryExpansionPoints(IteratorT start, IteratorT end) +{ + std::vector distances = { + min_turning_rad_, // Radius + 2.0 * min_turning_rad_, // Diameter + M_PI * min_turning_rad_, // 50% Circumference + 2.0 * M_PI * min_turning_rad_ // Circumference + }; + + BoundaryExpansions boundary_expansions; + boundary_expansions.resize(distances.size()); + double curr_dist = 0.0; + double x_last = start->pose.position.x; + double y_last = start->pose.position.y; + geometry_msgs::msg::Point pt; + unsigned int curr_dist_idx = 0; + + for (IteratorT iter = start; iter != end; iter++) { + pt = iter->pose.position; + curr_dist += hypot(pt.x - x_last, pt.y - y_last); + x_last = pt.x; + y_last = pt.y; + + if (curr_dist >= distances[curr_dist_idx]) { + boundary_expansions[curr_dist_idx].path_end_idx = iter - start; + boundary_expansions[curr_dist_idx].original_path_length = curr_dist; + curr_dist_idx++; + } + + if (curr_dist_idx == boundary_expansions.size()) { + break; + } + } + + return boundary_expansions; +} + +void Smoother::enforceStartBoundaryConditions( + const geometry_msgs::msg::Pose & start_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment) +{ + // Find range of points for testing + BoundaryExpansions boundary_expansions = + generateBoundaryExpansionPoints(path.poses.begin(), path.poses.end()); + + // Generate the motion model and metadata from start -> test points + for (unsigned int i = 0; i != boundary_expansions.size(); i++) { + BoundaryExpansion & expansion = boundary_expansions[i]; + if (expansion.path_end_idx == 0.0) { + continue; + } + + if (!reversing_segment) { + findBoundaryExpansion( + start_pose, path.poses[expansion.path_end_idx].pose, expansion, + costmap); + } else { + findBoundaryExpansion( + path.poses[expansion.path_end_idx].pose, start_pose, expansion, + costmap); + } + } + + // Find the shortest kinematically feasible boundary expansion + unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); + if (best_expansion_idx > boundary_expansions.size()) { + return; + } + + // Override values to match curve + BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; + if (reversing_segment) { + std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); + } + for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { + path.poses[i].pose.position.x = best_expansion.pts[i].x; + path.poses[i].pose.position.y = best_expansion.pts[i].y; + path.poses[i].pose.orientation = orientationAroundZAxis(best_expansion.pts[i].theta); + } +} + +void Smoother::enforceEndBoundaryConditions( + const geometry_msgs::msg::Pose & end_pose, + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const bool & reversing_segment) +{ + // Find range of points for testing + BoundaryExpansions boundary_expansions = + generateBoundaryExpansionPoints(path.poses.rbegin(), path.poses.rend()); + + // Generate the motion model and metadata from start -> test points + unsigned int expansion_starting_idx; + for (unsigned int i = 0; i != boundary_expansions.size(); i++) { + BoundaryExpansion & expansion = boundary_expansions[i]; + if (expansion.path_end_idx == 0.0) { + continue; + } + expansion_starting_idx = path.poses.size() - expansion.path_end_idx - 1; + if (!reversing_segment) { + findBoundaryExpansion(path.poses[expansion_starting_idx].pose, end_pose, expansion, costmap); + } else { + findBoundaryExpansion(end_pose, path.poses[expansion_starting_idx].pose, expansion, costmap); + } + } + + // Find the shortest kinematically feasible boundary expansion + unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions); + if (best_expansion_idx > boundary_expansions.size()) { + return; + } + + // Override values to match curve + BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx]; + if (reversing_segment) { + std::reverse(best_expansion.pts.begin(), best_expansion.pts.end()); + } + expansion_starting_idx = path.poses.size() - best_expansion.path_end_idx - 1; + for (unsigned int i = 0; i != best_expansion.pts.size(); i++) { + path.poses[expansion_starting_idx + i].pose.position.x = best_expansion.pts[i].x; + path.poses[expansion_starting_idx + i].pose.position.y = best_expansion.pts[i].y; + path.poses[expansion_starting_idx + i].pose.orientation = orientationAroundZAxis( + best_expansion.pts[i].theta); + } +} + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/test/3planners.png b/nav2_smac_planner/test/3planners.png new file mode 100644 index 0000000000..3a6ef221c4 Binary files /dev/null and b/nav2_smac_planner/test/3planners.png differ diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt new file mode 100644 index 0000000000..76befe954c --- /dev/null +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -0,0 +1,118 @@ +# Test costmap downsampler +ament_add_gtest(test_costmap_downsampler + test_costmap_downsampler.cpp +) +ament_target_dependencies(test_costmap_downsampler + ${dependencies} +) +target_link_libraries(test_costmap_downsampler + ${library_name} +) + +# Test Node2D +ament_add_gtest(test_node2d + test_node2d.cpp +) +ament_target_dependencies(test_node2d + ${dependencies} +) +target_link_libraries(test_node2d + ${library_name} +) + +# Test NodeHybrid +ament_add_gtest(test_nodehybrid + test_nodehybrid.cpp +) +ament_target_dependencies(test_nodehybrid + ${dependencies} +) +target_link_libraries(test_nodehybrid + ${library_name} +) + +# Test NodeBasic +ament_add_gtest(test_nodebasic + test_nodebasic.cpp +) +ament_target_dependencies(test_nodebasic + ${dependencies} +) +target_link_libraries(test_nodebasic + ${library_name} +) + +# Test collision checker +ament_add_gtest(test_collision_checker + test_collision_checker.cpp +) +ament_target_dependencies(test_collision_checker + ${dependencies} +) +target_link_libraries(test_collision_checker + ${library_name} +) + +# Test A* +ament_add_gtest(test_a_star + test_a_star.cpp +) +ament_target_dependencies(test_a_star + ${dependencies} +) +target_link_libraries(test_a_star + ${library_name} +) + +# Test SMAC Hybrid +ament_add_gtest(test_smac_hybrid + test_smac_hybrid.cpp +) +ament_target_dependencies(test_smac_hybrid + ${dependencies} +) +target_link_libraries(test_smac_hybrid + ${library_name} +) + +# Test SMAC 2D +ament_add_gtest(test_smac_2d + test_smac_2d.cpp +) +ament_target_dependencies(test_smac_2d + ${dependencies} +) +target_link_libraries(test_smac_2d + ${library_name}_2d +) + +# Test SMAC lattice +ament_add_gtest(test_smac_lattice + test_smac_lattice.cpp +) +ament_target_dependencies(test_smac_lattice + ${dependencies} +) +target_link_libraries(test_smac_lattice + ${library_name}_lattice +) + +# Test SMAC Smoother +ament_add_gtest(test_smoother + test_smoother.cpp +) +ament_target_dependencies(test_smoother + ${dependencies} +) +target_link_libraries(test_smoother + ${library_name}_lattice + ${library_name} + ${library_name}_2d +) + +#Test Lattice node +ament_add_gtest(test_lattice_node test_nodelattice.cpp) + +ament_target_dependencies(test_lattice_node ${dependencies}) + +target_link_libraries(test_lattice_node ${library_name}) diff --git a/nav2_smac_planner/test/deprecated/options.hpp b/nav2_smac_planner/test/deprecated/options.hpp new file mode 100644 index 0000000000..5d33223dc4 --- /dev/null +++ b/nav2_smac_planner/test/deprecated/options.hpp @@ -0,0 +1,207 @@ +// 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__OPTIONS_HPP_ +#define DEPRECATED__OPTIONS_HPP_ + +#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_smac_planner +{ + +/** + * @struct nav2_smac_planner::SmootherParams + * @brief Parameters for the smoother cost function + */ +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(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.smoother."); + + // Smoother params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_curve", rclcpp::ParameterValue(1.5)); + node->get_parameter(local_name + "w_curve", curvature_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_cost", rclcpp::ParameterValue(0.0)); + node->get_parameter(local_name + "w_cost", costmap_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_dist", rclcpp::ParameterValue(0.0)); + node->get_parameter(local_name + "w_dist", distance_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_smooth", rclcpp::ParameterValue(15000.0)); + node->get_parameter(local_name + "w_smooth", smooth_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "cost_scaling_factor", rclcpp::ParameterValue(10.0)); + node->get_parameter(local_name + "cost_scaling_factor", costmap_factor); + } + + double smooth_weight{0.0}; + double costmap_weight{0.0}; + double distance_weight{0.0}; + double curvature_weight{0.0}; + double max_curvature{0.0}; + double costmap_factor{0.0}; + double max_time; +}; + +/** + * @struct nav2_smac_planner::OptimizerParams + * @brief Parameters for the ceres optimizer + */ +struct OptimizerParams +{ + OptimizerParams() + : debug(false), + max_iterations(50), + max_time(1e4), + param_tol(1e-8), + fn_tol(1e-6), + gradient_tol(1e-10) + { + } + + /** + * @struct AdvancedParams + * @brief Advanced parameters for the ceres optimizer + */ + struct AdvancedParams + { + AdvancedParams() + : min_line_search_step_size(1e-9), + max_num_line_search_step_size_iterations(20), + line_search_sufficient_function_decrease(1e-4), + max_num_line_search_direction_restarts(20), + max_line_search_step_contraction(1e-3), + min_line_search_step_contraction(0.6), + line_search_sufficient_curvature_decrease(0.9), + max_line_search_step_expansion(10) + { + } + + /** + * @brief Get advanced params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.optimizer.advanced."); + + // Optimizer advanced params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "min_line_search_step_size", + rclcpp::ParameterValue(1e-20)); + node->get_parameter( + local_name + "min_line_search_step_size", + min_line_search_step_size); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_num_line_search_step_size_iterations", + rclcpp::ParameterValue(50)); + node->get_parameter( + local_name + "max_num_line_search_step_size_iterations", + max_num_line_search_step_size_iterations); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "line_search_sufficient_function_decrease", + rclcpp::ParameterValue(1e-20)); + node->get_parameter( + local_name + "line_search_sufficient_function_decrease", + line_search_sufficient_function_decrease); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_num_line_search_direction_restarts", + rclcpp::ParameterValue(10)); + node->get_parameter( + local_name + "max_num_line_search_direction_restarts", + max_num_line_search_direction_restarts); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_line_search_step_expansion", + rclcpp::ParameterValue(50)); + node->get_parameter( + local_name + "max_line_search_step_expansion", + max_line_search_step_expansion); + } + + + double min_line_search_step_size; // Ceres default: 1e-9 + int max_num_line_search_step_size_iterations; // Ceres default: 20 + double line_search_sufficient_function_decrease; // Ceres default: 1e-4 + int max_num_line_search_direction_restarts; // Ceres default: 5 + + double max_line_search_step_contraction; // Ceres default: 1e-3 + double min_line_search_step_contraction; // Ceres default: 0.6 + double line_search_sufficient_curvature_decrease; // Ceres default: 0.9 + int max_line_search_step_expansion; // Ceres default: 10 + }; + + /** + * @brief Get params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.optimizer."); + + // Optimizer params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "param_tol", rclcpp::ParameterValue(1e-15)); + node->get_parameter(local_name + "param_tol", param_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "fn_tol", rclcpp::ParameterValue(1e-7)); + node->get_parameter(local_name + "fn_tol", fn_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "gradient_tol", rclcpp::ParameterValue(1e-10)); + node->get_parameter(local_name + "gradient_tol", gradient_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_iterations", rclcpp::ParameterValue(500)); + node->get_parameter(local_name + "max_iterations", max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_time", rclcpp::ParameterValue(0.100)); + node->get_parameter(local_name + "max_time", max_time); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "debug_optimizer", rclcpp::ParameterValue(false)); + node->get_parameter(local_name + "debug_optimizer", debug); + + advanced.get(node, name); + } + + bool debug; + int max_iterations; // Ceres default: 50 + double max_time; // Ceres default: 1e4 + + double param_tol; // Ceres default: 1e-8 + double fn_tol; // Ceres default: 1e-6 + double gradient_tol; // Ceres default: 1e-10 + + AdvancedParams advanced; +}; + +} // namespace nav2_smac_planner + +#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 0000000000..91cfcd257c --- /dev/null +++ b/nav2_smac_planner/test/deprecated/smoother.hpp @@ -0,0 +1,146 @@ +// 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; + +#ifdef _MSC_VER + std::vector parameters_vec(path.size() * 2); + double * parameters = parameters_vec.data(); +#else + double parameters[path.size() * 2]; // NOLINT +#endif + for (unsigned int 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 (unsigned int 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/test/deprecated/smoother_cost_function.hpp b/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp new file mode 100644 index 0000000000..acd6414567 --- /dev/null +++ b/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp @@ -0,0 +1,542 @@ +// 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_COST_FUNCTION_HPP_ +#define DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "nav2_smac_planner/types.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace nav2_smac_planner +{ + +/** + * @struct nav2_smac_planner::UnconstrainedSmootherCostFunction + * @brief Cost function for path smoothing with multiple terms + * including curvature, smoothness, collision, and avoid obstacles. + */ +class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction +{ +public: + /** + * @brief A constructor for nav2_smac_planner::UnconstrainedSmootherCostFunction + * @param original_path Original unsmoothed path to smooth + * @param costmap A costmap to get values for collision and obstacle avoidance + */ + UnconstrainedSmootherCostFunction( + std::vector * original_path, + nav2_costmap_2d::Costmap2D * costmap, + const SmootherParams & params) + : _original_path(original_path), + _num_params(2 * original_path->size()), + _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(); + } + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for nav2_smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = true; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0.0, 0.0}; + Eigen::Vector2d delta_xi_p{0.0, 0.0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + virtual bool Evaluate( + const double * parameters, + double * cost, + double * gradient) const + { + Eigen::Vector2d xi; + Eigen::Vector2d xi_p1; + Eigen::Vector2d xi_m1; + unsigned int x_index, y_index; + cost[0] = 0.0; + double cost_raw = 0.0; + double grad_x_raw = 0.0; + double grad_y_raw = 0.0; + unsigned int mx, my; + bool valid_coords = true; + double costmap_cost = 0.0; + + // cache some computations between the residual and jacobian + CurvatureComputations curvature_params; + + for (int i = 0; i != NumParameters() / 2; i++) { + x_index = 2 * i; + y_index = 2 * i + 1; + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + if (i < 1 || i >= NumParameters() / 2 - 1) { + continue; + } + + xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]); + xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]); + xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]); + + // compute cost + addSmoothingResidual(_params.smooth_weight, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(_params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, cost_raw); + addDistanceResidual(_params.distance_weight, xi, _original_path->at(i), cost_raw); + + 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, xi); + } + + if (gradient != NULL) { + // compute gradient + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + addSmoothingJacobian(_params.smooth_weight, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian( + _params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, + grad_x_raw, grad_y_raw); + addDistanceJacobian( + _params.distance_weight, xi, _original_path->at( + i), grad_x_raw, grad_y_raw); + + if (valid_coords) { + addCostJacobian(_params.costmap_weight, mx, my, costmap_cost, grad_x_raw, grad_y_raw); + } + + gradient[x_index] = grad_x_raw; + gradient[y_index] = grad_y_raw; + } + } + + cost[0] = cost_raw; + + return true; + } + + /** + * @brief Get number of parameter blocks + * @return Number of parameters in cost function + */ + virtual int NumParameters() const {return _num_params;} + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _params.max_curvature; + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + return; + } + + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + + // Old formulation we may require again. + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + + j0 += weight * jacobian[0]; // xi x component of partial-derivative + j1 += weight * jacobian[1]; // xi x component of partial-derivative + } + + /** + * @brief Cost function derivative term for steering away changes in pose + * @param weight Weight to apply to function + * @param xi Point Xi for evaluation + * @param xi_original original point Xi for evaluation + * @param r Residual (cost) of term + */ + inline void addDistanceResidual( + const double & weight, + const Eigen::Vector2d & xi, + const Eigen::Vector2d & xi_original, + double & r) const + { + r += weight * (xi - xi_original).dot(xi - xi_original); // objective function value + } + + /** + * @brief Cost function derivative term for steering away changes in pose + * @param weight Weight to apply to function + * @param xi Point Xi for evaluation + * @param xi_original original point Xi for evaluation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addDistanceJacobian( + const double & weight, + const Eigen::Vector2d & xi, + const Eigen::Vector2d & xi_original, + double & j0, + double & j1) const + { + j0 += weight * 2 * (xi[0] - xi_original[0]); // xi y component of partial-derivative + j1 += weight * 2 * (xi[1] - xi_original[1]); // xi y component of partial-derivative + } + + + /** + * @brief Cost function term for steering away from costs + * @param weight Weight to apply to function + * @param value Point Xi's cost' + * @param params computed values to reduce overhead + * @param r Residual (cost) of term + */ + inline void addCostResidual( + const double & weight, + const double & value, + 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); + } + + /** + * @brief Cost function derivative term for steering away from costs + * @param weight Weight to apply to function + * @param mx Point Xi's x coordinate in map frame + * @param mx Point Xi's y coordinate in map frame + * @param value Point Xi's cost' + * @param params computed values to reduce overhead + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCostJacobian( + const double & weight, + const unsigned int & mx, + const unsigned int & my, + const double & value, + double & j0, + double & j1) const + { + if (value == FREE) { + return; + } + + const Eigen::Vector2d grad = getCostmapGradient(mx, my); + const double common_prefix = -2.0 * _params.costmap_factor * weight * value * value; + + j0 += common_prefix * grad[0]; // xi x component of partial-derivative + j1 += common_prefix * grad[1]; // xi y component of partial-derivative + } + + /** + * @brief Computing the gradient of the costmap using + * the 2 point numerical differentiation method + * @param mx Point Xi's x coordinate in map frame + * @param mx Point Xi's y coordinate in map frame + * @param params Params reference to store gradients + */ + inline Eigen::Vector2d getCostmapGradient( + const unsigned int mx, + const unsigned int my) const + { + // find unit vector that describes that direction + // via 7 point taylor series approximation for gradient at Xi + Eigen::Vector2d gradient; + + double l_1 = 0.0; + double l_2 = 0.0; + double l_3 = 0.0; + double r_1 = 0.0; + double r_2 = 0.0; + double r_3 = 0.0; + + if (mx < _costmap->getSizeInCellsX()) { + r_1 = static_cast(_costmap->getCost(mx + 1, my)); + } + if (mx + 1 < _costmap->getSizeInCellsX()) { + r_2 = static_cast(_costmap->getCost(mx + 2, my)); + } + if (mx + 2 < _costmap->getSizeInCellsX()) { + r_3 = static_cast(_costmap->getCost(mx + 3, my)); + } + + if (mx > 0) { + l_1 = static_cast(_costmap->getCost(mx - 1, my)); + } + if (mx - 1 > 0) { + l_2 = static_cast(_costmap->getCost(mx - 2, my)); + } + if (mx - 2 > 0) { + l_3 = static_cast(_costmap->getCost(mx - 3, my)); + } + + gradient[1] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60; + + if (my < _costmap->getSizeInCellsY()) { + r_1 = static_cast(_costmap->getCost(mx, my + 1)); + } + if (my + 1 < _costmap->getSizeInCellsY()) { + r_2 = static_cast(_costmap->getCost(mx, my + 2)); + } + if (my + 2 < _costmap->getSizeInCellsY()) { + r_3 = static_cast(_costmap->getCost(mx, my + 3)); + } + + if (my > 0) { + l_1 = static_cast(_costmap->getCost(mx, my - 1)); + } + if (my - 1 > 0) { + l_2 = static_cast(_costmap->getCost(mx, my - 2)); + } + if (my - 2 > 0) { + l_3 = static_cast(_costmap->getCost(mx, my - 3)); + } + + gradient[0] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60; + + gradient.normalize(); + return gradient; + } + + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + std::vector * _original_path{nullptr}; + int _num_params; + nav2_costmap_2d::Costmap2D * _costmap{nullptr}; + SmootherParams _params; + // DynamicVoronoi voronoiDiagram; +}; + +} // namespace nav2_smac_planner + +#endif // DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_ diff --git a/nav2_smac_planner/test/deprecated/upsampler.hpp b/nav2_smac_planner/test/deprecated/upsampler.hpp new file mode 100644 index 0000000000..90e0f8b464 --- /dev/null +++ b/nav2_smac_planner/test/deprecated/upsampler.hpp @@ -0,0 +1,213 @@ +// 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__UPSAMPLER_HPP_ +#define DEPRECATED__UPSAMPLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/upsampler_cost_function.hpp" +#include "nav2_smac_planner/upsampler_cost_function_nlls.hpp" + +#include "ceres/ceres.h" +#include "Eigen/Core" + +namespace nav2_smac_planner +{ + +/** + * @class nav2_smac_planner::Upsampler + * @brief A Conjugate Gradient 2D path upsampler implementation + */ +class Upsampler +{ +public: + /** + * @brief A constructor for nav2_smac_planner::Upsampler + */ + Upsampler() {} + + /** + * @brief A destructor for nav2_smac_planner::Upsampler + */ + ~Upsampler() {} + + /** + * @brief Initialization of the Upsampler + */ + 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; // 5000 + _options.max_solver_time_in_seconds = params.max_time; // 5.0; // TODO + + _options.function_tolerance = params.fn_tol; + _options.gradient_tolerance = params.gradient_tol; + _options.parameter_tolerance = params.param_tol; // 1e-20; + + _options.min_line_search_step_size = params.advanced.min_line_search_step_size; // 1e-30; + _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; // 1e-30; + _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 Upsampling method + * @param path Reference to path + * @param upsample parameters weights + * @param upsample_ratio upsample ratio + * @return If Upsampler was successful + */ + bool upsample( + std::vector & path, + const SmootherParams & params, + const int & upsample_ratio) + { + _options.max_solver_time_in_seconds = params.max_time; + + if (upsample_ratio != 2 && upsample_ratio != 4) { + // invalid inputs + return false; + } + + const int param_ratio = upsample_ratio * 2.0; + const int total_size = 2 * (path.size() * upsample_ratio - upsample_ratio + 1); + double parameters[total_size]; // NOLINT + + // 20-4hz regularly, but dosnt work in faster cases + // Linearly distribute initial poses for optimization + // TODO(stevemacenski) generalize for 2x and 4x + unsigned int next_pt; + Eigen::Vector2d interpolated; + std::vector temp_path; + for (unsigned int pt = 0; pt != path.size() - 1; pt++) { + next_pt = pt + 1; + interpolated = (path[next_pt] + path[pt]) / 2.0; + + parameters[param_ratio * pt] = path[pt][0]; + parameters[param_ratio * pt + 1] = path[pt][1]; + temp_path.push_back(path[pt]); + + parameters[param_ratio * pt + 2] = interpolated[0]; + parameters[param_ratio * pt + 3] = interpolated[1]; + temp_path.push_back(interpolated); + } + + parameters[total_size - 2] = path.back()[0]; + parameters[total_size - 1] = path.back()[1]; + temp_path.push_back(path.back()); + + // Solve the upsampling problem + ceres::GradientProblemSolver::Summary summary; + ceres::GradientProblem problem(new UpsamplerCostFunction(temp_path, params, upsample_ratio)); + ceres::Solve(_options, problem, parameters, &summary); + + + path.resize(total_size / 2); + for (int i = 0; i != total_size / 2; i++) { + path[i][0] = parameters[2 * i]; + path[i][1] = parameters[2 * i + 1]; + } + + // 10-15 hz, regularly + // std::vector path_double_sampled; + // for (int i = 0; i != path.size() - 1; i++) { // last term should not be upsampled + // path_double_sampled.push_back(path[i]); + // path_double_sampled.push_back((path[i+1] + path[i]) / 2); + // } + + // std::unique_ptr problem = std::make_unique(); + // for (uint i = 1; i != path_double_sampled.size() - 1; i++) { + // ceres::CostFunction * cost_fn = + // new UpsamplerConstrainedCostFunction(path_double_sampled, params, 2, i); + // problem->AddResidualBlock( + // cost_fn, nullptr, &path_double_sampled[i][0], &path_double_sampled[i][1]); + // // locking initial coordinates unnecessary since there's no update between terms in NLLS + // } + + // ceres::Solver::Summary summary; + // _options.minimizer_type = ceres::LINE_SEARCH; + // ceres::Solve(_options, problem.get(), &summary); + + // if (upsample_ratio == 4) { + // std::vector path_quad_sampled; + // for (int i = 0; i != path_double_sampled.size() - 1; i++) { + // path_quad_sampled.push_back(path_double_sampled[i]); + // path_quad_sampled.push_back((path_double_sampled[i+1] + path_double_sampled[i]) / 2.0); + // } + + // std::unique_ptr problem2 = std::make_unique(); + // for (uint i = 1; i != path_quad_sampled.size() - 1; i++) { + // ceres::CostFunction * cost_fn = + // new UpsamplerConstrainedCostFunction(path_quad_sampled, params, 4, i); + // problem2->AddResidualBlock( + // cost_fn, nullptr, &path_quad_sampled[i][0], &path_quad_sampled[i][1]); + // } + + // ceres::Solve(_options, problem2.get(), &summary); + + // path = path_quad_sampled; + // } else { + // path = path_double_sampled; + // } + + if (_debug) { + std::cout << summary.FullReport() << '\n'; + } + + if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) { + return false; + } + + return true; + } + +private: + bool _debug; + ceres::GradientProblemSolver::Options _options; +}; + +} // namespace nav2_smac_planner + +#endif // DEPRECATED__UPSAMPLER_HPP_ diff --git a/nav2_smac_planner/test/deprecated/upsampler_cost_function.hpp b/nav2_smac_planner/test/deprecated/upsampler_cost_function.hpp new file mode 100644 index 0000000000..4a89800b5b --- /dev/null +++ b/nav2_smac_planner/test/deprecated/upsampler_cost_function.hpp @@ -0,0 +1,366 @@ +// 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__UPSAMPLER_COST_FUNCTION_HPP_ +#define DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace nav2_smac_planner +{ +/** + * @struct nav2_smac_planner::UpsamplerCostFunction + * @brief Cost function for path upsampling with multiple terms using unconstrained + * optimization including curvature, smoothness, collision, and avoid obstacles. + */ +class UpsamplerCostFunction : public ceres::FirstOrderFunction +{ +public: + /** + * @brief A constructor for nav2_smac_planner::UpsamplerCostFunction + * @param num_points Number of path points to consider + */ + UpsamplerCostFunction( + const std::vector & path, + const SmootherParams & params, + const int & upsample_ratio) + : _num_params(2 * path.size()), + _params(params), + _upsample_ratio(upsample_ratio), + _path(path) + { + } + // TODO(stevemacenski) removed upsample_ratio because temp upsampling on path size + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for nav2_smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = false; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0, 0}; + Eigen::Vector2d delta_xi_p{0, 0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + virtual bool Evaluate( + const double * parameters, + double * cost, + double * gradient) const + { + Eigen::Vector2d xi; + Eigen::Vector2d xi_p1; + Eigen::Vector2d xi_m1; + uint x_index, y_index; + cost[0] = 0.0; + double cost_raw = 0.0; + double grad_x_raw = 0.0; + double grad_y_raw = 0.0; + + // cache some computations between the residual and jacobian + CurvatureComputations curvature_params; + + for (int i = 0; i != NumParameters() / 2; i++) { + x_index = 2 * i; + y_index = 2 * i + 1; + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + if (i < 1 || i >= NumParameters() / 2 - 1) { + continue; + } + + // if original point's neighbors TODO + if (i % _upsample_ratio == 1) { + continue; + } + + xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]); + + // TODO(stevemacenski): from deep copy to make sure no feedback _path + xi_p1 = _path.at(i + 1); + xi_m1 = _path.at(i - 1); + // xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]); + // xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]); + + // compute cost + addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw); + + if (gradient != NULL) { + // compute gradient + addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw); + + gradient[x_index] = grad_x_raw; + gradient[y_index] = grad_y_raw; + } + } + + cost[0] = cost_raw; + return true; + } + + /** + * @brief Get number of parameter blocks + * @return Number of parameters in cost function + */ + virtual int NumParameters() const {return _num_params;} + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Get path curvature information + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + */ + inline void getCurvatureParams( + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio * + _params.max_curvature; + // TODO(stevemacenski) is use of upsample_ratio correct here? small number? + // TODO(stevemacenski) can remove the subtraction with a + // lower weight value, does have direction issue, maybe just tuning? + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + } + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + getCurvatureParams(pt, pt_p, pt_m, curvature_params); + + if (!curvature_params.isValid()) { + return; + } + + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + j0 += weight * jacobian[0]; // xi y component of partial-derivative + j1 += weight * jacobian[1]; // xi x component of partial-derivative + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + } + + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + int _num_params; + SmootherParams _params; + int _upsample_ratio; + std::vector _path; +}; + +} // namespace nav2_smac_planner + +#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_ diff --git a/nav2_smac_planner/test/deprecated/upsampler_cost_function_nlls.hpp b/nav2_smac_planner/test/deprecated/upsampler_cost_function_nlls.hpp new file mode 100644 index 0000000000..f1599bcd78 --- /dev/null +++ b/nav2_smac_planner/test/deprecated/upsampler_cost_function_nlls.hpp @@ -0,0 +1,334 @@ +// 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__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ +#define DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace nav2_smac_planner +{ +/** + * @struct nav2_smac_planner::UpsamplerConstrainedCostFunction + * @brief Cost function for path upsampling with multiple terms using NLLS + * including curvature, smoothness, collision, and avoid obstacles. + */ +class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1> +{ +public: + /** + * @brief A constructor for nav2_smac_planner::UpsamplerConstrainedCostFunction + * @param num_points Number of path points to consider + */ + UpsamplerConstrainedCostFunction( + const std::vector & path, + const SmootherParams & params, + const int & upsample_ratio, + const int & i) + : _path(path), + _params(params), + _upsample_ratio(upsample_ratio), + index(i) + { + } + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for nav2_smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = true; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0, 0}; + Eigen::Vector2d delta_xi_p{0, 0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + + bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const override + { + Eigen::Vector2d xi = Eigen::Vector2d(parameters[0][0], parameters[1][0]); + Eigen::Vector2d xi_p1 = _path.at(index + 1); + Eigen::Vector2d xi_m1 = _path.at(index - 1); + CurvatureComputations curvature_params; + double grad_x_raw = 0, grad_y_raw = 0, cost_raw = 0; + + // compute cost + addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw); + + residuals[0] = 0; + residuals[0] = cost_raw; // objective function value x + + if (jacobians != NULL && jacobians[0] != NULL) { + addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw); + + jacobians[0][0] = 0; + jacobians[1][0] = 0; + jacobians[0][0] = grad_x_raw; // x derivative + jacobians[1][0] = grad_y_raw; // y derivative + jacobians[0][1] = 0.0; + jacobians[1][1] = 0.0; + } + + return true; + } + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Get path curvature information + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + */ + inline void getCurvatureParams( + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio * + _params.max_curvature; + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + } + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + getCurvatureParams(pt, pt_p, pt_m, curvature_params); + + if (!curvature_params.isValid()) { + return; + } + + // objective function value + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + j0 += weight * jacobian[0]; // xi x component of partial-derivative + j1 += weight * jacobian[1]; // xi y component of partial-derivative + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + } + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + std::vector _path; + SmootherParams _params; + int _upsample_ratio; + int index; +}; + +} // namespace nav2_smac_planner + +#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ diff --git a/nav2_smac_planner/test/path.png b/nav2_smac_planner/test/path.png new file mode 100644 index 0000000000..fc98e7709b Binary files /dev/null and b/nav2_smac_planner/test/path.png differ diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp new file mode 100644 index 0000000000..68af08df50 --- /dev/null +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -0,0 +1,307 @@ +// 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 + +#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/node_lattice.hpp" +#include "nav2_smac_planner/a_star.hpp" +#include "nav2_smac_planner/collision_checker.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(AStarTest, test_a_star_2d) +{ + nav2_smac_planner::SearchInfo info; + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::MOORE, info); + int max_iterations = 10000; + float tolerance = 0.0; + float some_tolerance = 20.0; + int it_on_approach = 10; + double max_planning_time = 120.0; + int num_it = 0; + + a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 0.0, 1); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + // island in the middle of lethal cost to cross + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmapA->setCost(i, j, 254); + } + } + + // functional case testing + 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, 102); + + // check path is the right size and collision free + EXPECT_EQ(path.size(), 81u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + + // setting non-zero dim 3 for 2D search + EXPECT_THROW(a_star.setGoal(0, 0, 10), std::runtime_error); + EXPECT_THROW(a_star.setStart(0, 0, 10), std::runtime_error); + + path.clear(); + // 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, max_planning_time, 0, 1); + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + 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 + a_star_2.setGoal(0, 0, 0); // valid + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + a_star_2.setStart(0, 0, 0); // valid + a_star_2.setGoal(50, 50, 0); // invalid + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + num_it = 0; + // invalid goal but liberal tolerance + 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(), 42u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + + EXPECT_TRUE(a_star_2.getStart() != nullptr); + EXPECT_TRUE(a_star_2.getGoal() != nullptr); + EXPECT_EQ(a_star_2.getSizeX(), 100u); + EXPECT_EQ(a_star_2.getSizeY(), 100u); + EXPECT_EQ(a_star_2.getSizeDim3(), 1u); + EXPECT_EQ(a_star_2.getToleranceHeuristic(), 20.0); + EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10); + + delete costmapA; +} + +TEST(AStarTest, test_a_star_se2) +{ + nav2_smac_planner::SearchInfo info; + info.change_penalty = 0.1; + info.non_straight_penalty = 1.1; + info.reverse_penalty = 2.0; + info.minimum_turning_radius = 8; // in grid coordinates + info.retrospective_penalty = 0.015; + info.analytic_expansion_max_length = 20.0; // in grid coordinates + info.analytic_expansion_ratio = 3.5; + unsigned int size_theta = 72; + info.cost_penalty = 1.7; + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::DUBIN, info); + int max_iterations = 10000; + float tolerance = 10.0; + int it_on_approach = 10; + double max_planning_time = 120.0; + int num_it = 0; + + a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + // island in the middle of lethal cost to cross + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmapA->setCost(i, j, 254); + } + } + + std::unique_ptr checker = + std::make_unique(costmapA, size_theta); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + // functional case testing + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u); + 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, 3222); + EXPECT_EQ(path.size(), 62u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + // no skipped nodes + for (unsigned int i = 1; i != path.size(); i++) { + EXPECT_LT(hypotf(path[i].x - path[i - 1].x, path[i].y - path[i - 1].y), 2.1f); + } + + delete costmapA; +} + +TEST(AStarTest, test_a_star_lattice) +{ + nav2_smac_planner::SearchInfo info; + info.change_penalty = 0.05; + info.non_straight_penalty = 1.05; + info.reverse_penalty = 2.0; + info.retrospective_penalty = 0.1; + info.analytic_expansion_ratio = 3.5; + info.lattice_filepath = + ament_index_cpp::get_package_share_directory("nav2_smac_planner") + + "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" + + "/output.json"; + info.minimum_turning_radius = 8; // in grid coordinates 0.4/0.05 + info.analytic_expansion_max_length = 20.0; // in grid coordinates + unsigned int size_theta = 16; + info.cost_penalty = 2.0; + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::STATE_LATTICE, info); + int max_iterations = 10000; + float tolerance = 10.0; + int it_on_approach = 10; + double max_planning_time = 120.0; + int num_it = 0; + + a_star.initialize( + false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0.0, 0.0, 0); + // island in the middle of lethal cost to cross + for (unsigned int i = 20; i <= 30; ++i) { + for (unsigned int j = 20; j <= 30; ++j) { + costmapA->setCost(i, j, 254); + } + } + + std::unique_ptr checker = + std::make_unique(costmapA, size_theta); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + // functional case testing + a_star.setCollisionChecker(checker.get()); + a_star.setStart(5u, 5u, 0u); + a_star.setGoal(40u, 40u, 1u); + nav2_smac_planner::NodeLattice::CoordinateVector path; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + + // check path is the right size and collision free + EXPECT_EQ(num_it, 21); + EXPECT_EQ(path.size(), 48u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + // no skipped nodes + for (unsigned int i = 1; i != path.size(); i++) { + EXPECT_LT(hypotf(path[i].x - path[i - 1].x, path[i].y - path[i - 1].y), 2.1f); + } + + delete costmapA; +} + +TEST(AStarTest, test_se2_single_pose_path) +{ + nav2_smac_planner::SearchInfo info; + info.change_penalty = 0.1; + info.non_straight_penalty = 1.1; + info.reverse_penalty = 2.0; + info.retrospective_penalty = 0.0; + info.minimum_turning_radius = 8; // in grid coordinates + info.analytic_expansion_max_length = 20.0; // in grid coordinates + info.analytic_expansion_ratio = 3.5; + unsigned int size_theta = 72; + info.cost_penalty = 1.7; + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::DUBIN, info); + int max_iterations = 100; + float tolerance = 10.0; + int it_on_approach = 10; + double max_planning_time = 120.0; + int num_it = 0; + + a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + + std::unique_ptr checker = + std::make_unique(costmapA, size_theta); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + // functional case testing + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + // Goal is one costmap cell away + a_star.setGoal(12u, 10u, 0u); + nav2_smac_planner::NodeHybrid::CoordinateVector path; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + + // Check that the path is length one + // With the current implementation, this produces a longer path + // EXPECT_EQ(path.size(), 1u); + EXPECT_GE(path.size(), 1u); + + delete costmapA; +} + +TEST(AStarTest, test_constants) +{ + nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown + EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Unknown")); + mm = nav2_smac_planner::MotionModel::VON_NEUMANN; // vonneumann + EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Von Neumann")); + mm = nav2_smac_planner::MotionModel::MOORE; // moore + EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Moore")); + mm = nav2_smac_planner::MotionModel::DUBIN; // dubin + EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Dubin")); + mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp + EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp")); + + EXPECT_EQ( + nav2_smac_planner::fromString( + "VON_NEUMANN"), nav2_smac_planner::MotionModel::VON_NEUMANN); + EXPECT_EQ(nav2_smac_planner::fromString("MOORE"), nav2_smac_planner::MotionModel::MOORE); + EXPECT_EQ(nav2_smac_planner::fromString("DUBIN"), nav2_smac_planner::MotionModel::DUBIN); + EXPECT_EQ( + nav2_smac_planner::fromString( + "REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP); + EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN); +} diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp new file mode 100644 index 0000000000..40e73c82ad --- /dev/null +++ b/nav2_smac_planner/test/test_collision_checker.cpp @@ -0,0 +1,171 @@ +// Copyright (c) 2020 Shivang Patel +// Copyright (c) 2020 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 "gtest/gtest.h" +#include "nav2_smac_planner/collision_checker.hpp" + +using namespace nav2_costmap_2d; // NOLINT + +TEST(collision_footprint, test_basic) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); + + geometry_msgs::msg::Point p1; + p1.x = -0.5; + p1.y = 0.0; + geometry_msgs::msg::Point p2; + p2.x = 0.0; + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; + p3.y = 0.0; + geometry_msgs::msg::Point p4; + p4.x = 0.0; + p4.y = -0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + 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); + delete costmap_; +} + +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_, 72); + nav2_costmap_2d::Footprint footprint; + collision_checker.setFootprint(footprint, true /*radius / pointcose*/, 0.0); + + collision_checker.inCollision(5.0, 5.0, 0.0, false); + float cost = collision_checker.getCost(); + EXPECT_NEAR(cost, 0.0, 0.001); + delete costmap_; +} + +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_, 72); + nav2_costmap_2d::Footprint footprint; + collision_checker.setFootprint(footprint, true /*radius / point cost*/, 0.0); + + unsigned int x, y; + + collision_checker.worldToMap(1.0, 1.0, x, y); + + collision_checker.inCollision(x, y, 0.0, false); + float cost = collision_checker.getCost(); + + EXPECT_NEAR(cost, 0.0, 0.001); + + costmap_->setCost(50, 50, 200); + collision_checker.worldToMap(5.0, 5.0, x, y); + + collision_checker.inCollision(x, y, 0.0, false); + EXPECT_NEAR(collision_checker.getCost(), 200.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_footprint_at_pose_with_movement) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 254); + + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap_->setCost(i, j, 128); + } + } + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + 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, 128.0, 0.001); + + collision_checker.inCollision(50, 49, 0.0, false); + float up_value = collision_checker.getCost(); + EXPECT_NEAR(up_value, 254.0, 0.001); + + collision_checker.inCollision(50, 52, 0.0, false); + float down_value = collision_checker.getCost(); + EXPECT_NEAR(down_value, 254.0, 0.001); + delete costmap_; +} + +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, 128.0); + + costmap_->setCost(62, 50, 254); + costmap_->setCost(39, 60, 254); + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + 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, 128.0, 0.001); + + collision_checker.inCollision(49, 50, 0.0, false); + float left_value = collision_checker.getCost(); + EXPECT_NEAR(left_value, 254.0, 0.001); + + collision_checker.inCollision(52, 50, 0.0, false); + float right_value = collision_checker.getCost(); + EXPECT_NEAR(right_value, 254.0, 0.001); + delete costmap_; +} diff --git a/nav2_smac_planner/test/test_costmap_downsampler.cpp b/nav2_smac_planner/test/test_costmap_downsampler.cpp new file mode 100644 index 0000000000..1317c34309 --- /dev/null +++ b/nav2_smac_planner/test/test_costmap_downsampler.cpp @@ -0,0 +1,67 @@ +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_smac_planner/costmap_downsampler.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(CostmapDownsampler, costmap_downsample_test) +{ + nav2_util::LifecycleNode::SharedPtr node = std::make_shared( + "CostmapDownsamplerTest"); + nav2_smac_planner::CostmapDownsampler downsampler; + + // create basic costmap + nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + costmapA.setCost(0, 0, 100); + costmapA.setCost(5, 5, 50); + + // downsample it + downsampler.on_configure(node, "map", "unused_topic", &costmapA, 2); + nav2_costmap_2d::Costmap2D * downsampledCostmapA = downsampler.downsample(2); + + // validate it + EXPECT_EQ(downsampledCostmapA->getCost(0, 0), 100); + EXPECT_EQ(downsampledCostmapA->getCost(2, 2), 50); + EXPECT_EQ(downsampledCostmapA->getSizeInCellsX(), 5u); + EXPECT_EQ(downsampledCostmapA->getSizeInCellsY(), 5u); + + // give it another costmap of another size + nav2_costmap_2d::Costmap2D costmapB(4, 4, 0.10, 0.0, 0.0, 0); + + // downsample it + downsampler.on_configure(node, "map", "unused_topic", &costmapB, 4); + downsampler.on_activate(); + nav2_costmap_2d::Costmap2D * downsampledCostmapB = downsampler.downsample(4); + downsampler.on_deactivate(); + + // validate size + EXPECT_EQ(downsampledCostmapB->getSizeInCellsX(), 1u); + EXPECT_EQ(downsampledCostmapB->getSizeInCellsY(), 1u); + + downsampler.resizeCostmap(); +} diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp new file mode 100644 index 0000000000..f4520bd496 --- /dev/null +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -0,0 +1,150 @@ +// 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 "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_2d.hpp" +#include "nav2_smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(Node2DTest, test_node_2d) +{ + nav2_costmap_2d::Costmap2D costmapA(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 + unsigned char cost = static_cast(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(); + EXPECT_TRUE(std::isnan(testA.getCost())); + + // check collision checking + 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_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, nullptr), 15., 0.01); + + // check operator== works on index + unsigned char costC = '2'; + nav2_smac_planner::Node2D testC(1); + testC.setCost(costC); + 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(), 1u); + + // check static index functions + EXPECT_EQ(nav2_smac_planner::Node2D::getIndex(1u, 1u, 10u), 11u); + EXPECT_EQ(nav2_smac_planner::Node2D::getIndex(6u, 43u, 10u), 436u); + EXPECT_EQ(nav2_smac_planner::Node2D::getCoords(436u, 10u, 1u).x, 6u); + EXPECT_EQ(nav2_smac_planner::Node2D::getCoords(436u, 10u, 1u).y, 43u); + EXPECT_THROW(nav2_smac_planner::Node2D::getCoords(436u, 10u, 10u), std::runtime_error); +} + +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::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); + + 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); + EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -100); + EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 100); + EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[4], -101); + EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[5], -99); + EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[6], 99); + 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); + 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(1); + node->setCost(cost); + std::function neighborGetter = + [&, this](const unsigned int & index, nav2_smac_planner::Node2D * & neighbor_rtn) -> bool + { + return false; + }; + + nav2_smac_planner::Node2D::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_nodebasic.cpp b/nav2_smac_planner/test/test_nodebasic.cpp new file mode 100644 index 0000000000..00aea82465 --- /dev/null +++ b/nav2_smac_planner/test/test_nodebasic.cpp @@ -0,0 +1,55 @@ +// 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_basic.hpp" +#include "nav2_smac_planner/node_2d.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_smac_planner/node_lattice.hpp" +#include "nav2_smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(NodeBasicTest, test_node_basic) +{ + nav2_smac_planner::NodeBasic node(50); + + EXPECT_EQ(node.index, 50u); + EXPECT_EQ(node.graph_node_ptr, nullptr); + + nav2_smac_planner::NodeBasic node2(100); + + EXPECT_EQ(node2.index, 100u); + EXPECT_EQ(node2.graph_node_ptr, nullptr); + + nav2_smac_planner::NodeBasic node3(200); + + EXPECT_EQ(node3.index, 200u); + EXPECT_EQ(node3.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 0000000000..e304b97e42 --- /dev/null +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -0,0 +1,303 @@ +// 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; + info.retrospective_penalty = 0.1; + 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 neutral as well + // but now reduced by retrospective penalty (10%) + testB.setMotionPrimitiveIndex(1); + testA.setMotionPrimitiveIndex(0); + EXPECT_NEAR(testB.getTraversalCost(&testA), 2.088 * 0.9, 0.1); + // same direction as parent, testB + testA.setMotionPrimitiveIndex(1); + EXPECT_NEAR(testB.getTraversalCost(&testA), 2.297f * 0.9, 0.01); + // opposite direction as parent, testB + testA.setMotionPrimitiveIndex(2); + EXPECT_NEAR(testB.getTraversalCost(&testA), 2.506f * 0.9, 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_obstacle_heuristic) +{ + 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; + info.retrospective_penalty = 0.0; + 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); + + nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( + 100, 100, 0.1, 0.0, 0.0, 0); + // island in the middle of lethal cost to cross + for (unsigned int i = 20; i <= 80; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmapA->setCost(i, j, 254); + } + } + // path on the right is narrow and thus with high cost + for (unsigned int i = 20; i <= 80; ++i) { + for (unsigned int j = 61; j <= 70; ++j) { + costmapA->setCost(i, j, 250); + } + } + for (unsigned int i = 20; i <= 80; ++i) { + for (unsigned int j = 71; j < 100; ++j) { + costmapA->setCost(i, j, 254); + } + } + std::unique_ptr checker = + std::make_unique(costmapA, 72); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + nav2_smac_planner::NodeHybrid testA(0); + testA.pose.x = 10; + testA.pose.y = 50; + testA.pose.theta = 0; + + nav2_smac_planner::NodeHybrid testB(1); + testB.pose.x = 90; + testB.pose.y = 51; // goal is a bit closer to the high-cost passage + testB.pose.theta = 0; + + // first block the high-cost passage to make sure the cost spreads through the better path + for (unsigned int j = 61; j <= 70; ++j) { + costmapA->setCost(50, j, 254); + } + nav2_smac_planner::NodeHybrid::resetObstacleHeuristic( + costmapA, testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y); + float wide_passage_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic( + testA.pose, + testB.pose, + info.cost_penalty); + + EXPECT_NEAR(wide_passage_cost, 91.1f, 0.1f); + + // then unblock it to check if cost remains the same + // (it should, since the unblocked narrow path will have higher cost than the wide one + // and thus lower bound of the path cost should be unchanged) + for (unsigned int j = 61; j <= 70; ++j) { + costmapA->setCost(50, j, 250); + } + nav2_smac_planner::NodeHybrid::resetObstacleHeuristic( + costmapA, + testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y); + float two_passages_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic( + testA.pose, + testB.pose, + info.cost_penalty); + + EXPECT_EQ(wide_passage_cost, two_passages_cost); + + 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 + info.retrospective_penalty = 0.0; + 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 + info.retrospective_penalty = 0.0; + 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_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp new file mode 100644 index 0000000000..a2f8a10796 --- /dev/null +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -0,0 +1,290 @@ +// Copyright (c) 2021 Joshua Wallace +// 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 "nav2_smac_planner/node_lattice.hpp" +#include "gtest/gtest.h" +#include "ament_index_cpp/get_package_share_directory.hpp" + +using json = nlohmann::json; + +TEST(NodeLatticeTest, parser_test) +{ + std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); + std::string filePath = + pkg_share_dir + + "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" + + "/output.json"; + std::ifstream myJsonFile(filePath); + + ASSERT_TRUE(myJsonFile.is_open()); + + json j; + myJsonFile >> j; + + nav2_smac_planner::LatticeMetadata metaData; + nav2_smac_planner::MotionPrimitive myPrimitive; + nav2_smac_planner::MotionPose pose; + + json jsonMetaData = j["lattice_metadata"]; + json jsonPrimatives = j["primitives"]; + json jsonPose = jsonPrimatives[0]["poses"][0]; + + nav2_smac_planner::fromJsonToMetaData(jsonMetaData, metaData); + + // Checks for parsing meta data + EXPECT_NEAR(metaData.min_turning_radius, 0.5, 0.001); + EXPECT_NEAR(metaData.grid_resolution, 0.05, 0.001); + EXPECT_NEAR(metaData.number_of_headings, 16, 0.01); + EXPECT_NEAR(metaData.heading_angles[0], 0.0, 0.01); + EXPECT_EQ(metaData.number_of_trajectories, 80u); + EXPECT_EQ(metaData.motion_model, std::string("ackermann")); + + std::vector myPrimitives; + for (unsigned int i = 0; i < jsonPrimatives.size(); ++i) { + nav2_smac_planner::MotionPrimitive newPrimative; + nav2_smac_planner::fromJsonToMotionPrimitive(jsonPrimatives[i], newPrimative); + myPrimitives.push_back(newPrimative); + } + + // Checks for parsing primitives + EXPECT_EQ(myPrimitives.size(), 80u); + EXPECT_NEAR(myPrimitives[0].trajectory_id, 0, 0.01); + EXPECT_NEAR(myPrimitives[0].start_angle, 0.0, 0.01); + EXPECT_NEAR(myPrimitives[0].end_angle, 13, 0.01); + EXPECT_NEAR(myPrimitives[0].turning_radius, 0.5259, 0.01); + EXPECT_NEAR(myPrimitives[0].trajectory_length, 0.64856, 0.01); + EXPECT_NEAR(myPrimitives[0].arc_length, 0.58225, 0.01); + EXPECT_NEAR(myPrimitives[0].straight_length, 0.06631, 0.01); + + EXPECT_NEAR(myPrimitives[0].poses[0]._x, 0.04981, 0.01); + EXPECT_NEAR(myPrimitives[0].poses[0]._y, -0.00236, 0.01); + EXPECT_NEAR(myPrimitives[0].poses[0]._theta, 6.1883, 0.01); + + EXPECT_NEAR(myPrimitives[0].poses[1]._x, 0.09917, 0.01); + EXPECT_NEAR(myPrimitives[0].poses[1]._y, -0.00944, 0.01); + EXPECT_NEAR(myPrimitives[0].poses[1]._theta, 6.09345, 0.015); +} + +TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing) +{ + std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); + std::string filePath = + pkg_share_dir + + "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" + + "/output.json"; + + nav2_smac_planner::SearchInfo info; + info.minimum_turning_radius = 1.1; + info.non_straight_penalty = 1; + info.change_penalty = 1; + info.reverse_penalty = 1; + info.cost_penalty = 1; + info.retrospective_penalty = 0.0; + info.analytic_expansion_ratio = 1; + info.lattice_filepath = filePath; + info.cache_obstacle_heuristic = true; + info.allow_reverse_expansion = true; + + unsigned int x = 100; + unsigned int y = 100; + unsigned int angle_quantization = 16; + + nav2_smac_planner::NodeLattice::initMotionModel( + nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info); + + nav2_smac_planner::NodeLattice aNode(0); + aNode.setPose(nav2_smac_planner::NodeHybrid::Coordinates(0, 0, 0)); + nav2_smac_planner::MotionPrimitivePtrs projections = + nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&aNode); + + EXPECT_NEAR(projections[0]->poses.back()._x, 0.5, 0.01); + EXPECT_NEAR(projections[0]->poses.back()._y, -0.35, 0.01); + EXPECT_NEAR(projections[0]->poses.back()._theta, 5.176, 0.01); + + EXPECT_NEAR( + nav2_smac_planner::NodeLattice::motion_table.getLatticeMetadata( + filePath).grid_resolution, 0.05, 0.005); +} + +TEST(NodeLatticeTest, test_node_lattice_conversions) +{ + std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); + std::string filePath = + pkg_share_dir + + "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" + + "/output.json"; + + nav2_smac_planner::SearchInfo info; + info.minimum_turning_radius = 1.1; + info.non_straight_penalty = 1; + info.change_penalty = 1; + info.reverse_penalty = 1; + info.cost_penalty = 1; + info.retrospective_penalty = 0.0; + info.analytic_expansion_ratio = 1; + info.lattice_filepath = filePath; + info.cache_obstacle_heuristic = true; + + unsigned int x = 100; + unsigned int y = 100; + unsigned int angle_quantization = 16; + + nav2_smac_planner::NodeLattice::initMotionModel( + nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info); + + nav2_smac_planner::NodeLattice aNode(0); + aNode.setPose(nav2_smac_planner::NodeHybrid::Coordinates(0, 0, 0)); + + EXPECT_NEAR(aNode.motion_table.getAngleFromBin(0u), 0.0, 0.005); + EXPECT_NEAR(aNode.motion_table.getAngleFromBin(1u), 0.46364, 0.005); + EXPECT_NEAR(aNode.motion_table.getAngleFromBin(2u), 0.78539, 0.005); + + EXPECT_EQ(aNode.motion_table.getClosestAngularBin(0.0), 0u); + EXPECT_EQ(aNode.motion_table.getClosestAngularBin(0.5), 1u); + EXPECT_EQ(aNode.motion_table.getClosestAngularBin(1.5), 4u); +} + +TEST(NodeLatticeTest, test_node_lattice) +{ + std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); + std::string filePath = + pkg_share_dir + + "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" + + "/output.json"; + + nav2_smac_planner::SearchInfo info; + info.minimum_turning_radius = 1.1; + info.non_straight_penalty = 1; + info.change_penalty = 1; + info.reverse_penalty = 1; + info.cost_penalty = 1; + info.retrospective_penalty = 0.1; + info.analytic_expansion_ratio = 1; + info.lattice_filepath = filePath; + info.cache_obstacle_heuristic = true; + info.allow_reverse_expansion = true; + + unsigned int x = 100; + unsigned int y = 100; + unsigned int angle_quantization = 16; + + nav2_smac_planner::NodeLattice::initMotionModel( + nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info); + + // Check defaults + nav2_smac_planner::NodeLattice aNode(0); + nav2_smac_planner::NodeLattice testA(49); + EXPECT_EQ(testA.getIndex(), 49u); + EXPECT_EQ(testA.getAccumulatedCost(), std::numeric_limits::max()); + EXPECT_TRUE(std::isnan(testA.getCost())); + EXPECT_EQ(testA.getMotionPrimitive(), nullptr); + + // Test visited state / reset + EXPECT_EQ(testA.wasVisited(), false); + testA.visited(); + EXPECT_EQ(testA.wasVisited(), true); + testA.reset(); + EXPECT_EQ(testA.wasVisited(), false); + + 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 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); + + // check collision checking + EXPECT_EQ(testA.isNodeValid(false, checker.get()), true); + + // check operator== works on index + nav2_smac_planner::NodeLattice testC(49); + EXPECT_TRUE(testA == testC); + + // check accumulated costs are set + testC.setAccumulatedCost(100); + EXPECT_EQ(testC.getAccumulatedCost(), 100.0f); + + // check set pose and pose + testC.setPose(nav2_smac_planner::NodeLattice::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); + + delete costmapA; +} + + +TEST(NodeLatticeTest, test_get_neighbors) +{ + std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); + std::string filePath = + pkg_share_dir + + "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" + + "/output.json"; + + nav2_smac_planner::SearchInfo info; + info.minimum_turning_radius = 1.1; + info.non_straight_penalty = 1; + info.change_penalty = 1; + info.reverse_penalty = 1; + info.cost_penalty = 1; + info.analytic_expansion_ratio = 1; + info.retrospective_penalty = 0.0; + info.lattice_filepath = filePath; + info.cache_obstacle_heuristic = true; + info.allow_reverse_expansion = true; + + unsigned int x = 100; + unsigned int y = 100; + unsigned int angle_quantization = 16; + + nav2_smac_planner::NodeLattice::initMotionModel( + nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info); + + nav2_smac_planner::NodeLattice node(49); + + 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); + + std::function neighborGetter = + [&, this](const unsigned int & index, nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool + { + // because we don't return a real object + return false; + }; + + nav2_smac_planner::NodeLattice::NodeVector neighbors; + node.getNeighbors(neighborGetter, checker.get(), false, neighbors); + // should be empty since totally invalid + EXPECT_EQ(neighbors.size(), 0u); + + delete costmapA; +} diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp new file mode 100644 index 0000000000..ebbac996cd --- /dev/null +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -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. + +#include + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "gtest/gtest.h" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_smac_planner/a_star.hpp" +#include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_smac_planner/smac_planner_2d.hpp" +#include "nav2_smac_planner/smac_planner_hybrid.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +// SMAC smoke tests for plugin-level issues rather than algorithms +// (covered by more extensively testing in other files) +// System tests in nav2_system_tests will actually plan with this work + +TEST(SmacTest, test_smac_2d) { + rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = + std::make_shared("Smac2DTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + node2D->declare_parameter("test.smooth_path", true); + node2D->set_parameter(rclcpp::Parameter("test.smooth_path", true)); + node2D->declare_parameter("test.downsample_costmap", true); + node2D->set_parameter(rclcpp::Parameter("test.downsample_costmap", true)); + node2D->declare_parameter("test.downsampling_factor", 2); + node2D->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation.w = 1.0; + // goal = start; + goal.pose.position.x = 7.0; + goal.pose.position.y = 0.0; + goal.pose.orientation.w = 1.0; + auto planner_2d = std::make_unique(); + planner_2d->configure(node2D, "test", nullptr, costmap_ros); + planner_2d->activate(); + try { + planner_2d->createPlan(start, goal); + } catch (...) { + } + + planner_2d->deactivate(); + planner_2d->cleanup(); + + planner_2d.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + node2D.reset(); + costmap_ros.reset(); +} + +TEST(SmacTest, test_smac_2d_reconfigure) { + rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = + std::make_shared("Smac2DTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + auto planner_2d = std::make_unique(); + planner_2d->configure(node2D, "test", nullptr, costmap_ros); + planner_2d->activate(); + + auto rec_param = std::make_shared( + node2D->get_node_base_interface(), node2D->get_node_topics_interface(), + node2D->get_node_graph_interface(), + node2D->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.tolerance", 1.0), + rclcpp::Parameter("test.cost_travel_multiplier", 1.0), + rclcpp::Parameter("test.max_planning_time", 2.0), + rclcpp::Parameter("test.downsample_costmap", false), + rclcpp::Parameter("test.allow_unknown", false), + rclcpp::Parameter("test.downsampling_factor", 2), + rclcpp::Parameter("test.max_iterations", -1), + rclcpp::Parameter("test.max_on_approach_iterations", -1), + rclcpp::Parameter("test.motion_model_for_search", "UNKNOWN"), + rclcpp::Parameter("test.use_final_approach_orientation", false)}); + + rclcpp::spin_until_future_complete( + node2D->get_node_base_interface(), + results); + + EXPECT_EQ(node2D->get_parameter("test.tolerance").as_double(), 1.0); + EXPECT_EQ( + node2D->get_parameter("test.cost_travel_multiplier").as_double(), + 1.0); + EXPECT_EQ(node2D->get_parameter("test.max_planning_time").as_double(), 2.0); + EXPECT_EQ(node2D->get_parameter("test.downsample_costmap").as_bool(), false); + EXPECT_EQ(node2D->get_parameter("test.allow_unknown").as_bool(), false); + EXPECT_EQ(node2D->get_parameter("test.downsampling_factor").as_int(), 2); + EXPECT_EQ(node2D->get_parameter("test.max_iterations").as_int(), -1); + EXPECT_EQ(node2D->get_parameter("test.use_final_approach_orientation").as_bool(), false); + EXPECT_EQ( + node2D->get_parameter("test.max_on_approach_iterations").as_int(), + -1); + EXPECT_EQ( + node2D->get_parameter("test.motion_model_for_search").as_string(), + "UNKNOWN"); + + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.downsample_costmap", true)}); + + rclcpp::spin_until_future_complete( + node2D->get_node_base_interface(), + results); +} diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp new file mode 100644 index 0000000000..3eb077cfb0 --- /dev/null +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -0,0 +1,145 @@ +// 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 "geometry_msgs/msg/pose_stamped.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_hybrid.hpp" +#include "nav2_smac_planner/smac_planner_2d.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +// SMAC smoke tests for plugin-level issues rather than algorithms +// (covered by more extensively testing in other files) +// System tests in nav2_system_tests will actually plan with this work + +TEST(SmacTest, test_smac_se2) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 = + std::make_shared("SmacSE2Test"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + nodeSE2->declare_parameter("test.downsample_costmap", true); + nodeSE2->set_parameter(rclcpp::Parameter("test.downsample_costmap", true)); + nodeSE2->declare_parameter("test.downsampling_factor", 2); + nodeSE2->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation.w = 1.0; + 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(); + + try { + planner->createPlan(start, goal); + } catch (...) { + } + + planner->deactivate(); + planner->cleanup(); + + planner.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + costmap_ros.reset(); + nodeSE2.reset(); +} + +TEST(SmacTest, test_smac_se2_reconfigure) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 = + std::make_shared("SmacSE2Test"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + auto planner = std::make_unique(); + planner->configure(nodeSE2, "test", nullptr, costmap_ros); + planner->activate(); + + auto rec_param = std::make_shared( + nodeSE2->get_node_base_interface(), nodeSE2->get_node_topics_interface(), + nodeSE2->get_node_graph_interface(), + nodeSE2->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.downsample_costmap", true), + rclcpp::Parameter("test.downsampling_factor", 2), + rclcpp::Parameter("test.angle_quantization_bins", 100), + rclcpp::Parameter("test.allow_unknown", false), + rclcpp::Parameter("test.max_iterations", -1), + rclcpp::Parameter("test.minimum_turning_radius", 1.0), + rclcpp::Parameter("test.cache_obstacle_heuristic", true), + rclcpp::Parameter("test.reverse_penalty", 5.0), + rclcpp::Parameter("test.change_penalty", 1.0), + rclcpp::Parameter("test.non_straight_penalty", 2.0), + rclcpp::Parameter("test.cost_penalty", 2.0), + rclcpp::Parameter("test.retrospective_penalty", 0.2), + rclcpp::Parameter("test.analytic_expansion_ratio", 4.0), + rclcpp::Parameter("test.max_planning_time", 10.0), + rclcpp::Parameter("test.lookup_table_size", 30.0), + rclcpp::Parameter("test.smooth_path", false), + rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), + rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); + + rclcpp::spin_until_future_complete( + nodeSE2->get_node_base_interface(), + results); + + EXPECT_EQ(nodeSE2->get_parameter("test.downsample_costmap").as_bool(), true); + EXPECT_EQ(nodeSE2->get_parameter("test.downsampling_factor").as_int(), 2); + EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 100); + EXPECT_EQ(nodeSE2->get_parameter("test.allow_unknown").as_bool(), false); + EXPECT_EQ(nodeSE2->get_parameter("test.max_iterations").as_int(), -1); + EXPECT_EQ(nodeSE2->get_parameter("test.minimum_turning_radius").as_double(), 1.0); + EXPECT_EQ(nodeSE2->get_parameter("test.cache_obstacle_heuristic").as_bool(), true); + EXPECT_EQ(nodeSE2->get_parameter("test.reverse_penalty").as_double(), 5.0); + EXPECT_EQ(nodeSE2->get_parameter("test.change_penalty").as_double(), 1.0); + EXPECT_EQ(nodeSE2->get_parameter("test.non_straight_penalty").as_double(), 2.0); + EXPECT_EQ(nodeSE2->get_parameter("test.cost_penalty").as_double(), 2.0); + EXPECT_EQ(nodeSE2->get_parameter("test.retrospective_penalty").as_double(), 0.2); + EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_ratio").as_double(), 4.0); + EXPECT_EQ(nodeSE2->get_parameter("test.smooth_path").as_bool(), false); + EXPECT_EQ(nodeSE2->get_parameter("test.max_planning_time").as_double(), 10.0); + EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0); + EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_max_length").as_double(), 42.0); + EXPECT_EQ( + nodeSE2->get_parameter("test.motion_model_for_search").as_string(), + std::string("REEDS_SHEPP")); +} diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp new file mode 100644 index 0000000000..ff813f08c1 --- /dev/null +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -0,0 +1,144 @@ +// 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 "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 "geometry_msgs/msg/pose_stamped.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_lattice.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +// Simple wrapper to be able to call a private member +class LatticeWrap : public nav2_smac_planner::SmacPlannerLattice +{ +public: + void callDynamicParams(std::vector parameters) + { + dynamicParametersCallback(parameters); + } +}; + +// SMAC smoke tests for plugin-level issues rather than algorithms +// (covered by more extensively testing in other files) +// System tests in nav2_system_tests will actually plan with this work + +TEST(SmacTest, test_smac_lattice) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice = + std::make_shared("SmacLatticeTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation.w = 1.0; + goal.pose.position.x = 1.0; + goal.pose.position.y = 1.0; + goal.pose.orientation.w = 1.0; + auto planner = std::make_unique(); + try { + // Expect to throw due to invalid prims file in param + planner->configure(nodeLattice, "test", nullptr, costmap_ros); + } catch (...) { + } + planner->activate(); + + try { + planner->createPlan(start, goal); + } catch (...) { + } + + planner->deactivate(); + planner->cleanup(); + + planner.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + costmap_ros.reset(); + nodeLattice.reset(); +} + +TEST(SmacTest, test_smac_lattice_reconfigure) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice = + std::make_shared("SmacLatticeTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + auto planner = std::make_unique(); + try { + // Expect to throw due to invalid prims file in param + planner->configure(nodeLattice, "test", nullptr, costmap_ros); + } catch (...) { + } + planner->activate(); + + auto rec_param = std::make_shared( + nodeLattice->get_node_base_interface(), nodeLattice->get_node_topics_interface(), + nodeLattice->get_node_graph_interface(), + nodeLattice->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.allow_unknown", false), + rclcpp::Parameter("test.max_iterations", -1), + rclcpp::Parameter("test.cache_obstacle_heuristic", true), + rclcpp::Parameter("test.reverse_penalty", 5.0), + rclcpp::Parameter("test.change_penalty", 1.0), + rclcpp::Parameter("test.non_straight_penalty", 2.0), + rclcpp::Parameter("test.cost_penalty", 2.0), + rclcpp::Parameter("test.retrospective_penalty", 0.2), + rclcpp::Parameter("test.analytic_expansion_ratio", 4.0), + rclcpp::Parameter("test.max_planning_time", 10.0), + rclcpp::Parameter("test.lookup_table_size", 30.0), + rclcpp::Parameter("test.smooth_path", false), + rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), + rclcpp::Parameter("test.rotation_penalty", 42.0), + rclcpp::Parameter("test.allow_reverse_expansion", true)}); + + try { + // All of these params will re-init A* which will involve loading the control set file + // which will cause an exception because the file does not exist. This will cause an + // expected failure preventing parameter updates from being successfully processed + rclcpp::spin_until_future_complete( + nodeLattice->get_node_base_interface(), + results); + } catch (...) { + } + + // So instead, lets call manually on a change + std::vector parameters; + parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI"))); + EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error); +} diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp new file mode 100644 index 0000000000..d7d27f1d20 --- /dev/null +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -0,0 +1,178 @@ +// 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 + +#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/a_star.hpp" +#include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/smoother.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +using namespace nav2_smac_planner; // NOLINT + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class SmootherWrapper : public nav2_smac_planner::Smoother +{ +public: + explicit SmootherWrapper(const SmootherParams & params) + : nav2_smac_planner::Smoother(params) + {} + + std::vector findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path) + { + return findDirectionalPathSegments(path); + } +}; + +TEST(SmootherTest, test_full_smoother) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSmootherTest"); + nav2_smac_planner::SmootherParams params; + params.get(node, "test"); + double maxtime = 1.0; + + // Make smoother and costmap to smooth in + auto smoother = std::make_unique(params); + smoother->initialize(0.4 /*turning radius*/); + + nav2_costmap_2d::Costmap2D * costmap = + new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0.0, 0.0, 0); + // island in the middle of lethal cost to cross + for (unsigned int i = 20; i <= 30; ++i) { + for (unsigned int j = 20; j <= 30; ++j) { + costmap->setCost(i, j, 254); + } + } + + // Setup A* search to get path to smooth + nav2_smac_planner::SearchInfo info; + info.change_penalty = 0.05; + info.non_straight_penalty = 1.05; + info.reverse_penalty = 2.0; + info.cost_penalty = 2.0; + info.retrospective_penalty = 0.0; + info.analytic_expansion_ratio = 3.5; + info.minimum_turning_radius = 8; // in grid coordinates 0.4/0.05 + info.analytic_expansion_max_length = 20.0; // in grid coordinates + unsigned int size_theta = 72; + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::REEDS_SHEPP, info); + int max_iterations = 10000; + float tolerance = 10.0; + int it_on_approach = 10; + double max_planning_time = 120.0; + int num_it = 0; + + a_star.initialize( + false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); + std::unique_ptr checker = + std::make_unique(costmap, size_theta); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + // Create A* search to smooth + a_star.setCollisionChecker(checker.get()); + a_star.setStart(5u, 5u, 0u); + a_star.setGoal(45u, 45u, 36u); + nav2_smac_planner::NodeHybrid::CoordinateVector path; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + + // Convert to world coordinates and get length to compare to smoothed length + nav_msgs::msg::Path plan; + plan.header.stamp = node->now(); + plan.header.frame_id = "map"; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + 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; + double initial_length = 0.0; + double x_m = path[path.size() - 1].x, y_m = path[path.size() - 1].y; + plan.poses.reserve(path.size()); + for (int i = path.size() - 1; i >= 0; --i) { + pose.pose = nav2_smac_planner::getWorldCoords(path[i].x, path[i].y, costmap); + pose.pose.orientation = nav2_smac_planner::getWorldOrientation(path[i].theta); + plan.poses.push_back(pose); + initial_length += hypot(path[i].x - x_m, path[i].y - y_m); + x_m = path[i].x; + y_m = path[i].y; + } + + // Check that we accurately detect that this path has a reversing segment + EXPECT_EQ(smoother->findDirectionalPathSegmentsWrapper(plan).size(), 2u); + + // Test smoother, should succeed with same number of points + // and shorter overall length, while still being collision free. + auto path_size_in = plan.poses.size(); + EXPECT_TRUE(smoother->smooth(plan, costmap, maxtime)); + EXPECT_EQ(plan.poses.size(), path_size_in); // Should have same number of poses + double length = 0.0; + x_m = plan.poses[0].pose.position.x; + y_m = plan.poses[0].pose.position.y; + for (unsigned int i = 0; i != plan.poses.size(); i++) { + // Should be collision free + EXPECT_EQ(costmap->getCost(plan.poses[i].pose.position.x, plan.poses[i].pose.position.y), 0); + length += hypot(plan.poses[i].pose.position.x - x_m, plan.poses[i].pose.position.y - y_m); + x_m = plan.poses[i].pose.position.x; + y_m = plan.poses[i].pose.position.y; + } + EXPECT_LT(length, initial_length); // Should be shorter + + // Try again but with failure modes + + // Failure mode: not enough iterations to complete + params.max_its_ = 0; + auto smoother_bypass = std::make_unique(params); + EXPECT_FALSE(smoother_bypass->smooth(plan, costmap, maxtime)); + params.max_its_ = 1; + auto smoother_failure = std::make_unique(params); + EXPECT_FALSE(smoother_failure->smooth(plan, costmap, maxtime)); + + // Failure mode: Not enough time + double max_no_time = 0.0; + EXPECT_FALSE(smoother->smooth(plan, costmap, max_no_time)); + + // Failure mode: Path is in collision, do 2x to exercise overlapping point + // attempts to update orientation should also fail + pose.pose.position.x = 1.25; + pose.pose.position.y = 1.25; + plan.poses.push_back(pose); + plan.poses.push_back(pose); + EXPECT_FALSE(smoother->smooth(plan, costmap, maxtime)); + EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.z, 1.0, 1e-3); + EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.x, 0.0, 1e-3); + EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.y, 0.0, 1e-3); + EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.w, 0.0, 1e-3); + + delete costmap; +} diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt new file mode 100644 index 0000000000..0df85f54bd --- /dev/null +++ b/nav2_smoother/CMakeLists.txt @@ -0,0 +1,109 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_smoother) + +find_package(ament_cmake REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_common REQUIRED) +find_package(angles REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav_2d_utils REQUIRED) +find_package(nav_2d_msgs REQUIRED) +find_package(pluginlib REQUIRED) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +nav2_package() + +include_directories( + include +) + +set(executable_name smoother_server) +set(library_name ${executable_name}_core) + +set(dependencies + angles + rclcpp + rclcpp_components + rclcpp_action + rclcpp_components + std_msgs + nav2_msgs + nav_2d_utils + nav_2d_msgs + nav2_util + nav2_core + pluginlib +) + +# Main library +add_library(${library_name} SHARED + src/nav2_smoother.cpp +) +ament_target_dependencies(${library_name} + ${dependencies} +) + +# Main executable +add_executable(${executable_name} + src/main.cpp +) +ament_target_dependencies(${executable_name} + ${dependencies} +) +target_link_libraries(${executable_name} ${library_name}) + +# Simple Smoother plugin +add_library(simple_smoother SHARED + src/simple_smoother.cpp +) +ament_target_dependencies(simple_smoother + ${dependencies} +) +pluginlib_export_plugin_description_file(nav2_core plugins.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) +endif() + +rclcpp_components_register_nodes(${library_name} "nav2_smoother::SmootherServer") + +install( + TARGETS ${library_name} simple_smoother + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name} simple_smoother) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/nav2_smoother/README.md b/nav2_smoother/README.md new file mode 100644 index 0000000000..da007a9096 --- /dev/null +++ b/nav2_smoother/README.md @@ -0,0 +1,9 @@ +# Nav2 Smoother + +The Nav2 smoother is a Task Server in Nav2 that implements the `nav2_behavior_tree::SmoothPath` interface. + +A smoothing module implementing the `nav2_behavior_tree::SmoothPath` interface is responsible for improving path smoothness and/or quality, typically given an unsmoothed path from the planner module in `nav2_planner`. It loads a map of potential smoother plugins to do the path smoothing in different user-defined situations. + +See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available smoother plugins. + +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions. diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp new file mode 100644 index 0000000000..1a4d416503 --- /dev/null +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -0,0 +1,167 @@ +// Copyright (c) 2021 RoboTech Vision +// 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_SMOOTHER__NAV2_SMOOTHER_HPP_ +#define NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_ + +#include +#include +#include +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" +#include "nav2_costmap_2d/footprint_subscriber.hpp" +#include "nav2_msgs/action/smooth_path.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/simple_action_server.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "pluginlib/class_loader.hpp" + +namespace nav2_smoother +{ + +/** + * @class nav2_smoother::SmootherServer + * @brief This class hosts variety of plugins of different algorithms to + * smooth or refine a path from the exposed SmoothPath action server. + */ +class SmootherServer : public nav2_util::LifecycleNode +{ +public: + using SmootherMap = std::unordered_map; + + /** + * @brief A constructor for nav2_smoother::SmootherServer + * @param options Additional options to control creation of the node. + */ + explicit SmootherServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /** + * @brief Destructor for nav2_smoother::SmootherServer + */ + ~SmootherServer(); + +protected: + /** + * @brief Configures smoother parameters and member variables + * + * Configures smoother plugin and costmap; Initialize odom subscriber, + * velocity publisher and smooth path action server. + * @param state LifeCycle Node's state + * @return Success or Failure + * @throw pluginlib::PluginlibException When failed to initialize smoother + * plugin + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Loads smoother plugins from parameter file + * @return bool if successfully loaded the plugins + */ + bool loadSmootherPlugins(); + + /** + * @brief Activates member variables + * + * Activates smoother, costmap, velocity publisher and smooth path action + * server + * @param state LifeCycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Deactivates member variables + * + * Deactivates smooth path action server, smoother, costmap and velocity + * publisher. Before calling deactivate state, velocity is being set to zero. + * @param state LifeCycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Calls clean up states and resets member variables. + * + * Smoother and costmap clean up state is called, and resets rest of the + * variables + * @param state LifeCycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Called when in Shutdown state + * @param state LifeCycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + using Action = nav2_msgs::action::SmoothPath; + using ActionServer = nav2_util::SimpleActionServer; + + /** + * @brief SmoothPath action server callback. Handles action server updates and + * spins server until goal is reached + * + * Provides global path to smoother received from action client. Local + * section of the path is optimized using smoother. + * @throw nav2_core::PlannerException + */ + void smoothPlan(); + + /** + * @brief Find the valid smoother ID name for the given request + * + * @param c_name The requested smoother name + * @param name Reference to the name to use for control if any valid available + * @return bool Whether it found a valid smoother to use + */ + bool findSmootherId(const std::string & c_name, std::string & name); + + // Our action server implements the SmoothPath action + std::unique_ptr action_server_; + + // Transforms + std::shared_ptr tf_; + std::shared_ptr transform_listener_; + + // Publishers and subscribers + rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; + + // Smoother Plugins + pluginlib::ClassLoader lp_loader_; + SmootherMap smoothers_; + std::vector default_ids_; + std::vector default_types_; + std::vector smoother_ids_; + std::vector smoother_types_; + std::string smoother_ids_concat_, current_smoother_; + + // Utilities + std::shared_ptr costmap_sub_; + std::shared_ptr footprint_sub_; + std::shared_ptr collision_checker_; + + rclcpp::Clock steady_clock_; +}; + +} // namespace nav2_smoother + +#endif // NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_ diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp new file mode 100644 index 0000000000..4b841b3c09 --- /dev/null +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -0,0 +1,161 @@ +// Copyright (c) 2022, 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_SMOOTHER__SIMPLE_SMOOTHER_HPP_ +#define NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "angles/angles.h" +#include "tf2/utils.h" + +namespace nav2_smoother +{ + +/** + * @class nav2_smoother::PathSegment + * @brief A segment of a path in start/end indices + */ +struct PathSegment +{ + unsigned int start; + unsigned int end; +}; + +typedef std::vector::iterator PathIterator; +typedef std::vector::reverse_iterator ReversePathIterator; + +/** + * @class nav2_smoother::SimpleSmoother + * @brief A path smoother implementation + */ +class SimpleSmoother : public nav2_core::Smoother +{ +public: + /** + * @brief A constructor for nav2_smoother::SimpleSmoother + */ + SimpleSmoother() = default; + + /** + * @brief A destructor for nav2_smoother::SimpleSmoother + */ + ~SimpleSmoother() override = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string name, std::shared_ptr, + std::shared_ptr, + std::shared_ptr) override; + + /** + * @brief Method to cleanup resources. + */ + void cleanup() override {costmap_sub_.reset();} + + /** + * @brief Method to activate smoother and any threads involved in execution. + */ + void activate() override {} + + /** + * @brief Method to deactivate smoother and any threads involved in execution. + */ + void deactivate() override {} + + /** + * @brief Method to smooth given path + * + * @param path In-out path to be smoothed + * @param max_time Maximum duration smoothing should take + * @return If smoothing was completed (true) or interrupted by time limit (false) + */ + bool smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) override; + +protected: + /** + * @brief Smoother method - does the smoothing on a segment + * @param path Reference to path + * @param reversing_segment Return if this is a reversing segment + * @param costmap Pointer to minimal costmap + * @param max_time Maximum time to compute, stop early if over limit + * @return If smoothing was successful + */ + bool smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time); + + /** + * @brief Get the field value for a given dimension + * @param msg Current pose to sample + * @param dim Dimension ID of interest + * @return dim value + */ + inline double getFieldByDim( + const geometry_msgs::msg::PoseStamped & msg, + const unsigned int & dim); + + /** + * @brief Set the field value for a given dimension + * @param msg Current pose to sample + * @param dim Dimension ID of interest + * @param value to set the dimention to for the pose + */ + inline void setFieldByDim( + geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, + const double & value); + + /** + * @brief Finds the starting and end indices of path segments where + * the robot is traveling in the same direction (e.g. forward vs reverse) + * @param path Path in which to look for cusps + * @return Set of index pairs for each segment of the path in a given direction + */ + std::vector findDirectionalPathSegments(const nav_msgs::msg::Path & path); + + /** + * @brief For a given path, update the path point orientations based on smoothing + * @param path Path to approximate the path orientation in + * @param reversing_segment Return if this is a reversing segment + */ + inline void updateApproximatePathOrientations( + nav_msgs::msg::Path & path, + bool & reversing_segment); + + double tolerance_, data_w_, smooth_w_; + int max_its_, refinement_ctr_; + bool do_refinement_; + std::shared_ptr costmap_sub_; + rclcpp::Logger logger_{rclcpp::get_logger("SimpleSmoother")}; +}; + +} // namespace nav2_smoother + +#endif // NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_ diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml new file mode 100644 index 0000000000..e0fdca6bae --- /dev/null +++ b/nav2_smoother/package.xml @@ -0,0 +1,33 @@ + + + + nav2_smoother + 1.1.0 + Smoother action interface + Matej Vargovcik + Steve Macenski + Apache-2.0 + + ament_cmake + nav2_common + angles + rclcpp + rclcpp_components + rclcpp_action + std_msgs + nav2_util + nav2_msgs + nav_2d_utils + nav_2d_msgs + nav2_core + pluginlib + + ament_lint_common + ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest + + + ament_cmake + + diff --git a/nav2_smoother/plugins.xml b/nav2_smoother/plugins.xml new file mode 100644 index 0000000000..c72f7c25c8 --- /dev/null +++ b/nav2_smoother/plugins.xml @@ -0,0 +1,7 @@ + + + + Does a simple smoothing process with collision checking + + + diff --git a/nav2_smoother/src/main.cpp b/nav2_smoother/src/main.cpp new file mode 100644 index 0000000000..93065ae8f3 --- /dev/null +++ b/nav2_smoother/src/main.cpp @@ -0,0 +1,29 @@ +// Copyright (c) 2021 RoboTech Vision +// 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 "nav2_smoother/nav2_smoother.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp new file mode 100644 index 0000000000..ada1f664b0 --- /dev/null +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -0,0 +1,329 @@ +// Copyright (c) 2019 RoboTech Vision +// 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 +#include +#include +#include + +#include "nav2_core/exceptions.hpp" +#include "nav2_smoother/nav2_smoother.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav_2d_utils/conversions.hpp" +#include "nav_2d_utils/tf_help.hpp" +#include "tf2_ros/create_timer_ros.h" + +using namespace std::chrono_literals; + +namespace nav2_smoother +{ + +SmootherServer::SmootherServer(const rclcpp::NodeOptions & options) +: LifecycleNode("smoother_server", "", options), + lp_loader_("nav2_core", "nav2_core::Smoother"), + default_ids_{"simple_smoother"}, + default_types_{"nav2_smoother::SimpleSmoother"} +{ + RCLCPP_INFO(get_logger(), "Creating smoother server"); + + declare_parameter( + "costmap_topic", rclcpp::ParameterValue( + std::string( + "global_costmap/costmap_raw"))); + declare_parameter( + "footprint_topic", + rclcpp::ParameterValue( + std::string("global_costmap/published_footprint"))); + declare_parameter( + "robot_base_frame", + rclcpp::ParameterValue(std::string("base_link"))); + declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter("smoother_plugins", default_ids_); +} + +SmootherServer::~SmootherServer() +{ + smoothers_.clear(); +} + +nav2_util::CallbackReturn +SmootherServer::on_configure(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Configuring smoother server"); + + auto node = shared_from_this(); + + get_parameter("smoother_plugins", smoother_ids_); + if (smoother_ids_ == default_ids_) { + for (size_t i = 0; i < default_ids_.size(); ++i) { + nav2_util::declare_parameter_if_not_declared( + node, default_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_types_[i])); + } + } + + tf_ = std::make_shared(get_clock()); + auto timer_interface = std::make_shared( + get_node_base_interface(), get_node_timers_interface()); + tf_->setCreateTimerInterface(timer_interface); + transform_listener_ = std::make_shared(*tf_); + + std::string costmap_topic, footprint_topic, robot_base_frame; + double transform_tolerance; + this->get_parameter("costmap_topic", costmap_topic); + this->get_parameter("footprint_topic", footprint_topic); + this->get_parameter("transform_tolerance", transform_tolerance); + this->get_parameter("robot_base_frame", robot_base_frame); + costmap_sub_ = std::make_shared( + shared_from_this(), costmap_topic); + footprint_sub_ = std::make_shared( + shared_from_this(), footprint_topic, *tf_, robot_base_frame, transform_tolerance); + + collision_checker_ = + std::make_shared( + *costmap_sub_, *footprint_sub_, this->get_name()); + + if (!loadSmootherPlugins()) { + return nav2_util::CallbackReturn::FAILURE; + } + + // Initialize pubs & subs + plan_publisher_ = create_publisher("plan_smoothed", 1); + + // Create the action server that we implement with our smoothPath method + action_server_ = std::make_unique( + shared_from_this(), + "smooth_path", + std::bind(&SmootherServer::smoothPlan, this), + nullptr, + std::chrono::milliseconds(500), + true); + + return nav2_util::CallbackReturn::SUCCESS; +} + +bool SmootherServer::loadSmootherPlugins() +{ + auto node = shared_from_this(); + + smoother_types_.resize(smoother_ids_.size()); + + for (size_t i = 0; i != smoother_ids_.size(); i++) { + try { + smoother_types_[i] = + nav2_util::get_plugin_type_param(node, smoother_ids_[i]); + nav2_core::Smoother::Ptr smoother = + lp_loader_.createUniqueInstance(smoother_types_[i]); + RCLCPP_INFO( + get_logger(), "Created smoother : %s of type %s", + smoother_ids_[i].c_str(), smoother_types_[i].c_str()); + smoother->configure( + node, smoother_ids_[i], tf_, costmap_sub_, + footprint_sub_); + smoothers_.insert({smoother_ids_[i], smoother}); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + get_logger(), "Failed to create smoother. Exception: %s", + ex.what()); + return false; + } + } + + for (size_t i = 0; i != smoother_ids_.size(); i++) { + smoother_ids_concat_ += smoother_ids_[i] + std::string(" "); + } + + RCLCPP_INFO( + get_logger(), "Smoother Server has %s smoothers available.", + smoother_ids_concat_.c_str()); + + return true; +} + +nav2_util::CallbackReturn +SmootherServer::on_activate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + plan_publisher_->on_activate(); + SmootherMap::iterator it; + for (it = smoothers_.begin(); it != smoothers_.end(); ++it) { + it->second->activate(); + } + action_server_->activate(); + + // create bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +SmootherServer::on_deactivate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + action_server_->deactivate(); + SmootherMap::iterator it; + for (it = smoothers_.begin(); it != smoothers_.end(); ++it) { + it->second->deactivate(); + } + plan_publisher_->on_deactivate(); + + // destroy bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +SmootherServer::on_cleanup(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + // Cleanup the helper classes + SmootherMap::iterator it; + for (it = smoothers_.begin(); it != smoothers_.end(); ++it) { + it->second->cleanup(); + } + smoothers_.clear(); + + // Release any allocated resources + action_server_.reset(); + plan_publisher_.reset(); + transform_listener_.reset(); + tf_.reset(); + footprint_sub_.reset(); + costmap_sub_.reset(); + collision_checker_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +SmootherServer::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} + +bool SmootherServer::findSmootherId( + const std::string & c_name, + std::string & current_smoother) +{ + if (smoothers_.find(c_name) == smoothers_.end()) { + if (smoothers_.size() == 1 && c_name.empty()) { + RCLCPP_WARN_ONCE( + get_logger(), + "No smoother was specified in action call." + " Server will use only plugin loaded %s. " + "This warning will appear once.", + smoother_ids_concat_.c_str()); + current_smoother = smoothers_.begin()->first; + } else { + RCLCPP_ERROR( + get_logger(), + "SmoothPath called with smoother name %s, " + "which does not exist. Available smoothers are: %s.", + c_name.c_str(), smoother_ids_concat_.c_str()); + return false; + } + } else { + RCLCPP_DEBUG(get_logger(), "Selected smoother: %s.", c_name.c_str()); + current_smoother = c_name; + } + + return true; +} + +void SmootherServer::smoothPlan() +{ + auto start_time = steady_clock_.now(); + + RCLCPP_INFO(get_logger(), "Received a path to smooth."); + + auto result = std::make_shared(); + try { + std::string c_name = action_server_->get_current_goal()->smoother_id; + std::string current_smoother; + if (findSmootherId(c_name, current_smoother)) { + current_smoother_ = current_smoother; + } else { + action_server_->terminate_current(); + return; + } + + // Perform smoothing + auto goal = action_server_->get_current_goal(); + result->path = goal->path; + result->was_completed = smoothers_[current_smoother_]->smooth( + result->path, goal->max_smoothing_duration); + result->smoothing_duration = steady_clock_.now() - start_time; + + if (!result->was_completed) { + RCLCPP_INFO( + get_logger(), + "Smoother %s did not complete smoothing in specified time limit" + "(%lf seconds) and was interrupted after %lf seconds", + current_smoother_.c_str(), + rclcpp::Duration(goal->max_smoothing_duration).seconds(), + rclcpp::Duration(result->smoothing_duration).seconds()); + } + plan_publisher_->publish(result->path); + + // Check for collisions + if (goal->check_for_collisions) { + geometry_msgs::msg::Pose2D pose2d; + bool fetch_data = true; + for (const auto & pose : result->path.poses) { + pose2d.x = pose.pose.position.x; + pose2d.y = pose.pose.position.y; + pose2d.theta = tf2::getYaw(pose.pose.orientation); + + if (!collision_checker_->isCollisionFree(pose2d, fetch_data)) { + RCLCPP_ERROR( + get_logger(), + "Smoothed path leads to a collision at x: %lf, y: %lf, theta: %lf", + pose2d.x, pose2d.y, pose2d.theta); + action_server_->terminate_current(result); + return; + } + fetch_data = false; + } + } + + RCLCPP_DEBUG( + get_logger(), "Smoother succeeded (time: %lf), setting result", + rclcpp::Duration(result->smoothing_duration).seconds()); + + action_server_->succeeded_current(result); + } catch (nav2_core::PlannerException & e) { + RCLCPP_ERROR(this->get_logger(), e.what()); + action_server_->terminate_current(); + return; + } +} + +} // namespace nav2_smoother + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_smoother::SmootherServer) diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp new file mode 100644 index 0000000000..59a81fd3b1 --- /dev/null +++ b/nav2_smoother/src/simple_smoother.cpp @@ -0,0 +1,299 @@ +// Copyright (c) 2022, 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 "nav2_smoother/simple_smoother.hpp" + +namespace nav2_smoother +{ +using namespace nav2_util::geometry_utils; // NOLINT +using namespace std::chrono; // NOLINT +using nav2_util::declare_parameter_if_not_declared; + +void SimpleSmoother::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr costmap_sub, + std::shared_ptr/*footprint_sub*/) +{ + costmap_sub_ = costmap_sub; + + auto node = parent.lock(); + logger_ = node->get_logger(); + + declare_parameter_if_not_declared( + node, name + ".tolerance", rclcpp::ParameterValue(1e-10)); + declare_parameter_if_not_declared( + node, name + ".max_its", rclcpp::ParameterValue(1000)); + declare_parameter_if_not_declared( + node, name + ".w_data", rclcpp::ParameterValue(0.2)); + declare_parameter_if_not_declared( + node, name + ".w_smooth", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, name + ".do_refinement", rclcpp::ParameterValue(true)); + + node->get_parameter(name + ".tolerance", tolerance_); + node->get_parameter(name + ".max_its", max_its_); + node->get_parameter(name + ".w_data", data_w_); + node->get_parameter(name + ".w_smooth", smooth_w_); + node->get_parameter(name + ".do_refinement", do_refinement_); +} + +bool SimpleSmoother::smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) +{ + auto costmap = costmap_sub_->getCostmap(); + + refinement_ctr_ = 0; + steady_clock::time_point start = steady_clock::now(); + double time_remaining = max_time.seconds(); + + bool success = true, reversing_segment; + nav_msgs::msg::Path curr_path_segment; + curr_path_segment.header = path.header; + + std::vector path_segments = findDirectionalPathSegments(path); + + for (unsigned int i = 0; i != path_segments.size(); i++) { + if (path_segments[i].end - path_segments[i].start > 9) { + // Populate path segment + curr_path_segment.poses.clear(); + std::copy( + path.poses.begin() + path_segments[i].start, + path.poses.begin() + path_segments[i].end + 1, + std::back_inserter(curr_path_segment.poses)); + + // Make sure we're still able to smooth with time remaining + steady_clock::time_point now = steady_clock::now(); + time_remaining = max_time.seconds() - duration_cast>(now - start).count(); + + // Smooth path segment naively + success = success && smoothImpl( + curr_path_segment, reversing_segment, costmap.get(), time_remaining); + + // Assemble the path changes to the main path + std::copy( + curr_path_segment.poses.begin(), + curr_path_segment.poses.end(), + path.poses.begin() + path_segments[i].start); + } + } + + return success; +} + +bool SimpleSmoother::smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time) +{ + 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_i_org; + unsigned int mx, my; + + 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_WARN( + logger_, + "Number of iterations has exceeded limit of %i.", max_its_); + path = last_path; + updateApproximatePathOrientations(path, reversing_segment); + 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_WARN( + logger_, + "Smoothing time exceeded allowed duration of %0.2f.", max_time); + path = last_path; + updateApproximatePathOrientations(path, reversing_segment); + return false; + } + + for (unsigned int i = 1; i != path_size - 1; 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; + + // Smooth based on local 3 point neighborhood and original data locations + 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 > nav2_costmap_2d::MAX_NON_OBSTACLE && cost != nav2_costmap_2d::NO_INFORMATION) { + 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, reversing_segment); + return false; + } + } + + last_path = new_path; + } + + // 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_ && refinement_ctr_ < 4) { + refinement_ctr_++; + smoothImpl(new_path, reversing_segment, costmap, max_time); + } + + updateApproximatePathOrientations(new_path, reversing_segment); + path = new_path; + return true; +} + +double SimpleSmoother::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; + } +} + +void SimpleSmoother::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; + } +} + +std::vector SimpleSmoother::findDirectionalPathSegments( + const nav_msgs::msg::Path & path) +{ + std::vector segments; + PathSegment curr_segment; + curr_segment.start = 0; + + // Iterating through the path to determine the position of the cusp + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + double oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + double ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + double ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existance of cusp, in the path, using the dot product. + double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + + // Checking for the existance of a differential rotation in place. + double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); + double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); + double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); + if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + } + + curr_segment.end = path.poses.size() - 1; + segments.push_back(curr_segment); + return segments; +} + +void SimpleSmoother::updateApproximatePathOrientations( + nav_msgs::msg::Path & path, + bool & reversing_segment) +{ + double dx, dy, theta, pt_yaw; + reversing_segment = false; + + // Find if this path segment is in reverse + dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; + dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; + theta = atan2(dy, dx); + pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); + if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { + reversing_segment = true; + } + + // Find the angle relative the path position vectors + 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); + + // If points are overlapping, pass + if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { + continue; + } + + // Flip the angle if this path segment is in reverse + if (reversing_segment) { + theta += M_PI; // orientationAroundZAxis will normalize + } + + path.poses[i].pose.orientation = orientationAroundZAxis(theta); + } +} + +} // namespace nav2_smoother + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_smoother::SimpleSmoother, nav2_core::Smoother) diff --git a/nav2_smoother/test/CMakeLists.txt b/nav2_smoother/test/CMakeLists.txt new file mode 100644 index 0000000000..67c98e3e43 --- /dev/null +++ b/nav2_smoother/test/CMakeLists.txt @@ -0,0 +1,23 @@ +ament_add_gtest(test_smoother_server + test_smoother_server.cpp +) + +target_link_libraries(test_smoother_server + ${library_name} +) + +ament_target_dependencies(test_smoother_server + ${dependencies} +) + +ament_add_gtest(test_simple_smoother + test_simple_smoother.cpp +) + +target_link_libraries(test_simple_smoother + simple_smoother +) + +ament_target_dependencies(test_simple_smoother + ${dependencies} +) diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp new file mode 100644 index 0000000000..fccc2c2a7c --- /dev/null +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -0,0 +1,279 @@ +// Copyright (c) 2022, 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_msgs/msg/costmap.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_smoother/simple_smoother.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +using namespace nav2_smoother; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class SmootherWrapper : public nav2_smoother::SimpleSmoother +{ +public: + SmootherWrapper() + : nav2_smoother::SimpleSmoother() + { + } + + std::vector findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path) + { + return findDirectionalPathSegments(path); + } + + void setMaxItsToInvalid() + { + max_its_ = 0; + } +}; + +TEST(SmootherTest, test_simple_smoother) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + // island in the middle of lethal cost to cross + for (unsigned int i = 20; i <= 30; ++i) { + for (unsigned int j = 20; j <= 30; ++j) { + costmap_msg->data[j * 100 + i] = 254; + } + } + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + + // Test that an irregular distributed path becomes more distributed + nav_msgs::msg::Path straight_irregular_path; + straight_irregular_path.header.frame_id = "map"; + straight_irregular_path.header.stamp = node->now(); + straight_irregular_path.poses.resize(11); + straight_irregular_path.poses[0].pose.position.x = 0.5; + straight_irregular_path.poses[0].pose.position.y = 0.0; + straight_irregular_path.poses[1].pose.position.x = 0.5; + straight_irregular_path.poses[1].pose.position.y = 0.1; + straight_irregular_path.poses[2].pose.position.x = 0.5; + straight_irregular_path.poses[2].pose.position.y = 0.2; + straight_irregular_path.poses[3].pose.position.x = 0.5; + straight_irregular_path.poses[3].pose.position.y = 0.35; + straight_irregular_path.poses[4].pose.position.x = 0.5; + straight_irregular_path.poses[4].pose.position.y = 0.4; + straight_irregular_path.poses[5].pose.position.x = 0.5; + straight_irregular_path.poses[5].pose.position.y = 0.56; + straight_irregular_path.poses[6].pose.position.x = 0.5; + straight_irregular_path.poses[6].pose.position.y = 0.9; + straight_irregular_path.poses[7].pose.position.x = 0.5; + straight_irregular_path.poses[7].pose.position.y = 0.95; + straight_irregular_path.poses[8].pose.position.x = 0.5; + straight_irregular_path.poses[8].pose.position.y = 1.3; + straight_irregular_path.poses[9].pose.position.x = 0.5; + straight_irregular_path.poses[9].pose.position.y = 2.0; + straight_irregular_path.poses[10].pose.position.x = 0.5; + straight_irregular_path.poses[10].pose.position.y = 2.5; + + rclcpp::Duration no_time = rclcpp::Duration::from_seconds(0.0); // 0 seconds + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1); // 1 second + EXPECT_FALSE(smoother->smooth(straight_irregular_path, no_time)); + EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time)); + for (uint i = 0; i != straight_irregular_path.poses.size() - 1; i++) { + // Check distances are more evenly spaced out now + EXPECT_LT( + fabs( + straight_irregular_path.poses[i].pose.position.y - + straight_irregular_path.poses[i + 1].pose.position.y), 0.38); + } + + // Test regular path, should see no effective change + nav_msgs::msg::Path straight_regular_path; + straight_regular_path.header = straight_irregular_path.header; + straight_regular_path.poses.resize(11); + straight_regular_path.poses[0].pose.position.x = 0.5; + straight_regular_path.poses[0].pose.position.y = 0.0; + straight_regular_path.poses[1].pose.position.x = 0.5; + straight_regular_path.poses[1].pose.position.y = 0.1; + straight_regular_path.poses[2].pose.position.x = 0.5; + straight_regular_path.poses[2].pose.position.y = 0.2; + straight_regular_path.poses[3].pose.position.x = 0.5; + straight_regular_path.poses[3].pose.position.y = 0.3; + straight_regular_path.poses[4].pose.position.x = 0.5; + straight_regular_path.poses[4].pose.position.y = 0.4; + straight_regular_path.poses[5].pose.position.x = 0.5; + straight_regular_path.poses[5].pose.position.y = 0.5; + straight_regular_path.poses[6].pose.position.x = 0.5; + straight_regular_path.poses[6].pose.position.y = 0.6; + straight_regular_path.poses[7].pose.position.x = 0.5; + straight_regular_path.poses[7].pose.position.y = 0.7; + straight_regular_path.poses[8].pose.position.x = 0.5; + straight_regular_path.poses[8].pose.position.y = 0.8; + straight_regular_path.poses[9].pose.position.x = 0.5; + straight_regular_path.poses[9].pose.position.y = 0.9; + straight_regular_path.poses[10].pose.position.x = 0.5; + straight_regular_path.poses[10].pose.position.y = 1.0; + + EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time)); + for (uint i = 0; i != straight_regular_path.poses.size() - 1; i++) { + // Check distances are still very evenly spaced + EXPECT_NEAR( + fabs( + straight_regular_path.poses[i].pose.position.y - + straight_regular_path.poses[i + 1].pose.position.y), 0.1, 0.001); + } + + // test shorter and curved if given a right angle + nav_msgs::msg::Path right_angle_path; + right_angle_path = straight_regular_path; + straight_regular_path.poses[6].pose.position.x = 0.6; + straight_regular_path.poses[6].pose.position.y = 0.5; + straight_regular_path.poses[7].pose.position.x = 0.7; + straight_regular_path.poses[7].pose.position.y = 0.5; + straight_regular_path.poses[8].pose.position.x = 0.8; + straight_regular_path.poses[8].pose.position.y = 0.5; + straight_regular_path.poses[9].pose.position.x = 0.9; + straight_regular_path.poses[9].pose.position.y = 0.5; + straight_regular_path.poses[10].pose.position.x = 0.95; + straight_regular_path.poses[10].pose.position.y = 0.5; + EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time)); + EXPECT_NEAR(straight_regular_path.poses[5].pose.position.x, 0.637, 0.01); + EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.353, 0.01); + + // Test that collisions are rejected + nav_msgs::msg::Path collision_path; + collision_path.poses.resize(11); + collision_path.poses[0].pose.position.x = 0.0; + collision_path.poses[0].pose.position.y = 0.0; + collision_path.poses[1].pose.position.x = 0.2; + collision_path.poses[1].pose.position.y = 0.2; + collision_path.poses[2].pose.position.x = 0.4; + collision_path.poses[2].pose.position.y = 0.4; + collision_path.poses[3].pose.position.x = 0.6; + collision_path.poses[3].pose.position.y = 0.6; + collision_path.poses[4].pose.position.x = 0.8; + collision_path.poses[4].pose.position.y = 0.8; + collision_path.poses[5].pose.position.x = 1.0; + collision_path.poses[5].pose.position.y = 1.0; + collision_path.poses[6].pose.position.x = 1.1; + collision_path.poses[6].pose.position.y = 1.1; + collision_path.poses[7].pose.position.x = 1.2; + collision_path.poses[7].pose.position.y = 1.2; + collision_path.poses[8].pose.position.x = 1.3; + collision_path.poses[8].pose.position.y = 1.3; + collision_path.poses[9].pose.position.x = 1.4; + collision_path.poses[9].pose.position.y = 1.4; + collision_path.poses[10].pose.position.x = 1.5; + collision_path.poses[10].pose.position.y = 1.5; + EXPECT_FALSE(smoother->smooth(collision_path, max_time)); + + // test cusp / reversing segments + nav_msgs::msg::Path reversing_path; + reversing_path.poses.resize(11); + reversing_path.poses[0].pose.position.x = 0.5; + reversing_path.poses[0].pose.position.y = 0.0; + reversing_path.poses[1].pose.position.x = 0.5; + reversing_path.poses[1].pose.position.y = 0.1; + reversing_path.poses[2].pose.position.x = 0.5; + reversing_path.poses[2].pose.position.y = 0.2; + reversing_path.poses[3].pose.position.x = 0.5; + reversing_path.poses[3].pose.position.y = 0.3; + reversing_path.poses[4].pose.position.x = 0.5; + reversing_path.poses[4].pose.position.y = 0.4; + reversing_path.poses[5].pose.position.x = 0.5; + reversing_path.poses[5].pose.position.y = 0.5; + reversing_path.poses[6].pose.position.x = 0.5; + reversing_path.poses[6].pose.position.y = 0.4; + reversing_path.poses[7].pose.position.x = 0.5; + reversing_path.poses[7].pose.position.y = 0.3; + reversing_path.poses[8].pose.position.x = 0.5; + reversing_path.poses[8].pose.position.y = 0.2; + reversing_path.poses[9].pose.position.x = 0.5; + reversing_path.poses[9].pose.position.y = 0.1; + reversing_path.poses[10].pose.position.x = 0.5; + reversing_path.poses[10].pose.position.y = 0.0; + EXPECT_TRUE(smoother->smooth(reversing_path, max_time)); + + // // test rotate in place + tf2::Quaternion quat1, quat2; + quat1.setRPY(0.0, 0.0, 0.0); + quat2.setRPY(0.0, 0.0, 1.0); + straight_irregular_path.poses[5].pose.position.x = 0.5; + straight_irregular_path.poses[5].pose.position.y = 0.5; + straight_irregular_path.poses[5].pose.orientation = tf2::toMsg(quat1); + straight_irregular_path.poses[6].pose.position.x = 0.5; + straight_irregular_path.poses[6].pose.position.y = 0.5; + straight_irregular_path.poses[6].pose.orientation = tf2::toMsg(quat2); + EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time)); + + // test max iterations + smoother->setMaxItsToInvalid(); + nav_msgs::msg::Path max_its_path; + max_its_path.poses.resize(11); + max_its_path.poses[0].pose.position.x = 0.5; + max_its_path.poses[0].pose.position.y = 0.0; + max_its_path.poses[1].pose.position.x = 0.5; + max_its_path.poses[1].pose.position.y = 0.1; + max_its_path.poses[2].pose.position.x = 0.5; + max_its_path.poses[2].pose.position.y = 0.2; + max_its_path.poses[3].pose.position.x = 0.5; + max_its_path.poses[3].pose.position.y = 0.3; + max_its_path.poses[4].pose.position.x = 0.5; + max_its_path.poses[4].pose.position.y = 0.4; + max_its_path.poses[5].pose.position.x = 0.5; + max_its_path.poses[5].pose.position.y = 0.5; + max_its_path.poses[6].pose.position.x = 0.5; + max_its_path.poses[6].pose.position.y = 0.6; + max_its_path.poses[7].pose.position.x = 0.5; + max_its_path.poses[7].pose.position.y = 0.7; + max_its_path.poses[8].pose.position.x = 0.5; + max_its_path.poses[8].pose.position.y = 0.8; + max_its_path.poses[9].pose.position.x = 0.5; + max_its_path.poses[9].pose.position.y = 0.9; + max_its_path.poses[10].pose.position.x = 0.5; + max_its_path.poses[10].pose.position.y = 1.0; + EXPECT_FALSE(smoother->smooth(max_its_path, max_time)); +} diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp new file mode 100644 index 0000000000..8dbc5416db --- /dev/null +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -0,0 +1,444 @@ +// Copyright (c) 2021 RoboTech Vision +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" + +#include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_core/smoother.hpp" +#include "nav2_core/exceptions.hpp" +#include "nav2_msgs/action/smooth_path.hpp" +#include "nav2_smoother/nav2_smoother.hpp" +#include "tf2_ros/create_timer_ros.h" + +using SmoothAction = nav2_msgs::action::SmoothPath; +using ClientGoalHandle = rclcpp_action::ClientGoalHandle; + +using namespace std::chrono_literals; + +// A smoother for testing the base class + +class DummySmoother : public nav2_core::Smoother +{ +public: + DummySmoother() + : initialized_(false) {} + + ~DummySmoother() {} + + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string, std::shared_ptr, + std::shared_ptr, + std::shared_ptr) {} + + virtual void cleanup() {} + + virtual void activate() {} + + virtual void deactivate() {} + + virtual bool smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) + { + assert(path.poses.size() == 2); + + if (path.poses.front() == path.poses.back()) { + throw nav2_core::PlannerException("Start and goal pose must differ"); + } + + auto max_time_ms = max_time.to_chrono(); + std::this_thread::sleep_for(std::min(max_time_ms, 100ms)); + + // place dummy pose in the middle of the path + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = + (path.poses.front().pose.position.x + path.poses.back().pose.position.x) / 2; + pose.pose.position.y = + (path.poses.front().pose.position.y + path.poses.back().pose.position.y) / 2; + pose.pose.orientation.w = 1.0; + path.poses.push_back(pose); + + return max_time_ms > 100ms; + } + +private: + bool initialized_; + std::string command_; + std::chrono::system_clock::time_point start_time_; +}; + +// Mocked class loader +void onPluginDeletion(nav2_core::Smoother * obj) +{ + if (nullptr != obj) { + delete (obj); + } +} + +template<> +pluginlib::UniquePtr pluginlib::ClassLoader:: +createUniqueInstance(const std::string & lookup_name) +{ + if (lookup_name != "DummySmoother") { + // original method body + if (!isClassLoaded(lookup_name)) { + loadLibraryForClass(lookup_name); + } + try { + std::string class_type = getClassType(lookup_name); + pluginlib::UniquePtr obj = + lowlevel_class_loader_.createUniqueInstance(class_type); + return obj; + } catch (const class_loader::CreateClassException & ex) { + throw pluginlib::CreateClassException(ex.what()); + } + } + + // mocked plugin creation + return std::unique_ptr>( + new DummySmoother(), + onPluginDeletion); +} + +class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber +{ +public: + DummyCostmapSubscriber( + nav2_util::LifecycleNode::SharedPtr node, + const std::string & topic_name) + : CostmapSubscriber(node, topic_name) + { + auto costmap = std::make_shared(); + costmap->metadata.size_x = 100; + costmap->metadata.size_y = 100; + costmap->metadata.resolution = 0.1; + costmap->metadata.origin.position.x = -5.0; + costmap->metadata.origin.position.y = -5.0; + + costmap->data.resize(costmap->metadata.size_x * costmap->metadata.size_y, 0); + for (unsigned int i = 0; i < costmap->metadata.size_y; ++i) { + for (unsigned int j = 20; j < 40; ++j) { + costmap->data[i * costmap->metadata.size_x + j] = 254; + } + } + + setCostmap(costmap); + } + + void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg) + { + costmap_msg_ = msg; + costmap_received_ = true; + } +}; + +class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber +{ +public: + DummyFootprintSubscriber( + nav2_util::LifecycleNode::SharedPtr node, + const std::string & topic_name, + tf2_ros::Buffer & tf_) + : FootprintSubscriber(node, topic_name, tf_) + { + auto footprint = std::make_shared(); + footprint->header.frame_id = "base_link"; // global frame = robot frame to avoid tf lookup + footprint->header.stamp = node->get_clock()->now(); + geometry_msgs::msg::Point32 point; + point.x = -0.2f; + point.y = -0.2f; + footprint->polygon.points.push_back(point); + point.y = 0.2f; + footprint->polygon.points.push_back(point); + point.x = 0.2f; + point.y = 0.0f; + footprint->polygon.points.push_back(point); + + setFootprint(footprint); + } + + void setFootprint(geometry_msgs::msg::PolygonStamped::SharedPtr msg) + { + footprint_ = msg; + footprint_received_ = true; + } +}; + +class DummySmootherServer : public nav2_smoother::SmootherServer +{ +public: + DummySmootherServer() + { + // Override defaults + default_ids_.clear(); + default_ids_.resize(1, "SmoothPath"); + set_parameter(rclcpp::Parameter("smoother_plugins", default_ids_)); + default_types_.clear(); + default_types_.resize(1, "DummySmoother"); + } + + nav2_util::CallbackReturn + on_configure(const rclcpp_lifecycle::State & state) + { + auto result = SmootherServer::on_configure(state); + if (result != nav2_util::CallbackReturn::SUCCESS) { + return result; + } + + // Create dummy subscribers and collision checker + auto node = shared_from_this(); + costmap_sub_ = + std::make_shared( + node, "costmap_topic"); + footprint_sub_ = + std::make_shared( + node, "footprint_topic", *tf_); + collision_checker_ = + std::make_shared( + *costmap_sub_, *footprint_sub_, + node->get_name()); + + return result; + } +}; + +// Define a test class to hold the context for the tests + +class SmootherConfigTest : public ::testing::Test +{ +}; + +class SmootherTest : public ::testing::Test +{ +protected: + SmootherTest() {SetUp();} + ~SmootherTest() {} + + void SetUp() + { + node_lifecycle_ = + std::make_shared( + "LifecycleSmootherTestNode", rclcpp::NodeOptions()); + + smoother_server_ = std::make_shared(); + smoother_server_->set_parameter( + rclcpp::Parameter( + "smoother_plugins", + rclcpp::ParameterValue(std::vector(1, "DummySmoothPath")))); + smoother_server_->declare_parameter( + "DummySmoothPath.plugin", + rclcpp::ParameterValue(std::string("DummySmoother"))); + smoother_server_->configure(); + smoother_server_->activate(); + + client_ = rclcpp_action::create_client( + node_lifecycle_->get_node_base_interface(), + node_lifecycle_->get_node_graph_interface(), + node_lifecycle_->get_node_logging_interface(), + node_lifecycle_->get_node_waitables_interface(), "smooth_path"); + std::cout << "Setup complete." << std::endl; + } + + void TearDown() override + { + smoother_server_->deactivate(); + smoother_server_->cleanup(); + smoother_server_->shutdown(); + } + + bool sendGoal( + std::string smoother_id, double x_start, double y_start, double x_goal, + double y_goal, std::chrono::milliseconds max_time, bool check_for_collisions) + { + if (!client_->wait_for_action_server(4s)) { + std::cout << "Server not up" << std::endl; + return false; + } + + geometry_msgs::msg::PoseStamped pose; + pose.pose.orientation.w = 1.0; + + auto goal = SmoothAction::Goal(); + goal.smoother_id = smoother_id; + pose.pose.position.x = x_start; + pose.pose.position.y = y_start; + goal.path.poses.push_back(pose); + pose.pose.position.x = x_goal; + pose.pose.position.y = y_goal; + goal.path.poses.push_back(pose); + goal.check_for_collisions = check_for_collisions; + goal.max_smoothing_duration = rclcpp::Duration(max_time); + + auto future_goal = client_->async_send_goal(goal); + + if (rclcpp::spin_until_future_complete(node_lifecycle_, future_goal) != + rclcpp::FutureReturnCode::SUCCESS) + { + std::cout << "failed sending goal" << std::endl; + // failed sending the goal + return false; + } + + goal_handle_ = future_goal.get(); + + if (!goal_handle_) { + std::cout << "goal was rejected" << std::endl; + // goal was rejected by the action server + return false; + } + + return true; + } + + ClientGoalHandle::WrappedResult getResult() + { + std::cout << "Getting async result..." << std::endl; + auto future_result = client_->async_get_result(goal_handle_); + std::cout << "Waiting on future..." << std::endl; + rclcpp::spin_until_future_complete(node_lifecycle_, future_result); + std::cout << "future received!" << std::endl; + return future_result.get(); + } + + std::shared_ptr node_lifecycle_; + std::shared_ptr smoother_server_; + std::shared_ptr> client_; + std::shared_ptr> goal_handle_; +}; + +// Define the tests + +TEST_F(SmootherTest, testingSuccess) +{ + ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 1.0, 0.0, 500ms, true)); + auto result = getResult(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); + EXPECT_EQ(result.result->path.poses.size(), (std::size_t)3); + EXPECT_TRUE(result.result->was_completed); + SUCCEED(); +} + +TEST_F(SmootherTest, testingFailureOnInvalidSmootherId) +{ + ASSERT_TRUE(sendGoal("InvalidSmoother", 0.0, 0.0, 1.0, 0.0, 500ms, true)); + auto result = getResult(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); + SUCCEED(); +} + +TEST_F(SmootherTest, testingSuccessOnEmptyPlugin) +{ + ASSERT_TRUE(sendGoal("", 0.0, 0.0, 1.0, 0.0, 500ms, true)); + auto result = getResult(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); + SUCCEED(); +} + +TEST_F(SmootherTest, testingIncomplete) +{ + ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 1.0, 0.0, 50ms, true)); + auto result = getResult(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); + EXPECT_FALSE(result.result->was_completed); + SUCCEED(); +} + +TEST_F(SmootherTest, testingFailureOnException) +{ + ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 0.0, 0.0, 500ms, true)); + auto result = getResult(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); + SUCCEED(); +} + +TEST_F(SmootherTest, testingFailureOnCollision) +{ + ASSERT_TRUE(sendGoal("DummySmoothPath", -4.0, 0.0, 0.0, 0.0, 500ms, true)); + auto result = getResult(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED); + SUCCEED(); +} + +TEST_F(SmootherTest, testingCollisionCheckDisabled) +{ + ASSERT_TRUE(sendGoal("DummySmoothPath", -4.0, 0.0, 0.0, 0.0, 500ms, false)); + auto result = getResult(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); + SUCCEED(); +} + +TEST_F(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) +{ + auto smoother_server = std::make_shared(); + smoother_server->set_parameter( + rclcpp::Parameter( + "smoother_plugins", + rclcpp::ParameterValue(std::vector(1, "DummySmoothPath")))); + smoother_server->declare_parameter( + "DummySmoothPath.plugin", + rclcpp::ParameterValue(std::string("DummySmoother"))); + auto state = smoother_server->configure(); + EXPECT_EQ(state.id(), 2); // 1 on failure, 2 on success + SUCCEED(); +} + +TEST_F(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) +{ + auto smoother_server = std::make_shared(); + smoother_server->set_parameter( + rclcpp::Parameter( + "smoother_plugins", + rclcpp::ParameterValue(std::vector(1, "DummySmoothPath")))); + smoother_server->declare_parameter( + "DummySmoothPath.plugin", + rclcpp::ParameterValue(std::string("InvalidSmootherPlugin"))); + auto state = smoother_server->configure(); + EXPECT_EQ(state.id(), 1); // 1 on failure, 2 on success + SUCCEED(); +} + +TEST_F(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin) +{ + auto smoother_server = std::make_shared(); + auto state = smoother_server->configure(); + EXPECT_EQ(state.id(), 2); // 1 on failure, 2 on success + SUCCEED(); +} + +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_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 8d67f4a978..6cc6a32e49 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,15 +54,18 @@ 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) add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) - add_subdirectory(src/recoveries/spin) - add_subdirectory(src/recoveries/wait) - add_subdirectory(src/recoveries/backup) + add_subdirectory(src/behaviors/spin) + add_subdirectory(src/behaviors/wait) + add_subdirectory(src/behaviors/backup) + add_subdirectory(src/behaviors/drive_on_heading) + add_subdirectory(src/costmap_filters) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() diff --git a/nav2_system_tests/README.md b/nav2_system_tests/README.md index c210085882..b40cba4ec4 100644 --- a/nav2_system_tests/README.md +++ b/nav2_system_tests/README.md @@ -1,36 +1,14 @@ # System Tests -The package provides tests for [components](#1.-Component-Testing), [subsystems](#2.-Subsystem-Testing) and full [system](#3.-System-Testing) integration. +The package provides tests for Component-Testing, Subsystem-Testing, and Full-System integration. Its main goal is to provide a location for smoke and integration tests of the navigation system to ensure that things are working properly on a high level. Unit and specific subsystem testing happens in the packages specific to those algorithms. -Unit tests are not included, these should be provided within each component or package. +Most tests in this package will spin up Gazebo instances of a robot in an environment to have the robot complete some task in the space while tracking a specific modules results. Some examples include -## 1. Component Testing -Test a component's ROS API (Pub/Sub/Service). +- System tests of a robot in a sandbox environment trying navigate to pose, navigate through poses, and waypoint following navigation types +- Random planning of thousands of paths in a generated environment to ensure default planners are working properly +- Testing the system can be brought up and down on the lifecycle transitions successfully multiple times +- Testing that the keepout and speed restricted zones work in a practical environment without going into keepout zones and slowing in speed restricted areas +- Testing behaviors in a sandbox environment to ensure they trigger and complete collision checking properly +- Testing system failures are properly recorded and can be recovered from -- [Global Planning](src/planning/README.md) - -- Controller - -- [Localization](src/localization/README.md) - -- World Model - -- Costmaps - -## 2. Subsystem Testing -Test the integration of several components and subsystems. - -- Support modules (Mapping, Perception, Prediction, Localization - -- Navigation core (Navigator, Planner, Controller) - -- Support modules and navigation core - -- Command chain (Mission Planning, Mission Execution, Navigation System, Robot Interface) - -## 3. System Testing -Test the lifecycle startup and shutdown of nodes. - - [Updown Test](src/updown/README.md) - -Test the integration of all subsystems. - - [System Test](src/system/README.md) +This is primarily for use in Nav2 CI to establish a high degree of maintainer confidence when merging in large architectural changes to the Nav2 project. However, this is also useful to test installs of Nav2 locally or for additional information. diff --git a/nav2_system_tests/maps/keepout_mask.pgm b/nav2_system_tests/maps/keepout_mask.pgm new file mode 100644 index 0000000000..f305cb948b --- /dev/null +++ b/nav2_system_tests/maps/keepout_mask.pgm @@ -0,0 +1,5 @@ +P5 +# Created by GIMP version 2.10.18 PNM plug-in +384 384 +255 + \ No newline at end of file diff --git a/nav2_system_tests/maps/keepout_mask.yaml b/nav2_system_tests/maps/keepout_mask.yaml new file mode 100644 index 0000000000..aea4f80b8e --- /dev/null +++ b/nav2_system_tests/maps/keepout_mask.yaml @@ -0,0 +1,6 @@ +image: keepout_mask.pgm +resolution: 0.050000 +origin: [-10.000000, -10.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/nav2_system_tests/maps/speed_mask.pgm b/nav2_system_tests/maps/speed_mask.pgm new file mode 100644 index 0000000000..c5e9d4e8d2 --- /dev/null +++ b/nav2_system_tests/maps/speed_mask.pgm @@ -0,0 +1,5 @@ +P5 +# Created by GIMP version 2.10.18 PNM plug-in +384 384 +255 + \ No newline at end of file diff --git a/nav2_system_tests/maps/speed_mask.yaml b/nav2_system_tests/maps/speed_mask.yaml new file mode 100644 index 0000000000..e32df2e6be --- /dev/null +++ b/nav2_system_tests/maps/speed_mask.yaml @@ -0,0 +1,7 @@ +image: speed_mask.pgm +mode: scale +resolution: 0.050000 +origin: [-10.000000, -10.000000, 0.000000] +negate: 0 +occupied_thresh: 0.99 +free_thresh: 0.01 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 3f4ce11b0a..63c52ae919 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 @@ -476,6 +477,7 @@ + /tf:=tf 30 diff --git a/nav2_system_tests/models/turtlebot3_waffle/model.sdf b/nav2_system_tests/models/turtlebot3_waffle/model.sdf index 897d69699c..42701b5ee5 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 @@ -476,6 +477,7 @@ + /tf:=tf 30 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 26b923b508..bf47571cfd 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 @@ -409,6 +410,7 @@ + /tf:=tf 30 diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 01a41e39d0..0940173e04 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.7 + 1.1.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 3a3fc36d49..29f0e27828 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 0000000000..19eca116e0 --- /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 0000000000..ce8d26599e --- /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 0000000000..2108f37d58 --- /dev/null +++ b/nav2_system_tests/src/behavior_tree/server_handler.cpp @@ -0,0 +1,96 @@ +// 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"); + drive_on_heading_server = std::make_unique>( + node_, "drive_on_heading"); + 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(); + drive_on_heading_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 0000000000..f8869ea037 --- /dev/null +++ b/nav2_system_tests/src/behavior_tree/server_handler.hpp @@ -0,0 +1,108 @@ +// 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/drive_on_heading.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> drive_on_heading_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 0000000000..2f76823ae4 --- /dev/null +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -0,0 +1,643 @@ +// 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_smooth_path_action_bt_node", + "nav2_follow_path_action_bt_node", + "nav2_spin_action_bt_node", + "nav2_wait_action_bt_node", + "nav2_back_up_action_bt_node", + "nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node", + "nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition", + "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", + "nav2_controller_cancel_bt_node", + "nav2_path_longer_on_approach_bt_node", + "nav2_wait_cancel_bt_node", + "nav2_spin_cancel_bt_node", + "nav2_back_up_cancel_bt_node", + "nav2_drive_on_heading_cancel_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/behaviors/README.md b/nav2_system_tests/src/behaviors/README.md new file mode 100644 index 0000000000..e1ac535ad5 --- /dev/null +++ b/nav2_system_tests/src/behaviors/README.md @@ -0,0 +1,4 @@ +# Behavior Test + +Provides some simple tests for behavior plugins. +It creates an instance of the stack, with the behavior server loading different behavior plugins, and checks for successful behavior behaviors. diff --git a/nav2_system_tests/src/behaviors/backup/CMakeLists.txt b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt new file mode 100644 index 0000000000..05afe3c50d --- /dev/null +++ b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_backup_behavior test_backup_behavior_node) + +ament_add_gtest_executable(${test_backup_behavior} + test_backup_behavior_node.cpp + backup_behavior_tester.cpp +) + +ament_target_dependencies(${test_backup_behavior} + ${dependencies} +) + +ament_add_test(test_backup_recovery + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_behavior_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp similarity index 87% rename from nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp rename to nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp index 290deff031..a270c8ab08 100644 --- a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp +++ b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp @@ -22,7 +22,7 @@ #include #include -#include "backup_recovery_tester.hpp" +#include "backup_behavior_tester.hpp" #include "nav2_util/geometry_utils.hpp" using namespace std::chrono_literals; @@ -31,11 +31,11 @@ using namespace std::chrono; // NOLINT namespace nav2_system_tests { -BackupRecoveryTester::BackupRecoveryTester() +BackupBehaviorTester::BackupBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("backup_recovery_test"); + node_ = rclcpp::Node::make_shared("backup_behavior_test"); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -52,17 +52,19 @@ BackupRecoveryTester::BackupRecoveryTester() subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&BackupRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); + std::bind(&BackupBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); + + stamp_ = node_->now(); } -BackupRecoveryTester::~BackupRecoveryTester() +BackupBehaviorTester::~BackupBehaviorTester() { if (is_active_) { deactivate(); } } -void BackupRecoveryTester::activate() +void BackupBehaviorTester::activate() { if (is_active_) { throw std::runtime_error("Trying to activate while already active"); @@ -76,7 +78,7 @@ void BackupRecoveryTester::activate() rclcpp::spin_some(node_); } - // Wait for lifecycle_manager_navigation to activate recoveries_server + // Wait for lifecycle_manager_navigation to activate behavior_server std::this_thread::sleep_for(10s); if (!client_ptr_) { @@ -95,7 +97,7 @@ void BackupRecoveryTester::activate() is_active_ = true; } -void BackupRecoveryTester::deactivate() +void BackupBehaviorTester::deactivate() { if (!is_active_) { throw std::runtime_error("Trying to deactivate while already inactive"); @@ -103,8 +105,8 @@ void BackupRecoveryTester::deactivate() is_active_ = false; } -bool BackupRecoveryTester::defaultBackupRecoveryTest( - const float target_dist, +bool BackupBehaviorTester::defaultBackupBehaviorTest( + const BackUp::Goal goal_msg, const double tolerance) { if (!is_active_) { @@ -112,13 +114,9 @@ bool BackupRecoveryTester::defaultBackupRecoveryTest( return false; } - // Sleep to let recovery server be ready for serving in multiple runs + // Sleep to let behavior server be ready for serving in multiple runs std::this_thread::sleep_for(5s); - auto goal_msg = BackUp::Goal(); - goal_msg.target.x = target_dist; - goal_msg.speed = 0.2; - RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); geometry_msgs::msg::PoseStamped initial_pose; @@ -180,22 +178,22 @@ bool BackupRecoveryTester::defaultBackupRecoveryTest( double dist = nav2_util::geometry_utils::euclidean_distance(initial_pose, current_pose); - if (fabs(dist) > fabs(target_dist) + tolerance) { + if (fabs(dist) > fabs(goal_msg.target.x) + tolerance) { RCLCPP_ERROR( node_->get_logger(), "Distance from goal is %lf (tolerance %lf)", - fabs(dist - target_dist), tolerance); + fabs(dist - goal_msg.target.x), tolerance); return false; } return true; } -void BackupRecoveryTester::sendInitialPose() +void BackupBehaviorTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; - pose.header.stamp = rclcpp::Time(); + pose.header.stamp = stamp_; pose.pose.pose.position.x = -2.0; pose.pose.pose.position.y = -0.5; pose.pose.pose.position.z = 0.0; @@ -214,7 +212,7 @@ void BackupRecoveryTester::sendInitialPose() RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); } -void BackupRecoveryTester::amclPoseCallback( +void BackupBehaviorTester::amclPoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) { initial_pose_received_ = true; diff --git a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp similarity index 85% rename from nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp rename to nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp index 4c658860a0..bf42c9881a 100644 --- a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp +++ b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_ -#define RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_ +#ifndef BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_ #include #include @@ -38,19 +38,19 @@ namespace nav2_system_tests { -class BackupRecoveryTester +class BackupBehaviorTester { public: using BackUp = nav2_msgs::action::BackUp; using GoalHandleBackup = rclcpp_action::ClientGoalHandle; - BackupRecoveryTester(); - ~BackupRecoveryTester(); + BackupBehaviorTester(); + ~BackupBehaviorTester(); // Runs a single test with given target yaw - bool defaultBackupRecoveryTest( - float target_dist, - double tolerance = 0.1); + bool defaultBackupBehaviorTest( + const BackUp::Goal goal_msg, + const double tolerance); void activate(); @@ -68,6 +68,7 @@ class BackupRecoveryTester bool is_active_; bool initial_pose_received_; + rclcpp::Time stamp_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; @@ -86,4 +87,4 @@ class BackupRecoveryTester } // namespace nav2_system_tests -#endif // RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_ +#endif // BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py similarity index 92% rename from nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py rename to nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py index 7f23662f2e..bf7f4f04f1 100755 --- a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_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( @@ -59,13 +60,13 @@ def generate_launch_description(): # using a local copy of TB3 urdf file Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), @@ -88,7 +89,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], - name='test_backup_recovery_node', + name='test_backup_behavior_node', output='screen') lts = LaunchTestService() diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp new file mode 100644 index 0000000000..e629dca0bb --- /dev/null +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp @@ -0,0 +1,128 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "backup_behavior_tester.hpp" +#include "nav2_msgs/action/back_up.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::BackupBehaviorTester; + +struct TestParameters +{ + float x; + float y; + float speed; + float tolerance; +}; + + +std::string testNameGenerator(const testing::TestParamInfo &) +{ + static int test_index = 0; + std::string name = "BackUpTest" + std::to_string(test_index); + ++test_index; + return name; +} + +class BackupBehaviorTestFixture + : public ::testing::TestWithParam +{ +public: + static void SetUpTestCase() + { + backup_behavior_tester = new BackupBehaviorTester(); + if (!backup_behavior_tester->isActive()) { + backup_behavior_tester->activate(); + } + } + + static void TearDownTestCase() + { + delete backup_behavior_tester; + backup_behavior_tester = nullptr; + } + +protected: + static BackupBehaviorTester * backup_behavior_tester; +}; + +BackupBehaviorTester * BackupBehaviorTestFixture::backup_behavior_tester = nullptr; + +TEST_P(BackupBehaviorTestFixture, testBackupBehavior) +{ + auto test_params = GetParam(); + auto goal = nav2_msgs::action::BackUp::Goal(); + goal.target.x = test_params.x; + goal.target.y = test_params.y; + goal.speed = test_params.speed; + float tolerance = test_params.tolerance; + + if (!backup_behavior_tester->isActive()) { + backup_behavior_tester->activate(); + } + + bool success = false; + success = backup_behavior_tester->defaultBackupBehaviorTest(goal, tolerance); + + float dist_to_obstacle = 2.0f; + + if ( ((dist_to_obstacle - std::fabs(test_params.x)) < std::fabs(goal.speed)) || + std::fabs(goal.target.y) > 0) + { + EXPECT_FALSE(success); + } else { + EXPECT_TRUE(success); + } +} + +std::vector test_params = {TestParameters{-0.05, 0.0, -0.2, 0.01}, + TestParameters{-0.05, 0.1, -0.2, 0.01}, + TestParameters{-2.0, 0.0, -0.2, 0.1}}; + +INSTANTIATE_TEST_SUITE_P( + BackupBehaviorTests, + BackupBehaviorTestFixture, + ::testing::Values( + test_params[0], + test_params[1], + test_params[2]), + testNameGenerator +); + + +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_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt b/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt new file mode 100644 index 0000000000..fe17417e16 --- /dev/null +++ b/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_drive_on_heading_behavior test_drive_on_heading_behavior_node) + +ament_add_gtest_executable(${test_drive_on_heading_behavior} + test_drive_on_heading_behavior_node.cpp + drive_on_heading_behavior_tester.cpp +) + +ament_target_dependencies(${test_drive_on_heading_behavior} + ${dependencies} +) + +ament_add_test(test_drive_on_heading_recovery + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_drive_on_heading_behavior_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp similarity index 77% rename from nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp rename to nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp index ee9f6b63bc..8db6ef6406 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp @@ -22,7 +22,8 @@ #include #include -#include "spin_recovery_tester.hpp" +#include "drive_on_heading_behavior_tester.hpp" +#include "nav2_util/geometry_utils.hpp" using namespace std::chrono_literals; using namespace std::chrono; // NOLINT @@ -30,38 +31,40 @@ using namespace std::chrono; // NOLINT namespace nav2_system_tests { -SpinRecoveryTester::SpinRecoveryTester() +DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("spin_recovery_test"); + node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test"); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - client_ptr_ = rclcpp_action::create_client( + client_ptr_ = rclcpp_action::create_client( node_->get_node_base_interface(), node_->get_node_graph_interface(), node_->get_node_logging_interface(), node_->get_node_waitables_interface(), - "spin"); + "drive_on_heading"); publisher_ = node_->create_publisher("initialpose", 10); subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&SpinRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); + std::bind(&DriveOnHeadingBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); + + stamp_ = node_->now(); } -SpinRecoveryTester::~SpinRecoveryTester() +DriveOnHeadingBehaviorTester::~DriveOnHeadingBehaviorTester() { if (is_active_) { deactivate(); } } -void SpinRecoveryTester::activate() +void DriveOnHeadingBehaviorTester::activate() { if (is_active_) { throw std::runtime_error("Trying to activate while already active"); @@ -75,7 +78,7 @@ void SpinRecoveryTester::activate() rclcpp::spin_some(node_); } - // Wait for lifecycle_manager_navigation to activate recoveries_server + // Wait for lifecycle_manager_navigation to activate behavior_server std::this_thread::sleep_for(10s); if (!client_ptr_) { @@ -90,11 +93,11 @@ void SpinRecoveryTester::activate() return; } - RCLCPP_INFO(this->node_->get_logger(), "Spin action server is ready"); + RCLCPP_INFO(this->node_->get_logger(), "DriveOnHeading action server is ready"); is_active_ = true; } -void SpinRecoveryTester::deactivate() +void DriveOnHeadingBehaviorTester::deactivate() { if (!is_active_) { throw std::runtime_error("Trying to deactivate while already inactive"); @@ -102,8 +105,8 @@ void SpinRecoveryTester::deactivate() is_active_ = false; } -bool SpinRecoveryTester::defaultSpinRecoveryTest( - const float target_yaw, +bool DriveOnHeadingBehaviorTester::defaultDriveOnHeadingBehaviorTest( + const DriveOnHeading::Goal goal_msg, const double tolerance) { if (!is_active_) { @@ -111,12 +114,9 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( return false; } - // Sleep to let recovery server be ready for serving in multiple runs + // Sleep to let behavior server be ready for serving in multiple runs std::this_thread::sleep_for(5s); - auto goal_msg = Spin::Goal(); - goal_msg.target_yaw = target_yaw; - RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); geometry_msgs::msg::PoseStamped initial_pose; @@ -135,7 +135,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( return false; } - rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); if (!goal_handle) { RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); return false; @@ -152,7 +152,8 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( return false; } - rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = + result_future.get(); switch (wrapped_result.code) { case rclcpp_action::ResultCode::SUCCEEDED: break; @@ -176,27 +177,24 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( return false; } - double goal_yaw = angles::normalize_angle( - tf2::getYaw(initial_pose.pose.orientation) + target_yaw); - double dyaw = angles::shortest_angular_distance( - goal_yaw, tf2::getYaw(current_pose.pose.orientation)); + double dist = nav2_util::geometry_utils::euclidean_distance(initial_pose, current_pose); - if (fabs(dyaw) > tolerance) { + if (fabs(dist) > fabs(goal_msg.target.x) + tolerance) { RCLCPP_ERROR( node_->get_logger(), - "Angular distance from goal is %lf (tolerance %lf)", - fabs(dyaw), tolerance); + "Distance from goal is %lf (tolerance %lf)", + fabs(dist - goal_msg.target.x), tolerance); return false; } return true; } -void SpinRecoveryTester::sendInitialPose() +void DriveOnHeadingBehaviorTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; - pose.header.stamp = rclcpp::Time(); + pose.header.stamp = stamp_; pose.pose.pose.position.x = -2.0; pose.pose.pose.position.y = -0.5; pose.pose.pose.position.z = 0.0; @@ -215,7 +213,7 @@ void SpinRecoveryTester::sendInitialPose() RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); } -void SpinRecoveryTester::amclPoseCallback( +void DriveOnHeadingBehaviorTester::amclPoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) { initial_pose_received_ = true; diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp similarity index 70% rename from nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp rename to nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp index 29622efbe7..920164f162 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp +++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2020 Samsung Research // Copyright (c) 2018 Intel Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ -#define RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ +#ifndef BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_ #include #include @@ -25,7 +25,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "angles/angles.h" -#include "nav2_msgs/action/spin.hpp" +#include "nav2_msgs/action/drive_on_heading.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/node_thread.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -38,19 +38,19 @@ namespace nav2_system_tests { -class SpinRecoveryTester +class DriveOnHeadingBehaviorTester { public: - using Spin = nav2_msgs::action::Spin; - using GoalHandleSpin = rclcpp_action::ClientGoalHandle; + using DriveOnHeading = nav2_msgs::action::DriveOnHeading; + using GoalHandleDriveOnHeading = rclcpp_action::ClientGoalHandle; - SpinRecoveryTester(); - ~SpinRecoveryTester(); + DriveOnHeadingBehaviorTester(); + ~DriveOnHeadingBehaviorTester(); // Runs a single test with given target yaw - bool defaultSpinRecoveryTest( - float target_yaw, - double tolerance = 0.1); + bool defaultDriveOnHeadingBehaviorTest( + const DriveOnHeading::Goal goal_msg, + double tolerance); void activate(); @@ -68,6 +68,7 @@ class SpinRecoveryTester bool is_active_; bool initial_pose_received_; + rclcpp::Time stamp_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; @@ -80,10 +81,10 @@ class SpinRecoveryTester // Subscriber for amcl pose rclcpp::Subscription::SharedPtr subscription_; - // Action client to call spin action - rclcpp_action::Client::SharedPtr client_ptr_; + // Action client to call DriveOnHeading action + rclcpp_action::Client::SharedPtr client_ptr_; }; } // namespace nav2_system_tests -#endif // RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ +#endif // BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py new file mode 100755 index 0000000000..29530dd475 --- /dev/null +++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py @@ -0,0 +1,103 @@ +#! /usr/bin/env python3 +# Copyright (c) 2012 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 ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') + + # Replace the `use_astar` setting on the params file + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites='', + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # 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', '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']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_drive_on_heading_behavior_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/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp new file mode 100644 index 0000000000..b0813c1a25 --- /dev/null +++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp @@ -0,0 +1,133 @@ +// Copyright (c) 2020 Samsung Research +// 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 "rclcpp/rclcpp.hpp" + +#include "drive_on_heading_behavior_tester.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::DriveOnHeadingBehaviorTester; + +struct TestParameters +{ + float x; + float y; + float speed; + float time_allowance; + float tolerance; +}; + +std::string testNameGenerator(const testing::TestParamInfo &) +{ + static int test_index = 0; + std::string name = "DriveOnHeadingTest" + std::to_string(test_index); + ++test_index; + return name; +} + +class DriveOnHeadingBehaviorTestFixture + : public ::testing::TestWithParam +{ +public: + static void SetUpTestCase() + { + drive_on_heading_behavior_tester = new DriveOnHeadingBehaviorTester(); + if (!drive_on_heading_behavior_tester->isActive()) { + drive_on_heading_behavior_tester->activate(); + } + } + + static void TearDownTestCase() + { + delete drive_on_heading_behavior_tester; + drive_on_heading_behavior_tester = nullptr; + } + +protected: + static DriveOnHeadingBehaviorTester * drive_on_heading_behavior_tester; +}; + +DriveOnHeadingBehaviorTester * DriveOnHeadingBehaviorTestFixture::drive_on_heading_behavior_tester = + nullptr; + +TEST_P(DriveOnHeadingBehaviorTestFixture, testBackupBehavior) +{ + auto test_params = GetParam(); + auto goal = nav2_msgs::action::DriveOnHeading::Goal(); + goal.target.x = test_params.x; + goal.target.y = test_params.y; + goal.speed = test_params.speed; + goal.time_allowance.sec = test_params.time_allowance; + float tolerance = test_params.tolerance; + + if (!drive_on_heading_behavior_tester->isActive()) { + drive_on_heading_behavior_tester->activate(); + } + + bool success = false; + success = drive_on_heading_behavior_tester->defaultDriveOnHeadingBehaviorTest( + goal, + tolerance); + + float dist_to_obstacle = 2.0f; + if ( ((dist_to_obstacle - std::fabs(test_params.x)) < std::fabs(goal.speed)) || + std::fabs(goal.target.y) > 0 || + goal.time_allowance.sec < 2.0 || + !((goal.target.x > 0.0) == (goal.speed > 0.0))) + { + EXPECT_FALSE(success); + } else { + EXPECT_TRUE(success); + } +} + +std::vector test_params = {TestParameters{-0.05, 0.0, -0.2, 10.0, 0.01}, + TestParameters{-0.05, 0.1, -0.2, 10.0, 0.01}, + TestParameters{-2.0, 0.0, -0.2, 10.0, 0.1}, + TestParameters{-0.05, 0.0, -0.01, 1.0, 0.01}, + TestParameters{0.05, 0.0, -0.2, 10.0, 0.01}}; + +INSTANTIATE_TEST_SUITE_P( + DriveOnHeadingBehaviorTests, + DriveOnHeadingBehaviorTestFixture, + ::testing::Values( + test_params[0], + test_params[1], + test_params[2], + test_params[3], + test_params[4]), + testNameGenerator); + +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_system_tests/src/behaviors/spin/CMakeLists.txt b/nav2_system_tests/src/behaviors/spin/CMakeLists.txt new file mode 100644 index 0000000000..84ee7246e5 --- /dev/null +++ b/nav2_system_tests/src/behaviors/spin/CMakeLists.txt @@ -0,0 +1,34 @@ +set(test_spin_behavior_exec test_spin_behavior_node) + +ament_add_gtest_executable(${test_spin_behavior_exec} + test_spin_behavior_node.cpp + spin_behavior_tester.cpp +) + +ament_target_dependencies(${test_spin_behavior_exec} + ${dependencies} +) + +ament_add_test(test_spin_behavior + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) + +ament_add_test(test_spin_behavior_fake + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior_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/behaviors/spin/spin_behavior_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp new file mode 100644 index 0000000000..557f5b7959 --- /dev/null +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp @@ -0,0 +1,351 @@ +// 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 +#include +#include +#include +#include +#include + +#include "spin_behavior_tester.hpp" + +using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT + +namespace nav2_system_tests +{ + +SpinBehaviorTester::SpinBehaviorTester() +: is_active_(false), + initial_pose_received_(false) +{ + node_ = rclcpp::Node::make_shared("spin_behavior_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(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), + "spin"); + + publisher_ = + node_->create_publisher("initialpose", 10); + fake_costmap_publisher_ = + node_->create_publisher( + "local_costmap/costmap_raw", + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + fake_footprint_publisher_ = node_->create_publisher( + "local_costmap/published_footprint", rclcpp::SystemDefaultsQoS()); + + subscription_ = node_->create_subscription( + "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&SpinBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); + + stamp_ = node_->now(); +} + +SpinBehaviorTester::~SpinBehaviorTester() +{ + if (is_active_) { + deactivate(); + } +} + +void SpinBehaviorTester::activate() +{ + if (is_active_) { + throw std::runtime_error("Trying to activate while already active"); + return; + } + 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 behavior_server + std::this_thread::sleep_for(10s); + + if (!client_ptr_) { + RCLCPP_ERROR(node_->get_logger(), "Action client not initialized"); + is_active_ = false; + return; + } + + if (!client_ptr_->wait_for_action_server(10s)) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + is_active_ = false; + return; + } + + RCLCPP_INFO(this->node_->get_logger(), "Spin action server is ready"); + is_active_ = true; +} + +void SpinBehaviorTester::deactivate() +{ + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; +} + +bool SpinBehaviorTester::defaultSpinBehaviorTest( + const float target_yaw, + const double tolerance) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + // Sleep to let behavior 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; + + // 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")) { + RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + 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"); + + // Intialize fake costmap + if (make_fake_costmap_) { + sendFakeCostmap(target_yaw); + sendFakeOdom(0.0); + } + + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return false; + } + + // Wait for the server to be done with the goal + 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) + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: break; + case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was aborted"); + return false; + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was canceled"); + return false; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return false; + } + + RCLCPP_INFO(node_->get_logger(), "result received"); + + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) { + RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + return false; + } + + double goal_yaw = angles::normalize_angle( + tf2::getYaw(initial_pose.pose.orientation) + target_yaw); + double dyaw = angles::shortest_angular_distance( + 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)", + fabs(dyaw), tolerance); + return false; + } + + return true; +} + +void SpinBehaviorTester::sendFakeCostmap(float angle) +{ + nav2_msgs::msg::Costmap fake_costmap; + + fake_costmap.header.frame_id = "odom"; + fake_costmap.header.stamp = stamp_; + 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 SpinBehaviorTester::sendInitialPose() +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = stamp_; + pose.pose.pose.position.x = -2.0; + pose.pose.pose.position.y = -0.5; + pose.pose.pose.position.z = 0.0; + pose.pose.pose.orientation.x = 0.0; + pose.pose.pose.orientation.y = 0.0; + pose.pose.pose.orientation.z = 0.0; + pose.pose.pose.orientation.w = 1.0; + for (int i = 0; i < 35; i++) { + pose.pose.covariance[i] = 0.0; + } + pose.pose.covariance[0] = 0.08; + pose.pose.covariance[7] = 0.08; + pose.pose.covariance[35] = 0.05; + + publisher_->publish(pose); + RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); +} + +void SpinBehaviorTester::sendFakeOdom(float angle) +{ + geometry_msgs::msg::TransformStamped transformStamped; + + transformStamped.header.stamp = stamp_; + 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); + + geometry_msgs::msg::PolygonStamped footprint; + footprint.header.frame_id = "odom"; + footprint.header.stamp = stamp_; + footprint.polygon.points.resize(4); + footprint.polygon.points[0].x = 0.22; + footprint.polygon.points[0].y = 0.22; + footprint.polygon.points[1].x = 0.22; + footprint.polygon.points[1].y = -0.22; + footprint.polygon.points[2].x = -0.22; + footprint.polygon.points[2].y = -0.22; + footprint.polygon.points[3].x = -0.22; + footprint.polygon.points[3].y = 0.22; + fake_footprint_publisher_->publish(footprint); +} +void SpinBehaviorTester::amclPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) +{ + initial_pose_received_ = true; +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp new file mode 100644 index 0000000000..745cfe4212 --- /dev/null +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp @@ -0,0 +1,108 @@ +// 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 BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#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 +{ + +class SpinBehaviorTester +{ +public: + using Spin = nav2_msgs::action::Spin; + using GoalHandleSpin = rclcpp_action::ClientGoalHandle; + + SpinBehaviorTester(); + ~SpinBehaviorTester(); + + // Runs a single test with given target yaw + bool defaultSpinBehaviorTest( + float target_yaw, + double tolerance = 0.1); + + void activate(); + + void deactivate(); + + bool isActive() const + { + return is_active_; + } + +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_; + rclcpp::Time stamp_; + + 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_; + + // Action client to call spin action + rclcpp_action::Client::SharedPtr client_ptr_; +}; + +} // namespace nav2_system_tests + +#endif // BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_bringup/bringup/launch/navigation_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py old mode 100644 new mode 100755 similarity index 62% rename from nav2_bringup/bringup/launch/navigation_launch.py rename to nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py index c421d70973..1b0ace8f1b --- a/nav2_bringup/bringup/launch/navigation_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py @@ -1,4 +1,5 @@ -# Copyright (c) 2018 Intel Corporation +#! /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. @@ -13,46 +14,36 @@ # limitations under the License. import os +import sys from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +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(): - # Get the launch directory + 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_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') + 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') - lifecycle_nodes = ['controller_server', - 'planner_server', - 'recoveries_server', - 'bt_navigator', - 'waypoint_follower'] - - # 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')] - # 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, + '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} @@ -62,9 +53,20 @@ def generate_launch_description(): param_rewrites=param_substitutions, convert_types=True) + lifecycle_nodes = ['behavior_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([ - # Set env var to print messages to stdout immediately SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), DeclareLaunchArgument( 'namespace', default_value='', @@ -84,51 +86,45 @@ def generate_launch_description(): description='Full path to the ROS2 parameters file to use'), DeclareLaunchArgument( - 'default_bt_xml_filename', + 'default_nav_through_poses_bt_xml', default_value=os.path.join( get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), + '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='nav2_controller', - executable='controller_server', + package='tf2_ros', + executable='static_transform_publisher', output='screen', - parameters=[configured_params], - remappings=remappings), - + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']), Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', + package='tf2_ros', + executable='static_transform_publisher', output='screen', - parameters=[configured_params], - remappings=remappings), - + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), Node( - package='nav2_recoveries', - executable='recoveries_server', - name='recoveries_server', + package='tf2_ros', + executable='static_transform_publisher', output='screen', - parameters=[configured_params], - remappings=remappings), + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), Node( - package='nav2_bt_navigator', - executable='bt_navigator', - name='bt_navigator', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_waypoint_follower', - executable='waypoint_follower', - name='waypoint_follower', + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', output='screen', parameters=[configured_params], remappings=remappings), @@ -141,5 +137,25 @@ def generate_launch_description(): 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_behavior_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/behaviors/spin/test_spin_behavior_launch.py similarity index 95% rename from nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py rename to nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index f1b7c40a78..d26795834b 100755 --- a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_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( @@ -88,7 +89,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], - name='test_spin_recovery_node', + name='test_spin_behavior_node', output='screen') lts = LaunchTestService() diff --git a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp similarity index 74% rename from nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp rename to nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp index cb83ed981e..150a6599fa 100644 --- a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp @@ -13,7 +13,6 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include #include #include #include @@ -21,11 +20,11 @@ #include "rclcpp/rclcpp.hpp" -#include "spin_recovery_tester.hpp" +#include "spin_behavior_tester.hpp" using namespace std::chrono_literals; -using nav2_system_tests::SpinRecoveryTester; +using nav2_system_tests::SpinBehaviorTester; std::string testNameGenerator(const testing::TestParamInfo> & param) { @@ -35,13 +34,13 @@ std::string testNameGenerator(const testing::TestParamInfo> { public: static void SetUpTestCase() { - spin_recovery_tester = new SpinRecoveryTester(); + spin_recovery_tester = new SpinBehaviorTester(); if (!spin_recovery_tester->isActive()) { spin_recovery_tester->activate(); } @@ -54,12 +53,12 @@ class SpinRecoveryTestFixture } protected: - static SpinRecoveryTester * spin_recovery_tester; + static SpinBehaviorTester * spin_recovery_tester; }; -SpinRecoveryTester * SpinRecoveryTestFixture::spin_recovery_tester = nullptr; +SpinBehaviorTester * SpinBehaviorTestFixture::spin_recovery_tester = nullptr; -TEST_P(SpinRecoveryTestFixture, testSpinRecovery) +TEST_P(SpinBehaviorTestFixture, testSpinRecovery) { float target_yaw = std::get<0>(GetParam()); float tolerance = std::get<1>(GetParam()); @@ -67,18 +66,23 @@ TEST_P(SpinRecoveryTestFixture, testSpinRecovery) bool success = false; int num_tries = 3; for (int i = 0; i != num_tries; i++) { - success = success || spin_recovery_tester->defaultSpinRecoveryTest(target_yaw, tolerance); + success = success || spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance); if (success) { 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_CASE_P( +INSTANTIATE_TEST_SUITE_P( SpinRecoveryTests, - SpinRecoveryTestFixture, + SpinBehaviorTestFixture, ::testing::Values( std::make_tuple(-M_PIf32 / 6.0, 0.1), std::make_tuple(M_PI_4f32, 0.1), diff --git a/nav2_system_tests/src/behaviors/wait/CMakeLists.txt b/nav2_system_tests/src/behaviors/wait/CMakeLists.txt new file mode 100644 index 0000000000..d6fb45401d --- /dev/null +++ b/nav2_system_tests/src/behaviors/wait/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_wait_behavior_exec test_wait_behavior_node) + +ament_add_gtest_executable(${test_wait_behavior_exec} + test_wait_behavior_node.cpp + wait_behavior_tester.cpp +) + +ament_target_dependencies(${test_wait_behavior_exec} + ${dependencies} +) + +ament_add_test(test_wait_behavior + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wait_behavior_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_behavior.xml +) diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py similarity index 95% rename from nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py rename to nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index 484c16fda1..37f2ebd34c 100755 --- a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_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( @@ -88,7 +89,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], - name='test_wait_recovery_node', + name='test_wait_behavior_node', output='screen') lts = LaunchTestService() diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_node.cpp similarity index 74% rename from nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp rename to nav2_system_tests/src/behaviors/wait/test_wait_behavior_node.cpp index 39ee9871d2..5aa00c56e9 100644 --- a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_node.cpp @@ -21,11 +21,11 @@ #include "rclcpp/rclcpp.hpp" -#include "wait_recovery_tester.hpp" +#include "wait_behavior_tester.hpp" using namespace std::chrono_literals; -using nav2_system_tests::WaitRecoveryTester; +using nav2_system_tests::WaitBehaviorTester; std::string testNameGenerator(const testing::TestParamInfo> & param) { @@ -35,31 +35,31 @@ std::string testNameGenerator(const testing::TestParamInfo> { public: static void SetUpTestCase() { - wait_recovery_tester = new WaitRecoveryTester(); - if (!wait_recovery_tester->isActive()) { - wait_recovery_tester->activate(); + wait_behavior_tester = new WaitBehaviorTester(); + if (!wait_behavior_tester->isActive()) { + wait_behavior_tester->activate(); } } static void TearDownTestCase() { - delete wait_recovery_tester; - wait_recovery_tester = nullptr; + delete wait_behavior_tester; + wait_behavior_tester = nullptr; } protected: - static WaitRecoveryTester * wait_recovery_tester; + static WaitBehaviorTester * wait_behavior_tester; }; -WaitRecoveryTester * WaitRecoveryTestFixture::wait_recovery_tester = nullptr; +WaitBehaviorTester * WaitBehaviorTestFixture::wait_behavior_tester = nullptr; -TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) +TEST_P(WaitBehaviorTestFixture, testSWaitBehavior) { float wait_time = std::get<0>(GetParam()); float cancel = std::get<1>(GetParam()); @@ -68,9 +68,9 @@ TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) int num_tries = 3; for (int i = 0; i != num_tries; i++) { if (cancel == 1.0) { - success = success || wait_recovery_tester->recoveryTestCancel(wait_time); + success = success || wait_behavior_tester->behaviorTestCancel(wait_time); } else { - success = success || wait_recovery_tester->recoveryTest(wait_time); + success = success || wait_behavior_tester->behaviorTest(wait_time); } if (success) { break; @@ -80,9 +80,9 @@ TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) EXPECT_EQ(true, success); } -INSTANTIATE_TEST_CASE_P( - WaitRecoveryTests, - WaitRecoveryTestFixture, +INSTANTIATE_TEST_SUITE_P( + WaitBehaviorTests, + WaitBehaviorTestFixture, ::testing::Values( std::make_tuple(1.0, 0.0), std::make_tuple(2.0, 0.0), diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp similarity index 91% rename from nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp rename to nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp index 7112289b5a..ea6e58afb7 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp @@ -23,7 +23,7 @@ #include #include -#include "wait_recovery_tester.hpp" +#include "wait_behavior_tester.hpp" using namespace std::chrono_literals; using namespace std::chrono; // NOLINT @@ -31,11 +31,11 @@ using namespace std::chrono; // NOLINT namespace nav2_system_tests { -WaitRecoveryTester::WaitRecoveryTester() +WaitBehaviorTester::WaitBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("wait_recovery_test"); + node_ = rclcpp::Node::make_shared("wait_behavior_test"); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -52,17 +52,17 @@ WaitRecoveryTester::WaitRecoveryTester() subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&WaitRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); + std::bind(&WaitBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); } -WaitRecoveryTester::~WaitRecoveryTester() +WaitBehaviorTester::~WaitBehaviorTester() { if (is_active_) { deactivate(); } } -void WaitRecoveryTester::activate() +void WaitBehaviorTester::activate() { if (is_active_) { throw std::runtime_error("Trying to activate while already active"); @@ -76,7 +76,7 @@ void WaitRecoveryTester::activate() rclcpp::spin_some(node_); } - // Wait for lifecycle_manager_navigation to activate recoveries_server + // Wait for lifecycle_manager_navigation to activate behavior_server std::this_thread::sleep_for(10s); if (!client_ptr_) { @@ -95,7 +95,7 @@ void WaitRecoveryTester::activate() is_active_ = true; } -void WaitRecoveryTester::deactivate() +void WaitBehaviorTester::deactivate() { if (!is_active_) { throw std::runtime_error("Trying to deactivate while already inactive"); @@ -103,7 +103,7 @@ void WaitRecoveryTester::deactivate() is_active_ = false; } -bool WaitRecoveryTester::recoveryTest( +bool WaitBehaviorTester::behaviorTest( const float wait_time) { if (!is_active_) { @@ -111,7 +111,7 @@ bool WaitRecoveryTester::recoveryTest( return false; } - // Sleep to let recovery server be ready for serving in multiple runs + // Sleep to let behavior server be ready for serving in multiple runs std::this_thread::sleep_for(5s); auto start_time = node_->now(); @@ -172,7 +172,7 @@ bool WaitRecoveryTester::recoveryTest( return true; } -bool WaitRecoveryTester::recoveryTestCancel( +bool WaitBehaviorTester::behaviorTestCancel( const float wait_time) { if (!is_active_) { @@ -180,7 +180,7 @@ bool WaitRecoveryTester::recoveryTestCancel( return false; } - // Sleep to let recovery server be ready for serving in multiple runs + // Sleep to let behavior server be ready for serving in multiple runs std::this_thread::sleep_for(5s); auto start_time = node_->now(); @@ -249,7 +249,7 @@ bool WaitRecoveryTester::recoveryTestCancel( return false; } -void WaitRecoveryTester::sendInitialPose() +void WaitBehaviorTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; @@ -272,7 +272,7 @@ void WaitRecoveryTester::sendInitialPose() RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); } -void WaitRecoveryTester::amclPoseCallback( +void WaitBehaviorTester::amclPoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) { initial_pose_received_ = true; diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp similarity index 88% rename from nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp rename to nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp index 4dfb90c662..71181e24d3 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp @@ -14,8 +14,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef RECOVERIES__WAIT__WAIT_RECOVERY_TESTER_HPP_ -#define RECOVERIES__WAIT__WAIT_RECOVERY_TESTER_HPP_ +#ifndef BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_ #include #include @@ -39,20 +39,20 @@ namespace nav2_system_tests { -class WaitRecoveryTester +class WaitBehaviorTester { public: using Wait = nav2_msgs::action::Wait; using GoalHandleWait = rclcpp_action::ClientGoalHandle; - WaitRecoveryTester(); - ~WaitRecoveryTester(); + WaitBehaviorTester(); + ~WaitBehaviorTester(); // Runs a single test with given target yaw - bool recoveryTest( + bool behaviorTest( float time); - bool recoveryTestCancel(float time); + bool behaviorTestCancel(float time); void activate(); @@ -88,4 +88,4 @@ class WaitRecoveryTester } // namespace nav2_system_tests -#endif // RECOVERIES__WAIT__WAIT_RECOVERY_TESTER_HPP_ +#endif // BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/costmap_filters/CMakeLists.txt b/nav2_system_tests/src/costmap_filters/CMakeLists.txt new file mode 100644 index 0000000000..8a48ff0859 --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/CMakeLists.txt @@ -0,0 +1,47 @@ +ament_add_test(test_keepout_filter + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_keepout_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_MASK=${PROJECT_SOURCE_DIR}/maps/keepout_mask.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + PARAMS_FILE=${CMAKE_CURRENT_SOURCE_DIR}/keepout_params.yaml + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + ASTAR=False +) + +ament_add_test(test_speed_filter_global + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_speed_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_MASK=${PROJECT_SOURCE_DIR}/maps/speed_mask.yaml + 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_to_pose_w_replanning_and_recovery.xml + ASTAR=False +) + +ament_add_test(test_speed_filter_local + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_speed_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_MASK=${PROJECT_SOURCE_DIR}/maps/speed_mask.yaml + 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_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 new file mode 100644 index 0000000000..d81b723c4c --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -0,0 +1,354 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: true + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field_prob" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::OmniMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + # '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_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_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_truncate_path_local_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_goal_updated_controller_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_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +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_plugins: ["goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + 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: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + filters: ["keepout_filter"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + 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: + plugin: "nav2_costmap_2d::KeepoutFilter" + enabled: True + filter_info_topic: "/costmap_filter_info" + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + filters: ["keepout_filter"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + keepout_filter: + plugin: "nav2_costmap_2d::KeepoutFilter" + enabled: True + filter_info_topic: "/costmap_filter_info" + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: False + allow_unknown: True + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +smoother_server: + ros__parameters: + use_sim_time: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "waypoint_task_executor" + waypoint_task_executor: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 0 + +costmap_filter_info_server: + ros__parameters: + use_sim_time: true + type: 0 + filter_info_topic: "/costmap_filter_info" + mask_topic: "/filter_mask" + base: 0.0 + multiplier: 1.0 + +filter_mask_server: + ros__parameters: + use_sim_time: true + frame_id: "map" + topic_name: "/filter_mask" + yaml_filename: "keepout_mask.yaml" diff --git a/nav2_system_tests/src/costmap_filters/keepout_plan.png b/nav2_system_tests/src/costmap_filters/keepout_plan.png new file mode 100644 index 0000000000..f08b540eb7 Binary files /dev/null and b/nav2_system_tests/src/costmap_filters/keepout_plan.png differ diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml similarity index 74% rename from nav2_bringup/bringup/params/nav2_params.yaml rename to nav2_system_tests/src/costmap_filters/speed_global_params.yaml index bae3aa36c0..a8943fa162 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -16,7 +16,7 @@ amcl: laser_likelihood_max_dist: 2.0 laser_max_range: 100.0 laser_min_range: -1.0 - laser_model_type: "likelihood_field" + laser_model_type: "beam" max_beams: 60 max_particles: 2000 min_particles: 500 @@ -26,7 +26,7 @@ amcl: recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 - robot_model_type: "differential" + robot_model_type: "nav2_amcl::DifferentialMotionModel" save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true @@ -53,26 +53,32 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom - 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 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_smooth_path_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_back_up_action_bt_node + - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node - nav2_initial_pose_received_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_truncate_path_local_action_bt_node - nav2_goal_updater_node_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node @@ -80,6 +86,18 @@ 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_goal_updated_controller_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_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -92,8 +110,9 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + speed_limit_topic: "/speed_limit" progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: ["goal_checker"] controller_plugins: ["FollowPath"] # Progress checker parameters @@ -210,6 +229,7 @@ global_costmap: resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + filters: ["speed_filter"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -227,6 +247,11 @@ global_costmap: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.55 + 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 global_costmap_client: ros__parameters: @@ -246,7 +271,7 @@ map_saver: save_map_timeout: 5000 free_thresh_default: 0.25 occupied_thresh_default: 0.65 - map_subscribe_transient_local: False + map_subscribe_transient_local: True planner_server: ros__parameters: @@ -256,28 +281,34 @@ 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: use_sim_time: True -recoveries_server: +smoother_server: + ros__parameters: + use_sim_time: True + +behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "back_up", "wait"] + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] spin: - plugin: "nav2_recoveries/Spin" - back_up: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors/Wait" global_frame: odom robot_base_frame: base_link - transform_timeout: 0.1 + transform_tolerance: 0.1 use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 @@ -287,3 +318,29 @@ recoveries_server: robot_state_publisher: ros__parameters: use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "waypoint_task_executor" + waypoint_task_executor: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 0 + +costmap_filter_info_server: + ros__parameters: + use_sim_time: true + type: 1 + filter_info_topic: "/costmap_filter_info" + mask_topic: "/filter_mask" + base: 100.0 + multiplier: -1.0 + +filter_mask_server: + ros__parameters: + use_sim_time: true + frame_id: "map" + topic_name: "/filter_mask" + yaml_filename: "speed_mask.yaml" diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml new file mode 100644 index 0000000000..4f9320e370 --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -0,0 +1,346 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "beam" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +amcl_map_client: + ros__parameters: + use_sim_time: True + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + # '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_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_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_truncate_path_local_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_goal_updated_controller_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_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +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 + speed_limit_topic: "/speed_limit" + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + filters: ["speed_filter"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + 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: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5000 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: False + allow_unknown: True + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +smoother_server: + ros__parameters: + use_sim_time: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "waypoint_task_executor" + waypoint_task_executor: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 0 + +costmap_filter_info_server: + ros__parameters: + use_sim_time: true + type: 1 + filter_info_topic: "/costmap_filter_info" + mask_topic: "/filter_mask" + base: 100.0 + multiplier: -1.0 + +filter_mask_server: + ros__parameters: + use_sim_time: true + frame_id: "map" + topic_name: "/filter_mask" + yaml_filename: "speed_mask.yaml" diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py new file mode 100755 index 0000000000..d8ad112392 --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 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. + +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 ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + filter_mask_file = os.getenv('TEST_MASK') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + script_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(script_dir, 'keepout_params.yaml') + + # Replace the `use_astar` setting on the params file + param_substitutions = { + 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), + 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, + 'yaml_filename': filter_mask_file} + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + + context = LaunchContext() + new_yaml = configured_params.perform(context) + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # 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', '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_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_filters', + output='screen', + parameters=[{ + 'node_names': + [ + 'filter_mask_server', 'costmap_filter_info_server', 'bt_navigator' + ] + }, + {'autostart': True}]), + + # Nodes required for Costmap Filters configuration + Node( + package='nav2_map_server', + executable='map_server', + name='filter_mask_server', + output='screen', + parameters=[new_yaml]), + + Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + parameters=[new_yaml]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + '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')]), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-t', 'keepout', '-r', '-2.0', '-0.5', '0.0', '-0.5'], + name='tester_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/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py new file mode 100755 index 0000000000..2603ea0df1 --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 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. + +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 ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + filter_mask_file = os.getenv('TEST_MASK') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.getenv('PARAMS_FILE') + + # Replace the `use_astar` setting on the params file + param_substitutions = { + 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR'), + 'filter_mask_server.ros__parameters.yaml_filename': filter_mask_file, + 'yaml_filename': filter_mask_file} + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + context = LaunchContext() + new_yaml = configured_params.perform(context) + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # 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', '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_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_filters', + output='screen', + parameters=[{ + 'node_names': + [ + 'filter_mask_server', 'costmap_filter_info_server', 'bt_navigator' + ] + }, + {'autostart': True}]), + + # Nodes required for Costmap Filters configuration + Node( + package='nav2_map_server', + executable='map_server', + name='filter_mask_server', + output='screen', + parameters=[new_yaml]), + + Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + parameters=[new_yaml]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-t', 'speed', '-r', '-2.0', '-0.5', '0.0', '-0.5'], + name='tester_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/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py new file mode 100755 index 0000000000..0a61116254 --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -0,0 +1,537 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation. +# Copyright (c) 2020 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. + +import argparse +from enum import Enum +import math +import sys +import time +from typing import Optional + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import NavigateToPose +from nav2_msgs.msg import SpeedLimit +from nav2_msgs.srv import ManageLifecycleNodes +from nav_msgs.msg import OccupancyGrid +from nav_msgs.msg import Path + +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): + KEEPOUT = 0 + SPEED = 1 + + +class FilterMask(): + + def __init__( + self, + filter_mask: OccupancyGrid + ): + self.filter_mask = filter_mask + + # Converts world coordinates into filter mask map coordinate. + # Returns filter mask map coordinates or (-1, -1) in case + # if world coordinates are out of mask bounds. + def worldToMap(self, wx: float, wy: float): + origin_x = self.filter_mask.info.origin.position.x + origin_y = self.filter_mask.info.origin.position.y + size_x = self.filter_mask.info.width + size_y = self.filter_mask.info.height + resolution = self.filter_mask.info.resolution + + if wx < origin_x or wy < origin_y: + return -1, -1 + + mx = int((wx - origin_x) / resolution) + my = int((wy - origin_y) / resolution) + + if mx < size_x and my < size_y: + return mx, my + + return -1, -1 + + # Gets filter_mask[mx, my] value + def getValue(self, mx, my): + size_x = self.filter_mask.info.width + return self.filter_mask.data[mx + my * size_x] + + +class NavTester(Node): + + def __init__( + self, + test_type: TestType, + 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) + self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) + + transient_local_qos = QoSProfile( + 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.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 + self.limits = [50.0, 0.0] + # Permissive array: all received speed limits must match to "limits" from above + self.limit_passed = [False, False] + self.plan_sub = self.create_subscription(SpeedLimit, 'speed_limit', + self.speedLimitCallback, volatile_qos) + + self.mask_received = False + self.mask_sub = self.create_subscription(OccupancyGrid, 'filter_mask', + self.maskCallback, transient_local_qos) + + self.initial_pose_received = False + self.initial_pose = initial_pose + self.goal_pose = goal_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') + + 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 publishGoalPose(self, goal_pose: Optional[Pose] = None): + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose + self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose)) + + 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.goal_pose = goal_pose if goal_pose is not None else self.goal_pose + goal_msg = NavigateToPose.Goal() + goal_msg.pose = 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(f'Goal failed with status code: {status}') + return False + + self.info_msg('Goal succeeded!') + return True + + def isInKeepout(self, x, y): + mx, my = self.filter_mask.worldToMap(x, y) + if mx == -1 and my == -1: # Out of mask's area + return False + if self.filter_mask.getValue(mx, my) == 100: # Occupied + return True + return False + + # Checks that (x, y) position does not belong to a keepout zone. + def checkKeepout(self, x, y): + if not self.mask_received: + self.warn_msg('Filter mask was not received') + elif self.isInKeepout(x, y): + self.filter_test_result = False + self.error_msg(f'Pose ({x}, {y}) belongs to keepout zone') + return False + return True + + # Checks that currently received speed_limit is equal to the it-th item + # of expected speed "limits" array. + # If so, sets it-th item of permissive array "limit_passed" to be true. + # Otherwise it will be remained to be false. + # Also verifies that speed limit messages received no more than N-times + # (where N - is the length of "limits" array), + # otherwise sets overall "filter_test_result" to be false. + def checkSpeed(self, it, speed_limit): + if it >= len(self.limits): + self.error_msg('Got excess speed limit') + self.filter_test_result = False + return + if speed_limit == self.limits[it]: + self.limit_passed[it] = True + else: + self.error_msg('Incorrect speed limit received: ' + str(speed_limit) + + ', but should be: ' + str(self.limits[it])) + + def poseCallback(self, msg): + self.info_msg('Received amcl_pose') + self.current_pose = msg.pose.pose + self.initial_pose_received = True + if self.test_type == TestType.KEEPOUT: + if not self.checkKeepout(msg.pose.pose.position.x, msg.pose.pose.position.y): + self.error_msg('Robot goes into keepout zone') + + def planCallback(self, msg): + self.info_msg('Received plan') + for pose in msg.poses: + if not self.checkKeepout(pose.pose.position.x, pose.pose.position.y): + 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(f'Received speed limit: {msg.speed_limit}') + self.checkSpeed(self.speed_it, msg.speed_limit) + self.speed_it += 1 + + def maskCallback(self, msg): + self.info_msg('Received filter mask') + self.filter_mask = FilterMask(msg) + self.mask_received = True + + def wait_for_filter_mask(self, timeout): + start_time = time.time() + + while not self.mask_received: + self.info_msg('Waiting for filter mask to be received ...') + rclpy.spin_once(self, timeout_sec=1) + if (time.time() - start_time) > timeout: + self.error_msg('Time out to waiting filter mask') + 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() + + while not goalReached: + rclpy.spin_once(self, timeout_sec=1) + if self.distanceFromGoal() < distance: + goalReached = True + self.info_msg('*** GOAL REACHED ***') + return True + elif timeout is not None: + if (time.time() - start_time) > timeout: + self.error_msg('Robot timed out reaching its goal!') + return False + + def distanceFromGoal(self): + d_x = self.current_pose.position.x - self.goal_pose.position.x + d_y = self.current_pose.position.y - self.goal_pose.position.y + distance = math.sqrt(d_x * d_x + d_y * d_y) + self.info_msg(f'Distance from goal is: {distance}') + return distance + + def wait_for_node_active(self, node_name: str): + # Waits for the node within the tester namespace to become active + self.info_msg(f'Waiting for {node_name} to become active') + node_service = f'{node_name}/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{node_service} service not available, waiting...') + req = GetState.Request() # empty request + state = 'UNKNOWN' + while (state != 'active'): + self.info_msg(f'Getting {node_name} state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label + self.info_msg(f'Result of get_state: {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(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down navigation lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg( + 'Shutting down navigation lifecycle manager complete.') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + transition_service = 'lifecycle_manager_localization/manage_nodes' + mgr_client = self.create_client( + ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down localization lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg( + 'Shutting down localization lifecycle manager complete') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + def wait_for_initial_pose(self): + 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 test_RobotMovesToGoal(robot_tester): + robot_tester.info_msg('Setting goal pose') + robot_tester.publishGoalPose() + robot_tester.info_msg('Waiting 60 seconds for robot to reach goal') + return robot_tester.reachesGoal(timeout=60, distance=0.5) + + +# Tests that all received speed limits are correct: +# If overall "filter_test_result" is true +# checks that all items in "limit_passed" permissive array are also true. +# In other words, it verifies that all speed limits are received +# exactly (by count and values) as expected by "limits" array. +def test_SpeedLimitsAllCorrect(robot_tester): + if not robot_tester.filter_test_result: + return False + for passed in robot_tester.limit_passed: + if not passed: + robot_tester.error_msg('Did not meet one of the speed limit') + return False + return True + + +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.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) + + # 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_tester(args): + + # Requested tester for one robot + type_str = args.type + init_x, init_y, final_x, final_y = args.robot[0] + test_type = TestType.KEEPOUT # Default value + if type_str == 'speed': + test_type = TestType.SPEED + + tester = NavTester( + test_type, + 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 + '.') + return tester + + +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.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) + 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 tester for the robot + tester = get_tester(args) + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + passed = run_all_tests(tester) + + # stop and shutdown the nav stack to exit cleanly + tester.shutdown() + tester.info_msg('Done Shutting Down.') + + if not passed: + tester.info_msg('Exiting failed') + exit(1) + else: + tester.info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp index 32ecf21451..092b5e9e84 100644 --- a/nav2_system_tests/src/localization/test_localization_node.cpp +++ b/nav2_system_tests/src/localization/test_localization_node.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" #include "nav2_amcl/amcl_node.hpp" #include "std_msgs/msg/string.hpp" #include "geometry_msgs/msg/pose_array.hpp" diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt index 9c67eadddf..61355a82c4 100644 --- a/nav2_system_tests/src/planning/CMakeLists.txt +++ b/nav2_system_tests/src/planning/CMakeLists.txt @@ -46,11 +46,18 @@ ament_add_test(test_planner_random TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm ) -ament_add_gtest(test_planner_plugin_failures +ament_add_gtest(test_planner_plugins test_planner_plugins.cpp + TIMEOUT 10 ) -ament_target_dependencies(test_planner_plugin_failures rclcpp geometry_msgs nav2_msgs ${dependencies}) -target_link_libraries(test_planner_plugin_failures +ament_target_dependencies(test_planner_plugins rclcpp geometry_msgs nav2_msgs ${dependencies}) +target_link_libraries(test_planner_plugins stdc++fs ) + +ament_add_gtest(test_planner_is_path_valid + test_planner_is_path_valid.cpp + planner_tester.cpp) + +ament_target_dependencies(test_planner_is_path_valid rclcpp geometry_msgs nav2_msgs ${dependencies}) diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 1367735aa2..0a2368bab6 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -76,6 +76,7 @@ void PlannerTester::activate() planner_tester_->onConfigure(state); publishRobotTransform(); map_pub_ = this->create_publisher("map", 1); + path_valid_client_ = this->create_client("is_path_valid"); rclcpp::Rate r(1); r.sleep(); planner_tester_->onActivate(state); @@ -133,7 +134,7 @@ void PlannerTester::updateRobotPosition(const geometry_msgs::msg::Point & positi } std::cout << now().nanoseconds() << std::endl; - base_transform_->header.stamp = now() + rclcpp::Duration(250000000); + base_transform_->header.stamp = now() + rclcpp::Duration(0.25s); base_transform_->transform.translation.x = position.x; base_transform_->transform.translation.y = position.y; base_transform_->transform.rotation.w = 1.0; @@ -344,7 +345,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 +369,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; @@ -396,6 +397,27 @@ TaskStatus PlannerTester::createPlan( return TaskStatus::FAILED; } +bool PlannerTester::isPathValid(nav_msgs::msg::Path & path) +{ + planner_tester_->setCostmap(costmap_.get()); + // create a fake service request + auto request = std::make_shared(); + request->path = path; + auto result = path_valid_client_->async_send_request(request); + + RCLCPP_INFO(this->get_logger(), "Waiting for service complete"); + if (rclcpp::spin_until_future_complete( + this->planner_tester_, result, + std::chrono::milliseconds(100)) == + rclcpp::FutureReturnCode::SUCCESS) + { + return result.get()->is_valid; + } else { + RCLCPP_INFO(get_logger(), "Failed to call is_path_valid service"); + return false; + } +} + bool PlannerTester::isCollisionFree(const ComputePathToPoseResult & path) { // At each point of the path, check if the corresponding cell is free diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index c667b9d7b4..b849e89908 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -27,6 +27,7 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_msgs/msg/costmap.hpp" #include "nav2_msgs/srv/get_costmap.hpp" +#include "nav2_msgs/srv/is_path_valid.hpp" #include "visualization_msgs/msg/marker.hpp" #include "nav2_util/costmap.hpp" #include "nav2_util/node_thread.hpp" @@ -150,11 +151,14 @@ class PlannerTester : public rclcpp::Node ComputePathToPoseResult & path, const double deviation_tolerance = 1.0); + // Runs multiple tests with random initial and goal poses bool defaultPlannerRandomTests( const unsigned int number_tests, const float acceptable_fail_ratio); + bool isPathValid(nav_msgs::msg::Path & path); + private: void setCostmap(); @@ -184,6 +188,9 @@ class PlannerTester : public rclcpp::Node // The global planner std::shared_ptr planner_tester_; + // The is path valid client + rclcpp::Client::SharedPtr path_valid_client_; + // A thread for spinning the ROS node std::unique_ptr spin_thread_; diff --git a/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp b/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp new file mode 100644 index 0000000000..958891e02a --- /dev/null +++ b/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp @@ -0,0 +1,75 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include + +#include "nav2_msgs/srv/is_path_valid.hpp" +#include "rclcpp/rclcpp.hpp" +#include "planner_tester.hpp" +#include "nav2_util/lifecycle_utils.hpp" + +using nav2_system_tests::PlannerTester; +using nav2_util::TestCostmap; + +TEST(testIsPathValid, testIsPathValid) +{ + auto planner_tester = std::make_shared(); + planner_tester->activate(); + planner_tester->loadSimpleCostmap(TestCostmap::top_left_obstacle); + + nav_msgs::msg::Path path; + + // empty path + bool is_path_valid = planner_tester->isPathValid(path); + EXPECT_FALSE(is_path_valid); + + // invalid path + for (float i = 0; i < 10; i += 1.0) { + for (float j = 0; j < 10; j += 1.0) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + pose.pose.position.y = j; + path.poses.push_back(pose); + } + } + is_path_valid = planner_tester->isPathValid(path); + EXPECT_FALSE(is_path_valid); + + // valid path + path.poses.clear(); + for (float i = 0; i < 10; i += 1.0) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 1.0; + pose.pose.position.y = i; + path.poses.push_back(pose); + } + is_path_valid = planner_tester->isPathValid(path); + EXPECT_TRUE(is_path_valid); +} + +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_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index c99c1c36bb..a56a7216b1 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -20,6 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "planner_tester.hpp" #include "nav2_util/lifecycle_utils.hpp" +#include "nav2_util/geometry_utils.hpp" using namespace std::chrono_literals; @@ -33,6 +34,85 @@ void callback(const nav_msgs::msg::Path::ConstSharedPtr /*grid*/) { } +void testSmallPathValidityAndOrientation(std::string plugin, double length) +{ + auto obj = std::make_shared(); + rclcpp_lifecycle::State state; + obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin)); + obj->declare_parameter( + "GridBased.use_final_approach_orientation", rclcpp::ParameterValue(false)); + obj->onConfigure(state); + + geometry_msgs::msg::PoseStamped start; + geometry_msgs::msg::PoseStamped goal; + + start.pose.position.x = 0.5; + start.pose.position.y = 0.5; + start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2); + start.header.frame_id = "map"; + + goal.pose.position.x = 0.5; + goal.pose.position.y = start.pose.position.y + length; + goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); + goal.header.frame_id = "map"; + + // Test without use_final_approach_orientation + // expecting end path pose orientation to be equal to goal orientation + auto path = obj->getPlan(start, goal, "GridBased"); + EXPECT_GT((int)path.poses.size(), 0); + EXPECT_NEAR(tf2::getYaw(path.poses.back().pose.orientation), -M_PI, 0.01); + // obj->onCleanup(state); + obj.reset(); +} + +void testSmallPathValidityAndNoOrientation(std::string plugin, double length) +{ + auto obj = std::make_shared(); + rclcpp_lifecycle::State state; + obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin)); + + // Test WITH use_final_approach_orientation + // expecting end path pose orientation to be equal to approach orientation + // which in the one pose corner case should be the start pose orientation + obj->declare_parameter( + "GridBased.use_final_approach_orientation", rclcpp::ParameterValue(true)); + obj->set_parameter(rclcpp::Parameter("GridBased.use_final_approach_orientation", true)); + obj->onConfigure(state); + + geometry_msgs::msg::PoseStamped start; + geometry_msgs::msg::PoseStamped goal; + + start.pose.position.x = 0.5; + start.pose.position.y = 0.5; + start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2); + start.header.frame_id = "map"; + + goal.pose.position.x = 0.5; + goal.pose.position.y = start.pose.position.y + length; + goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); + goal.header.frame_id = "map"; + + auto path = obj->getPlan(start, goal, "GridBased"); + EXPECT_GT((int)path.poses.size(), 0); + + int path_size = path.poses.size(); + if (path_size == 1) { + EXPECT_NEAR( + tf2::getYaw(path.poses.back().pose.orientation), + tf2::getYaw(start.pose.orientation), + 0.01); + } else { + double dx = path.poses.back().pose.position.x - path.poses.front().pose.position.x; + double dy = path.poses.back().pose.position.y - path.poses.front().pose.position.y; + EXPECT_NEAR( + tf2::getYaw(path.poses.back().pose.orientation), + atan2(dy, dx), + 0.01); + } + // obj->onCleanup(state); + obj.reset(); +} + TEST(testPluginMap, Failures) { auto obj = std::make_shared(); @@ -55,6 +135,156 @@ TEST(testPluginMap, Failures) obj->onCleanup(state); } +TEST(testPluginMap, Smac2dEqualStartGoal) +{ + testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.0); +} + +TEST(testPluginMap, Smac2dEqualStartGoalN) +{ + testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.0); +} + +TEST(testPluginMap, Smac2dVerySmallPath) +{ + testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.00001); +} + +TEST(testPluginMap, Smac2dVerySmallPathN) +{ + testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.00001); +} + +TEST(testPluginMap, Smac2dBelowCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.09); +} + +TEST(testPluginMap, Smac2dBelowCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.09); +} + +TEST(testPluginMap, Smac2dJustAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.102); +} + +TEST(testPluginMap, Smac2dJustAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.102); +} + +TEST(testPluginMap, Smac2dAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 1.5); +} + +TEST(testPluginMap, Smac2dAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 1.5); +} + +TEST(testPluginMap, NavFnEqualStartGoal) +{ + testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.0); +} + +TEST(testPluginMap, NavFnEqualStartGoalN) +{ + testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.0); +} + +TEST(testPluginMap, NavFnVerySmallPath) +{ + testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.00001); +} + +TEST(testPluginMap, NavFnVerySmallPathN) +{ + testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.00001); +} + +TEST(testPluginMap, NavFnBelowCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.09); +} + +TEST(testPluginMap, NavFnBelowCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.09); +} + +TEST(testPluginMap, NavFnJustAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.102); +} + +TEST(testPluginMap, NavFnJustAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.102); +} + +TEST(testPluginMap, NavFnAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 1.5); +} + +TEST(testPluginMap, NavFnAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 1.5); +} + +TEST(testPluginMap, ThetaStarEqualStartGoal) +{ + testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.0); +} + +TEST(testPluginMap, ThetaStarEqualStartGoalN) +{ + testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.0); +} + +TEST(testPluginMap, ThetaStarVerySmallPath) +{ + testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.00001); +} + +TEST(testPluginMap, ThetaStarVerySmallPathN) +{ + testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.00001); +} + +TEST(testPluginMap, ThetaStarBelowCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.09); +} + +TEST(testPluginMap, ThetaStarBelowCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.09); +} + +TEST(testPluginMap, ThetaStarJustAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.102); +} + +TEST(testPluginMap, ThetaStarJustAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.102); +} + +TEST(testPluginMap, ThetaStarAboveCostmapResolution) +{ + testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5); +} + +TEST(testPluginMap, ThetaStarAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_system_tests/src/recoveries/README.md b/nav2_system_tests/src/recoveries/README.md deleted file mode 100644 index e4b764aaee..0000000000 --- a/nav2_system_tests/src/recoveries/README.md +++ /dev/null @@ -1,4 +0,0 @@ -# Recoveries Test - -Provides some simple tests for recovery plugins. -It creates an instance of the stack, with the recovery server loading different recovery plugins, and checks for successful recovery behaviors. diff --git a/nav2_system_tests/src/recoveries/backup/CMakeLists.txt b/nav2_system_tests/src/recoveries/backup/CMakeLists.txt deleted file mode 100644 index d21387dec4..0000000000 --- a/nav2_system_tests/src/recoveries/backup/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -set(test_backup_recovery_exec test_backup_recovery_node) - -ament_add_gtest_executable(${test_backup_recovery_exec} - test_backup_recovery_node.cpp - backup_recovery_tester.cpp -) - -ament_target_dependencies(${test_backup_recovery_exec} - ${dependencies} -) - -ament_add_test(test_backup_recovery - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_recovery_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 180 - ENV - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - 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 -) 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 deleted file mode 100644 index 82c6effbd9..0000000000 --- a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright (c) 2020 Samsung Research -// 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 "rclcpp/rclcpp.hpp" - -#include "backup_recovery_tester.hpp" - -using namespace std::chrono_literals; - -using nav2_system_tests::BackupRecoveryTester; - -std::string testNameGenerator(const testing::TestParamInfo> & param) -{ - std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string( - std::get<1>(param.param)); - name.erase(std::remove(name.begin(), name.end(), '.'), name.end()); - return name; -} - -class BackupRecoveryTestFixture - : public ::testing::TestWithParam> -{ -public: - static void SetUpTestCase() - { - backup_recovery_tester = new BackupRecoveryTester(); - if (!backup_recovery_tester->isActive()) { - backup_recovery_tester->activate(); - } - } - - static void TearDownTestCase() - { - delete backup_recovery_tester; - backup_recovery_tester = nullptr; - } - -protected: - static BackupRecoveryTester * backup_recovery_tester; -}; - -BackupRecoveryTester * BackupRecoveryTestFixture::backup_recovery_tester = nullptr; - -TEST_P(BackupRecoveryTestFixture, testBackupRecovery) -{ - float target_dist = std::get<0>(GetParam()); - float tolerance = std::get<1>(GetParam()); - - if (!backup_recovery_tester->isActive()) { - backup_recovery_tester->activate(); - } - - bool success = false; - 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; - // } - - EXPECT_EQ(true, success); -} - -// TODO(stevemacenski): See issue #1779, while the 3rd test collides, -// it returns success due to technical debt in the BT. This test will -// remain as a reminder to update this to a `false` case once the -// recovery server returns true values. - -INSTANTIATE_TEST_CASE_P( - BackupRecoveryTests, - BackupRecoveryTestFixture, - ::testing::Values( - std::make_tuple(-0.1, 0.1), - std::make_tuple(-0.2, 0.1), - std::make_tuple(-2.0, 0.1)), - testNameGenerator); - -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_system_tests/src/recoveries/spin/CMakeLists.txt b/nav2_system_tests/src/recoveries/spin/CMakeLists.txt deleted file mode 100644 index 34abacca01..0000000000 --- a/nav2_system_tests/src/recoveries/spin/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -set(test_spin_recovery_exec test_spin_recovery_node) - -ament_add_gtest_executable(${test_spin_recovery_exec} - test_spin_recovery_node.cpp - spin_recovery_tester.cpp -) - -ament_target_dependencies(${test_spin_recovery_exec} - ${dependencies} -) - -ament_add_test(test_spin_recovery - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_recovery_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 180 - ENV - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - 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 -) diff --git a/nav2_system_tests/src/recoveries/wait/CMakeLists.txt b/nav2_system_tests/src/recoveries/wait/CMakeLists.txt deleted file mode 100644 index 06755020d5..0000000000 --- a/nav2_system_tests/src/recoveries/wait/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -set(test_wait_recovery_exec test_wait_recovery_node) - -ament_add_gtest_executable(${test_wait_recovery_exec} - test_wait_recovery_node.cpp - wait_recovery_tester.cpp -) - -ament_target_dependencies(${test_wait_recovery_exec} - ${dependencies} -) - -ament_add_test(test_wait_recovery - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wait_recovery_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 180 - ENV - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - 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 -) diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index b176fcddfc..ddbd22ab40 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -11,7 +11,25 @@ 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_wrong_init_pose + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wrong_init_pose_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.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + 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 @@ -27,13 +45,14 @@ 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 +ament_add_test(test_bt_navigator_2 GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" @@ -43,9 +62,9 @@ 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 ) @@ -60,7 +79,25 @@ 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 @@ -77,7 +114,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 d78e928465..7160a7bdf4 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 0000000000..de3b823c6d --- /dev/null +++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py @@ -0,0 +1,344 @@ +#! /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.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(f'Goal failed with status code: {status}') + return False + + self.info_msg('Goal succeeded!') + return True + + def runFakeNavigateAction(self): + # 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...") + + goal_msg = NavigateThroughPoses.Goal() + + 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(f'Goal failed with status code: {status}') + return False + + self.info_msg('Goal succeeded!') + return True + + def runNavigatePreemptionAction(self, block): + # 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...") + + goal_msg = NavigateThroughPoses.Goal() + goal_msg.poses = [self.getStampedPoseMsg(self.initial_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 + + if not block: + return True + + 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(f'Goal failed with status code: {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(f'Waiting for {node_name} to become active') + node_service = f'{node_name}/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{node_service} service not available, waiting...') + req = GetState.Request() # empty request + state = 'UNKNOWN' + while (state != 'active'): + self.info_msg(f'Getting {node_name} state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label + self.info_msg(f'Result of get_state: {state}') + else: + self.error_msg(f'Exception while calling service: {future.exception()!r}') + time.sleep(5) + + def shutdown(self): + 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(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down navigation lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down navigation lifecycle manager complete.') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + transition_service = 'lifecycle_manager_localization/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down localization lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down localization lifecycle manager complete') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + def wait_for_initial_pose(self): + 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() + # Test empty navigation request + result = result and not robot_tester.runFakeNavigateAction() + # Test preempting NHP with the same goal + result = result and robot_tester.runNavigatePreemptionAction(False) + result = result and robot_tester.runNavigatePreemptionAction(True) + + # 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 62% rename from nav2_system_tests/src/system/tester_node.py rename to nav2_system_tests/src/system/nav_to_pose_tester_node.py index d84724ea5c..8fd1bc6034 100755 --- a/nav2_system_tests/src/system/tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -16,7 +16,6 @@ import argparse import math -import os import sys import time @@ -37,30 +36,26 @@ from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy from rclpy.qos import QoSProfile -import zmq - class NavTester(Node): - def __init__( - self, - initial_pose: Pose, - goal_pose: Pose, - namespace: str = '' - ): + 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) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) 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, - depth=1) - - self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, - 'amcl_pose', self.poseCallback, pose_qos) + 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 @@ -99,13 +94,6 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg("'NavigateToPose' action server not available, waiting...") - if (os.getenv('GROOT_MONITORING') == 'True'): - if self.grootMonitoringGetStatus(): - self.error_msg('Behavior Tree must not be running already!') - self.error_msg('Are you running multiple goals/bts..?') - return False - self.info_msg('This Error above MUST Fail and is o.k.!') - self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateToPose.Goal() goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) @@ -124,23 +112,12 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): get_result_future = goal_handle.get_result_async() future_return = True - if (os.getenv('GROOT_MONITORING') == 'True'): - try: - if not self.grootMonitoringReloadTree(): - self.error_msg('Failed GROOT_BT - Reload Tree from ZMQ Server') - future_return = False - if not self.grootMonitoringGetStatus(): - self.error_msg('Failed GROOT_BT - Get Status from ZMQ Publisher') - future_return = False - except Exception as e: - self.error_msg('Failed GROOT_BT - ZMQ Tests: ' + e.__doc__ + e.message) - future_return = False 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)) + self.info_msg(f'Goal failed with status code: {status}') return False if not future_return: @@ -149,75 +126,6 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): self.info_msg('Goal succeeded!') return True - def grootMonitoringReloadTree(self): - # ZeroMQ Context - context = zmq.Context() - - sock = context.socket(zmq.REQ) - port = 1667 # default server port for groot monitoring - # # Set a Timeout so we do not spin till infinity - sock.setsockopt(zmq.RCVTIMEO, 1000) - # sock.setsockopt(zmq.LINGER, 0) - - sock.connect('tcp://localhost:' + str(port)) - self.info_msg('ZMQ Server Port: ' + str(port)) - - # this should fail - try: - sock.recv() - self.error_msg('ZMQ Reload Tree Test 1/3 - This should have failed!') - # Only works when ZMQ server receives a request first - sock.close() - return False - except zmq.error.ZMQError: - self.info_msg('ZMQ Reload Tree Test 1/3: Check') - try: - # request tree from server - sock.send_string('') - # receive tree from server as flat_buffer - sock.recv() - self.info_msg('ZMQ Reload Tree Test 2/3: Check') - except zmq.error.Again: - self.info_msg('ZMQ Reload Tree Test 2/3 - Failed to load tree') - sock.close() - return False - - # this should fail - try: - sock.recv() - self.error_msg('ZMQ Reload Tree Test 3/3 - This should have failed!') - # Tree should only be loadable ONCE after ZMQ server received a request - sock.close() - return False - except zmq.error.ZMQError: - self.info_msg('ZMQ Reload Tree Test 3/3: Check') - - return True - - def grootMonitoringGetStatus(self): - # ZeroMQ Context - context = zmq.Context() - # Define the socket using the 'Context' - sock = context.socket(zmq.SUB) - # Set a Timeout so we do not spin till infinity - sock.setsockopt(zmq.RCVTIMEO, 2000) - # sock.setsockopt(zmq.LINGER, 0) - - # Define subscription and messages with prefix to accept. - sock.setsockopt_string(zmq.SUBSCRIBE, '') - port = 1666 # default publishing port for groot monitoring - sock.connect('tcp://127.0.0.1:' + str(port)) - - for request in range(3): - try: - sock.recv() - except zmq.error.Again: - self.error_msg('ZMQ - Did not receive any status') - sock.close() - return False - self.info_msg('ZMQ - Did receive status') - return True - def poseCallback(self, msg): self.info_msg('Received amcl_pose') self.current_pose = msg.pose.pose @@ -242,27 +150,29 @@ def distanceFromGoal(self): d_x = self.current_pose.position.x - self.goal_pose.position.x d_y = self.current_pose.position.y - self.goal_pose.position.y distance = math.sqrt(d_x * d_x + d_y * d_y) - self.info_msg('Distance from goal is: ' + str(distance)) + self.info_msg(f'Distance from goal is: {distance}') return distance 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' + self.info_msg(f'Waiting for {node_name} to become active') + node_service = f'{node_name}/get_state' state_client = self.create_client(GetState, node_service) while not state_client.wait_for_service(timeout_sec=1.0): - self.info_msg(node_service + ' service not available, waiting...') + self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' - while (state != 'active'): - self.info_msg('Getting ' + node_name + ' state...') + while state != 'active': + self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None: state = future.result().current_state.label - self.info_msg('Result of get_state: %s' % state) + self.info_msg(f'Result of get_state: {state}') else: - self.error_msg('Exception while calling service: %r' % future.exception()) + self.error_msg( + f'Exception while calling service: {future.exception()!r}' + ) time.sleep(5) def shutdown(self): @@ -272,7 +182,7 @@ def shutdown(self): 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...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -282,12 +192,12 @@ 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: - self.error_msg('Service call failed %r' % (e,)) + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') transition_service = 'lifecycle_manager_localization/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + ' service not available, waiting...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -297,8 +207,8 @@ 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: - self.error_msg('Service call failed %r' % (e,)) + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') def wait_for_initial_pose(self): self.initial_pose_received = False @@ -319,18 +229,18 @@ def test_RobotMovesToGoal(robot_tester): def run_all_tests(robot_tester): # set transforms to use_sim_time result = True - if (result): + 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() - if (result): + if result: result = test_RobotMovesToGoal(robot_tester) # Add more tests here if desired - if (result): + if result: robot_tester.info_msg('Test PASSED') else: robot_tester.error_msg('Test FAILED') @@ -358,10 +268,19 @@ def get_testers(args): 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))) + 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 + '.') + 'Starting tester, robot going from ' + + init_x + + ', ' + + init_y + + ' to ' + + final_x + + ', ' + + final_y + + '.' + ) testers.append(tester) return testers @@ -371,29 +290,62 @@ def get_testers(args): tester = NavTester( namespace=namespace, initial_pose=fwd_pose(float(init_x), float(init_y)), - goal_pose=fwd_pose(float(final_x), float(final_y))) + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) tester.info_msg( - 'Starting tester for ' + namespace + - ' going from ' + init_x + ', ' + init_y + - ' to ' + final_x + ', ' + final_y) + 'Starting tester for ' + + namespace + + ' going from ' + + init_x + + ', ' + + init_y + + ' to ' + + final_x + + ', ' + + final_y + ) testers.append(tester) return testers +def check_args(expect_failure: str): + # Check if --expect_failure is True or False + if expect_failure != 'True' and expect_failure != 'False': + print( + '\033[1;37;41m' + ' -e flag must be set to True or False only. ' + '\033[0m' + ) + exit(1) + else: + return eval(expect_failure) + + def main(argv=sys.argv[1:]): # The robot(s) positions from the input arguments parser = argparse.ArgumentParser(description='System-level navigation tester node') + parser.add_argument('-e', '--expect_failure') 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.') - group.add_argument('-rs', '--robots', action='append', nargs=5, - metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), - help="The robot's namespace and starting and final positions. " + - 'Repeating the argument for multiple robots is supported.') + 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.', + ) + group.add_argument( + '-rs', + '--robots', + action='append', + nargs=5, + metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), + help="The robot's namespace and starting and final positions. " + + 'Repeating the argument for multiple robots is supported.', + ) args, unknown = parser.parse_known_args() + expect_failure = check_args(args.expect_failure) + rclpy.init() # Create testers for each robot @@ -404,7 +356,7 @@ def main(argv=sys.argv[1:]): for tester in testers: passed = run_all_tests(tester) - if not passed: + if passed != expect_failure: break for tester in testers: @@ -413,7 +365,7 @@ def main(argv=sys.argv[1:]): testers[0].info_msg('Done Shutting Down.') - if not passed: + if passed != expect_failure: testers[0].info_msg('Exiting failed') exit(1) else: 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 87be85d613..4c22101eca 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -52,8 +52,8 @@ def generate_launch_description(): # Launch Gazebo server for simulation start_gazebo_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', - '--minimal_comms', world], + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '-s', 'libgazebo_ros_factory.so', '--minimal_comms', world], output='screen') # Define commands for spawing the robots into Gazebo @@ -61,18 +61,21 @@ def generate_launch_description(): for robot in robots: spawn_robots_cmds.append( Node( - package='nav2_gazebo_spawner', - executable='nav2_gazebo_spawner', + package='gazebo_ros', + executable='spawn_entity.py', output='screen', arguments=[ - '--robot_name', TextSubstitution(text=robot['name']), - '--robot_namespace', TextSubstitution(text=robot['name']), - '--sdf', TextSubstitution(text=sdf), + '-entity', TextSubstitution(text=robot['name']), + '-robot_namespace', TextSubstitution(text=robot['name']), + '-file', TextSubstitution(text=sdf), '-x', TextSubstitution(text=str(robot['x_pose'])), '-y', TextSubstitution(text=str(robot['y_pose'])), '-z', TextSubstitution(text=str(robot['z_pose']))] )) + with open(urdf, 'r') as infp: + robot_description = infp.read() + # Define commands for launching the robot state publishers robot_state_pubs_cmds = [] for robot in robots: @@ -82,14 +85,13 @@ def generate_launch_description(): executable='robot_state_publisher', namespace=TextSubstitution(text=robot['name']), output='screen', - parameters=[{'use_sim_time': 'True'}], - remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], - arguments=[urdf])) + parameters=[{'use_sim_time': True, 'robot_description': robot_description}], + remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')])) # Define commands for launching the navigation instances nav_instances_cmds = [] for robot in robots: - params_file = eval(robot['name'] + '_params_file') + params_file = eval(f"{robot['name']}_params_file") group = GroupAction([ # Instances use the robot's name for namespace @@ -109,7 +111,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) @@ -126,9 +129,10 @@ def main(argv=sys.argv[1:]): # TODO(orduno) remove duplicated definition of robots on `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')), '-rs', 'robot1', '0.0', '0.5', '1.0', '0.5', - '-rs', 'robot2', '0.0', '-0.5', '1.0', '-0.5'], + '-rs', 'robot2', '0.0', '-0.5', '1.0', '-0.5', + '-e', 'True'], name='tester_node', output='screen') diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 8a15b9a63c..6b067deea4 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -50,9 +50,6 @@ def generate_launch_description(): if (os.getenv('ASTAR') == 'True'): param_substitutions.update({'use_astar': 'True'}) - 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( @@ -67,7 +64,8 @@ def generate_launch_description(): 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( @@ -106,8 +104,9 @@ def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), - '-r', '-2.0', '-0.5', '0.0', '2.0'], + cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-r', '-2.0', '-0.5', '0.0', '2.0', + '-e', 'True'], name='tester_node', output='screen') diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py new file mode 100755 index 0000000000..740c267fa2 --- /dev/null +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# Copyright (c) 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 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 ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') + + # Replace the default parameter values for testing special features + # without having multiple params_files inside the nav2 stack + context = LaunchContext() + param_substitutions = {} + + if (os.getenv('ASTAR') == 'True'): + param_substitutions.update({'use_astar': 'True'}) + + param_substitutions.update( + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}) + param_substitutions.update( + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}) + + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + + new_yaml = configured_params.perform(context) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # 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', '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']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True', + 'use_composition': 'False'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-r', '-200000.0', '-200000.0', '0.0', '2.0', + '-e', 'False'], + name='tester_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/system_failure/CMakeLists.txt b/nav2_system_tests/src/system_failure/CMakeLists.txt index 885234adc2..6bc4620535 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 4afd4eaac3..1671ff9da4 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 c41e78f304..56d4d44fdb 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 eac29bc654..59b1fbd99d 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -50,17 +50,18 @@ 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, - depth=1) + 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, 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') @@ -93,7 +94,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() @@ -116,7 +118,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status if status != GoalStatus.STATUS_ABORTED: - self.info_msg('Goal failed with status code: {0}'.format(status)) + self.info_msg(f'Goal failed with status code: {status}') return False self.info_msg('Goal failed, as expected!') @@ -146,27 +148,28 @@ def distanceFromGoal(self): d_x = self.current_pose.position.x - self.goal_pose.position.x d_y = self.current_pose.position.y - self.goal_pose.position.y distance = math.sqrt(d_x * d_x + d_y * d_y) - self.info_msg('Distance from goal is: ' + str(distance)) + self.info_msg(f'Distance from goal is: {distance}') return distance 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' + self.info_msg(f'Waiting for {node_name} to become active') + node_service = f'{node_name}/get_state' state_client = self.create_client(GetState, node_service) while not state_client.wait_for_service(timeout_sec=1.0): - self.info_msg(node_service + ' service not available, waiting...') + self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request state = 'UNKNOWN' while (state != 'active'): - self.info_msg('Getting ' + node_name + ' state...') + self.info_msg(f'Getting {node_name} state...') future = state_client.call_async(req) rclpy.spin_until_future_complete(self, future) if future.result() is not None: state = future.result().current_state.label - self.info_msg('Result of get_state: %s' % state) + self.info_msg(f'Result of get_state: {state}') else: - self.error_msg('Exception while calling service: %r' % future.exception()) + self.error_msg( + f'Exception while calling service: {future.exception()!r}') time.sleep(5) def shutdown(self): @@ -174,9 +177,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( + f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -185,13 +190,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.error_msg('Service call failed %r' % (e,)) + self.info_msg( + 'Shutting down navigation lifecycle manager complete.') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') transition_service = 'lifecycle_manager_localization/manage_nodes' - mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + 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( + f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -200,9 +208,10 @@ 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.error_msg('Service call failed %r' % (e,)) + self.info_msg( + 'Shutting down localization lifecycle manager complete') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') def wait_for_initial_pose(self): self.initial_pose_received = False @@ -264,13 +273,15 @@ def get_testers(args): def main(argv=sys.argv[1:]): # The robot(s) positions from the input arguments - parser = argparse.ArgumentParser(description='System-level navigation tester node') + 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.') group.add_argument('-rs', '--robots', action='append', nargs=5, - metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), + metavar=('name', 'init_x', 'init_y', + 'final_x', 'final_y'), help="The robot's namespace and starting and final positions. " + 'Repeating the argument for multiple robots is supported.') diff --git a/nav2_system_tests/src/updown/README.md b/nav2_system_tests/src/updown/README.md index abfbf0a900..6756d52465 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/updown/test_updown.cpp b/nav2_system_tests/src/updown/test_updown.cpp index 9047c2a10e..092e0edf46 100644 --- a/nav2_system_tests/src/updown/test_updown.cpp +++ b/nav2_system_tests/src/updown/test_updown.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" @@ -26,8 +27,9 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); RCLCPP_INFO(rclcpp::get_logger("test_updown"), "Initializing test"); - nav2_lifecycle_manager::LifecycleManagerClient client_nav("lifecycle_manager_navigation"); - nav2_lifecycle_manager::LifecycleManagerClient client_loc("lifecycle_manager_localization"); + auto node = std::make_shared("lifecycle_manager_service_client"); + nav2_lifecycle_manager::LifecycleManagerClient client_nav("lifecycle_manager_navigation", node); + nav2_lifecycle_manager::LifecycleManagerClient client_loc("lifecycle_manager_localization", node); bool test_passed = true; // Wait for a few seconds to let all of the nodes come up diff --git a/nav2_system_tests/src/waypoint_follower/CMakeLists.txt b/nav2_system_tests/src/waypoint_follower/CMakeLists.txt index 2cd3c53a92..6ba85bc7bf 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 06d1298f15..98819e6e63 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 7f5db902fb..b44d886571 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, @@ -50,8 +50,8 @@ def __init__(self): def setInitialPose(self, pose): self.init_pose = PoseWithCovarianceStamped() - self.init_pose.pose.pose.position.x = -2.0 - self.init_pose.pose.pose.position.y = -0.5 + self.init_pose.pose.pose.position.x = pose[0] + self.init_pose.pose.pose.position.y = pose[1] self.init_pose.header.frame_id = 'map' self.publishInitialPose() time.sleep(5) @@ -61,9 +61,7 @@ def poseCallback(self, msg): self.initial_pose_received = True def setWaypoints(self, waypoints): - if not self.waypoints: - self.waypoints = [] - + self.waypoints = [] for wp in waypoints: msg = PoseStamped() msg.header.frame_id = 'map' @@ -78,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 @@ -88,8 +86,8 @@ 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: - self.error_msg('Service call failed %r' % (e,)) + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') if not self.goal_handle.accepted: self.error_msg('Goal rejected') @@ -101,20 +99,21 @@ 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: - self.error_msg('Service call failed %r' % (e,)) + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') if status != GoalStatus.STATUS_SUCCEEDED: - self.info_msg('Goal failed with status code: {0}'.format(status)) + self.info_msg(f'Goal failed with status code: {status}') return False if len(result.missed_waypoints) > 0: self.info_msg('Goal failed to process all waypoints,' ' missed {0} wps.'.format(len(result.missed_waypoints))) + return False self.info_msg('Goal succeeded!') return True @@ -124,10 +123,14 @@ def publishInitialPose(self): def shutdown(self): self.info_msg('Shutting down') + + self.action_client.destroy() + self.info_msg('Destroyed follow_waypoints action client') + 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...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -135,13 +138,15 @@ def shutdown(self): try: rclpy.spin_until_future_complete(self, future) future.result() - except Exception as e: - self.error_msg('Service call failed %r' % (e,)) + except Exception as e: # noqa: B902 + self.error_msg(f'{transition_service} service call failed {e!r}') + + self.info_msg(f'{transition_service} finished') 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...') + self.info_msg(f'{transition_service} service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -149,8 +154,14 @@ def shutdown(self): try: rclpy.spin_until_future_complete(self, future) future.result() - except Exception as e: - self.error_msg('Service call failed %r' % (e,)) + except Exception as e: # noqa: B902 + self.error_msg(f'{transition_service} service call failed {e!r}') + + self.info_msg(f'{transition_service} finished') + + def cancel_goal(self): + cancel_future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) def cancel_goal(self): cancel_future = self.goal_handle.cancel_goal_async() diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo.world index 52020409c9..9a9e1bdbe8 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 @@ -454,6 +455,7 @@ + /tf:=tf diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world index b14f46fd13..b8c0d0d128 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 @@ -454,6 +455,7 @@ + /tf:=tf diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt new file mode 100644 index 0000000000..8cf8a28c35 --- /dev/null +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -0,0 +1,81 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_theta_star_planner) + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2_ros REQUIRED) + +nav2_package() #Calls the nav2_package.cmake file +add_compile_options(-O3) + +include_directories( + include +) + +set(library_name ${PROJECT_NAME}) + +set(dependencies ament_cmake + builtin_interfaces + nav2_common + nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + tf2_ros +) + + +add_library(${library_name} SHARED + src/theta_star.cpp + src/theta_star_planner.cpp +) + +ament_target_dependencies(${library_name} ${dependencies}) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_core theta_star_planner.xml) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(FILES theta_star_planner.xml + DESTINATION share/${PROJECT_NAME} +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(gtest_disable_pthreads OFF) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_theta_star test/test_theta_star.cpp) + ament_target_dependencies(test_theta_star ${dependencies}) + target_link_libraries(test_theta_star ${library_name}) +endif() + + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md new file mode 100644 index 0000000000..edaf5e5299 --- /dev/null +++ b/nav2_theta_star_planner/README.md @@ -0,0 +1,94 @@ +# Nav2 Theta Star Planner +The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta\* Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. + +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-thetastar.html) for additional parameter descriptions. + +## Features +- The planner uses A\* search along with line of sight (LOS) checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* +- As it also considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases) +- Uses the costs from the costmap to penalise high cost regions +- Allows to control the path behavior to either be any angle directed or to be in the middle of the spaces +- Is well suited for smaller robots of the omni-directional and differential drive kind +- The algorithmic part of the planner has been segregated from the plugin part to allow for reusability + +## Metrics +For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - +![example.png](img/00-37.png) + +The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` + +## Cost Function Details +### Symbols and their meanings +**g(a)** - cost function cost for the node 'a' + +**h(a)** - heuristic function cost for the node 'a' + +**f(a)** - total cost (g(a) + h(a)) for the node 'a' + +**LETHAL_COST** - a value of the costmap traversal cost that inscribes an obstacle with +respect to a function, value = 253 + +**curr** - represents the node whose neighbours are being added to the list + +**neigh** - one of the neighboring nodes of curr + +**par** - parent node of curr + +**euc_cost(a,b)** - euclidean distance between the node type 'a' and type 'b' + +**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 254 = unknown value) between the node type 'a' and type 'b' + +### Cost function +``` +g(neigh) = g(curr) + w_euc_cost*euc_cost(curr, neigh) + w_traversal_cost*(costmap_cost(curr,neigh)/LETHAL_COST)^2 +h(neigh) = w_heuristic_cost * euc_cost(neigh, goal) +f(neigh) = g(neigh) + h(neigh) +``` +Because of how the program works when the 'neigh' init_rclcpp is to be expanded, depending +on the result of the LOS check, (if the LOS check returns true) the value of g(neigh) might change to `g(par) + +w1*euc_cost(par, neigh) + w2*(costmap(par,neigh)/LETHAL_COST)^2` + +## Parameters +The parameters of the planner are : +- ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 +- ` .w_euc_cost ` : weight applied on the length of the path. +- ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs. +Below are the default values of the parameters : +``` +planner_server: + ros__parameters: + planner_plugin_types: ["nav2_theta_star_planner/ThetaStarPlanner"] + use_sim_time: True + planner_plugin_ids: ["GridBased"] + GridBased: + how_many_corners: 8 + w_euc_cost: 1.0 + w_traversal_cost: 2.0 +``` + +## Usage Notes + +### Tuning the Parameters +Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (ie before being multiplied to its parameter and being substituted in g(init_rclcpp)) varies from 0 to 1. + +This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (thus leading to only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor: 10.0`, `inflation_radius: 5.5` and then decrease the value of `cost_scaling_factor` to achieve the said potential field. + +Providing a gentle potential field over the region allows the planner to exchange the increase in path lengths for a decrease in the accumulated traversal cost, thus leading to an increase in the distance from the obstacles. This around a corner allows for naturally smoothing the turns and removes the requirement for an external path smoother. + +`w_heuristic_cost` is an internal setting that is not user changable. It has been provided to have an admissible heuristic, restricting its value to the minimum of `w_euc_cost` and `1.0` to make sure the heuristic and travel costs are admissible and consistent. + +To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversly make the paths less taut. So it is recommend ed that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. + +Also note that changes to `w_traversal_cost` might cause slow downs, in case the number of node expanisions increase thus tuning it along with `w_euc_cost` is the way to go to. + +While tuning the planner's parameters you can also change the `inflation_layer`'s parameters (of the global costmap) to tune the behavior of the paths. + +### Path Smoothing +Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is, and the number of cells in that turn. + +This planner is recommended to be used with local planners like DWB or TEB (or other any local planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. + +While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so because it might increase the query times from the planner. Test the planners performance on the higher cm/px costmaps before making a switch to finer costmaps. + +### When to use this planner? +This planner could be used in scenarios where planning speed matters over an extremely smooth path, which could anyways be handled by using a local planner/controller as mentioned above. Because of the any-angled nature of paths you can traverse environments diagonally (wherever it is allowed, eg: in a wide open region). Another use case would be when you have corridors that are misaligned in the map image, in such a case this planner would be able to give straight-line like paths as it considers line of sight and thus avoid giving jagged paths which arises with other planners because of the finite directions of motion they consider. diff --git a/nav2_theta_star_planner/img/00-37.png b/nav2_theta_star_planner/img/00-37.png new file mode 100644 index 0000000000..fa4115bfb9 Binary files /dev/null and b/nav2_theta_star_planner/img/00-37.png differ diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp new file mode 100644 index 0000000000..dacd8a57bc --- /dev/null +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -0,0 +1,306 @@ +// Copyright 2020 Anshumaan Singh +// +// 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_THETA_STAR_PLANNER__THETA_STAR_HPP_ +#define NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_ + +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +const double INF_COST = DBL_MAX; +const int LETHAL_COST = 252; + +struct coordsM +{ + int x, y; +}; + +struct coordsW +{ + double x, y; +}; + +struct tree_node +{ + int x, y; + double g = INF_COST; + double h = INF_COST; + const tree_node * parent_id = nullptr; + bool is_in_queue = false; + double f = INF_COST; +}; + +struct comp +{ + bool operator()(const tree_node * p1, const tree_node * p2) + { + return (p1->f) > (p2->f); + } +}; + +namespace theta_star +{ +class ThetaStar +{ +public: + coordsM src_{}, dst_{}; + nav2_costmap_2d::Costmap2D * costmap_{}; + + /// weight on the costmap traversal cost + double w_traversal_cost_; + /// weight on the euclidean distance cost (used for calculations of g_cost) + double w_euc_cost_; + /// weight on the heuristic cost (used for h_cost calculations) + double w_heuristic_cost_; + /// parameter to set the number of adjacent nodes to be searched on + int how_many_corners_; + /// the x-directional and y-directional lengths of the map respectively + int size_x_, size_y_; + + ThetaStar(); + + ~ThetaStar() = default; + + /** + * @brief it iteratively searches upon the nodes in the queue (open list) until the + * current node is the goal pose or the size of queue becomes 0 + * @param raw_path is used to return the path obtained by executing the algorithm + * @return true if a path is found, false if no path is found in between the start and goal pose + */ + bool generatePath(std::vector & raw_path); + + /** + * @brief this function checks whether the cost of a point(cx, cy) on the costmap is less than the LETHAL_COST + * @return the result of the check + */ + inline bool isSafe(const int & cx, const int & cy) const + { + return costmap_->getCost(cx, cy) < LETHAL_COST; + } + + /** + * @brief initialises the values of the start and goal points + */ + void setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal); + + /** + * @brief checks whether the start and goal points have costmap costs greater than LETHAL_COST + * @return true if the cost of any one of the points is greater than LETHAL_COST + */ + bool isUnsafeToPlan() const + { + return !(isSafe(src_.x, src_.y)) || !(isSafe(dst_.x, dst_.y)); + } + + int nodes_opened = 0; + +protected: + /// for the coordinates (x,y), it stores at node_position_[size_x_ * y + x], + /// the pointer to the location at which the data of the node is present in nodes_data_ + /// it is initialised with size_x_ * size_y_ elements + /// and its number of elements increases to account for a change in map size + std::vector node_position_; + + /// the vector nodes_data_ stores the coordinates, costs and index of the parent node, + /// and whether or not the node is present in queue_, for all the nodes searched + /// it is initialised with no elements + /// and its size increases depending on the number of nodes searched + std::vector nodes_data_; + + /// this is the priority queue (open_list) to select the next node to be expanded + std::priority_queue, comp> queue_; + + /// it is a counter like variable used to generate consecutive indices + /// such that the data for all the nodes (in open and closed lists) could be stored + /// consecutively in nodes_data_ + int index_generated_; + + const coordsM moves[8] = {{0, 1}, + {0, -1}, + {1, 0}, + {-1, 0}, + {1, -1}, + {-1, 1}, + {1, 1}, + {-1, -1}}; + + tree_node * exp_node; + + + /** @brief it performs a line of sight (los) check between the current node and the parent node of its parent node; + * if an los is found and the new costs calculated are lesser, then the cost and parent node + * of the current node is updated + * @param data of the current node + */ + void resetParent(tree_node * curr_data); + + /** + * @brief this function expands the current node + * @param curr_data used to send the data of the current node + * @param curr_id used to send the index of the current node as stored in nodes_position_ + */ + void setNeighbors(const tree_node * curr_data); + + /** + * @brief performs the line of sight check using Bresenham's Algorithm, + * and has been modified to calculate the traversal cost incurred in a straight line path between + * the two points whose coordinates are (x0, y0) and (x1, y1) + * @param sl_cost is used to return the cost thus incurred + * @return true if a line of sight exists between the points + */ + bool losCheck( + const int & x0, const int & y0, const int & x1, const int & y1, + double & sl_cost) const; + + /** + * @brief it returns the path by backtracking from the goal to the start, by using their parent nodes + * @param raw_points used to return the path thus found + * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position + */ + void backtrace(std::vector & raw_points, const tree_node * curr_n) const; + + /** + * @brief it is an overloaded function to ease the cost calculations while performing the LOS check + * @param cost denotes the total straight line traversal cost; it adds the traversal cost for the node (cx, cy) at every instance; it is also being returned + * @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise + */ + bool isSafe(const int & cx, const int & cy, double & cost) const + { + double curr_cost = getCost(cx, cy); + if (curr_cost < LETHAL_COST) { + cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + return true; + } else { + return false; + } + } + + /* + * @brief this function scales the costmap cost by shifting the origin to 25 and then multiply + * the actual costmap cost by 0.9 to keep the output in the range of [25, 255) + */ + inline double getCost(const int & cx, const int & cy) const + { + return 26 + 0.9 * costmap_->getCost(cx, cy); + } + + /** + * @brief for the point(cx, cy), its traversal cost is calculated by + * *()^2/()^2 + * @return the traversal cost thus calculated + */ + inline double getTraversalCost(const int & cx, const int & cy) + { + double curr_cost = getCost(cx, cy); + return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + } + + /** + * @brief calculates the piecewise straight line euclidean distances by + * * + * @return the distance thus calculated + */ + inline double getEuclideanCost(const int & ax, const int & ay, const int & bx, const int & by) + { + return w_euc_cost_ * std::hypot(ax - bx, ay - by); + } + + /** + * @brief for the point(cx, cy), its heuristic cost is calculated by + * * + * @return the heuristic cost + */ + inline double getHCost(const int & cx, const int & cy) + { + return w_heuristic_cost_ * std::hypot(cx - dst_.x, cy - dst_.y); + } + + /** + * @brief checks if the given coordinates(cx, cy) lies within the map + * @return the result of the check + */ + inline bool withinLimits(const int & cx, const int & cy) const + { + return cx >= 0 && cx < size_x_ && cy >= 0 && cy < size_y_; + } + + /** + * @brief checks if the coordinates of a node is the goal or not + * @return the result of the check + */ + inline bool isGoal(const tree_node & this_node) const + { + return this_node.x == dst_.x && this_node.y == dst_.y; + } + + /** + * @brief initialises the node_position_ vector by storing -1 as index for all points(x, y) within the limits of the map + * @param size_inc is used to increase the number of elements in node_position_ in case the size of the map increases + */ + void initializePosn(int size_inc = 0); + + /** + * @brief it stores id_this in node_position_ at the index [ size_x_*cy + cx ] + * @param id_this a pointer to the location at which the data of the point(cx, cy) is stored in nodes_data_ + */ + inline void addIndex(const int & cx, const int & cy, tree_node * node_this) + { + node_position_[size_x_ * cy + cx] = node_this; + } + + /** + * @brief retrieves the pointer of the location at which the data of the point(cx, cy) is stored in nodes_data + * @return id_this is the pointer to that location + */ + inline tree_node * getIndex(const int & cx, const int & cy) + { + return node_position_[size_x_ * cy + cx]; + } + + /** + * @brief this function depending on the size of the nodes_data_ vector allots space to store the data for a node(x, y) + * @param id_this is the index at which the data is stored/has to be stored for that node + */ + void addToNodesData(const int & id_this) + { + if (static_cast(nodes_data_.size()) <= id_this) { + nodes_data_.push_back({}); + } else { + nodes_data_[id_this] = {}; + } + } + + /** + * @brief initialises the values of global variables at beginning of the execution of the generatePath function + */ + void resetContainers(); + + /** + * @brief clears the priority queue after each execution of the generatePath function + */ + void clearQueue() + { + queue_ = std::priority_queue, comp>(); + } +}; +} // namespace theta_star + +#endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_ diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp new file mode 100644 index 0000000000..211962090d --- /dev/null +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -0,0 +1,101 @@ +// Copyright 2020 Anshumaan Singh +// +// 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_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_ +#define NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_theta_star_planner/theta_star.hpp" +#include "nav2_util/geometry_utils.hpp" + +using rcl_interfaces::msg::ParameterType; + +namespace nav2_theta_star_planner +{ + +class ThetaStarPlanner : public nav2_core::GlobalPlanner +{ +public: + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + void cleanup() override; + + void activate() override; + + void deactivate() override; + + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + +protected: + std::shared_ptr tf_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("ThetaStarPlanner")}; + std::string global_frame_, name_; + bool use_final_approach_orientation_; + + // parent node weak ptr + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node_; + + std::unique_ptr planner_; + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + + + /** + * @brief the function responsible for calling the algorithm and retrieving a path from it + * @return global_path is the planned path to be taken + */ + void getPlan(nav_msgs::msg::Path & global_path); + + /** + * @brief interpolates points between the consecutive waypoints of the path + * @param raw_path is used to send in the path received from the planner + * @param dist_bw_points is used to send in the interpolation_resolution (which has been set as the costmap resolution) + * @return the final path with waypoints at a distance of the value of interpolation_resolution of each other + */ + static nav_msgs::msg::Path linearInterpolation( + const std::vector & raw_path, + const double & dist_bw_points); + + /** + * @brief Callback executed when a paramter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); +}; +} // namespace nav2_theta_star_planner + +#endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_ diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml new file mode 100644 index 0000000000..c621ec96da --- /dev/null +++ b/nav2_theta_star_planner/package.xml @@ -0,0 +1,34 @@ + + + + nav2_theta_star_planner + 1.1.0 + Theta* Global Planning Plugin + Steve Macenski + Anshumaan Singh + Apache-2.0 + + ament_cmake + + builtin_interfaces + nav2_common + nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + tf2_ros + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + + + diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp new file mode 100644 index 0000000000..e0267b34c0 --- /dev/null +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -0,0 +1,262 @@ +// Copyright 2020 Anshumaan Singh +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include "nav2_theta_star_planner/theta_star.hpp" + +namespace theta_star +{ + +ThetaStar::ThetaStar() +: w_traversal_cost_(1.0), + w_euc_cost_(2.0), + w_heuristic_cost_(1.0), + how_many_corners_(8), + size_x_(0), + size_y_(0), + index_generated_(0) +{ + exp_node = new tree_node; +} + +void ThetaStar::setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + unsigned int s[2], d[2]; + costmap_->worldToMap(start.pose.position.x, start.pose.position.y, s[0], s[1]); + costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, d[0], d[1]); + + src_ = {static_cast(s[0]), static_cast(s[1])}; + dst_ = {static_cast(d[0]), static_cast(d[1])}; +} + +bool ThetaStar::generatePath(std::vector & raw_path) +{ + resetContainers(); + addToNodesData(index_generated_); + double src_g_cost = getTraversalCost(src_.x, src_.y), src_h_cost = getHCost(src_.x, src_.y); + nodes_data_[index_generated_] = + {src_.x, src_.y, src_g_cost, src_h_cost, &nodes_data_[index_generated_], true, + src_g_cost + src_h_cost}; + queue_.push({&nodes_data_[index_generated_]}); + addIndex(src_.x, src_.y, &nodes_data_[index_generated_]); + tree_node * curr_data = &nodes_data_[index_generated_]; + index_generated_++; + nodes_opened = 0; + + while (!queue_.empty()) { + nodes_opened++; + + if (isGoal(*curr_data)) { + break; + } + + resetParent(curr_data); + setNeighbors(curr_data); + + curr_data = queue_.top(); + queue_.pop(); + } + + if (queue_.empty()) { + raw_path.clear(); + return false; + } + + backtrace(raw_path, curr_data); + clearQueue(); + + return true; +} + +void ThetaStar::resetParent(tree_node * curr_data) +{ + double g_cost, los_cost = 0; + curr_data->is_in_queue = false; + const tree_node * curr_par = curr_data->parent_id; + const tree_node * maybe_par = curr_par->parent_id; + + if (losCheck(curr_data->x, curr_data->y, maybe_par->x, maybe_par->y, los_cost)) { + g_cost = maybe_par->g + + getEuclideanCost(curr_data->x, curr_data->y, maybe_par->x, maybe_par->y) + los_cost; + + if (g_cost < curr_data->g) { + curr_data->parent_id = maybe_par; + curr_data->g = g_cost; + curr_data->f = g_cost + curr_data->h; + } + } +} + +void ThetaStar::setNeighbors(const tree_node * curr_data) +{ + int mx, my; + tree_node * m_id = nullptr; + double g_cost, h_cost, cal_cost; + + for (int i = 0; i < how_many_corners_; i++) { + mx = curr_data->x + moves[i].x; + my = curr_data->y + moves[i].y; + + if (withinLimits(mx, my)) { + if (!isSafe(mx, my)) { + continue; + } + } else { + continue; + } + + g_cost = curr_data->g + getEuclideanCost(curr_data->x, curr_data->y, mx, my) + + getTraversalCost(mx, my); + + m_id = getIndex(mx, my); + + if (m_id == nullptr) { + addToNodesData(index_generated_); + m_id = &nodes_data_[index_generated_]; + addIndex(mx, my, m_id); + index_generated_++; + } + + exp_node = m_id; + + h_cost = getHCost(mx, my); + cal_cost = g_cost + h_cost; + if (exp_node->f > cal_cost) { + exp_node->g = g_cost; + exp_node->h = h_cost; + exp_node->f = cal_cost; + exp_node->parent_id = curr_data; + if (!exp_node->is_in_queue) { + exp_node->x = mx; + exp_node->y = my; + exp_node->is_in_queue = true; + queue_.push({m_id}); + } + } + } +} + +void ThetaStar::backtrace(std::vector & raw_points, const tree_node * curr_n) const +{ + std::vector path_rev; + coordsW world{}; + do { + costmap_->mapToWorld(curr_n->x, curr_n->y, world.x, world.y); + path_rev.push_back(world); + if (path_rev.size() > 1) { + curr_n = curr_n->parent_id; + } + } while (curr_n->parent_id != curr_n); + costmap_->mapToWorld(curr_n->x, curr_n->y, world.x, world.y); + path_rev.push_back(world); + + raw_points.reserve(path_rev.size()); + for (int i = static_cast(path_rev.size()) - 1; i >= 0; i--) { + raw_points.push_back(path_rev[i]); + } +} + +bool ThetaStar::losCheck( + const int & x0, const int & y0, const int & x1, const int & y1, + double & sl_cost) const +{ + sl_cost = 0; + + int cx, cy; + int dy = abs(y1 - y0), dx = abs(x1 - x0), f = 0; + int sx, sy; + sx = x1 > x0 ? 1 : -1; + sy = y1 > y0 ? 1 : -1; + + int u_x = (sx - 1) / 2; + int u_y = (sy - 1) / 2; + cx = x0; + cy = y0; + + if (dx >= dy) { + while (cx != x1) { + f += dy; + if (f >= dx) { + if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + cy += sy; + f -= dx; + } + if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + if (dy == 0 && !isSafe(cx + u_x, cy, sl_cost) && !isSafe(cx + u_x, cy - 1, sl_cost)) { + return false; + } + cx += sx; + } + } else { + while (cy != y1) { + f = f + dx; + if (f >= dy) { + if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + cx += sx; + f -= dy; + } + if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + if (dx == 0 && !isSafe(cx, cy + u_y, sl_cost) && !isSafe(cx - 1, cy + u_y, sl_cost)) { + return false; + } + cy += sy; + } + } + return true; +} + +void ThetaStar::resetContainers() +{ + index_generated_ = 0; + int last_size_x = size_x_; + int last_size_y = size_y_; + int curr_size_x = static_cast(costmap_->getSizeInCellsX()); + int curr_size_y = static_cast(costmap_->getSizeInCellsY()); + if (((last_size_x != curr_size_x) || (last_size_y != curr_size_y)) && + static_cast(node_position_.size()) < (curr_size_x * curr_size_y)) + { + initializePosn(curr_size_y * curr_size_x - last_size_y * last_size_x); + nodes_data_.reserve(curr_size_x * curr_size_y); + } else { + initializePosn(); + } + size_x_ = curr_size_x; + size_y_ = curr_size_y; +} + +void ThetaStar::initializePosn(int size_inc) +{ + int i = 0; + + if (!node_position_.empty()) { + for (; i < size_x_ * size_y_; i++) { + node_position_[i] = nullptr; + } + } + + for (; i < size_inc; i++) { + node_position_.push_back(nullptr); + } +} +} // namespace theta_star diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp new file mode 100644 index 0000000000..cc98649ccc --- /dev/null +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -0,0 +1,229 @@ +// Copyright 2020 Anshumaan Singh +// +// 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_theta_star_planner/theta_star_planner.hpp" +#include "nav2_theta_star_planner/theta_star.hpp" + +namespace nav2_theta_star_planner +{ +void ThetaStarPlanner::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) +{ + planner_ = std::make_unique(); + parent_node_ = parent; + auto node = parent_node_.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); + name_ = name; + tf_ = tf; + planner_->costmap_ = costmap_ros->getCostmap(); + global_frame_ = costmap_ros->getGlobalFrameID(); + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".how_many_corners", rclcpp::ParameterValue(8)); + + node->get_parameter(name_ + ".how_many_corners", planner_->how_many_corners_); + + if (planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4) { + planner_->how_many_corners_ = 8; + RCLCPP_WARN(logger_, "Your value for - .how_many_corners was overridden, and is now set to 8"); + } + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".w_euc_cost", rclcpp::ParameterValue(1.0)); + node->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); + + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(2.0)); + node->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_); + + planner_->w_heuristic_cost_ = planner_->w_euc_cost_ < 1.0 ? planner_->w_euc_cost_ : 1.0; + + nav2_util::declare_parameter_if_not_declared( + node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".use_final_approach_orientation", use_final_approach_orientation_); +} + +void ThetaStarPlanner::cleanup() +{ + RCLCPP_INFO(logger_, "CleaningUp plugin %s of type nav2_theta_star_planner", name_.c_str()); + planner_.reset(); +} + +void ThetaStarPlanner::activate() +{ + RCLCPP_INFO(logger_, "Activating plugin %s of type nav2_theta_star_planner", name_.c_str()); + // Add callback for dynamic parameters + auto node = parent_node_.lock(); + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&ThetaStarPlanner::dynamicParametersCallback, this, std::placeholders::_1)); +} + +void ThetaStarPlanner::deactivate() +{ + RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str()); +} + +nav_msgs::msg::Path ThetaStarPlanner::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + nav_msgs::msg::Path global_path; + auto start_time = std::chrono::steady_clock::now(); + + // Corner case of start and goal beeing on the same cell + unsigned int mx_start, my_start, mx_goal, my_goal; + planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start); + planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal); + if (mx_start == mx_goal && my_start == my_goal) { + if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { + RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles"); + return global_path; + } + global_path.header.stamp = clock_->now(); + global_path.header.frame_id = global_frame_; + geometry_msgs::msg::PoseStamped pose; + pose.header = global_path.header; + pose.pose.position.z = 0.0; + + pose.pose = start.pose; + // if we have a different start and goal orientation, set the unique path pose to the goal + // orientation, unless use_final_approach_orientation=true where we need it to be the start + // orientation to avoid movement from the local planner + if (start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_) { + pose.pose.orientation = goal.pose.orientation; + } + global_path.poses.push_back(pose); + return global_path; + } + + planner_->setStartAndGoal(start, goal); + RCLCPP_DEBUG( + logger_, "Got the src and dst... (%i, %i) && (%i, %i)", + planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); + getPlan(global_path); + // check if a plan is generated + size_t plan_size = global_path.poses.size(); + if (plan_size > 0) { + global_path.poses.back().pose.orientation = goal.pose.orientation; + } + + // If use_final_approach_orientation=true, interpolate the last pose orientation from the + // previous pose to set the orientation to the 'final approach' orientation of the robot so + // it does not rotate. + // And deal with corner case of plan of length 1 + if (use_final_approach_orientation_) { + if (plan_size == 1) { + global_path.poses.back().pose.orientation = start.pose.orientation; + } else if (plan_size > 1) { + double dx, dy, theta; + auto last_pose = global_path.poses.back().pose.position; + auto approach_pose = global_path.poses[plan_size - 2].pose.position; + dx = last_pose.x - approach_pose.x; + dy = last_pose.y - approach_pose.y; + theta = atan2(dy, dx); + global_path.poses.back().pose.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(theta); + } + } + + auto stop_time = std::chrono::steady_clock::now(); + auto dur = std::chrono::duration_cast(stop_time - start_time); + RCLCPP_DEBUG(logger_, "the time taken is : %i", static_cast(dur.count())); + RCLCPP_DEBUG(logger_, "the nodes_opened are: %i", planner_->nodes_opened); + return global_path; +} + +void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) +{ + std::vector path; + if (planner_->isUnsafeToPlan()) { + RCLCPP_ERROR(logger_, "Either of the start or goal pose are an obstacle! "); + global_path.poses.clear(); + } else if (planner_->generatePath(path)) { + global_path = linearInterpolation(path, planner_->costmap_->getResolution()); + } else { + RCLCPP_ERROR(logger_, "Could not generate path between the given poses"); + global_path.poses.clear(); + } + global_path.header.stamp = clock_->now(); + global_path.header.frame_id = global_frame_; +} + +nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation( + const std::vector & raw_path, + const double & dist_bw_points) +{ + nav_msgs::msg::Path pa; + + geometry_msgs::msg::PoseStamped p1; + for (unsigned int j = 0; j < raw_path.size() - 1; j++) { + coordsW pt1 = raw_path[j]; + p1.pose.position.x = pt1.x; + p1.pose.position.y = pt1.y; + pa.poses.push_back(p1); + + coordsW pt2 = raw_path[j + 1]; + double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); + int loops = static_cast(distance / dist_bw_points); + double sin_alpha = (pt2.y - pt1.y) / distance; + double cos_alpha = (pt2.x - pt1.x) / distance; + for (int k = 1; k < loops; k++) { + p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha; + p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha; + pa.poses.push_back(p1); + } + } + + return pa; +} + +rcl_interfaces::msg::SetParametersResult +ThetaStarPlanner::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_INTEGER) { + if (name == name_ + ".how_many_corners") { + planner_->how_many_corners_ = parameter.as_int(); + } + } else if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == name_ + ".w_euc_cost") { + planner_->w_euc_cost_ = parameter.as_double(); + } else if (name == name_ + ".w_traversal_cost") { + planner_->w_traversal_cost_ = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == name_ + ".use_final_approach_orientation") { + use_final_approach_orientation_ = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_theta_star_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_theta_star_planner::ThetaStarPlanner, nav2_core::GlobalPlanner) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp new file mode 100644 index 0000000000..7776179963 --- /dev/null +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -0,0 +1,187 @@ +// Copyright 2020 Anshumaan Singh +// +// 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 "rclcpp/rclcpp.hpp" +#include "nav2_theta_star_planner/theta_star.hpp" +#include "nav2_theta_star_planner/theta_star_planner.hpp" + +class init_rclcpp +{ +public: + init_rclcpp() {rclcpp::init(0, nullptr);} + ~init_rclcpp() {rclcpp::shutdown();} +}; + +/// class created to access the protected members of the ThetaStar class +/// u is used as shorthand for use +class test_theta_star : public theta_star::ThetaStar +{ +public: + int getSizeOfNodePosition() + { + return static_cast(node_position_.size()); + } + + bool ulosCheck(const int & x0, const int & y0, const int & x1, const int & y1, double & sl_cost) + { + return losCheck(x0, y0, x1, y1, sl_cost); + } + + bool uwithinLimits(const int & cx, const int & cy) {return withinLimits(cx, cy);} + + bool uisGoal(const tree_node & this_node) {return isGoal(this_node);} + + void uinitializePosn(int size_inc = 0) + { + node_position_.reserve(size_x_ * size_y_); initializePosn(size_inc); + } + + void uaddIndex(const int & cx, const int & cy) {addIndex(cx, cy, &nodes_data_[0]);} + + tree_node * ugetIndex(const int & cx, const int & cy) {return getIndex(cx, cy);} + + tree_node * test_getIndex() {return &nodes_data_[0];} + + void uaddToNodesData(const int & id) {addToNodesData(id);} + + void uresetContainers() {nodes_data_.clear(); resetContainers();} + + bool runAlgo(std::vector & path) + { + if (!isUnsafeToPlan()) { + return generatePath(path); + } + return false; + } +}; + +init_rclcpp node; + +// Tests meant to test the algorithm itself and its helper functions +TEST(ThetaStarTest, test_theta_star) { + auto planner_ = std::make_unique(); + planner_->costmap_ = new nav2_costmap_2d::Costmap2D(50, 50, 1.0, 0.0, 0.0, 0); + for (int i = 7; i <= 14; i++) { + for (int j = 7; j <= 14; j++) { + planner_->costmap_->setCost(i, j, 253); + } + } + coordsM s = {5, 5}, g = {18, 18}; + int size_x = 20, size_y = 20; + planner_->size_x_ = size_x; + planner_->size_y_ = size_y; + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = s.x; + start.pose.position.y = s.y; + start.pose.orientation.w = 1.0; + goal.pose.position.x = g.x; + goal.pose.position.y = g.y; + goal.pose.orientation.w = 1.0; + /// Check if the setStartAndGoal function works properly + planner_->setStartAndGoal(start, goal); + EXPECT_TRUE(planner_->src_.x == s.x && planner_->src_.y == s.y); + EXPECT_TRUE(planner_->dst_.x == g.x && planner_->dst_.y == g.y); + + /// Check if the initializePosn function works properly + planner_->uinitializePosn(size_x * size_y); + EXPECT_EQ(planner_->getSizeOfNodePosition(), (size_x * size_y)); + + /// Check if the withinLimits function works properly + EXPECT_TRUE(planner_->uwithinLimits(18, 18)); + EXPECT_FALSE(planner_->uwithinLimits(120, 140)); + + tree_node n = {g.x, g.y, 120, 0, NULL, false, 20}; + n.parent_id = &n; + /// Check if the isGoal function works properly + EXPECT_TRUE(planner_->uisGoal(n)); // both (x,y) are the goal coordinates + n.x = 25; + EXPECT_FALSE(planner_->uisGoal(n)); // only y coordinate matches with that of goal + n.x = g.x; + n.y = 20; + EXPECT_FALSE(planner_->uisGoal(n)); // only x coordinate matches with that of goal + n.x = 30; + EXPECT_FALSE(planner_->uisGoal(n)); // both (x, y) are different from the goal coordinate + + /// Check if the isSafe functions work properly + EXPECT_TRUE(planner_->isSafe(5, 5)); // cost at this point is 0 + EXPECT_FALSE(planner_->isSafe(10, 10)); // cost at this point is 253 (>LETHAL_COST) + + /// Check if the functions addIndex & getIndex work properly + coordsM c = {20, 30}; + planner_->uaddToNodesData(0); + planner_->uaddIndex(c.x, c.y); + tree_node * c_node = planner_->ugetIndex(c.x, c.y); + EXPECT_EQ(c_node, planner_->test_getIndex()); + + double sl_cost = 0.0; + /// Checking for the case where the losCheck should return the value as true + EXPECT_TRUE(planner_->ulosCheck(2, 2, 7, 20, sl_cost)); + /// and as false + EXPECT_FALSE(planner_->ulosCheck(2, 2, 18, 18, sl_cost)); + + planner_->uresetContainers(); + std::vector path; + /// Check if the planner returns a path for the case where a path exists + EXPECT_TRUE(planner_->runAlgo(path)); + EXPECT_GT(static_cast(path.size()), 0); + /// and where it doesn't exist + path.clear(); + planner_->src_ = {10, 10}; + EXPECT_FALSE(planner_->runAlgo(path)); + EXPECT_EQ(static_cast(path.size()), 0); +} + +// Smoke tests meant to detect issues arising from the plugin part rather than the algorithm +TEST(ThetaStarPlanner, test_theta_star_planner) { + rclcpp_lifecycle::LifecycleNode::SharedPtr life_node = + std::make_shared("ThetaStarPlannerTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation.w = 1.0; + goal = start; + auto planner_2d = std::make_unique(); + planner_2d->configure(life_node, "test", nullptr, costmap_ros); + planner_2d->activate(); + + nav_msgs::msg::Path path = planner_2d->createPlan(start, goal); + EXPECT_GT(static_cast(path.poses.size()), 0); + + // test if the goal is unsafe + for (int i = 7; i <= 14; i++) { + for (int j = 7; j <= 14; j++) { + costmap_ros->getCostmap()->setCost(i, j, 254); + } + } + goal.pose.position.x = 1.0; + goal.pose.position.y = 1.0; + path = planner_2d->createPlan(start, goal); + EXPECT_EQ(static_cast(path.poses.size()), 0); + + planner_2d->deactivate(); + planner_2d->cleanup(); + + planner_2d.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + life_node.reset(); + costmap_ros.reset(); +} diff --git a/nav2_theta_star_planner/theta_star_planner.xml b/nav2_theta_star_planner/theta_star_planner.xml new file mode 100644 index 0000000000..1890ab14c3 --- /dev/null +++ b/nav2_theta_star_planner/theta_star_planner.xml @@ -0,0 +1,5 @@ + + + An implementation of the Theta* Algorithm + + diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 6221118d15..86fa5fdaee 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -12,9 +12,11 @@ find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(rclcpp_action REQUIRED) -find_package(test_msgs REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(bondcpp REQUIRED) +find_package(bond REQUIRED) +find_package(action_msgs REQUIRED) set(dependencies nav2_msgs @@ -26,8 +28,10 @@ set(dependencies rclcpp lifecycle_msgs rclcpp_action - test_msgs rclcpp_lifecycle + bondcpp + bond + action_msgs ) nav2_package() diff --git a/nav2_util/README.md b/nav2_util/README.md index 98a53e3c07..844370983c 100644 --- a/nav2_util/README.md +++ b/nav2_util/README.md @@ -1,7 +1,12 @@ # Nav2 Util -The `nav2_util` package contains utilities abstracted from individual packages which may find use in other uses. Examples include the particle filter implementation from AMCL, motion models, ROS2 node utilities, and more. +The `nav2_util` package contains utilities abstracted from individual packages which may find use in other uses. Some examples of things you'll find here: -## ROS1 Comparison +- Geometry utilities for computing distances and values in paths +- A Nav2 specific lifecycle node wrapper for boilerplate code and useful common utilities like `declare_parameter_if_not_declared()` +- Simplified service clients +- Simplified action servers +- Transformation and robot pose helpers -This package does not have a direct counter-part in Navigation. This was created to abstract out sections of the code-base from their implementations should the base algorithms/utilities find use elsewhere. +The long-term aim is for these utilities to find more permanent homes in other packages (within and outside of Nav2) or migrate to the raw tools made available in ROS 2. + \ No newline at end of file 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 570feb79c8..8b401e8a59 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 c811007e4c..0d868df6e0 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 52bd5ebb41..884b7b366e 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -19,15 +19,22 @@ #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.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,31 +42,141 @@ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle) return tf2::toMsg(q); } +/** + * @brief Get the euclidean distance between 2 geometry_msgs::Points + * @param pos1 First point + * @param pos1 Second point + * @param is_3d True if a true L2 distance is desired (default false) + * @return double L2 distance + */ inline double euclidean_distance( const geometry_msgs::msg::Point & pos1, - const geometry_msgs::msg::Point & pos2) + const geometry_msgs::msg::Point & pos2, + const bool is_3d = false) { double dx = pos1.x - pos2.x; double dy = pos1.y - pos2.y; - double dz = pos1.z - pos2.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); + + if (is_3d) { + double dz = pos1.z - pos2.z; + return std::hypot(dx, dy, dz); + } + + return std::hypot(dx, dy); } +/** + * @brief Get the L2 distance between 2 geometry_msgs::Poses + * @param pos1 First pose + * @param pos1 Second pose + * @param is_3d True if a true L2 distance is desired (default false) + * @return double euclidean distance + */ inline double euclidean_distance( const geometry_msgs::msg::Pose & pos1, - const geometry_msgs::msg::Pose & pos2) + const geometry_msgs::msg::Pose & pos2, + const bool is_3d = false) { double dx = pos1.position.x - pos2.position.x; double dy = pos1.position.y - pos2.position.y; - double dz = pos1.position.z - pos2.position.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); + + if (is_3d) { + double dz = pos1.position.z - pos2.position.z; + return std::hypot(dx, dy, dz); + } + + return std::hypot(dx, dy); } +/** + * @brief Get the L2 distance between 2 geometry_msgs::PoseStamped + * @param pos1 First pose + * @param pos1 Second pose + * @param is_3d True if a true L2 distance is desired (default false) + * @return double L2 distance + */ inline double euclidean_distance( const geometry_msgs::msg::PoseStamped & pos1, - const geometry_msgs::msg::PoseStamped & pos2) + const geometry_msgs::msg::PoseStamped & pos2, + const bool is_3d = false) +{ + return euclidean_distance(pos1.pose, pos2.pose, is_3d); +} + +/** + * @brief Get the L2 distance between 2 geometry_msgs::Pose2D + * @param pos1 First pose + * @param pos1 Second pose + * @return double L2 distance + */ +inline double euclidean_distance( + const geometry_msgs::msg::Pose2D & pos1, + const geometry_msgs::msg::Pose2D & pos2) +{ + double dx = pos1.x - pos2.x; + double dy = pos1.y - pos2.y; + + return std::hypot(dx, dy); +} + +/** + * 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; +} + +/** + * Find first element in iterator that is greater integrated distance than comparevalue + */ +template +inline Iter first_after_integrated_distance(Iter begin, Iter end, Getter getCompareVal) +{ + if (begin == end) { + return end; + } + Getter dist = 0.0; + for (Iter it = begin; it != end - 1; it++) { + dist += euclidean_distance(*it, *(it + 1)); + if (dist > getCompareVal) { + return it + 1; + } + } + return end; +} + +/** + * @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) { - return euclidean_distance(pos1.pose, pos2.pose); + 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 diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index de543515a7..d4ba168849 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -22,24 +22,30 @@ #include "nav2_util/node_thread.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" +#include "bondcpp/bond.hpp" +#include "bond/msg/constants.hpp" namespace nav2_util { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -// The following is a temporary wrapper for rclcpp_lifecycle::LifecycleNode. This class -// adds the optional creation of an rclcpp::Node that can be used by derived classes -// 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 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 options Node options + */ LifecycleNode( const std::string & node_name, - const std::string & namespace_ = "", - bool use_rclcpp_node = false, + const std::string & ns = "", const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); virtual ~LifecycleNode(); @@ -57,7 +63,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 = "", @@ -73,7 +86,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, @@ -94,7 +115,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, @@ -115,12 +144,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( @@ -129,18 +167,24 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode return nav2_util::CallbackReturn::SUCCESS; } -protected: - void print_lifecycle_node_notification(); + /** + * @brief Create bond connection to lifecycle manager + */ + void createBond(); - // Whether or not to create a local rclcpp::Node which can be used for ROS2 classes that don't - // yet support lifecycle nodes - bool use_rclcpp_node_; + /** + * @brief Destroy bond connection to lifecycle manager + */ + void destroyBond(); - // The local node - rclcpp::Node::SharedPtr rclcpp_node_; +protected: + /** + * @brief Print notifications for lifecycle node + */ + void printLifecycleNodeNotification(); - // When creating a local node, this class will launch a separate thread created to spin the node - std::unique_ptr rclcpp_thread_; + // Connection to tell that server is still up + std::unique_ptr bond_{nullptr}; }; } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp index 4bfaead087..cceb539368 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 { @@ -39,7 +40,7 @@ class LifecycleServiceClient /** * Throws std::runtime_error on failure */ - void change_state( + bool change_state( const uint8_t transition, // takes a lifecycle_msgs::msg::Transition id const std::chrono::seconds timeout); @@ -50,7 +51,7 @@ class LifecycleServiceClient /** * Throws std::runtime_error on failure */ - uint8_t get_state(const std::chrono::seconds timeout = std::chrono::seconds::max()); + uint8_t get_state(const std::chrono::seconds timeout = std::chrono::seconds(2)); protected: rclcpp::Node::SharedPtr node_; diff --git a/nav2_util/include/nav2_util/line_iterator.hpp b/nav2_util/include/nav2_util/line_iterator.hpp index a9ff0c1f43..82ea12425e 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 97ab46cee6..78d84fb8db 100644 --- a/nav2_util/include/nav2_util/node_thread.hpp +++ b/nav2_util/include/nav2_util/node_thread.hpp @@ -21,23 +21,43 @@ namespace nav2_util { - +/** + * @class nav2_util::NodeThread + * @brief A background thread to process node/executor 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 executor's callbacks constructor + * @param executor Interface to executor to spin in thread + */ + explicit NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor); + + /** + * @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: rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_; std::unique_ptr thread_; - rclcpp::executors::SingleThreadedExecutor executor_; + rclcpp::Executor::SharedPtr executor_; }; } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 51281ff10a..484ff35dd1 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/occ_grid_values.hpp b/nav2_util/include/nav2_util/occ_grid_values.hpp new file mode 100644 index 0000000000..d366c0c3c7 --- /dev/null +++ b/nav2_util/include/nav2_util/occ_grid_values.hpp @@ -0,0 +1,50 @@ +// Copyright (c) 2020 Samsung Research Russia +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 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. +// +// Author: Alexey Merzlyakov + +#ifndef NAV2_UTIL__OCC_GRID_VALUES_HPP_ +#define NAV2_UTIL__OCC_GRID_VALUES_HPP_ + +namespace nav2_util +{ + +/** + * @brief OccupancyGrid data constants + */ +static constexpr int8_t OCC_GRID_UNKNOWN = -1; +static constexpr int8_t OCC_GRID_FREE = 0; +static constexpr int8_t OCC_GRID_OCCUPIED = 100; + +} // namespace nav2_util + +#endif // NAV2_UTIL__OCC_GRID_VALUES_HPP_ diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp index 9a1d49d504..185a86d147 100644 --- a/nav2_util/include/nav2_util/odometry_utils.hpp +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -43,23 +43,50 @@ class OdomSmoother public: /** * @brief Constructor that subscribes to an Odometry topic - * @param nh NodeHandle for creating subscriber + * @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( - rclcpp::Node::SharedPtr nh, + const rclcpp::Node::WeakPtr & parent, double filter_duration = 0.3, - std::string odom_topic = "odom"); + 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); - void updateState(); - rclcpp::Node::SharedPtr node_; + /** + * @brief Update internal state of the smoother after getting new data + */ + void updateState(); rclcpp::Subscription::SharedPtr odom_sub_; nav_msgs::msg::Odometry odom_cumulate_; diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 0575dad5d9..5281ff8813 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -25,16 +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); + const std::string robot_frame = "base_link", const double transform_timeout = 0.1, + const rclcpp::Time stamp = rclcpp::Time()); + +/** +* @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 diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index e1eb698381..4af107abf1 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 e4767abf5f..40f1a93860 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -24,30 +24,66 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_util/node_thread.hpp" namespace nav2_util { -template +/** + * @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 + * @param spin_thread Whether to spin with a dedicated thread internally + * @param options Options to pass to the underlying rcl_action_server_t + */ + template explicit SimpleActionServer( - typename nodeT::SharedPtr node, + NodeT node, const std::string & action_name, ExecuteCallback execute_callback, - bool autostart = true, - std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500)) + CompletionCallback completion_callback = nullptr, + std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500), + bool spin_thread = false, + const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) : 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, autostart, server_timeout) + action_name, execute_callback, completion_callback, server_timeout, spin_thread, options) {} + /** + * @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 + * @param spin_thread Whether to spin with a dedicated thread internally + * @param options Options to pass to the underlying rcl_action_server_t + */ explicit SimpleActionServer( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, @@ -55,22 +91,25 @@ class SimpleActionServer rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, const std::string & action_name, ExecuteCallback execute_callback, - bool autostart = true, - std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500)) + CompletionCallback completion_callback = nullptr, + std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500), + bool spin_thread = false, + const rcl_action_server_options_t & options = rcl_action_server_get_default_options()) : node_base_interface_(node_base_interface), node_clock_interface_(node_clock_interface), node_logging_interface_(node_logging_interface), node_waitables_interface_(node_waitables_interface), action_name_(action_name), execute_callback_(execute_callback), - server_timeout_(server_timeout) + completion_callback_(completion_callback), + server_timeout_(server_timeout), + spin_thread_(spin_thread) { - if (autostart) { - server_active_ = true; - } - using namespace std::placeholders; // NOLINT - + if (spin_thread_) { + callback_group_ = node_base_interface->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + } action_server_ = rclcpp_action::create_server( node_base_interface_, node_clock_interface_, @@ -79,9 +118,22 @@ class SimpleActionServer action_name_, std::bind(&SimpleActionServer::handle_goal, this, _1, _2), std::bind(&SimpleActionServer::handle_cancel, this, _1), - std::bind(&SimpleActionServer::handle_accepted, this, _1)); + std::bind(&SimpleActionServer::handle_accepted, this, _1), + options, + callback_group_); + if (spin_thread_) { + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, node_base_interface_); + executor_thread_ = std::make_unique(executor_); + } } + /** + * @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*/) @@ -96,14 +148,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_); @@ -136,6 +206,9 @@ class SimpleActionServer } } + /** + * @brief Computed background work and processes stop requests + */ void work() { while (rclcpp::ok() && !stop_execution_ && is_active(current_handle_)) { @@ -147,6 +220,7 @@ class SimpleActionServer node_logging_interface_->get_logger(), "Action server failed while executing action callback: \"%s\"", ex.what()); terminate_all(); + completion_callback_(); return; } @@ -156,12 +230,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_)) { @@ -175,6 +251,9 @@ class SimpleActionServer debug_msg("Worker thread done."); } + /** + * @brief Active action server + */ void activate() { std::lock_guard lock(update_mutex_); @@ -182,6 +261,9 @@ class SimpleActionServer stop_execution_ = false; } + /** + * @brief Deactive action server + */ void deactivate() { debug_msg("Deactivating..."); @@ -208,6 +290,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"); } } @@ -215,6 +298,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() && @@ -222,18 +309,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_); @@ -257,6 +356,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_); @@ -269,6 +390,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_); @@ -287,6 +428,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()) @@ -297,6 +442,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()) @@ -305,6 +454,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()) @@ -318,6 +471,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_)) { @@ -337,6 +494,7 @@ class SimpleActionServer std::string action_name_; ExecuteCallback execute_callback_; + CompletionCallback completion_callback_; std::future execution_future_; bool stop_execution_{false}; @@ -349,18 +507,35 @@ class SimpleActionServer std::shared_ptr> pending_handle_; typename rclcpp_action::Server::SharedPtr action_server_; - + bool spin_thread_; + rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr executor_thread_; + + /** + * @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 = @@ -380,6 +555,9 @@ class SimpleActionServer } } + /** + * @brief Info logging + */ void info_msg(const std::string & msg) const { RCLCPP_INFO( @@ -387,6 +565,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( @@ -394,6 +575,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( @@ -401,6 +585,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 1f3478a68d..06d93b82a2 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 9a645cbd70..60a2b0a093 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.7 + 1.1.0 TODO Michael Jeronimo Mohammad Haghighipanah @@ -21,8 +21,9 @@ tf2_ros tf2_geometry_msgs lifecycle_msgs + bondcpp + bond rclcpp_action - test_msgs rclcpp_lifecycle launch launch_testing_ament_cmake @@ -36,7 +37,10 @@ launch launch_testing_ament_cmake std_srvs + test_msgs action_msgs + launch_testing_ros + ament_cmake_pytest ament_cmake diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index bcaf94b5ba..a639a0e59e 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -20,6 +20,7 @@ ament_target_dependencies(${library_name} lifecycle_msgs rclcpp_lifecycle tf2_geometry_msgs + bondcpp ) add_executable(lifecycle_bringup @@ -29,13 +30,6 @@ target_link_libraries(lifecycle_bringup ${library_name}) find_package(Boost REQUIRED COMPONENTS program_options) -add_executable(dump_params dump_params.cpp) -ament_target_dependencies(dump_params rclcpp) - -target_include_directories(dump_params PUBLIC ${Boost_INCLUDE_DIRS}) - -target_link_libraries(dump_params ${Boost_PROGRAM_OPTIONS_LIBRARY}) - install(TARGETS ${library_name} ARCHIVE DESTINATION lib @@ -45,6 +39,5 @@ install(TARGETS install(TARGETS lifecycle_bringup - dump_params RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_util/src/costmap.cpp b/nav2_util/src/costmap.cpp index 7dd6a65c8e..91f3b2fbc8 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/dump_params.cpp b/nav2_util/src/dump_params.cpp deleted file mode 100644 index 247a09d936..0000000000 --- a/nav2_util/src/dump_params.cpp +++ /dev/null @@ -1,407 +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 -#ifndef _WIN32 -#include -#endif -#include -#include -#include -#include - -#include "boost/program_options.hpp" -#include "boost/tokenizer.hpp" -#include "boost/foreach.hpp" -#include "boost/algorithm/algorithm.hpp" -#include "boost/algorithm/string/split.hpp" -#include "boost/algorithm/string/classification.hpp" -#include "rcl_interfaces/srv/list_parameters.hpp" -#include "rcl_interfaces/srv/get_parameters.hpp" -#include "rcl_interfaces/msg/parameter_value.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace po = boost::program_options; -namespace alg = boost::algorithm; - -using namespace std::chrono_literals; - -static std::vector -get_param_names_for_node(rclcpp::Node::SharedPtr node, std::string node_name) -{ - auto client = node->create_client( - node_name + "/list_parameters"); - - while (!client->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { - throw std::runtime_error("client interrupted while waiting for service to appear."); - } - - throw std::runtime_error( - std::string("ListParameters service for ") + - node_name + " not available"); - } - - auto request = std::make_shared(); - auto result_future = client->async_send_request(request); - - if (rclcpp::spin_until_future_complete(node, result_future, 1s) != - rclcpp::FutureReturnCode::SUCCESS) - { - throw std::runtime_error(std::string("service call to \"") + node_name + "\" failed"); - } - - return result_future.get()->result.names; -} - -static std::vector -get_param_values_for_node( - rclcpp::Node::SharedPtr node, std::string node_name, - std::vector & param_names) -{ - auto client = node->create_client( - node_name + "/get_parameters"); - - while (!client->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { - throw std::runtime_error("client interrupted while waiting for service to appear."); - } - - throw std::runtime_error( - std::string("GetParameters service for ") + - node_name + " not available"); - } - - auto request = std::make_shared(); - request->names = param_names; - - auto result_future = client->async_send_request(request); - - if (rclcpp::spin_until_future_complete(node, result_future, 1s) != - rclcpp::FutureReturnCode::SUCCESS) - { - throw std::runtime_error(std::string("service call to \"") + node_name + "\" failed"); - } - - return result_future.get()->values; -} - -static std::vector -get_param_descriptors_for_node( - rclcpp::Node::SharedPtr node, std::string node_name, - std::vector & param_names) -{ - auto client = node->create_client( - node_name + "/describe_parameters"); - - while (!client->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { - throw std::runtime_error("client interrupted while waiting for service to appear."); - } - - throw std::runtime_error( - std::string("DescribeParameters service for ") + - node_name + " not available"); - } - - auto request = std::make_shared(); - request->names = param_names; - - auto result_future = client->async_send_request(request); - - if (rclcpp::spin_until_future_complete(node, result_future, 1s) != - rclcpp::FutureReturnCode::SUCCESS) - { - throw std::runtime_error(std::string("service call to \"") + node_name + "\" failed"); - } - - return result_future.get()->descriptors; -} - -// A local version to avoid trailing zeros -static std::string to_string(const rcl_interfaces::msg::ParameterValue & param_value) -{ - switch (param_value.type) { - case rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET: - return "NOT_SET"; - - case rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE: - { - // remove trailing zeroes - std::ostringstream out; - out << param_value.double_value; - return out.str(); - } - - case rcl_interfaces::msg::ParameterType::PARAMETER_STRING: - return std::string("\"") + param_value.string_value + std::string("\""); - - case rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY: - { - std::stringstream stream; - stream << "["; - auto num_items = param_value.string_array_value.size(); - for (unsigned i = 0; i < num_items; i++) { - stream << "\"" << param_value.string_array_value[i] << "\""; - if (i != num_items - 1) { - stream << ", "; - } - } - stream << "]"; - return stream.str(); - }; - - case rcl_interfaces::msg::ParameterType::PARAMETER_BOOL: - { - return param_value.bool_value ? "True" : "False"; - }; - - case rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY: - { - std::stringstream stream; - stream << "["; - auto num_items = param_value.bool_array_value.size(); - for (unsigned i = 0; i < num_items; i++) { - stream << (param_value.bool_array_value[i] ? "True" : "False"); - if (i != num_items - 1) { - stream << ", "; - } - } - stream << "]"; - return stream.str(); - }; - - default: - { - return rclcpp::Parameter{"", param_value}.value_to_string(); - } - } -} - -static void -print_yaml( - const std::string node_name, std::vector & param_names, - const std::vector & param_values, - const std::vector & param_descriptors, bool verbose) -{ - std::cout << node_name << ":" << std::endl; - std::cout << " ros__parameters:" << std::endl; - - for (unsigned i = 0; i < param_names.size(); i++) { - auto param_str = to_string(param_values[i]); - - // Use a field width wide enough for all of the headers - auto fw = 30; - - if (verbose) { - std::cout << " " << std::left << std::setw(fw + 2) << - param_names[i] + ":" << param_str << "\n"; - } else { - std::cout << " " << param_names[i] << ": " << param_str << "\n"; - } - - if (verbose) { - std::cout << " # " << std::left << std::setw(fw) << "Range: "; - if (param_descriptors[i].floating_point_range.size()) { - auto range = param_descriptors[i].floating_point_range[0]; - std::cout << range.from_value << ";" << - range.to_value << ";" << - range.step << "\n"; - } else if (param_descriptors[i].integer_range.size()) { - auto range = param_descriptors[i].integer_range[0]; - std::cout << range.from_value << ";" << - range.to_value << ";" << - range.step << "\n"; - } else { - std::cout << "N/A\n"; - } - - std::cout << " # " << std::left << std::setw(fw) << "Description: " << - param_descriptors[i].description << "\n"; - std::cout << " # " << std::left << std::setw(fw) << "Additional constraints: " << - param_descriptors[i].additional_constraints << "\n"; - std::cout << " # " << std::left << std::setw(fw) << "Read-only: " << - (param_descriptors[i].read_only ? "True" : "False") << "\n"; - - std::cout << std::endl; - } - } - - std::cout << std::endl; -} - -static void -print_markdown( - const std::string node_name, std::vector & param_names, - const std::vector & param_values, - const std::vector & param_descriptors, - bool verbose) -{ - std::cout << "## " << node_name << " Parameters" << "\n"; - - if (verbose) { - std::cout << "|Parameter|Default Value|Range|Description|Additional Constraints|Read-Only|" << - "\n"; - std::cout << "|---|---|---|---|---|---|" << "\n"; - } else { - std::cout << "|Parameter|Default Value|" << "\n"; - std::cout << "|---|---|" << "\n"; - } - - for (unsigned i = 0; i < param_names.size(); i++) { - auto param_str = to_string(param_values[i]); - - std::cout << "|" << param_names[i] << "|" << param_str; - - if (verbose) { - if (param_descriptors[i].floating_point_range.size()) { - auto range = param_descriptors[i].floating_point_range[0]; - std::cout << "|" << - range.from_value << ";" << - range.to_value << ";" << - range.step << "|"; - } else if (param_descriptors[i].integer_range.size()) { - auto range = param_descriptors[i].integer_range[0]; - std::cout << "|" << - range.from_value << ";" << - range.to_value << ";" << - range.step << "|"; - } else { - // No range specified - std::cout << "|N/A"; - } - - std::cout << "|" << - param_descriptors[i].description << "|" << - param_descriptors[i].additional_constraints << "|" << - (param_descriptors[i].read_only ? "True" : "False"); - } - - // End the parameter - std::cout << "|\n"; - } - - std::cout << std::endl; -} - -template -struct option_sequence -{ - std::vector values; -}; - -template -void -validate( - boost::any & v, const std::vector & values, - option_sequence * /*target_type*/, int) -{ - std::vector result; - typedef std::vector strings; - for (strings::const_iterator iter = values.begin(); iter != values.end(); ++iter) { - strings tks; - alg::split(tks, *iter, alg::is_any_of(",")); - for (strings::const_iterator tk = tks.begin(); tk != tks.end(); ++tk) { - result.push_back(boost::lexical_cast(*tk)); - } - } - v = option_sequence(); - boost::any_cast &>(v).values.swap(result); -} - -#ifdef _WIN32 -static const char * basename(const char * filepath) -{ - const char * base = std::strrchr(filepath, '/'); - return base ? (base + 1) : filepath; -} -#endif - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto dump_params_node = rclcpp::Node::make_shared("dump_params"); - - try { - po::options_description desc("Options"); - - /* *INDENT-OFF* */ - desc.add_options()("help,h", "Print help message") - ("node_names,n", po::value>(), - "A list of comma-separated node names") - ("format,f", po::value(), "The format to dump ('yaml' or 'markdown')") - ("verbose,v", "Verbose option") - ; - /* *INDENT-ON* */ - - po::variables_map vm; - po::store(po::parse_command_line(argc, argv, desc), vm); - po::notify(vm); - - if (vm.count("help")) { - std::cout << "Usage: " << basename(argv[0]) << "\n"; - std::cout << desc << "\n"; - return 1; - } - - std::vector node_names; - - if (vm.count("node_names")) { - node_names = vm["node_names"].as>().values; - } else { - node_names = dump_params_node->get_node_names(); - } - - bool verbose = vm.count("verbose"); - - for (std::string target_node_name : node_names) { - // Skip hidden nodes - if (target_node_name[1] == '_') { - continue; - } - - try { - auto param_names = get_param_names_for_node(dump_params_node, target_node_name); - auto param_values = - get_param_values_for_node(dump_params_node, target_node_name, param_names); - auto param_descriptors = get_param_descriptors_for_node( - dump_params_node, target_node_name, - param_names); - - if (!vm.count("format")) { - // Default to YAML if the format hasn't been specified - print_yaml(target_node_name, param_names, param_values, param_descriptors, verbose); - } else { - auto format = vm["format"].as(); - if (format == "md" || format == "markdown") { - print_markdown(target_node_name, param_names, param_values, param_descriptors, verbose); - } else { - if (format != "yaml") { - std::cerr << "Unknown output format specified, defaulting to 'yaml'" << std::endl; - } - print_yaml(target_node_name, param_names, param_values, param_descriptors, verbose); - } - } - } catch (std::exception & e) { - std::cerr << "Error: " << e.what() << "\n" << std::endl; - } - } - } catch (po::error & e) { - std::cerr << "Error: " << e.what() << std::endl; - } - - rclcpp::shutdown(); - return 0; -} diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index e3b9bd6279..9868a38bbb 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -23,45 +23,24 @@ namespace nav2_util { -// The nav2_util::LifecycleNode class is temporary until we get the -// required support for lifecycle nodes in MessageFilter, TransformListener, -// and TransforBroadcaster. We have submitted issues for these and will -// be submitting PRs to add the fixes: -// -// https://github.com/ros2/geometry2/issues/95 -// https://github.com/ros2/geometry2/issues/94 -// https://github.com/ros2/geometry2/issues/70 -// -// Until then, this class can provide a normal ROS node that has a thread -// that processes the node's messages. If a derived class needs to interface -// to one of these classes - MessageFilter, etc. - that don't yet support -// lifecycle nodes, it can simply set the use_rclcpp_node flag in the -// constructor and then provide the rclcpp_node_ to the helper classes, like -// MessageFilter. -// - LifecycleNode::LifecycleNode( const std::string & node_name, - const std::string & namespace_, bool use_rclcpp_node, + const std::string & ns, const rclcpp::NodeOptions & options) -: rclcpp_lifecycle::LifecycleNode(node_name, namespace_, options), - use_rclcpp_node_(use_rclcpp_node) +: rclcpp_lifecycle::LifecycleNode(node_name, ns, options) { - if (use_rclcpp_node_) { - std::vector new_args = options.arguments(); - new_args.push_back("--ros-args"); - new_args.push_back("-r"); - 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)); - rclcpp_thread_ = std::make_unique(rclcpp_node_); - } - print_lifecycle_node_notification(); + // server side never times out from lifecycle manager + this->declare_parameter(bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true); + this->set_parameter( + rclcpp::Parameter( + bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true)); + + printLifecycleNodeNotification(); } LifecycleNode::~LifecycleNode() { + RCLCPP_INFO(get_logger(), "Destroying"); // In case this lifecycle node wasn't properly shut down, do it here if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) @@ -71,7 +50,30 @@ LifecycleNode::~LifecycleNode() } } -void LifecycleNode::print_lifecycle_node_notification() +void LifecycleNode::createBond() +{ + RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name()); + + bond_ = std::make_unique( + std::string("bond"), + this->get_name(), + shared_from_this()); + + bond_->setHeartbeatPeriod(0.10); + bond_->setHeartbeatTimeout(4.0); + bond_->start(); +} + +void LifecycleNode::destroyBond() +{ + RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name()); + + if (bond_) { + bond_.reset(); + } +} + +void LifecycleNode::printLifecycleNodeNotification() { RCLCPP_INFO( get_logger(), diff --git a/nav2_util/src/lifecycle_service_client.cpp b/nav2_util/src/lifecycle_service_client.cpp index dfad1efe38..788c44123a 100644 --- a/nav2_util/src/lifecycle_service_client.cpp +++ b/nav2_util/src/lifecycle_service_client.cpp @@ -25,6 +25,7 @@ using nav2_util::generate_internal_node; using std::chrono::seconds; using std::make_shared; using std::string; +using namespace std::chrono_literals; namespace nav2_util { @@ -34,6 +35,8 @@ LifecycleServiceClient::LifecycleServiceClient(const string & lifecycle_node_nam change_state_(lifecycle_node_name + "/change_state", node_), get_state_(lifecycle_node_name + "/get_state", node_) { + // Block until server is up + get_state_.wait_for_service(); } LifecycleServiceClient::LifecycleServiceClient( @@ -43,22 +46,31 @@ LifecycleServiceClient::LifecycleServiceClient( change_state_(lifecycle_node_name + "/change_state", node_), get_state_(lifecycle_node_name + "/get_state", node_) { + // Block until server is up + get_state_.wait_for_service(); } -void LifecycleServiceClient::change_state( +bool LifecycleServiceClient::change_state( const uint8_t transition, const seconds timeout) { - change_state_.wait_for_service(timeout); + if (!change_state_.wait_for_service(timeout)) { + throw std::runtime_error("change_state service is not available!"); + } + auto request = std::make_shared(); request->transition.id = transition; - change_state_.invoke(request, timeout); + auto response = change_state_.invoke(request, timeout); + return response.get(); } bool LifecycleServiceClient::change_state( std::uint8_t transition) { - change_state_.wait_for_service(); + if (!change_state_.wait_for_service(5s)) { + throw std::runtime_error("change_state service is not available!"); + } + auto request = std::make_shared(); auto response = std::make_shared(); request->transition.id = transition; @@ -68,7 +80,10 @@ bool LifecycleServiceClient::change_state( uint8_t LifecycleServiceClient::get_state( const seconds timeout) { - get_state_.wait_for_service(timeout); + if (!get_state_.wait_for_service(timeout)) { + throw std::runtime_error("get_state service is not available!"); + } + auto request = std::make_shared(); auto result = get_state_.invoke(request, timeout); return result->current_state.id; diff --git a/nav2_util/src/node_thread.cpp b/nav2_util/src/node_thread.cpp index e498e1371d..c19fbd9847 100644 --- a/nav2_util/src/node_thread.cpp +++ b/nav2_util/src/node_thread.cpp @@ -22,19 +22,25 @@ namespace nav2_util NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) : node_(node_base) { + executor_ = std::make_shared(); thread_ = std::make_unique( [&]() { - executor_.add_node(node_); - executor_.spin(); - executor_.remove_node(node_); + executor_->add_node(node_); + executor_->spin(); + executor_->remove_node(node_); }); } +NodeThread::NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor) +: executor_(executor) +{ + thread_ = std::make_unique([&]() {executor_->spin();}); +} NodeThread::~NodeThread() { - executor_.cancel(); + executor_->cancel(); thread_->join(); } diff --git a/nav2_util/src/odometry_utils.cpp b/nav2_util/src/odometry_utils.cpp index f866bd9244..9bf585bdfe 100644 --- a/nav2_util/src/odometry_utils.cpp +++ b/nav2_util/src/odometry_utils.cpp @@ -24,13 +24,33 @@ namespace nav2_util { OdomSmoother::OdomSmoother( - rclcpp::Node::SharedPtr nh, + const rclcpp::Node::WeakPtr & parent, double filter_duration, - std::string odom_topic) -: node_(nh), - odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) + const std::string & odom_topic) +: odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) { - odom_sub_ = nh->create_subscription( + 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; +} + +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)); diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 6f735e6d25..e99d9e3334 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -26,33 +26,42 @@ namespace nav2_util bool getCurrentPose( geometry_msgs::msg::PoseStamped & global_pose, tf2_ros::Buffer & tf_buffer, const std::string global_frame, - const std::string robot_frame, const double transform_timeout) + const std::string robot_frame, const double transform_timeout, + const rclcpp::Time stamp) { - 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 = stamp; + + 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 +69,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/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 4d3b5511dc..9f1ae99bbc 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -3,7 +3,8 @@ ament_add_gtest(test_execution_timer test_execution_timer.cpp) ament_add_gtest(test_node_utils test_node_utils.cpp) target_link_libraries(test_node_utils ${library_name}) -find_package(std_srvs) +find_package(std_srvs REQUIRED) +find_package(test_msgs REQUIRED) ament_add_gtest(test_service_client test_service_client.cpp) ament_target_dependencies(test_service_client std_srvs) @@ -40,38 +41,3 @@ target_link_libraries(test_odometry_utils ${library_name}) ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) - -# This test is disabled due to failing services -# https://github.com/ros-planning/navigation2/issues/1836 - -add_launch_test( - "test_dump_params/test_dump_params_default.test.py" - TARGET "test_dump_params_default" - TIMEOUT 10 - ENV - TEST_EXECUTABLE=$ -) - -add_launch_test( - "test_dump_params/test_dump_params_yaml.test.py" - TARGET "test_dump_params_yaml" - TIMEOUT 10 - ENV - TEST_EXECUTABLE=$ -) - -add_launch_test( - "test_dump_params/test_dump_params_md.test.py" - TARGET "test_dump_params_md" - TIMEOUT 10 - ENV - TEST_EXECUTABLE=$ -) - -add_launch_test( - "test_dump_params/test_dump_params_multiple.test.py" - TARGET "test_dump_params_multiple" - TIMEOUT 10 - ENV - TEST_EXECUTABLE=$ -) diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 84cef8576c..35043d5df2 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -236,6 +236,8 @@ class ActionTest : public ::testing::Test TEST_F(ActionTest, test_simple_action) { + node_->activate_server(); + // The goal for this invocation auto goal = Fibonacci::Goal(); goal.order = 12; diff --git a/nav2_util/test/test_dump_params/dump_params_default.txt b/nav2_util/test/test_dump_params/dump_params_default.txt deleted file mode 100644 index 1d9f982e1d..0000000000 --- a/nav2_util/test/test_dump_params/dump_params_default.txt +++ /dev/null @@ -1,4 +0,0 @@ -/dump_params: - ros__parameters: - use_sim_time: False - diff --git a/nav2_util/test/test_dump_params/dump_params_error.txt b/nav2_util/test/test_dump_params/dump_params_error.txt deleted file mode 100644 index 814e4ce6e1..0000000000 --- a/nav2_util/test/test_dump_params/dump_params_error.txt +++ /dev/null @@ -1 +0,0 @@ -Error: ListParameters service for test_dump_params_error not available diff --git a/nav2_util/test/test_dump_params/dump_params_help.txt b/nav2_util/test/test_dump_params/dump_params_help.txt deleted file mode 100644 index 765385262b..0000000000 --- a/nav2_util/test/test_dump_params/dump_params_help.txt +++ /dev/null @@ -1,7 +0,0 @@ -Usage: dump_params -Options: - -h [ --help ] Print help message - -n [ --node_names ] arg A list of comma-separated node names - -f [ --format ] arg The format to dump ('yaml' or 'markdown') - -v [ --verbose ] Verbose option - diff --git a/nav2_util/test/test_dump_params/dump_params_md.txt b/nav2_util/test/test_dump_params/dump_params_md.txt deleted file mode 100644 index 25144298f8..0000000000 --- a/nav2_util/test/test_dump_params/dump_params_md.txt +++ /dev/null @@ -1,15 +0,0 @@ -## test_dump_params Parameters -|Parameter|Default Value| -|---|---| -|use_sim_time|False| -|param_not_set|NOT_SET| -|param_bool|True| -|param_int|1234| -|param_double|3.14| -|param_string|"foobar"| -|param_bool_array|[True, False]| -|param_int_array|[1, 2, 3]| -|param_double_array|[1.5, 23.5012, 123.001]| -|param_string_array|["foo", "bar"]| -|param_byte_array|[0x1, 0x2, 0x3, 0x4]| - 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 deleted file mode 100644 index 929c8c93fe..0000000000 --- a/nav2_util/test/test_dump_params/dump_params_md_verbose.txt +++ /dev/null @@ -1,15 +0,0 @@ -## test_dump_params Parameters -|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| -|param_string|"foobar"|N/A|||False| -|param_bool_array|[True, False]|N/A|||False| -|param_int_array|[1, 2, 3]|N/A|||False| -|param_double_array|[1.5, 23.5012, 123.001]|-1000.5;1000.5;0.0001||||False| -|param_string_array|["foo", "bar"]|N/A|||False| -|param_byte_array|[0x1, 0x2, 0x3, 0x4]|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 deleted file mode 100644 index 5e79078155..0000000000 --- a/nav2_util/test/test_dump_params/dump_params_multiple.txt +++ /dev/null @@ -1,28 +0,0 @@ -test_dump_params: - ros__parameters: - use_sim_time: False - param_not_set: NOT_SET - param_bool: True - param_int: 1234 - param_double: 3.14 - param_string: "foobar" - param_bool_array: [True, False] - param_int_array: [1, 2, 3] - param_double_array: [1.5, 23.5012, 123.001] - param_string_array: ["foo", "bar"] - param_byte_array: [0x1, 0x2, 0x3, 0x4] - -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 - param_string: "foobar" - param_bool_array: [True, False] - param_int_array: [1, 2, 3] - param_double_array: [1.5, 23.5012, 123.001] - param_string_array: ["foo", "bar"] - param_byte_array: [0x1, 0x2, 0x3, 0x4] - diff --git a/nav2_util/test/test_dump_params/dump_params_yaml.txt b/nav2_util/test/test_dump_params/dump_params_yaml.txt deleted file mode 100644 index 7c81e3fd5d..0000000000 --- a/nav2_util/test/test_dump_params/dump_params_yaml.txt +++ /dev/null @@ -1,14 +0,0 @@ -test_dump_params: - ros__parameters: - use_sim_time: False - param_not_set: NOT_SET - param_bool: True - param_int: 1234 - param_double: 3.14 - param_string: "foobar" - param_bool_array: [True, False] - param_int_array: [1, 2, 3] - param_double_array: [1.5, 23.5012, 123.001] - param_string_array: ["foo", "bar"] - param_byte_array: [0x1, 0x2, 0x3, 0x4] - 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 deleted file mode 100644 index 2efd430a8a..0000000000 --- a/nav2_util/test/test_dump_params/dump_params_yaml_verbose.txt +++ /dev/null @@ -1,69 +0,0 @@ -test_dump_params: - ros__parameters: - use_sim_time: False - # Range: N/A - # Description: - # 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 - # Additional constraints: - # Read-only: True - - param_int: 1234 - # Range: -1000;10000;2 - # Description: - # Additional constraints: - # Read-only: False - - param_double: 3.14 - # Range: N/A - # Description: - # Additional constraints: - # Read-only: False - - param_string: "foobar" - # Range: N/A - # Description: - # Additional constraints: - # Read-only: False - - param_bool_array: [True, False] - # Range: N/A - # Description: - # Additional constraints: - # Read-only: False - - param_int_array: [1, 2, 3] - # Range: N/A - # Description: - # Additional constraints: - # Read-only: False - - param_double_array: [1.5, 23.5012, 123.001] - # Range: -1000.5;1000.5;0.0001 - # Description: - # Additional constraints: - # Read-only: False - - param_string_array: ["foo", "bar"] - # Range: N/A - # Description: - # Additional constraints: - # Read-only: False - - param_byte_array: [0x1, 0x2, 0x3, 0x4] - # Range: N/A - # Description: - # Additional constraints: - # Read-only: False - - diff --git a/nav2_util/test/test_dump_params/test_dump_params_default.test.py b/nav2_util/test/test_dump_params/test_dump_params_default.test.py deleted file mode 100644 index e0849b398f..0000000000 --- a/nav2_util/test/test_dump_params/test_dump_params_default.test.py +++ /dev/null @@ -1,100 +0,0 @@ -#! /usr/bin/env python3 -# 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. - -import os - -import unittest - -from launch import LaunchDescription -from launch.actions import ExecuteProcess - -import launch_testing -import launch_testing.actions -import launch_testing.asserts -import launch_testing.util -import launch_testing_ros - -import pytest - - -@pytest.mark.launch_test -def generate_test_description(): - launch_description = LaunchDescription() - launch_description.add_action( - ExecuteProcess( - cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], - name='test_dump_params') - ) - processes_to_test = [ - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-h'], - name='test_dump_params_help', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE')], - name='test_dump_params_default', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params_error'], - name='test_dump_params_error', - output='screen') - ] - for process in processes_to_test: - launch_description.add_action(process) - launch_description.add_action( - launch_testing.actions.ReadyToTest() - ) - return launch_description, {'processes_to_test': processes_to_test} - - -# Tests without a unittest to run concurrently with -# the processes under test throw an exception -# The following is a dummy test to suppress the traceback -# https://github.com/ros2/launch/issues/380 - -class TestLoggingOutputFormat(unittest.TestCase): - - def test_logging_output(self, proc_info, processes_to_test): - for process_name in processes_to_test: - proc_info.assertWaitForShutdown(process=process_name, timeout=10) - - -@launch_testing.post_shutdown_test() -class TestDumpParams(unittest.TestCase): - - def test_processes_output(self, proc_output, processes_to_test): - """Test all processes output against expectations.""" - from launch_testing.tools.output import get_default_filtered_prefixes - output_filter = launch_testing_ros.tools.basic_output_filter( - filtered_prefixes=get_default_filtered_prefixes() - ) - output_files = [ - os.path.join(os.path.dirname(__file__), out) - for out in ['dump_params_help', - 'dump_params_default', - 'dump_params_error'] - ] - for process, output_file in zip(processes_to_test[:-1], output_files[:-1]): - launch_testing.asserts.assertInStdout( - proc_output, - expected_output=launch_testing.tools.expected_output_from_file( - path=output_file), - process=process, output_filter=output_filter - ) - launch_testing.asserts.assertInStderr( - proc_output, - expected_output=launch_testing.tools.expected_output_from_file(path=output_files[-1]), - process=processes_to_test[-1], - output_filter=output_filter) diff --git a/nav2_util/test/test_dump_params/test_dump_params_md.test.py b/nav2_util/test/test_dump_params/test_dump_params_md.test.py deleted file mode 100644 index 61a85c514c..0000000000 --- a/nav2_util/test/test_dump_params/test_dump_params_md.test.py +++ /dev/null @@ -1,90 +0,0 @@ -#! /usr/bin/env python3 -# 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. - -import os - -import unittest - -from launch import LaunchDescription -from launch.actions import ExecuteProcess - -import launch_testing -import launch_testing.actions -import launch_testing.asserts -import launch_testing.util -import launch_testing_ros - -import pytest - - -@pytest.mark.launch_test -def generate_test_description(): - launch_description = LaunchDescription() - launch_description.add_action( - ExecuteProcess( - cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], - name='test_dump_params') - ) - processes_to_test = [ - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params'], - name='test_dump_params_markdown', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params', '-v'], - name='test_dump_params_markdown_verbose', - output='screen') - ] - for process in processes_to_test: - launch_description.add_action(process) - launch_description.add_action( - launch_testing.actions.ReadyToTest() - ) - return launch_description, {'processes_to_test': processes_to_test} - - -# Tests without a unittest to run concurrently with -# the processes under test throw an exception -# The following is a dummy test to suppress the traceback -# https://github.com/ros2/launch/issues/380 - -class TestLoggingOutputFormat(unittest.TestCase): - - def test_logging_output(self, proc_info, processes_to_test): - for process_name in processes_to_test: - proc_info.assertWaitForShutdown(process=process_name, timeout=10) - - -@launch_testing.post_shutdown_test() -class TestDumpParams(unittest.TestCase): - - def test_processes_output(self, proc_output, processes_to_test): - """Test all processes output against expectations.""" - from launch_testing.tools.output import get_default_filtered_prefixes - output_filter = launch_testing_ros.tools.basic_output_filter( - filtered_prefixes=get_default_filtered_prefixes() - ) - output_files = [ - os.path.join(os.path.dirname(__file__), out) - for out in ['dump_params_md', - 'dump_params_md_verbose'] - ] - for process, output_file in zip(processes_to_test, output_files): - launch_testing.asserts.assertInStdout( - proc_output, - expected_output=launch_testing.tools.expected_output_from_file( - path=output_file), - process=process, output_filter=output_filter - ) diff --git a/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py b/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py deleted file mode 100644 index 4733c19f58..0000000000 --- a/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py +++ /dev/null @@ -1,96 +0,0 @@ -#! /usr/bin/env python3 -# 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. - -import os - -import unittest - -from launch import LaunchDescription -from launch.actions import ExecuteProcess - -import launch_testing -import launch_testing.actions -import launch_testing.asserts -import launch_testing.util -import launch_testing_ros - -import pytest - - -@pytest.mark.launch_test -def generate_test_description(): - launch_description = LaunchDescription() - launch_description.add_action( - ExecuteProcess( - cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], - name='test_dump_params') - ) - launch_description.add_action( - ExecuteProcess( - cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py'), - 'test_dump_params_copy'], - name='test_dump_params_copy') - ) - processes_to_test = [ - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'error', '-n', 'test_dump_params'], - name='test_dump_params_bad_format', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params,test_dump_params_copy'], - name='test_dump_params_multiple', - output='screen') - ] - for process in processes_to_test: - launch_description.add_action(process) - launch_description.add_action( - launch_testing.actions.ReadyToTest() - ) - return launch_description, {'processes_to_test': processes_to_test} - - -# Tests without a unittest to run concurrently with -# the processes under test throw an exception -# The following is a dummy test to suppress the traceback -# https://github.com/ros2/launch/issues/380 - -class TestLoggingOutputFormat(unittest.TestCase): - - def test_logging_output(self, proc_info, processes_to_test): - for process_name in processes_to_test: - proc_info.assertWaitForShutdown(process=process_name, timeout=10) - - -@launch_testing.post_shutdown_test() -class TestDumpParams(unittest.TestCase): - - def test_processes_output(self, proc_output, processes_to_test): - """Test all processes output against expectations.""" - from launch_testing.tools.output import get_default_filtered_prefixes - output_filter = launch_testing_ros.tools.basic_output_filter( - filtered_prefixes=get_default_filtered_prefixes() - ) - output_files = [ - os.path.join(os.path.dirname(__file__), out) - for out in ['dump_params_yaml', - 'dump_params_multiple'] - ] - for process, output_file in zip(processes_to_test[:-1], output_files[:-1]): - launch_testing.asserts.assertInStdout( - proc_output, - expected_output=launch_testing.tools.expected_output_from_file( - path=output_file), - process=process, output_filter=output_filter - ) 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 deleted file mode 100755 index 4e579df08d..0000000000 --- a/nav2_util/test/test_dump_params/test_dump_params_node.py +++ /dev/null @@ -1,67 +0,0 @@ -#! /usr/bin/env python3 -# 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. - -import sys - -from rcl_interfaces.msg import FloatingPointRange, IntegerRange -import rclpy -from rclpy.node import Node, ParameterDescriptor - - -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)) - self.declare_parameter('param_int', - 1234, - ParameterDescriptor(integer_range=[IntegerRange( - from_value=-1000, to_value=10000, step=2)])) - self.declare_parameter('param_double', 3.14) - self.declare_parameter('param_string', 'foobar') - self.declare_parameter('param_bool_array', [True, False]) - self.declare_parameter('param_int_array', [1, 2, 3]) - self.declare_parameter('param_double_array', - [1.50000, 23.50120, 123.0010], - ParameterDescriptor(floating_point_range=[FloatingPointRange( - from_value=-1000.5, to_value=1000.5, step=0.0001)]) - ) - self.declare_parameter('param_string_array', ['foo', 'bar']) - self.declare_parameter('param_byte_array', [b'\x01', b'\x02', b'\x03', b'\x04']) - - -def main(args=None): - rclpy.init(args=args) - - name = 'test_dump_params' - if len(sys.argv) > 1: - name = sys.argv[1] - - node = TestDumpParamsNode(name) - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py b/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py deleted file mode 100644 index 545bf4934c..0000000000 --- a/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py +++ /dev/null @@ -1,90 +0,0 @@ -#! /usr/bin/env python3 -# 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. - -import os - -import unittest - -from launch import LaunchDescription -from launch.actions import ExecuteProcess - -import launch_testing -import launch_testing.actions -import launch_testing.asserts -import launch_testing.util -import launch_testing_ros - -import pytest - - -@pytest.mark.launch_test -def generate_test_description(): - launch_description = LaunchDescription() - launch_description.add_action( - ExecuteProcess( - cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], - name='test_dump_params') - ) - processes_to_test = [ - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params'], - name='test_dump_params_yaml', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params', '-v'], - name='test_dump_params_yaml_verbose', - output='screen') - ] - for process in processes_to_test: - launch_description.add_action(process) - launch_description.add_action( - launch_testing.actions.ReadyToTest() - ) - return launch_description, {'processes_to_test': processes_to_test} - - -# Tests without a unittest to run concurrently with -# the processes under test throw an exception -# The following is a dummy test to suppress the traceback -# https://github.com/ros2/launch/issues/380 - -class TestLoggingOutputFormat(unittest.TestCase): - - def test_logging_output(self, proc_info, processes_to_test): - for process_name in processes_to_test: - proc_info.assertWaitForShutdown(process=process_name, timeout=10) - - -@launch_testing.post_shutdown_test() -class TestDumpParams(unittest.TestCase): - - def test_processes_output(self, proc_output, processes_to_test): - """Test all processes output against expectations.""" - from launch_testing.tools.output import get_default_filtered_prefixes - output_filter = launch_testing_ros.tools.basic_output_filter( - filtered_prefixes=get_default_filtered_prefixes() - ) - output_files = [ - os.path.join(os.path.dirname(__file__), out) - for out in ['dump_params_yaml', - 'dump_params_yaml_verbose'] - ] - for process, output_file in zip(processes_to_test, output_files): - launch_testing.asserts.assertInStdout( - proc_output, - expected_output=launch_testing.tools.expected_output_from_file( - path=output_file), - process=process, output_filter=output_filter - ) diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index 9c379575bb..38f27ad304 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -16,11 +16,13 @@ #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) +TEST(GeometryUtils, euclidean_distance_point_3d) { geometry_msgs::msg::Point point1; point1.x = 3.0; @@ -32,10 +34,40 @@ TEST(GeometryUtils, euclidean_distance_point) point2.y = 2.0; point2.z = 3.0; - ASSERT_NEAR(euclidean_distance(point1, point2), 2.82843, 1e-5); + ASSERT_NEAR(euclidean_distance(point1, point2, true), 2.82843, 1e-5); } -TEST(GeometryUtils, euclidean_distance_pose) +TEST(GeometryUtils, euclidean_distance_point_2d) +{ + geometry_msgs::msg::Point point1; + point1.x = 3.0; + point1.y = 2.0; + point1.z = 1.0; + + geometry_msgs::msg::Point point2; + point2.x = 1.0; + point2.y = 2.0; + point2.z = 3.0; + + ASSERT_NEAR(euclidean_distance(point1, point2), 2.0, 1e-5); +} + +TEST(GeometryUtils, euclidean_distance_pose_3d) +{ + geometry_msgs::msg::Pose pose1; + pose1.position.x = 7.0; + pose1.position.y = 4.0; + pose1.position.z = 3.0; + + geometry_msgs::msg::Pose pose2; + pose2.position.x = 17.0; + pose2.position.y = 6.0; + pose2.position.z = 2.0; + + ASSERT_NEAR(euclidean_distance(pose1, pose2, true), 10.24695, 1e-5); +} + +TEST(GeometryUtils, euclidean_distance_pose_2d) { geometry_msgs::msg::Pose pose1; pose1.position.x = 7.0; @@ -47,5 +79,52 @@ TEST(GeometryUtils, euclidean_distance_pose) pose2.position.y = 6.0; pose2.position.z = 2.0; - ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.24695, 1e-5); + ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.19804, 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_lifecycle_node.cpp b/nav2_util/test/test_lifecycle_node.cpp index 358e83ddbe..fd204df47f 100644 --- a/nav2_util/test/test_lifecycle_node.cpp +++ b/nav2_util/test/test_lifecycle_node.cpp @@ -33,7 +33,7 @@ RclCppFixture g_rclcppfixture; TEST(LifecycleNode, RclcppNodeExitsCleanly) { // Make sure the node exits cleanly when using an rclcpp_node and associated thread - auto node1 = std::make_shared("test_node", "", true); + auto node1 = std::make_shared("test_node", ""); std::this_thread::sleep_for(std::chrono::seconds(1)); SUCCEED(); } @@ -41,11 +41,8 @@ TEST(LifecycleNode, RclcppNodeExitsCleanly) TEST(LifecycleNode, MultipleRclcppNodesExitCleanly) { // Try a couple nodes w/ rclcpp_node and threads - auto node1 = std::make_shared("test_node1", "", true); - auto node2 = std::make_shared("test_node2", "", true); - - // Another one without rclcpp_node and on the stack - nav2_util::LifecycleNode node3("test_node3", "", false); + auto node1 = std::make_shared("test_node1", ""); + auto node2 = std::make_shared("test_node2", ""); std::this_thread::sleep_for(std::chrono::seconds(1)); SUCCEED(); diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index d3b50b23eb..7c43927161 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -88,6 +88,7 @@ TEST(DeclareParameterIfNotDeclared, DeclareParameterIfNotDeclared) TEST(GetPluginTypeParam, GetPluginTypeParam) { + ::testing::FLAGS_gtest_death_test_style = "threadsafe"; auto node = std::make_shared("test_node"); node->declare_parameter("Foo.plugin", "bar"); ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar"); diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp index a3530048a4..bd277515ab 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 bc1b551812..521d3f3056 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 0782111271..baff10d684 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,11 +223,35 @@ 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 - int dz = int(z1) - int(z0); // NOLINT + // 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, min_x0, min_y0, min_z0; + if (dist > 0.0) { + scale = std::min(1.0, max_length / dist); + + // Updating starting point to the point at distance min_length from the initial point + min_x0 = x0 + (x1 - x0) / dist * min_length; + min_y0 = y0 + (y1 - y0) / dist * min_length; + min_z0 = z0 + (z1 - z0) / dist * min_length; + } else { + // dist can be 0 if [x0, y0, z0]==[x1, y1, z1]. + // In this case only this voxel should be processed. + scale = 1.0; + min_x0 = x0; + min_y0 = y0; + min_z0 = z0; + } + + int dx = int(x1) - int(min_x0); // NOLINT + int dy = int(y1) - int(min_y0); // NOLINT + int dz = int(z1) - int(min_z0); // NOLINT unsigned int abs_dx = abs(dx); unsigned int abs_dy = abs(dy); @@ -237,17 +261,12 @@ 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; + 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); - // 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)); - double scale = std::min(1.0, max_length / dist); - // is x dominant if (abs_dx >= max(abs_dy, abs_dz)) { int error_y = abs_dx / 2; diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 7eaccca6e4..e11e73b3db 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.7 + 1.1.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 c41bc40533..44a63e2e31 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/CMakeLists.txt b/nav2_voxel_grid/test/CMakeLists.txt index eb68ad8b3a..ff01e4bde1 100644 --- a/nav2_voxel_grid/test/CMakeLists.txt +++ b/nav2_voxel_grid/test/CMakeLists.txt @@ -1,2 +1,5 @@ ament_add_gtest(voxel_grid_tests voxel_grid_tests.cpp) target_link_libraries(voxel_grid_tests voxel_grid) + +ament_add_gtest(voxel_grid_bresenham_3d voxel_grid_bresenham_3d.cpp) +target_link_libraries(voxel_grid_bresenham_3d voxel_grid) diff --git a/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp new file mode 100644 index 0000000000..cce36e7fc4 --- /dev/null +++ b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp @@ -0,0 +1,156 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2021 Samsung Research Russia +* 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 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. +* +* Author: Alexey Merzlyakov +*********************************************************************/ +#include +#include + +class TestVoxel +{ +public: + explicit TestVoxel(uint32_t * data, int sz_x, int sz_y) + : data_(data) + { + size_ = sz_x * sz_y; + } + inline void operator()(unsigned int off, unsigned int val) + { + ASSERT_TRUE(off < size_); + data_[off] = val; + } + inline unsigned int operator()(unsigned int off) + { + return data_[off]; + } + +private: + uint32_t * data_; + unsigned int size_; +}; + +TEST(voxel_grid, bresenham3DBoundariesCheck) +{ + const int sz_x = 60; + const int sz_y = 60; + const int sz_z = 2; + const unsigned int max_length = 60; + const unsigned int min_length = 6; + nav2_voxel_grid::VoxelGrid vg(sz_x, sz_y, sz_z); + TestVoxel tv(vg.getData(), sz_x, sz_y); + + // Initial point - some assymetrically standing point in order to cover most corner cases + const double x0 = 2.2; + const double y0 = 3.8; + const double z0 = 0.4; + // z-axis won't be domimant + const double z1 = 0.5; + // (x1, y1) point will move + double x1, y1; + + // Epsilon for outer boundaries of voxel grid array + const double epsilon = 0.02; + + // Running on (x, 0) edge + y1 = 0.0; + for (int i = 0; i <= sz_x; i++) { + if (i != sz_x) { + x1 = i; + } else { + x1 = i - epsilon; + } + vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length); + } + + // Running on (x, sz_y) edge + y1 = sz_y - epsilon; + for (int i = 0; i <= sz_x; i++) { + if (i != sz_x) { + x1 = i; + } else { + x1 = i - epsilon; + } + vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length); + } + + // Running on (0, y) edge + x1 = 0.0; + for (int j = 0; j <= sz_y; j++) { + if (j != sz_y) { + y1 = j; + } else { + y1 = j - epsilon; + } + vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length); + } + + // Running on (sz_x, y) edge + x1 = sz_x - epsilon; + for (int j = 0; j <= sz_y; j++) { + if (j != sz_y) { + y1 = j; + } else { + y1 = j - epsilon; + } + vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length); + } +} + +TEST(voxel_grid, bresenham3DSamePoint) +{ + const int sz_x = 60; + const int sz_y = 60; + const int sz_z = 2; + const unsigned int max_length = 60; + const unsigned int min_length = 0; + nav2_voxel_grid::VoxelGrid vg(sz_x, sz_y, sz_z); + TestVoxel tv(vg.getData(), sz_x, sz_y); + + // Initial point + const double x0 = 2.2; + const double y0 = 3.8; + const double z0 = 0.4; + + unsigned int offset = static_cast(y0) * sz_x + static_cast(x0); + unsigned int val_before = tv(offset); + // Same point to check + vg.raytraceLine(tv, x0, y0, z0, x0, y0, z0, max_length, min_length); + unsigned int val_after = tv(offset); + ASSERT_FALSE(val_before == val_after); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nav2_voxel_grid/test/voxel_grid_tests.cpp b/nav2_voxel_grid/test/voxel_grid_tests.cpp index cfc596f6ae..92e0b0b46f 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 71925e74b5..a3b46942bc 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -1,18 +1,32 @@ cmake_minimum_required(VERSION 3.5) project(nav2_waypoint_follower) +# Try for OpenCV 4.X, but settle for whatever is installed +find_package(OpenCV 4 QUIET) +if(NOT OpenCV_FOUND) + find_package(OpenCV REQUIRED) +endif() +message(STATUS "Found OpenCV version ${OpenCV_VERSION}") + +find_package(image_transport REQUIRED) +find_package(cv_bridge REQUIRED) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(nav_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(tf2_ros REQUIRED) +find_package(nav2_core REQUIRED) +find_package(pluginlib REQUIRED) nav2_package() +link_libraries(stdc++fs) + include_directories( include ) @@ -33,10 +47,16 @@ set(dependencies rclcpp rclcpp_action rclcpp_lifecycle + rclcpp_components nav_msgs nav2_msgs nav2_util tf2_ros + nav2_core + pluginlib + image_transport + cv_bridge + OpenCV ) ament_target_dependencies(${executable_name} @@ -49,7 +69,18 @@ ament_target_dependencies(${library_name} ${dependencies} ) -install(TARGETS ${library_name} +add_library(wait_at_waypoint SHARED plugins/wait_at_waypoint.cpp) +ament_target_dependencies(wait_at_waypoint ${dependencies}) + +add_library(photo_at_waypoint SHARED plugins/photo_at_waypoint.cpp) +ament_target_dependencies(photo_at_waypoint ${dependencies}) + +add_library(input_at_waypoint SHARED plugins/input_at_waypoint.cpp) +ament_target_dependencies(input_at_waypoint ${dependencies}) + +rclcpp_components_register_nodes(${library_name} "nav2_waypoint_follower::WaypointFollower") + +install(TARGETS ${library_name} wait_at_waypoint photo_at_waypoint input_at_waypoint ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -65,9 +96,14 @@ 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) +ament_export_libraries(wait_at_waypoint photo_at_waypoint input_at_waypoint ${library_name}) +ament_export_dependencies(${dependencies}) +pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml) ament_package() diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index f9cb777d17..cd0a2366b9 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -1,7 +1,29 @@ # 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. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). -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. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-waypoint-follower.html) for additional parameter descriptions. + +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. There is a parameterization `stop_on_failure` whether to stop processing the waypoint following action on a single waypoint failure. When false, it will continue onto the next waypoint when the current waypoint fails. The action will exist when either all the waypoint navigation tasks have terminated or when `stop_on_failure`, a single waypoint as failed. + +## An aside on autonomy / waypoint following + +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 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. +- Dumb robot; smart centralized dispatcher +- Smart robot; dumb centralized dispatcher + +In the first, the ``nav2_waypoint_follower`` is weakly sufficient to create a production-grade on-robot solution. Since the autonomy system / dispatcher is taking into account things like the robot's pose, battery level, current task, and more when assigning tasks, the application on the robot just needs to worry about the task at hand and not the other complexities of the system complete the requested task. In this situation, you should think of a request to the waypoint follower as 1 unit of work (e.g. 1 pick in a warehouse, 1 security patrole loop, 1 aisle, etc) to do a task and then return to the dispatcher for the next task or request to recharge. In this school of thought, the waypoint following application is just one step above navigation and below the system autonomy application. + +In the second, the ``nav2_waypoint_follower`` is a nice sample application / proof of concept, but you really need your waypoint following / autonomy system on the robot to carry more weight in making a robust solution. In this case, you should use the ``nav2_behavior_tree`` package to create a custom application-level behavior tree using navigation to complete the task. This can include subtrees like checking for the charge status mid-task for returning to dock or handling more than 1 unit of work in a more complex task. Soon, there will be a ``nav2_bt_waypoint_follower`` (name subject to adjustment) that will allow you to create this application more easily. In this school of thought, the waypoint following application is more closely tied to the system autonomy, or in many cases, is the system autonomy. + +Neither is better than the other, it highly depends on the tasks your robot(s) are completing, in what type of environment, and with what cloud resources available. Often this distinction is very clear for a given business case. diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp new file mode 100644 index 0000000000..2a8537007e --- /dev/null +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp @@ -0,0 +1,85 @@ +// 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_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_ +#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_ +#pragma once + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/empty.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/waypoint_task_executor.hpp" + +namespace nav2_waypoint_follower +{ + +/** + * @brief Simple plugin based on WaypointTaskExecutor, lets robot to wait for a + * user input at waypoint arrival. + */ +class InputAtWaypoint : public nav2_core::WaypointTaskExecutor +{ +public: +/** + * @brief Construct a new Input At Waypoint Arrival object + * + */ + InputAtWaypoint(); + + /** + * @brief Destroy the Input At Waypoint Arrival object + * + */ + ~InputAtWaypoint(); + + /** + * @brief declares and loads parameters used + * @param parent parent node + * @param plugin_name name of plugin + */ + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name); + + /** + * @brief Processor + * @param curr_pose current pose of the robot + * @param curr_waypoint_index current waypoint, that robot just arrived + * @return if task execution failed + */ + bool processAtWaypoint( + const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index); + +protected: + /** + * @brief Processor callback + * @param msg Empty message + */ + void Cb(const std_msgs::msg::Empty::SharedPtr msg); + + bool input_received_; + bool is_enabled_; + rclcpp::Duration timeout_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; + rclcpp::Clock::SharedPtr clock_; + std::mutex mutex_; + rclcpp::Subscription::SharedPtr subscription_; +}; + +} // namespace nav2_waypoint_follower + +#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_ diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp new file mode 100644 index 0000000000..fc4aee5c5e --- /dev/null +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp @@ -0,0 +1,116 @@ +// Copyright (c) 2020 Fetullah Atas +// +// 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_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_ +#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_ + +/** + * While C++17 isn't the project standard. We have to force LLVM/CLang + * to ignore deprecated declarations + */ +#define _LIBCPP_NO_EXPERIMENTAL_DEPRECATION_WARNING_FILESYSTEM + + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#include "sensor_msgs/msg/image.hpp" +#include "nav2_core/waypoint_task_executor.hpp" +#include "opencv4/opencv2/core.hpp" +#include "opencv4/opencv2/opencv.hpp" +#include "cv_bridge/cv_bridge.h" +#include "image_transport/image_transport.hpp" + + +namespace nav2_waypoint_follower +{ + +class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor +{ +public: + /** + * @brief Construct a new Photo At Waypoint object + * + */ + PhotoAtWaypoint(); + + /** + * @brief Destroy the Photo At Waypoint object + * + */ + ~PhotoAtWaypoint(); + + /** + * @brief declares and loads parameters used + * + * @param parent parent node that plugin will be created within + * @param plugin_name should be provided in nav2_params.yaml==> waypoint_follower + */ + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name); + + + /** + * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint + * + * @param curr_pose current pose of the robot + * @param curr_waypoint_index current waypoint, that robot just arrived + * @return true if task execution was successful + * @return false if task execution failed + */ + bool processAtWaypoint( + const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index); + + /** + * @brief + * + * @param msg + */ + void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg); + + /** + * @brief given a shared pointer to sensor::msg::Image type, make a deep copy to inputted cv Mat + * + * @param msg + * @param mat + */ + static void deepCopyMsg2Mat(const sensor_msgs::msg::Image::SharedPtr & msg, cv::Mat & mat); + +protected: + // to ensure safety when accessing global var curr_frame_ + std::mutex global_mutex_; + // the taken photos will be saved under this directory + std::experimental::filesystem::path save_dir_; + // .png ? .jpg ? or some other well known format + std::string image_format_; + // the topic to subscribe in order capture a frame + std::string image_topic_; + // whether plugin is enabled + bool is_enabled_; + // current frame; + sensor_msgs::msg::Image::SharedPtr curr_frame_msg_; + // global logger + rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; + // ros susbcriber to get camera image + rclcpp::Subscription::SharedPtr camera_image_subscriber_; +}; +} // namespace nav2_waypoint_follower + +#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_ diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp new file mode 100644 index 0000000000..911ae441a2 --- /dev/null +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp @@ -0,0 +1,79 @@ +// Copyright (c) 2020 Fetullah Atas +// +// 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_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_ +#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_ +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/waypoint_task_executor.hpp" + +namespace nav2_waypoint_follower +{ + +/** + * @brief Simple plugin based on WaypointTaskExecutor, lets robot to sleep for a + * specified amount of time at waypoint arrival. You can reference this class to define + * your own task and rewrite the body for it. + * + */ +class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor +{ +public: +/** + * @brief Construct a new Wait At Waypoint Arrival object + * + */ + WaitAtWaypoint(); + + /** + * @brief Destroy the Wait At Waypoint Arrival object + * + */ + ~WaitAtWaypoint(); + + /** + * @brief declares and loads parameters used (waypoint_pause_duration_) + * + * @param parent parent node that plugin will be created withing(waypoint_follower in this case) + * @param plugin_name + */ + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name); + + + /** + * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint + * + * @param curr_pose current pose of the robot + * @param curr_waypoint_index current waypoint, that robot just arrived + * @return true if task execution was successful + * @return false if task execution failed + */ + bool processAtWaypoint( + const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index); + +protected: + // the robot will sleep waypoint_pause_duration_ milliseconds + int waypoint_pause_duration_; + bool is_enabled_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; +}; + +} // namespace nav2_waypoint_follower +#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_ 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 83ffa14f69..b7cd82d684 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" @@ -26,6 +27,11 @@ #include "nav2_util/simple_action_server.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_core/waypoint_task_executor.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" + namespace nav2_waypoint_follower { @@ -52,8 +58,9 @@ class WaypointFollower : public nav2_util::LifecycleNode /** * @brief A constructor for nav2_waypoint_follower::WaypointFollower class + * @param options Additional options to control creation of the node. */ - WaypointFollower(); + explicit WaypointFollower(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief A destructor for nav2_waypoint_follower::WaypointFollower class */ @@ -63,7 +70,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 */ @@ -106,20 +113,38 @@ class WaypointFollower : public nav2_util::LifecycleNode /** * @brief Action client goal response callback - * @param future Shared future to goalhandle + * @param goal Response of action server updated asynchronously + */ + void goalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message */ - void goalResponseCallback( - std::shared_future::SharedPtr> future); + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Our action server std::unique_ptr action_server_; ActionClient::SharedPtr nav_to_pose_client_; - rclcpp::Node::SharedPtr client_node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::shared_future::SharedPtr> future_goal_handle_; bool stop_on_failure_; ActionStatus current_goal_status_; int loop_rate_; std::vector failed_ids_; + + // Task Execution At Waypoint Plugin + pluginlib::ClassLoader + waypoint_task_executor_loader_; + pluginlib::UniquePtr + waypoint_task_executor_; + std::string waypoint_task_executor_id_; + std::string waypoint_task_executor_type_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index a8aa4bb935..5bb9db52d0 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,25 +2,33 @@ nav2_waypoint_follower - 0.4.7 + 1.1.0 A waypoint follower navigation server Steve Macenski Apache-2.0 ament_cmake + nav2_common + cv_bridge + pluginlib + image_transport rclcpp rclcpp_action rclcpp_lifecycle nav_msgs nav2_msgs nav2_util + nav2_core tf2_ros ament_lint_common ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest ament_cmake + diff --git a/nav2_waypoint_follower/plugins.xml b/nav2_waypoint_follower/plugins.xml new file mode 100644 index 0000000000..ab8c01f050 --- /dev/null +++ b/nav2_waypoint_follower/plugins.xml @@ -0,0 +1,18 @@ + + + + Lets robot sleep for a specified amount of time at waypoint arrival + + + + + Run-time plugin that takes photos at waypoint arrivals when using waypoint follower node. + Saves the taken photos to specified directory. + + + + + Lets robot wait for input at waypoint arrival + + + diff --git a/nav2_waypoint_follower/plugins/input_at_waypoint.cpp b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp new file mode 100644 index 0000000000..5c613219ff --- /dev/null +++ b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp @@ -0,0 +1,116 @@ +// 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 "nav2_waypoint_follower/plugins/input_at_waypoint.hpp" + +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +#include "nav2_util/node_utils.hpp" + +namespace nav2_waypoint_follower +{ + +using std::placeholders::_1; + +InputAtWaypoint::InputAtWaypoint() +: input_received_(false), + is_enabled_(true), + timeout_(10.0, 0.0) +{ +} + +InputAtWaypoint::~InputAtWaypoint() +{ +} + +void InputAtWaypoint::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) +{ + auto node = parent.lock(); + + if (!node) { + throw std::runtime_error{"Failed to lock node in input at waypoint plugin!"}; + } + + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + double timeout; + std::string input_topic; + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".timeout", + rclcpp::ParameterValue(10.0)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".enabled", + rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".input_topic", + rclcpp::ParameterValue("input_at_waypoint/input")); + node->get_parameter(plugin_name + ".timeout", timeout); + node->get_parameter(plugin_name + ".enabled", is_enabled_); + node->get_parameter(plugin_name + ".input_topic", input_topic); + + timeout_ = rclcpp::Duration(timeout, 0.0); + + RCLCPP_INFO( + logger_, "InputAtWaypoint: Subscribing to input topic %s.", input_topic.c_str()); + subscription_ = node->create_subscription( + input_topic, 1, std::bind(&InputAtWaypoint::Cb, this, _1)); +} + +void InputAtWaypoint::Cb(const std_msgs::msg::Empty::SharedPtr /*msg*/) +{ + std::lock_guard lock(mutex_); + input_received_ = true; +} + +bool InputAtWaypoint::processAtWaypoint( + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, + const int & curr_waypoint_index) +{ + if (!is_enabled_) { + return true; + } + + input_received_ = false; + + rclcpp::Time start = clock_->now(); + rclcpp::Rate r(50); + bool input_received = false; + while (clock_->now() - start < timeout_) { + { + std::lock_guard lock(mutex_); + input_received = input_received_; + } + + if (input_received) { + return true; + } + + r.sleep(); + } + + RCLCPP_WARN( + logger_, "Unable to get external input at wp %i. Moving on.", curr_waypoint_index); + return false; +} + +} // namespace nav2_waypoint_follower + +PLUGINLIB_EXPORT_CLASS( + nav2_waypoint_follower::InputAtWaypoint, + nav2_core::WaypointTaskExecutor) diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp new file mode 100644 index 0000000000..8a6cb74b96 --- /dev/null +++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp @@ -0,0 +1,159 @@ +// Copyright (c) 2020 Fetullah Atas +// +// 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_waypoint_follower/plugins/photo_at_waypoint.hpp" + +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +#include "nav2_util/node_utils.hpp" + +namespace nav2_waypoint_follower +{ +PhotoAtWaypoint::PhotoAtWaypoint() +{ +} + +PhotoAtWaypoint::~PhotoAtWaypoint() +{ +} + +void PhotoAtWaypoint::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) +{ + auto node = parent.lock(); + + curr_frame_msg_ = std::make_shared(); + + 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")); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".save_dir", + rclcpp::ParameterValue("/tmp/waypoint_images")); + 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_); + node->get_parameter(plugin_name + ".image_topic", image_topic_); + node->get_parameter(plugin_name + ".save_dir", save_dir_as_string); + node->get_parameter(plugin_name + ".image_format", image_format_); + + // get inputted save directory and make sure it exists, if not log and create it + save_dir_ = save_dir_as_string; + try { + if (!std::experimental::filesystem::exists(save_dir_)) { + RCLCPP_WARN( + logger_, + "Provided save directory for photo at waypoint plugin does not exist," + "provided directory is: %s, the directory will be created automatically.", + save_dir_.c_str() + ); + if (!std::experimental::filesystem::create_directory(save_dir_)) { + RCLCPP_ERROR( + logger_, + "Failed to create directory!: %s required by photo at waypoint plugin, " + "exiting the plugin with failure!", + save_dir_.c_str() + ); + is_enabled_ = false; + } + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + logger_, "Exception (%s) thrown while attempting to create image capture directory." + " This task executor is being disabled as it cannot save images.", e.what()); + is_enabled_ = false; + } + + if (!is_enabled_) { + RCLCPP_INFO( + logger_, "Photo at waypoint plugin is disabled."); + } else { + RCLCPP_INFO( + logger_, "Initializing photo at waypoint plugin, subscribing to camera topic named; %s", + image_topic_.c_str()); + camera_image_subscriber_ = node->create_subscription( + image_topic_, rclcpp::SystemDefaultsQoS(), + std::bind(&PhotoAtWaypoint::imageCallback, this, std::placeholders::_1)); + } +} + +bool PhotoAtWaypoint::processAtWaypoint( + const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index) +{ + if (!is_enabled_) { + RCLCPP_WARN( + logger_, + "Photo at waypoint plugin is disabled. Not performing anything" + ); + return true; + } + try { + // construct the full path to image filename + std::experimental::filesystem::path file_name = std::to_string( + curr_waypoint_index) + "_" + + std::to_string(curr_pose.header.stamp.sec) + "." + image_format_; + std::experimental::filesystem::path full_path_image_path = save_dir_ / file_name; + + // save the taken photo at this waypoint to given directory + std::lock_guard guard(global_mutex_); + cv::Mat curr_frame_mat; + deepCopyMsg2Mat(curr_frame_msg_, curr_frame_mat); + cv::imwrite(full_path_image_path.c_str(), curr_frame_mat); + RCLCPP_INFO( + logger_, + "Photo has been taken sucessfully at waypoint %i", curr_waypoint_index); + } catch (const std::exception & e) { + RCLCPP_ERROR( + logger_, + "Couldn't take photo at waypoint %i! Caught exception: %s \n" + "Make sure that the image topic named: %s is valid and active!", + curr_waypoint_index, + e.what(), image_topic_.c_str()); + return false; + } + return true; +} + +void PhotoAtWaypoint::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg) +{ + std::lock_guard guard(global_mutex_); + curr_frame_msg_ = msg; +} + +void PhotoAtWaypoint::deepCopyMsg2Mat( + const sensor_msgs::msg::Image::SharedPtr & msg, + cv::Mat & mat) +{ + cv_bridge::CvImageConstPtr cv_bridge_ptr = cv_bridge::toCvShare(msg, msg->encoding); + cv::Mat frame = cv_bridge_ptr->image; + if (msg->encoding == "rgb8") { + cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR); + } + frame.copyTo(mat); +} + +} // namespace nav2_waypoint_follower +PLUGINLIB_EXPORT_CLASS( + nav2_waypoint_follower::PhotoAtWaypoint, + nav2_core::WaypointTaskExecutor) diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp new file mode 100644 index 0000000000..917655e21a --- /dev/null +++ b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp @@ -0,0 +1,86 @@ +// Copyright (c) 2020 Fetullah Atas +// +// 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_waypoint_follower/plugins/wait_at_waypoint.hpp" + +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +#include "nav2_util/node_utils.hpp" + +namespace nav2_waypoint_follower +{ +WaitAtWaypoint::WaitAtWaypoint() +: waypoint_pause_duration_(0), + is_enabled_(true) +{ +} + +WaitAtWaypoint::~WaitAtWaypoint() +{ +} + +void WaitAtWaypoint::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) +{ + auto node = parent.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node in wait at waypoint plugin!"}; + } + logger_ = node->get_logger(); + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".waypoint_pause_duration", + rclcpp::ParameterValue(0)); + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".enabled", + rclcpp::ParameterValue(true)); + node->get_parameter( + plugin_name + ".waypoint_pause_duration", + waypoint_pause_duration_); + node->get_parameter( + plugin_name + ".enabled", + is_enabled_); + if (waypoint_pause_duration_ == 0) { + is_enabled_ = false; + RCLCPP_INFO( + logger_, + "Waypoint pause duration is set to zero, disabling task executor plugin."); + } else if (!is_enabled_) { + RCLCPP_INFO( + logger_, "Waypoint task executor plugin is disabled."); + } +} + +bool WaitAtWaypoint::processAtWaypoint( + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, const int & curr_waypoint_index) +{ + if (!is_enabled_) { + return true; + } + RCLCPP_INFO( + logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds", + curr_waypoint_index, + waypoint_pause_duration_); + rclcpp::sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); + return true; +} +} // namespace nav2_waypoint_follower +PLUGINLIB_EXPORT_CLASS( + nav2_waypoint_follower::WaitAtWaypoint, + nav2_core::WaypointTaskExecutor) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index a86154d9bd..6f8d9e1dab 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -24,18 +24,28 @@ namespace nav2_waypoint_follower { -WaypointFollower::WaypointFollower() -: nav2_util::LifecycleNode("WaypointFollower", "", false) +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("waypoint_follower", "", options), + waypoint_task_executor_loader_("nav2_waypoint_follower", + "nav2_core::WaypointTaskExecutor") { RCLCPP_INFO(get_logger(), "Creating"); declare_parameter("stop_on_failure", true); declare_parameter("loop_rate", 20); + nav2_util::declare_parameter_if_not_declared( + this, std::string("waypoint_task_executor_plugin"), + rclcpp::ParameterValue(std::string("wait_at_waypoint"))); + nav2_util::declare_parameter_if_not_declared( + this, std::string("wait_at_waypoint.plugin"), + rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint"))); } WaypointFollower::~WaypointFollower() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn @@ -43,26 +53,46 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); + auto node = shared_from_this(); + stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); + waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); - std::vector new_args = rclcpp::NodeOptions().arguments(); - new_args.push_back("--ros-args"); - new_args.push_back("-r"); - new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); - new_args.push_back("--"); - client_node_ = std::make_shared( - "_", "", rclcpp::NodeOptions().arguments(new_args)); + callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, get_node_base_interface()); nav_to_pose_client_ = rclcpp_action::create_client( - client_node_, "navigate_to_pose"); + get_node_base_interface(), + get_node_graph_interface(), + get_node_logging_interface(), + get_node_waitables_interface(), + "navigate_to_pose", callback_group_); action_server_ = std::make_unique( get_node_base_interface(), get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), - "FollowWaypoints", std::bind(&WaypointFollower::followWaypoints, this), false); + "follow_waypoints", std::bind(&WaypointFollower::followWaypoints, this)); + + try { + waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( + this, + waypoint_task_executor_id_); + waypoint_task_executor_ = waypoint_task_executor_loader_.createUniqueInstance( + waypoint_task_executor_type_); + RCLCPP_INFO( + get_logger(), "Created waypoint_task_executor : %s of type %s", + waypoint_task_executor_id_.c_str(), waypoint_task_executor_type_.c_str()); + waypoint_task_executor_->initialize(node, waypoint_task_executor_id_); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + get_logger(), + "Failed to create waypoint_task_executor. Exception: %s", ex.what()); + } return nav2_util::CallbackReturn::SUCCESS; } @@ -74,6 +104,14 @@ WaypointFollower::on_activate(const rclcpp_lifecycle::State & /*state*/) action_server_->activate(); + auto node = shared_from_this(); + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&WaypointFollower::dynamicParametersCallback, this, _1)); + + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -83,6 +121,10 @@ WaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); action_server_->deactivate(); + dyn_params_handler_.reset(); + + // destroy bond connection + destroyBond(); return nav2_util::CallbackReturn::SUCCESS; } @@ -135,9 +177,9 @@ WaypointFollower::followWaypoints() // Check if asked to stop processing action if (action_server_->is_cancel_requested()) { auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(); - rclcpp::spin_until_future_complete(client_node_, cancel_future); + callback_group_executor_.spin_until_future_complete(cancel_future); // for result callback processing - spin_some(client_node_); + callback_group_executor_.spin_some(); action_server_->terminate_all(); return; } @@ -188,8 +230,29 @@ WaypointFollower::followWaypoints() } } else if (current_goal_status_ == ActionStatus::SUCCEEDED) { RCLCPP_INFO( - get_logger(), "Succeeded processing waypoint %i, " - "moving to next.", goal_index); + get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution", + goal_index); + bool is_task_executed = waypoint_task_executor_->processAtWaypoint( + goal->poses[goal_index], goal_index); + RCLCPP_INFO( + get_logger(), "Task execution at waypoint %i %s", goal_index, + is_task_executed ? "succeeded" : "failed!"); + // if task execution was failed and stop_on_failure_ is on , terminate action + if (!is_task_executed && stop_on_failure_) { + failed_ids_.push_back(goal_index); + RCLCPP_WARN( + get_logger(), "Failed to execute task at waypoint %i " + " stop on failure is enabled." + " Terminating action.", goal_index); + result->missed_waypoints = failed_ids_; + action_server_->terminate_current(result); + failed_ids_.clear(); + return; + } else { + RCLCPP_INFO( + get_logger(), "Handled task execution on waypoint %i," + " moving to next.", goal_index); + } } if (current_goal_status_ != ActionStatus::PROCESSING && @@ -200,7 +263,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); @@ -214,7 +277,7 @@ WaypointFollower::followWaypoints() "Processing waypoint %i...", goal_index); } - rclcpp::spin_some(client_node_); + callback_group_executor_.spin_some(); r.sleep(); } } @@ -241,10 +304,9 @@ WaypointFollower::resultCallback( void WaypointFollower::goalResponseCallback( - std::shared_future::SharedPtr> future) + const rclcpp_action::ClientGoalHandle::SharedPtr & goal) { - auto goal_handle = future.get(); - if (!goal_handle) { + if (!goal) { RCLCPP_ERROR( get_logger(), "navigate_to_pose action client failed to send goal to server."); @@ -252,4 +314,36 @@ WaypointFollower::goalResponseCallback( } } +rcl_interfaces::msg::SetParametersResult +WaypointFollower::dynamicParametersCallback(std::vector parameters) +{ + // No locking required as action server is running on same single threaded executor + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_INTEGER) { + if (name == "loop_rate") { + loop_rate_ = parameter.as_int(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == "stop_on_failure") { + stop_on_failure_ = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; +} + } // namespace nav2_waypoint_follower + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_waypoint_follower::WaypointFollower) diff --git a/nav2_waypoint_follower/test/CMakeLists.txt b/nav2_waypoint_follower/test/CMakeLists.txt new file mode 100644 index 0000000000..a6a7388ead --- /dev/null +++ b/nav2_waypoint_follower/test/CMakeLists.txt @@ -0,0 +1,21 @@ +# Test test executors +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 +) + +# Test dynamic parameters +ament_add_gtest(test_dynamic_parameters + test_dynamic_parameters.cpp +) +ament_target_dependencies(test_dynamic_parameters + ${dependencies} +) +target_link_libraries(test_dynamic_parameters + ${library_name} +) diff --git a/nav2_waypoint_follower/test/test_dynamic_parameters.cpp b/nav2_waypoint_follower/test/test_dynamic_parameters.cpp new file mode 100644 index 0000000000..345db77d85 --- /dev/null +++ b/nav2_waypoint_follower/test/test_dynamic_parameters.cpp @@ -0,0 +1,76 @@ +// 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 "gtest/gtest.h" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_waypoint_follower/waypoint_follower.hpp" +#include "rclcpp/rclcpp.hpp" + +class WPShim : public nav2_waypoint_follower::WaypointFollower +{ +public: + WPShim() + : nav2_waypoint_follower::WaypointFollower(rclcpp::NodeOptions()) + { + } + + void configure() + { + rclcpp_lifecycle::State state; + this->on_configure(state); + } + + void activate() + { + rclcpp_lifecycle::State state; + this->on_activate(state); + } +}; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(WPTest, test_dynamic_parameters) +{ + auto follower = std::make_shared(); + follower->configure(); + follower->activate(); + + auto rec_param = std::make_shared( + follower->get_node_base_interface(), follower->get_node_topics_interface(), + follower->get_node_graph_interface(), + follower->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("loop_rate", 100), + rclcpp::Parameter("stop_on_failure", false)}); + + rclcpp::spin_until_future_complete( + follower->get_node_base_interface(), + results); + + EXPECT_EQ(follower->get_parameter("loop_rate").as_int(), 100); + EXPECT_EQ(follower->get_parameter("stop_on_failure").as_bool(), false); +} 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 0000000000..da0a0e9944 --- /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 59ac3f63ca..763c95c1e9 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 8b6811af54..3a7088f0e0 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.7 + 1.1.0 ROS2 Navigation Stack @@ -14,21 +14,28 @@ nav2_amcl + nav2_behavior_tree nav2_bt_navigator - nav2_costmap_2d + nav2_controller nav2_core + nav2_costmap_2d nav2_dwb_controller nav2_lifecycle_manager nav2_map_server - nav2_recoveries - nav2_planner nav2_msgs nav2_navfn_planner + nav2_planner + nav2_behaviors + nav2_smoother + nav2_regulated_pure_pursuit_controller + nav2_rotation_shim_controller nav2_rviz_plugins - nav2_behavior_tree + nav2_simple_commander + nav2_smac_planner + nav2_smoother + nav2_theta_star_planner nav2_util nav2_voxel_grid - nav2_controller nav2_waypoint_follower smac_planner nav2_regulated_pure_pursuit_controller diff --git a/tools/bt2img.py b/tools/bt2img.py index 31c84eecec..31480f19be 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -89,7 +89,7 @@ def main(): legend.render(args.legend) dot.format = 'png' if args.save_dot: - print("Saving dot to " + str(args.save_dot)) + print(f'Saving dot to {args.save_dot}') args.save_dot.write(dot.source) dot.render(args.image_out, view=args.display) @@ -119,7 +119,7 @@ def find_behavior_tree(xml_tree, tree_name): if tree_name == tree.get('ID'): return tree - raise RuntimeError("No behavior tree for name " + tree_name + " found in the XML file") + raise RuntimeError(f'No behavior tree for name {tree_name} found in the XML file') # Generate a dot description of the root of the behavior tree. def convert2dot(behavior_tree): diff --git a/tools/build_all.sh b/tools/build_all.sh deleted file mode 100755 index 4b2e3c7d45..0000000000 --- a/tools/build_all.sh +++ /dev/null @@ -1,68 +0,0 @@ -#!/bin/bash - -# All the parenthesis around the calls to setup.bash and the build are to -# prevent the ROS variables from entering the global scope of this script. -# It seems that the ros1_bridge build will fail if the ros2 environment is -# sourced before ROS 1, so we need to make sure that no ROS 2 variables are -# set at the time we start the last step, the ros1_bridge build. - -if [ "$ROS2_DISTRO" = "" ]; then - export ROS2_DISTRO=foxy -fi -if [ "$ROS2_DISTRO" != "foxy" ]; then - echo "ROS2_DISTRO variable must be set to foxy" - exit 1 -fi - -set -e - -# so you can call from anywhere in the navigation2_ws, ros2_ws, or deps branches -while [[ "$PWD" =~ ros2_ws ]]; do - cd ../ -done - -while [[ "$PWD" =~ ros2_nav_dependencies_ws ]]; do - cd ../ -done - -while [[ "$PWD" =~ navigation2_ws ]]; do - cd ../ -done - -CWD=`pwd` - -# Determine which repos to build. If ROS 2 base directories are -# not present, then don't build -if [ -d "ros2_ws" ]; then - ENABLE_ROS2=true - ROS2_SETUP_FILE=$CWD/ros2_ws/install/setup.bash -else - ENABLE_ROS2=false - ROS2_SETUP_FILE=/opt/ros/$ROS2_DISTRO/setup.bash -fi - -# Build ROS 2 base -if [ "$ENABLE_ROS2" = true ]; then - cd $CWD/ros2_ws - colcon build --symlink-install --packages-skip ros1_bridge -fi - -# Build our ROS 2 dependencies -cd $CWD/ros2_nav_dependencies_ws -export ROSDISTRO_INDEX_URL='https://raw.githubusercontent.com/ros2/rosdistro/ros2/index.yaml' -(. $ROS2_SETUP_FILE && - colcon build --symlink-install) - -# Build our code -cd $CWD/navigation2_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) - -# Update the ROS1 bridge -if test "$ENABLE_ROS1" = true && test "$ENABLE_ROS2" = true ; then - cd $CWD - . navigation2_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 3867927bf9..f4510238c6 100755 --- a/tools/code_coverage_report.bash +++ b/tools/code_coverage_report.bash @@ -36,58 +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 -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/*" \ - --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 e65d6d2e26..c0756a070a 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/release.Dockerfile b/tools/distro.Dockerfile similarity index 80% rename from tools/release.Dockerfile rename to tools/distro.Dockerfile index 2b89b53400..6ae66fb66e 100644 --- a/tools/release.Dockerfile +++ b/tools/distro.Dockerfile @@ -6,14 +6,14 @@ # # Example build command: # export DOCKER_BUILDKIT=1 -# export FROM_IMAGE="ros:eloquent" -# export OVERLAY_MIXINS="release ccache" -# docker build -t nav2:release_branch \ +# export FROM_IMAGE="ros:rolling" +# export OVERLAY_MIXINS="release ccache lld" +# docker build -t nav2:rolling \ # --build-arg FROM_IMAGE \ # --build-arg OVERLAY_MIXINS \ -# -f release_branch.Dockerfile ./ +# -f distro.Dockerfile ../ -ARG FROM_IMAGE=ros:eloquent +ARG FROM_IMAGE=ros:rolling ARG OVERLAY_WS=/opt/overlay_ws # multi-stage for caching @@ -43,6 +43,7 @@ RUN mkdir -p /tmp/opt && \ # multi-stage for building FROM $FROM_IMAGE AS builder +ARG DEBIAN_FRONTEND=noninteractive # edit apt for caching RUN cp /etc/apt/apt.conf.d/docker-clean /etc/apt/ && \ @@ -55,6 +56,7 @@ RUN --mount=type=cache,target=/var/cache/apt \ apt-get update && apt-get install -q -y \ ccache \ lcov \ + lld \ && rosdep update # install overlay dependencies @@ -71,7 +73,7 @@ RUN --mount=type=cache,target=/var/cache/apt \ # build overlay source COPY --from=cacher $OVERLAY_WS/src ./src -ARG OVERLAY_MIXINS="release ccache" +ARG OVERLAY_MIXINS="release ccache lld" RUN --mount=type=cache,target=/root/.ccache \ . /opt/ros/$ROS_DISTRO/setup.sh && \ colcon build \ @@ -88,15 +90,12 @@ RUN sed --in-place \ 's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \ /ros_entrypoint.sh -# ARG RUN_TESTS -# ARG FAIL_ON_TEST_FAILURE -# RUN if [ -z "$RUN_TESTS" ]; then \ -# colcon test \ -# --mixin $OVERLAY_MIXINS \ -# --ctest-args --test-regex "test_.*"; \ -# if [ -z "$FAIL_ON_TEST_FAILURE" ]; then \ -# colcon test-result; \ -# else \ -# colcon test-result || true; \ -# fi \ -# fi +# test overlay build +ARG RUN_TESTS +ARG FAIL_ON_TEST_FAILURE=True +RUN if [ -n "$RUN_TESTS" ]; then \ + . install/setup.sh && \ + colcon test && \ + colcon test-result \ + || ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \ + fi diff --git a/tools/initial_ros_setup.sh b/tools/initial_ros_setup.sh deleted file mode 100755 index 5e633e2037..0000000000 --- a/tools/initial_ros_setup.sh +++ /dev/null @@ -1,123 +0,0 @@ -#!/bin/bash - -ENABLE_BUILD=true -ENABLE_ROS2=true - -if [ "$ROS2_DISTRO" = "" ]; then - export ROS2_DISTRO=foxy -fi -if [ "$ROS2_DISTRO" != "foxy" ]; then - echo "ROS2_DISTRO variable must be set to foxy" - exit 1 -fi - -for opt in "$@" ; do - case $opt in - --no-ros2) - ENABLE_ROS2=false - shift - ;; - --download-only) - ENABLE_BUILD=false - shift - ;; - *) - echo "Invalid option: $opt" - echo "Valid options:" - echo "--no-ros2 Uses the binary distribution of ROS2 foxy" - echo "--download-only Skips the build step and only downloads the code" - exit 1 - ;; - esac -done - -set -e -CHECKPOINT_FILES='' - -CWD=`pwd` -return_to_root_dir() { - cd $CWD -} - -download_navstack() { - echo "Downloading the ROS 2 navstack" - mkdir -p navigation2_ws/src - cd navigation2_ws - if [ -f "custom_nav2.repos" ]; then #override default location for testing - vcs import src < custom_nav2.repos - else - cd src - git clone https://github.com/ros-planning/navigation2.git - fi - return_to_root_dir -} - -download_ros2() { - echo "Downloading ROS 2 Release Latest" - mkdir -p ros2_ws/src - cd ros2_ws - wget https://raw.githubusercontent.com/ros2/ros2/master/ros2.repos - vcs import src < ros2.repos - return_to_root_dir -} - -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/ros2_dependencies.repos - return_to_root_dir -} - -checkpoint() { - local CHECKPOINT_FILE_NAME=.INITIAL_SETUP_$1 - CHECKPOINT_FILES="${CHECKPOINT_FILES} ${CHECKPOINT_FILE_NAME}" - if [ ! -f ${CHECKPOINT_FILE_NAME} ]; then - $1 - touch ${CHECKPOINT_FILE_NAME} - else - echo "${CHECKPOINT_FILE_NAME} exists. Skipping $1" - fi -} - -download_all() { - checkpoint download_navstack - checkpoint download_ros2_dependencies - if [ "$ENABLE_ROS2" = true ]; then - checkpoint download_ros2 - fi -} - -rosdep_install() { - rosdep install -y -r -q --from-paths . --ignore-src --rosdistro $ROS2_DISTRO --skip-keys "catkin" -} - -echo "This script will download the ROS 2 latest release workspace, the" -echo "dependencies workspace and the ros_navstack_port workspace to the" -echo "current directory and then build them all. There should be no ROS" -echo "environment variables set at this time." -echo -echo "The current directory is $CWD" -echo -echo "Are you sure you want to continue? [yN]" -read -r REPLY -echo -if [ "$REPLY" = "y" ]; then - download_all - rosdep_install - if [ "$ENABLE_BUILD" = true ]; then - $CWD/navigation2_ws/src/navigation2/tools/build_all.sh - fi - - cd ${CWD} - rm ${CHECKPOINT_FILES} - echo - 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 - 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" -fi diff --git a/tools/planner_benchmarking/100by100_10.pgm b/tools/planner_benchmarking/100by100_10.pgm new file mode 100644 index 0000000000..c69b8a7fb0 Binary files /dev/null and b/tools/planner_benchmarking/100by100_10.pgm differ diff --git a/tools/planner_benchmarking/100by100_15.pgm b/tools/planner_benchmarking/100by100_15.pgm new file mode 100644 index 0000000000..6b8ecb1ff3 Binary files /dev/null and b/tools/planner_benchmarking/100by100_15.pgm differ diff --git a/tools/planner_benchmarking/100by100_20.pgm b/tools/planner_benchmarking/100by100_20.pgm new file mode 100644 index 0000000000..0159f9763e Binary files /dev/null and b/tools/planner_benchmarking/100by100_20.pgm differ diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py new file mode 100644 index 0000000000..3865b0bbdb --- /dev/null +++ b/tools/planner_benchmarking/metrics.py @@ -0,0 +1,157 @@ +#! /usr/bin/env python3 +# Copyright 2022 Joshua Wallace +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy +from ament_index_python.packages import get_package_share_directory + +import math +import os +import pickle +import numpy as np + +from random import seed +from random import randint +from random import uniform + +from transforms3d.euler import euler2quat + +# Note: Map origin is assumed to be (0,0) + + +def getPlannerResults(navigator, initial_pose, goal_pose, planners): + results = [] + for planner in planners: + path = navigator.getPath(initial_pose, goal_pose, planner) + if path is not None: + results.append(path) + return results + + +def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res): + start = PoseStamped() + start.header.frame_id = 'map' + start.header.stamp = time_stamp + while True: + row = randint(side_buffer, costmap.shape[0]-side_buffer) + col = randint(side_buffer, costmap.shape[1]-side_buffer) + + if costmap[row, col] < max_cost: + start.pose.position.x = col*res + start.pose.position.y = row*res + + yaw = uniform(0, 1) * 2*math.pi + quad = euler2quat(0.0, 0.0, yaw) + start.pose.orientation.w = quad[0] + start.pose.orientation.x = quad[1] + start.pose.orientation.y = quad[2] + start.pose.orientation.z = quad[3] + break + return start + + +def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): + goal = PoseStamped() + goal.header.frame_id = 'map' + goal.header.stamp = time_stamp + while True: + row = randint(side_buffer, costmap.shape[0]-side_buffer) + col = randint(side_buffer, costmap.shape[1]-side_buffer) + + start_x = start.pose.position.x + start_y = start.pose.position.y + goal_x = col*res + goal_y = row*res + x_diff = goal_x - start_x + y_diff = goal_y - start_y + dist = math.sqrt(x_diff ** 2 + y_diff ** 2) + + if costmap[row, col] < max_cost and dist > 3.0: + goal.pose.position.x = goal_x + goal.pose.position.y = goal_y + + yaw = uniform(0, 1) * 2*math.pi + quad = euler2quat(0.0, 0.0, yaw) + goal.pose.orientation.w = quad[0] + goal.pose.orientation.x = quad[1] + goal.pose.orientation.y = quad[2] + goal.pose.orientation.z = quad[3] + break + return goal + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our experiment'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 = 1.0 + initial_pose.pose.position.y = 1.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate + navigator.waitUntilNav2Active() + + # Get the costmap for start/goal validation + costmap_msg = navigator.getGlobalCostmap() + costmap = np.asarray(costmap_msg.data) + costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) + + planners = ['NavFn', 'Smac2d', 'SmacLattice', 'SmacHybrid'] + max_cost = 210 + side_buffer = 100 + time_stamp = navigator.get_clock().now().to_msg() + results = [] + seed(33) + + random_pairs = 100 + res = costmap_msg.metadata.resolution + for i in range(random_pairs): + print("Cycle: ", i, "out of: ", random_pairs) + start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res) + goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res) + print("Start", start) + print("Goal", goal) + result = getPlannerResults(navigator, start, goal, planners) + if len(result) == len(planners): + results.append(result) + else: + print("One of the planners was invalid") + + print("Write Results...") + nav2_planner_metrics_dir = get_package_share_directory('nav2_planner_metrics') + with open(os.path.join(nav2_planner_metrics_dir, 'results.pickle'), 'wb') as f: + pickle.dump(results, f, pickle.HIGHEST_PROTOCOL) + + with open(os.path.join(nav2_planner_metrics_dir, 'costmap.pickle'), 'wb') as f: + pickle.dump(costmap_msg, f, pickle.HIGHEST_PROTOCOL) + + with open(os.path.join(nav2_planner_metrics_dir, 'planners.pickle'), 'wb') as f: + pickle.dump(planners, f, pickle.HIGHEST_PROTOCOL) + print("Write Complete") + + navigator.lifecycleShutdown() + exit(0) + + +if __name__ == '__main__': + main() diff --git a/tools/planner_benchmarking/process_data.py b/tools/planner_benchmarking/process_data.py new file mode 100644 index 0000000000..443da00266 --- /dev/null +++ b/tools/planner_benchmarking/process_data.py @@ -0,0 +1,174 @@ +#! /usr/bin/env python3 +# Copyright 2022 Joshua Wallace +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import numpy as np +import math + +import os +from ament_index_python.packages import get_package_share_directory +import pickle + +import seaborn as sns +import matplotlib.pylab as plt +from tabulate import tabulate + + +def getPaths(results): + paths = [] + for result in results: + for path in result: + paths.append(path.path) + return paths + + +def getTimes(results): + times = [] + for result in results: + for time in result: + times.append(time.planning_time.nanosec/1e09 + time.planning_time.sec) + return times + + +def getMapCoordsFromPaths(paths, resolution): + coords = [] + for path in paths: + x = [] + y = [] + for pose in path.poses: + x.append(pose.pose.position.x/resolution) + y.append(pose.pose.position.y/resolution) + coords.append(x) + coords.append(y) + return coords + + +def getPathLength(path): + path_length = 0 + x_prev = path.poses[0].pose.position.x + y_prev = path.poses[0].pose.position.y + for i in range(1, len(path.poses)): + x_curr = path.poses[i].pose.position.x + y_curr = path.poses[i].pose.position.y + path_length = path_length + math.sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2) + x_prev = x_curr + y_prev = y_curr + return path_length + + +def plotResults(costmap, paths): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + data = np.where(data <= 253, 0, data) + + plt.figure(3) + ax = sns.heatmap(data, cmap='Greys', cbar=False) + for i in range(0, len(coords), 2): + ax.plot(coords[i], coords[i+1], linewidth=0.7) + plt.axis('off') + ax.set_aspect('equal', 'box') + plt.show() + + +def averagePathCost(paths, costmap, num_of_planners): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + + average_path_costs = [] + for i in range(num_of_planners): + average_path_costs.append([]) + + k = 0 + for i in range(0, len(coords), 2): + costs = [] + for j in range(len(coords[i])): + costs.append(data[math.floor(coords[i+1][j])][math.floor(coords[i][j])]) + average_path_costs[k % num_of_planners].append(sum(costs)/len(costs)) + k += 1 + + return average_path_costs + + +def maxPathCost(paths, costmap, num_of_planners): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + + max_path_costs = [] + for i in range(num_of_planners): + max_path_costs.append([]) + + k = 0 + for i in range(0, len(coords), 2): + max_cost = 0 + for j in range(len(coords[i])): + cost = data[math.floor(coords[i+1][j])][math.floor(coords[i][j])] + if max_cost < cost: + max_cost = cost + max_path_costs[k % num_of_planners].append(max_cost) + k += 1 + + return max_path_costs + + +def main(): + + nav2_planner_metrics_dir = get_package_share_directory('nav2_planner_metrics') + print("Read data") + with open(os.path.join(nav2_planner_metrics_dir, 'results.pickle'), 'rb') as f: + results = pickle.load(f) + + with open(os.path.join(nav2_planner_metrics_dir, 'planners.pickle'), 'rb') as f: + planners = pickle.load(f) + + with open(os.path.join(nav2_planner_metrics_dir, 'costmap.pickle'), 'rb') as f: + costmap = pickle.load(f) + + paths = getPaths(results) + path_lengths = [] + + for path in paths: + path_lengths.append(getPathLength(path)) + path_lengths = np.asarray(path_lengths) + total_paths = len(paths) + + path_lengths.resize((int(total_paths/len(planners)), len(planners))) + path_lengths = path_lengths.transpose() + + times = getTimes(results) + times = np.asarray(times) + times.resize((int(total_paths/len(planners)), len(planners))) + times = np.transpose(times) + + # Costs + average_path_costs = np.asarray(averagePathCost(paths, costmap, len(planners))) + max_path_costs = np.asarray(maxPathCost(paths, costmap, len(planners))) + + # Generate table + planner_table = [['Planner', 'Average path length (m)', 'Average Time (s)', + 'Average cost', 'Max cost']] + + for i in range(0, len(planners)): + planner_table.append([planners[i], np.average(path_lengths[i]), np.average(times[i]), + np.average(average_path_costs[i]), np.average(max_path_costs[i])]) + + # Visualize results + print(tabulate(planner_table)) + plotResults(costmap, paths) + + +if __name__ == '__main__': + main() diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos deleted file mode 100644 index 73d6b1f1e2..0000000000 --- a/tools/ros2_dependencies.repos +++ /dev/null @@ -1,29 +0,0 @@ -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 diff --git a/tools/ros2_minimal_dependencies.repos b/tools/ros2_minimal_dependencies.repos index 427e110e99..f7e0ea5859 100644 --- a/tools/ros2_minimal_dependencies.repos +++ b/tools/ros2_minimal_dependencies.repos @@ -1,9 +1,9 @@ repositories: - BehaviorTree.CPP: + BehaviorTree/BehaviorTree.CPP: type: git url: https://github.com/BehaviorTree/BehaviorTree.CPP.git - version: 3.5.3 - angles: + version: master + ros/angles: type: git url: https://github.com/ros/angles.git version: ros2 diff --git a/tools/run_sanitizers b/tools/run_sanitizers index 7ded4aa359..fb263eda27 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/run_test_suite.bash b/tools/run_test_suite.bash index 51323c566a..3c807c5a9f 100755 --- a/tools/run_test_suite.bash +++ b/tools/run_test_suite.bash @@ -5,10 +5,10 @@ set -ex SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" # gets the directory of this script # Skip flaky tests. Nav2 system tests will be run later. -colcon test --packages-skip nav2_system_tests nav2_recoveries +colcon test --packages-skip nav2_system_tests nav2_behaviors -# run the stable tests in nav2_recoveries -colcon test --packages-select nav2_recoveries --ctest-args --exclude-regex "test_recoveries" +# run the stable tests in nav2_behaviors +colcon test --packages-select nav2_behaviors --ctest-args --exclude-regex "test_recoveries" # run the linters in nav2_system_tests. They only need to be run once. colcon test --packages-select nav2_system_tests --ctest-args --exclude-regex "test_.*" # run the linters diff --git a/tools/skip_keys.txt b/tools/skip_keys.txt index 0875072611..8285089c34 100644 --- a/tools/skip_keys.txt +++ b/tools/skip_keys.txt @@ -1,7 +1,7 @@ console_bridge fastcdr fastrtps -libopensplice67 libopensplice69 rti-connext-dds-5.3.1 +slam_toolbox urdfdom_headers \ No newline at end of file diff --git a/tools/source.Dockerfile b/tools/source.Dockerfile index a090b305b9..90cdc223ea 100644 --- a/tools/source.Dockerfile +++ b/tools/source.Dockerfile @@ -1,168 +1,247 @@ -# This dockerfile expects to be contained in the /navigation2 root folder for file copy +# syntax=docker/dockerfile:experimental + +# Use experimental buildkit for faster builds +# https://github.com/moby/buildkit/blob/master/frontend/dockerfile/docs/experimental.md +# Use `--progress=plain` to use plane stdout for docker build # # Example build command: # This determines which version of the ROS2 code base to pull -# export ROS2_BRANCH=master +# export ROS2_BRANCH=main +# export DOCKER_BUILDKIT=1 # docker build \ -# --no-cache \ -# --tag nav2:full_ros_build \ -# --file full_ros_build.Dockerfile ./ +# --tag nav2:source \ +# --file source.Dockerfile ../ +# +# Use `--no-cache` to break the local docker build cache. +# Use `--pull` to pull the latest parent image from the remote registry. +# Use `--target=` to build stages not used for final stage. # -# Omit the `--no-cache` if you know you don't need to break the cache. # We're only building on top of a ros2 devel image to get the basics # prerequisites installed such as the apt source, rosdep, etc. We don't want to # actually use any of the ros release packages. Instead we are going to build # everything from source in one big workspace. ARG FROM_IMAGE=osrf/ros2:devel +ARG UNDERLAY_WS=/opt/underlay_ws +ARG OVERLAY_WS=/opt/overlay_ws # multi-stage for caching -FROM $FROM_IMAGE AS cache +FROM $FROM_IMAGE AS cacher + +# clone ros2 source +ARG ROS2_BRANCH=master +ARG ROS2_REPO=https://github.com/ros2/ros2.git +WORKDIR $ROS2_WS/src +RUN git clone $ROS2_REPO -b $ROS2_BRANCH && \ + vcs import ./ < ros2/ros2.repos && \ + find ./ -name ".git" | xargs rm -rf # clone underlay source -ENV UNDERLAY_WS /opt/underlay_ws -RUN mkdir -p $UNDERLAY_WS/src -WORKDIR $UNDERLAY_WS -COPY ./tools/ros2_dependencies.repos ./ -RUN vcs import src < ros2_dependencies.repos +ARG UNDERLAY_WS +WORKDIR $UNDERLAY_WS/src +COPY ./tools/underlay.repos ../ +RUN vcs import ./ < ../underlay.repos && \ + find ./ -name ".git" | xargs rm -rf # copy overlay source -ENV OVERLAY_WS /opt/overlay_ws -RUN mkdir -p $OVERLAY_WS/src -WORKDIR $OVERLAY_WS -COPY ./ src/navigation2 +ARG OVERLAY_WS +WORKDIR $OVERLAY_WS/src +COPY ./ ./ros-planning/navigation2 +RUN colcon list --names-only | cat >> /opt/packages.txt -# copy manifests for caching +# remove skiped packages WORKDIR /opt -RUN find ./ -name "package.xml" | \ - xargs cp --parents -t /tmp && \ - find ./ -name "COLCON_IGNORE" | \ - xargs cp --parents -t /tmp +RUN find ./ \ + -name "AMENT_IGNORE" -o \ + -name "CATKIN_IGNORE" -o \ + -name "COLCON_IGNORE" \ + | xargs dirname | xargs rm -rf || true && \ + colcon list --paths-only \ + --packages-skip-up-to \ + $(cat packages.txt | xargs) \ + | xargs rm -rf + +# copy manifests for caching +RUN mkdir -p /tmp/opt && \ + find ./ -name "package.xml" | \ + xargs cp --parents -t /tmp/opt -# multi-stage for building -FROM $FROM_IMAGE AS build +# multi-stage for ros2 dependencies +FROM $FROM_IMAGE AS ros2_depender +ARG DEBIAN_FRONTEND=noninteractive + +# edit apt for caching +RUN cp /etc/apt/apt.conf.d/docker-clean /etc/apt/ && \ + echo 'Binary::apt::APT::Keep-Downloaded-Packages "true";' \ + > /etc/apt/apt.conf.d/docker-clean # install packages -RUN apt-get update && apt-get install -q -y \ +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && apt-get install -q -y \ + ccache \ libasio-dev \ libtinyxml2-dev \ - wget \ - && rm -rf /var/lib/apt/lists/* + lld \ + && rosdep update -ARG ROS2_BRANCH=master -ENV ROS2_BRANCH=$ROS2_BRANCH ENV ROS_VERSION=2 \ ROS_PYTHON_VERSION=3 +# install ros2 dependencies WORKDIR $ROS2_WS +COPY --from=cacher /tmp/$ROS2_WS ./ +COPY ./tools/skip_keys.txt /tmp/ +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && rosdep install -q -y \ + --from-paths src \ + --ignore-src \ + --skip-keys " \ + $(cat /tmp/skip_keys.txt | xargs) \ + " + +# multi-stage for building ros2 +FROM ros2_depender AS ros2_builder + +# build ros2 source +COPY --from=cacher $ROS2_WS ./ +ARG ROS2_MIXINS="release ccache lld" +RUN --mount=type=cache,target=/root/.ccache \ + colcon build \ + --symlink-install \ + --mixin $ROS2_MIXINS -# get ros2 source code -RUN wget https://raw.githubusercontent.com/ros2/ros2/$ROS2_BRANCH/ros2.repos \ - && vcs import src < ros2.repos +# multi-stage for testing ros2 +FROM ros2_builder AS ros2_tester -# get skip keys -COPY ./tools/skip_keys.txt ./ +# test overlay build +ARG RUN_TESTS +ARG FAIL_ON_TEST_FAILURE=True +RUN if [ -n "$RUN_TESTS" ]; then \ + . install/setup.sh && \ + colcon test && \ + colcon test-result \ + || ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \ + fi -RUN rosdep update +# multi-stage for underlay dependencies +FROM ros2_depender AS underlay_depender -# copy underlay manifests -COPY --from=cache /tmp/underlay_ws src/underlay -RUN cd src/underlay && colcon list --names-only | \ - cat > packages.txt && \ - cd ../../ && colcon list --names-only \ - --packages-up-to \ - $(cat src/underlay/packages.txt | xargs) | \ - cat > packages.txt +# copy manifests for caching +COPY --from=cacher /tmp/$ROS2_WS $ROS2_WS # install underlay dependencies -RUN apt-get update && rosdep install -y \ +ARG UNDERLAY_WS +WORKDIR $UNDERLAY_WS +COPY --from=cacher /tmp/$UNDERLAY_WS ./ +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && rosdep install -q -y \ --from-paths src \ + $ROS2_WS/src \ --ignore-src \ - --skip-keys \ - "$(cat skip_keys.txt | xargs)" \ - src/underlay \ - && rm -rf /var/lib/apt/lists/* + --skip-keys " \ + $(cat /tmp/skip_keys.txt | xargs) \ + " -# build ros2 source -ARG ROS2_MIXINS="release" -RUN colcon build \ - --symlink-install \ - --mixin \ - $ROS2_MIXINS \ - --packages-up-to \ - $(cat src/underlay/packages.txt | xargs) \ - --packages-skip \ - $(cat src/underlay/packages.txt | xargs) \ - --cmake-args --no-warn-unused-cli +# multi-stage for building underlay +FROM underlay_depender AS underlay_builder -# copy underlay source -COPY --from=cache /opt/underlay_ws src/underlay +# copy workspace for caching +COPY --from=ros2_builder $ROS2_WS $ROS2_WS # build underlay source -ARG UNDERLAY_MIXINS="release" -RUN colcon build \ +COPY --from=cacher $UNDERLAY_WS ./ +ARG UNDERLAY_MIXINS="release ccache lld" +RUN --mount=type=cache,target=/root/.ccache \ + . $ROS2_WS/install/setup.sh && \ + colcon build \ --symlink-install \ - --mixin \ - $UNDERLAY_MIXINS \ - --packages-up-to \ - $(cat src/underlay/packages.txt | xargs) \ - --packages-skip-build-finished \ - --cmake-args --no-warn-unused-cli - -# copy overlay manifests -COPY --from=cache /tmp/overlay_ws src/overlay -RUN cd src/overlay && colcon list --names-only | \ - cat > packages.txt && \ - cd ../../ && colcon list --names-only \ - --packages-up-to \ - $(cat src/overlay/packages.txt | xargs) | \ - cat > packages.txt + --mixin $UNDERLAY_MIXINS + +# multi-stage for testing underlay +FROM underlay_builder AS underlay_tester + +# test overlay build +ARG RUN_TESTS +ARG FAIL_ON_TEST_FAILURE=True +RUN if [ -n "$RUN_TESTS" ]; then \ + . install/setup.sh && \ + colcon test && \ + colcon test-result \ + || ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \ + fi + +# multi-stage for overlay dependencies +FROM underlay_depender AS overlay_depender + +# copy manifests for caching +COPY --from=cacher /tmp/$ROS2_WS $ROS2_WS +COPY --from=cacher /tmp/$UNDERLAY_WS $UNDERLAY_WS # install overlay dependencies -RUN apt-get update && rosdep install -y \ +ARG OVERLAY_WS +WORKDIR $OVERLAY_WS +COPY --from=cacher /tmp/$OVERLAY_WS ./ +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && rosdep install -q -y \ --from-paths src \ + $ROS2_WS/src \ + $UNDERLAY_WS/src \ --ignore-src \ - --skip-keys \ - "$(cat skip_keys.txt | xargs)" \ - src/overlay \ - && rm -rf /var/lib/apt/lists/* + --skip-keys " \ + $(cat /tmp/skip_keys.txt | xargs) \ + " -# build ros2 source -RUN colcon build \ - --symlink-install \ - --mixin \ - $ROS2_MIXINS \ - --packages-up-to \ - $(cat src/overlay/packages.txt | xargs) \ - --packages-skip \ - $(cat src/overlay/packages.txt | xargs) \ - --packages-skip-build-finished \ - --cmake-args --no-warn-unused-cli +# multi-stage for building overlay +FROM overlay_depender AS overlay_builder -# copy overlay source -COPY --from=cache /opt/overlay_ws src/overlay +# copy workspace for caching +COPY --from=ros2_builder $ROS2_WS $ROS2_WS +COPY --from=underlay_builder $UNDERLAY_WS $UNDERLAY_WS # build overlay source -ARG OVERLAY_MIXINS="build-testing-on release" -RUN colcon build \ +COPY --from=cacher $OVERLAY_WS ./ +ARG OVERLAY_MIXINS="release ccache lld" +RUN --mount=type=cache,target=/root/.ccache \ + . $UNDERLAY_WS/install/setup.sh && \ + colcon build \ --symlink-install \ - --mixin \ - $OVERLAY_MIXINS \ - --packages-up-to \ - $(cat src/overlay/packages.txt | xargs) \ - --packages-skip-build-finished \ - --cmake-args --no-warn-unused-cli - -# test overlay source + --mixin $OVERLAY_MIXINS + +# multi-stage for testing overlay +FROM overlay_builder AS overlay_tester + +# test overlay build ARG RUN_TESTS -ARG FAIL_ON_TEST_FAILURE -RUN if [ ! -z "$RUN_TESTS" ]; then \ - colcon test \ - --packages-select \ - $(cat src/overlay/packages.txt | xargs); \ - if [ ! -z "$FAIL_ON_TEST_FAILURE" ]; then \ - colcon test-result; \ - else \ - colcon test-result || true; \ - fi \ +ARG FAIL_ON_TEST_FAILURE=True +RUN if [ -n "$RUN_TESTS" ]; then \ + . install/setup.sh && \ + colcon test && \ + colcon test-result \ + || ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \ fi + +# multi-stage for testing workspaces +FROM overlay_builder AS workspaces_tester + +# copy workspace test results +COPY --from=ros2_tester $ROS2_WS/log $ROS2_WS/log +COPY --from=underlay_tester $UNDERLAY_WS/log $UNDERLAY_WS/log +COPY --from=overlay_tester $OVERLAY_WS/log $OVERLAY_WS/log + +# multi-stage for shipping overlay +FROM overlay_builder AS overlay_shipper + +# restore apt for docker +RUN mv /etc/apt/docker-clean /etc/apt/apt.conf.d/ && \ + rm -rf /var/lib/apt/lists/ + +# 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/tools/underlay.repos b/tools/underlay.repos new file mode 100644 index 0000000000..658f35d7a3 --- /dev/null +++ b/tools/underlay.repos @@ -0,0 +1,29 @@ +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/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 + # ros-simulation/gazebo_ros_pkgs: + # type: git + # url: https://github.com/ros-simulation/gazebo_ros_pkgs.git + # version: ros2 diff --git a/tools/update_bt_diagrams.bash b/tools/update_bt_diagrams.bash index 94e5d00d0d..f467fdc6e3 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