diff --git a/.circleci/config.yml b/.circleci/config.yml index 5293fc64afa..8f60f876b41 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v8\ + - "<< parameters.key >>-v11\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v8\ + - "<< parameters.key >>-v11\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v8\ + key: "<< parameters.key >>-v11\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/.github/mergify.yml b/.github/mergify.yml index 3713a4262a7..491c2c10763 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -1,4 +1,13 @@ pull_request_rules: + - name: backport to humble at reviewers discretion + conditions: + - base=main + - "label=backport-humble" + actions: + backport: + branches: + - humble + - name: backport to galactic at reviewers discretion conditions: - base=main diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index 7b2b646221e..9358ea96295 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -63,9 +63,9 @@ jobs: runs-on: ubuntu-latest steps: - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v1 + uses: docker/setup-buildx-action@v2 - name: Login to Docker Hub - uses: docker/login-action@v1 + uses: docker/login-action@v2 with: registry: ghcr.io username: ${{ github.repository_owner }} @@ -94,7 +94,7 @@ jobs: - name: Build and push if: steps.config.outputs.trigger == 'true' id: docker_build - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v3 with: pull: true push: true diff --git a/.gitignore b/.gitignore index 88c775d303c..21696be8722 100644 --- a/.gitignore +++ b/.gitignore @@ -44,9 +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/README.md b/README.md index 4f278d5eda3..0acfb8fe5c8 100644 --- a/README.md +++ b/README.md @@ -54,33 +54,39 @@ please cite this work in your papers! ## Build Status -| 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) | +| Service | Foxy | Galactic | Humble | 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/) | [![Build Status](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/) | N/A | +| Circle CI | N/A | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-planning/navigation2/tree/main.svg?style=svg)](https://circleci.com/gh/ros-planning/navigation2/tree/main) | -| Package | Foxy Source | Foxy Debian | Galactic Source | Galactic Debian | -| :---: | :---: | :---: | :---: | :---: | -| Navigation2 | [![Build Status](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__navigation2__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__navigation2__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__navigation2__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | -| nav2_amcl | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_amcl__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_amcl__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/) | -| nav2_behavior_tree | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_behavior_tree__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_behavior_tree__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/) | -| nav2_bringup | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_bringup__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_bringup__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/) | -| nav2_bt_navigator | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_bt_navigator__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_bt_navigator__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/) | -| nav2_common | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_common__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_common__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_common__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_common__ubuntu_focal_amd64__binary/) | -| nav2_controller | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/) | -| nav2_core | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_core__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_core__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_core__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_core__ubuntu_focal_amd64__binary/) | -| nav2_costmap_2d | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_costmap_2d__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_costmap_2d__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/) | -| nav2_dwb_controller | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_dwb_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_dwb_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/) | -| nav2_lifecycle_manager | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/) | -| nav2_map_server | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_map_server__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_map_server__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/) | -| nav2_msgs | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_msgs__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_msgs__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/) | -| nav2_navfn_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_navfn_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_navfn_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/) | -| nav2_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/) | -| nav2_recoveries | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_recoveries__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_recoveries__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/) | -| nav2_regulated_pure_pursuit | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Fbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Gbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/) | -| nav2_rviz_plugins | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/) -| nav2_smac_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__smac_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__smac_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__smac_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__smac_planner__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_smac_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_smac_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_smac_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_smac_planner__ubuntu_focal_amd64__binary/) | -| nav2_system_tests | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_system_tests__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_system_tests__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/) | -| nav2_util | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_util__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_util__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_util__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_util__ubuntu_focal_amd64__binary/) | -| nav2_waypoint_follower | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/) | +| Package | Foxy Source | Foxy Debian | Galactic Source | Galactic Debian | Humble Source | Humble Debian | +| :---: | :---: | :---: | :---: | :---: | :---: | :---: | +| Navigation2 | [![Build Status](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__navigation2__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__navigation2__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__navigation2__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__navigation2__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_amcl__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | +| nav2_{recoveries, behaviors} | [![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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_bringup__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_common__ubuntu_jammy_amd64__binary/) | +| nav2_constrained_smoother | N/A | N/A | N/A | N/A | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_controller__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_core__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_map_server__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_msgs__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_planner__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | +| nav2_rotation_shim_controller | N/A | N/A | N/A | N/A | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | +| nav2_simple_commander | N/A | N/A | [![Build Status](https://build.ros2.org/job/Gsrc_uF__nav2_simple_commander__ubuntu_focal__source/badge/icon)](https://build.ros2.org/job/Gsrc_uF__nav2_simple_commander__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/job/Gbin_uF64__nav2_simple_commander__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Gbin_uF64__nav2_simple_commander__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | +| nav2_smoother | N/A | N/A | N/A | N/A | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_smoother__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | +| nav2_theta_star_planner | N/A | N/A | [![Build Status](https://build.ros2.org/job/Gsrc_uF__nav2_theta_star_planner__ubuntu_focal__source/badge/icon)](https://build.ros2.org/job/Gsrc_uF__nav2_theta_star_planner__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/job/Gbin_uF64__nav2_theta_star_planner__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Gbin_uF64__nav2_theta_star_planner__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_util__ubuntu_jammy_amd64__binary/) | +| nav2_voxel_grid | [![Build Status](https://build.ros2.org/job/Fsrc_uF__nav2_voxel_grid__ubuntu_focal__source/badge/icon)](https://build.ros2.org/job/Fsrc_uF__nav2_voxel_grid__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/job/Fbin_uF64__nav2_voxel_grid__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Fbin_uF64__nav2_voxel_grid__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Gsrc_uF__nav2_voxel_grid__ubuntu_focal__source/badge/icon)](https://build.ros2.org/job/Gsrc_uF__nav2_voxel_grid__ubuntu_focal__source/) | [![Build Status](https://build.ros2.org/job/Gbin_uF64__nav2_voxel_grid__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Gbin_uF64__nav2_voxel_grid__ubuntu_focal_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | +| 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/) | [![Build Status](https://build.ros2.org/job/Hsrc_uJ__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uJ__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index e2446e0c7bf..2360d09c253 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -91,10 +91,26 @@ class AmclNode : public nav2_util::LifecycleNode */ 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 { @@ -132,7 +148,7 @@ class AmclNode : public nav2_util::LifecycleNode 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; @@ -155,7 +171,8 @@ class AmclNode : public nav2_util::LifecycleNode * @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_; @@ -238,7 +255,6 @@ class AmclNode : public nav2_util::LifecycleNode */ static pf_vector_t uniformPoseGenerator(void * arg); pf_t * pf_{nullptr}; - std::mutex pf_mutex_; bool pf_init_; pf_vector_t pf_odom_pose_; int resample_count_{0}; 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 index cac356e0c79..2d173602042 100644 --- a/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp +++ b/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp @@ -19,8 +19,8 @@ * */ -#ifndef DIFFERENTIAL_MOTION_MODEL_HPP -#define DIFFERENTIAL_MOTION_MODEL_HPP +#ifndef NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_ +#define NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_ #include #include @@ -44,4 +44,4 @@ class DifferentialMotionModel : public nav2_amcl::MotionModel double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_; }; } // namespace nav2_amcl -#endif +#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 58010aaffe4..4cd1c6a8798 100644 --- a/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp +++ b/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp @@ -18,9 +18,10 @@ #define NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_ #include +#include + #include "nav2_amcl/pf/pf.hpp" #include "nav2_amcl/pf/pf_pdf.hpp" -#include namespace nav2_amcl { @@ -55,7 +56,6 @@ class MotionModel * @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 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 index 3201028341c..ae586d4d6c5 100644 --- a/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp +++ b/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp @@ -19,8 +19,8 @@ * */ -#ifndef DIFFERENTIAL_MOTION_MODEL_HPP -#define DIFFERENTIAL_MOTION_MODEL_HPP +#ifndef NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_ +#define NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_ #include #include @@ -44,4 +44,4 @@ class OmniMotionModel : public nav2_amcl::MotionModel double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_; }; } // namespace nav2_amcl -#endif +#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 ae0d698b17e..ea53e1e870d 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/portable_utils.hpp b/nav2_amcl/include/nav2_amcl/portable_utils.hpp new file mode 100644 index 00000000000..13d8a795c03 --- /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/package.xml b/nav2_amcl/package.xml index b2d5b5d5593..3ff62b68707 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.0.0 + 1.1.0

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 293bbdb4177..a28b4927a3e 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -48,9 +48,10 @@ #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 @@ -58,7 +59,7 @@ namespace nav2_amcl using nav2_util::geometry_utils::orientationAroundZAxis; AmclNode::AmclNode(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("amcl", "", true, options) +: nav2_util::LifecycleNode("amcl", "", options) { RCLCPP_INFO(get_logger(), "Creating"); @@ -221,6 +222,10 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) 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() @@ -231,7 +236,8 @@ 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(); @@ -240,7 +246,9 @@ AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/) initPubSub(); initServices(); initOdometry(); - + 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; } @@ -274,6 +282,13 @@ 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(); @@ -291,6 +306,9 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/) pose_pub_->on_deactivate(); particle_cloud_pub_->on_deactivate(); + // reset dynamic parameter handler + dyn_params_handler_.reset(); + // destroy bond connection destroyBond(); @@ -302,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(); @@ -461,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"); @@ -487,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"); @@ -522,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; @@ -586,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 @@ -1055,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_); @@ -1109,6 +1130,206 @@ AmclNode::initParameters() } } +/** + * @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) { @@ -1123,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", @@ -1210,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; @@ -1226,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( diff --git a/nav2_amcl/src/include/portable_utils.h b/nav2_amcl/src/include/portable_utils.h deleted file mode 100644 index 1577e6875b1..00000000000 --- 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/motion_model/omni_motion_model.cpp b/nav2_amcl/src/motion_model/omni_motion_model.cpp index 009efb862ad..969fb7f0d6c 100644 --- a/nav2_amcl/src/motion_model/omni_motion_model.cpp +++ b/nav2_amcl/src/motion_model/omni_motion_model.cpp @@ -88,7 +88,7 @@ OmniMotionModel::odometryUpdate( } } -} // namespace nav2_amcl +} // 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 0eec4ce1a1c..a5a84ae5df6 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. */ diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index d2426906afa..63d71b99bd0 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 diff --git a/nav2_amcl/src/pf/pf_pdf.c b/nav2_amcl/src/pf/pf_pdf.c index 858bea7df88..98a7748ffac 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_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 6773f30f3d0..0569c58bc94 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -60,6 +60,18 @@ 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) @@ -69,6 +81,9 @@ 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) @@ -87,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) @@ -117,9 +141,15 @@ 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) @@ -150,6 +180,9 @@ 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/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index ace1542b111..8de0b27977b 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 @@ -90,7 +90,14 @@ class BtActionNode : public BT::ActionNodeBase // 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(); + static constexpr std::chrono::milliseconds wait_for_server_timeout = + std::chrono::milliseconds(1000); + if (!action_client_->wait_for_action_server(wait_for_server_timeout)) { + RCLCPP_ERROR( + node_->get_logger(), "\"%s\" action server not available after waiting for %li ms", + action_name.c_str(), wait_for_server_timeout.count()); + throw std::runtime_error("Action server not available"); + } } /** @@ -132,9 +139,12 @@ class BtActionNode : public BT::ActionNodeBase /** * @brief Function to perform some user-defined operation after a timeout - * waiting for a result that hasn't been received yet + * 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() + virtual void on_wait_for_result(std::shared_ptr/*feedback*/) { } @@ -206,7 +216,10 @@ class BtActionNode : public BT::ActionNodeBase // 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(); + 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 || @@ -340,6 +353,11 @@ 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; + }; future_goal_handle_ = std::make_shared< std::shared_future::SharedPtr>>( @@ -406,6 +424,9 @@ 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_; 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 index e423afcb458..75580803476 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -40,7 +40,8 @@ class BtActionServer typedef std::function OnGoalReceivedCallback; typedef std::function OnLoopCallback; typedef std::function OnPreemptCallback; - typedef std::function OnCompletionCallback; + typedef std::function OnCompletionCallback; /** * @brief A constructor for nav2_behavior_tree::BtActionServer class @@ -168,7 +169,7 @@ class BtActionServer * @brief Getter function for the current BT tree * @return BT::Tree Current behavior tree */ - BT::Tree getTree() const + const BT::Tree & getTree() const { return tree_; } 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 index dfce6d284a8..592d1257a27 100644 --- 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 @@ -80,7 +80,8 @@ bool BtActionServer::on_configure() auto options = rclcpp::NodeOptions().arguments( {"--ros-args", "-r", - std::string("__node:=") + std::string(node->get_name()) + client_node_name + "_rclcpp_node", + std::string("__node:=") + + std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node", "--"}); // Support for handling the topic-based goal pose from rviz @@ -171,7 +172,13 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena std::istreambuf_iterator()); // Create the Behavior Tree from the XML input - tree_ = bt_->createTreeFromText(xml_string, blackboard_); + try { + tree_ = bt_->createTreeFromText(xml_string, blackboard_); + } catch (const std::exception & e) { + RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); + return false; + } + topic_logger_ = std::make_unique(client_node_, tree_); current_bt_xml_filename_ = filename; @@ -216,7 +223,7 @@ void BtActionServer::executeCallback() // 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); + on_completion_callback_(result, rc); switch (rc) { case nav2_behavior_tree::BtStatus::SUCCEEDED: 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 b18f01e662b..636f24415c1 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 @@ -145,9 +145,10 @@ class BtServiceNode : public BT::ActionNodeBase /** * @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() + virtual BT::NodeStatus on_completion(std::shared_ptr/*response*/) { return BT::NodeStatus::SUCCESS; } @@ -168,7 +169,7 @@ class BtServiceNode : public BT::ActionNodeBase 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(); + BT::NodeStatus status = on_completion(future_result_.get()); return status; } 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 00000000000..d59bbff5a57 --- /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/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index 811ee005a27..f9f233c2337 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -68,7 +68,9 @@ class ComputePathThroughPosesAction "Destinations to plan through"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), - BT::InputPort("planner_id", ""), + BT::InputPort( + "planner_id", "", + "Mapped name to the planner plugin type to use"), }); } }; 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 2528a5ce20b..105b52fbf6c 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 @@ -63,7 +63,9 @@ class ComputePathToPoseAction : public BtActionNode("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), - BT::InputPort("planner_id", ""), + BT::InputPort( + "planner_id", "", + "Mapped name to the planner plugin type to use"), }); } }; 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 00000000000..5632a7551e6 --- /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 00000000000..a2f13a94fef --- /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 ef6ccf0d63b..a97993c7738 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" @@ -48,8 +49,10 @@ class FollowPathAction : public BtActionNode /** * @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() override; + void on_wait_for_result( + std::shared_ptr feedback) override; /** * @brief Creates list of BT ports diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index 209a11b6bab..7e7f3d5c30b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -56,6 +56,7 @@ class NavigateThroughPosesAction : public BtActionNode("goals", "Destinations to plan through"), + BT::InputPort("behavior_tree", "Behavior tree to run"), }); } }; 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 aa5f7a1e82a..3708017f16a 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 @@ -56,6 +56,7 @@ class NavigateToPoseAction : public BtActionNode("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/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index dec29248ea7..76496ba7ed2 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 @@ -54,9 +54,13 @@ class SpinAction : public BtActionNode return providedBasicPorts( { BT::InputPort("spin_dist", 1.57, "Spin distance"), - BT::InputPort("time_allowance", 10.0, "Allowed time for spinning") + 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 00000000000..0e747fa70d0 --- /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_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp new file mode 100644 index 00000000000..113e0d83e72 --- /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_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp new file mode 100644 index 00000000000..1545e9da2e5 --- /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/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 00000000000..64f77f1473d --- /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/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 00000000000..a04b1263f4b --- /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/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 00000000000..fb2f42f3d4d --- /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/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp new file mode 100644 index 00000000000..4ac0ab44eeb --- /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/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 00000000000..6e41516434c --- /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/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 319492de75d..3f5fa4b522f 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -1,7 +1,7 @@ @@ -11,33 +11,71 @@ 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 @@ -55,17 +93,27 @@ Path to follow Goal checker + Service name + Server timeout Goal + Service name + Server timeout + Behavior tree to run Goals + Service name + Server timeout + Behavior tree to run + Service name + Server timeout @@ -74,6 +122,17 @@ 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 @@ -95,15 +154,21 @@ Spin distance Allowed time for spinning + Service name + Server timeout Wait time + Service name + Server timeout Destination + Reference frame + Robot base frame @@ -115,6 +180,8 @@ + + Min battery % or voltage before triggering Topic for battery info @@ -131,8 +198,20 @@ 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 + + @@ -149,6 +228,8 @@ Distance + Reference frame + Robot base frame @@ -167,5 +248,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 472de10433a..81e24a8cb30 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.0.0 + 1.1.0 TODO Michael Jeronimo Carlos Orduno @@ -51,6 +51,7 @@ ament_lint_common ament_lint_auto ament_cmake_gtest + test_msgs ament_cmake 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 00000000000..3baa1ffb1e3 --- /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/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp new file mode 100644 index 00000000000..81a93a63628 --- /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 00000000000..b29de63df55 --- /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 d3353a8981d..0fc0dc5b574 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -35,7 +35,8 @@ void FollowPathAction::on_tick() getInput("goal_checker_id", goal_.goal_checker_id); } -void FollowPathAction::on_wait_for_result() +void FollowPathAction::on_wait_for_result( + std::shared_ptr/*feedback*/) { // Grab the new path nav_msgs::msg::Path new_path; diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp index 657b191491d..693fbfa146c 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -36,6 +36,7 @@ void NavigateThroughPosesAction::on_tick() "NavigateThroughPosesAction: goal not provided"); return; } + getInput("behavior_tree", goal_.behavior_tree); } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp index c932bca2571..82cdab44f84 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -36,6 +36,7 @@ void NavigateToPoseAction::on_tick() "NavigateToPoseAction: goal not provided"); return; } + getInput("behavior_tree", goal_.behavior_tree); } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index 5682a2ae9d8..73dc8c589fb 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -32,11 +32,14 @@ SpinAction::SpinAction( getInput("time_allowance", time_allowance); goal_.target_yaw = dist; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + getInput("is_recovery", is_recovery_); } void SpinAction::on_tick() { - increment_recovery_count(); + if (is_recovery_) { + increment_recovery_count(); + } } } // namespace nav2_behavior_tree 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 00000000000..62ddbc4719c --- /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 00000000000..934f2b86bb8 --- /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_cancel_node.cpp b/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp new file mode 100644 index 00000000000..9365b7e06fa --- /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 00000000000..b6afc0d5a4f --- /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 ddbb5402dc5..34930bb582e 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -23,8 +23,7 @@ GoalUpdatedCondition::GoalUpdatedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf) -{ -} +{} BT::NodeStatus GoalUpdatedCondition::tick() { 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 00000000000..4a02dede9aa --- /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/path_expiring_timer_condition.cpp b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp new file mode 100644 index 00000000000..e018e025352 --- /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/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp new file mode 100644 index 00000000000..f8a2d8cefc2 --- /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/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp new file mode 100644 index 00000000000..d4b40b0964e --- /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/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index ed55b085349..7bfc003cf1c 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -87,6 +87,10 @@ BehaviorTreeEngine::createTreeFromFile( 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 a2819658529..3c6e595ba7d 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -1,5 +1,7 @@ +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}) +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) @@ -9,6 +11,10 @@ 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}) @@ -17,6 +23,23 @@ ament_add_gtest(test_action_controller_cancel_action test_controller_cancel_node 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}) @@ -53,6 +76,10 @@ 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}) 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 00000000000..e1a04c82a79 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -0,0 +1,172 @@ +// 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(); + client_.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_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp index 3ca86a832b1..cf754c52291 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp @@ -83,6 +83,7 @@ class CancelControllerActionTestFixture : public ::testing::Test config_ = nullptr; node_.reset(); action_server_.reset(); + client_.reset(); factory_.reset(); } 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 00000000000..31f9af0427b --- /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 00000000000..6c6eb57233f --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp @@ -0,0 +1,176 @@ +// 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(); + client_.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_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp new file mode 100644 index 00000000000..978dc2d78f5 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -0,0 +1,172 @@ +// 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(); + client_.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 00000000000..4947c6bc8c9 --- /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_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp new file mode 100644 index 00000000000..f1ed0750d86 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -0,0 +1,172 @@ +// 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(); + client_.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 b1350f74537..21fd0b29409 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 00000000000..d41a6d424cc --- /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 00000000000..21728b4fcc9 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp @@ -0,0 +1,127 @@ +// 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; + server_.reset(); + node_.reset(); + factory_.reset(); + 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_; +}; + +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 00000000000..385b1cfed8e --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp @@ -0,0 +1,100 @@ +// 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; + node_.reset(); + 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/decorator/CMakeLists.txt b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt index 330ff3b2518..ed6504a6824 100644 --- a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt @@ -17,3 +17,11 @@ 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 00000000000..d8e2a24fb08 --- /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 00000000000..7a71f0a74b0 --- /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_recoveries/CHANGELOG.rst b/nav2_behaviors/CHANGELOG.rst similarity index 100% rename from nav2_recoveries/CHANGELOG.rst rename to nav2_behaviors/CHANGELOG.rst diff --git a/nav2_recoveries/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt similarity index 64% rename from nav2_recoveries/CMakeLists.txt rename to nav2_behaviors/CMakeLists.txt index 8af7c5b2f03..273c5fe664e 100644 --- a/nav2_recoveries/CMakeLists.txt +++ b/nav2_behaviors/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(nav2_recoveries) +project(nav2_behaviors) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) @@ -25,8 +25,8 @@ 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 @@ -47,35 +47,43 @@ 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} ) -add_library(nav2_backup_recovery SHARED - plugins/back_up.cpp +add_library(nav2_wait_behavior SHARED +plugins/wait.cpp +) + +ament_target_dependencies(nav2_wait_behavior +${dependencies} +) + +add_library(nav2_drive_on_heading_behavior SHARED + plugins/drive_on_heading.cpp ) -ament_target_dependencies(nav2_backup_recovery +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} ) -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} SHARED - src/recovery_server.cpp + src/behavior_server.cpp ) ament_target_dependencies(${library_name} @@ -93,12 +101,13 @@ ament_target_dependencies(${executable_name} ${dependencies} ) -rclcpp_components_register_nodes(${library_name} "recovery_server::RecoveryServer") +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 @@ -112,10 +121,14 @@ install(DIRECTORY include/ DESTINATION include/ ) -install(FILES recovery_plugin.xml +install(FILES behavior_plugin.xml DESTINATION share/${PROJECT_NAME} ) +install(DIRECTORY plugins/ + DESTINATION share/${PROJECT_NAME}/plugins/ +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -125,9 +138,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_dependencies(${dependencies}) ament_package() diff --git a/nav2_behaviors/README.md b/nav2_behaviors/README.md new file mode 100644 index 00000000000..5ecd14af5c5 --- /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 00000000000..5989f566a99 --- /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 72% rename from nav2_recoveries/include/nav2_recoveries/recovery_server.hpp rename to nav2_behaviors/include/nav2_behaviors/behavior_server.hpp index dfcb95bb0a4..ea7a2d4cfa2 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp @@ -23,33 +23,33 @@ #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 recovery_server::RecoveryServer - * @brief An server hosting a map of recovery plugins + * @class behavior_server::BehaviorServer + * @brief An server hosting a map of behavior plugins */ -class RecoveryServer : public nav2_util::LifecycleNode +class BehaviorServer : public nav2_util::LifecycleNode { public: /** - * @brief A constructor for recovery_server::RecoveryServer + * @brief A constructor for behavior_server::BehaviorServer * @param options Additional options to control creation of the node. */ - explicit RecoveryServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - ~RecoveryServer(); + explicit BehaviorServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~BehaviorServer(); /** - * @brief Loads recovery plugins from parameter file + * @brief Loads behavior plugins from parameter file * @return bool if successfully loaded the plugins */ - bool loadRecoveryPlugins(); + bool loadBehaviorPlugins(); protected: /** @@ -81,12 +81,12 @@ class RecoveryServer : public nav2_util::LifecycleNode std::shared_ptr transform_listener_; // Plugins - pluginlib::ClassLoader plugin_loader_; - std::vector> recoveries_; + 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_; @@ -94,6 +94,6 @@ class RecoveryServer : public nav2_util::LifecycleNode std::shared_ptr collision_checker_; }; -} // 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 80% rename from nav2_recoveries/include/nav2_recoveries/recovery.hpp rename to nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 1a7060e0b97..06528be8872 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,13 +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 @@ -48,28 +48,27 @@ enum class Status : int8_t using namespace std::chrono_literals; //NOLINT /** - * @class nav2_recoveries::Recovery - * @brief An action server recovery base class implementing the action server and basic factory. + * @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; /** - * @brief A Recovery constructor + * @brief A TimedBehavior constructor */ - Recovery() + TimedBehavior() : action_server_(nullptr), cycle_frequency_(10.0), - enabled_(false) + enabled_(false), + transform_tolerance_(0.0) { } - virtual ~Recovery() - { - } + virtual ~TimedBehavior() = default; // Derived classes can override this method to catch the command and perform some checks // before getting into the main loop. The method will only be called @@ -80,7 +79,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; @@ -109,7 +108,7 @@ class Recovery : public nav2_core::Recovery RCLCPP_INFO(logger_, "Configuring %s", name.c_str()); - recovery_name_ = name; + behavior_name_ = name; tf_ = tf; node->get_parameter("cycle_frequency", cycle_frequency_); @@ -118,8 +117,8 @@ class Recovery : public nav2_core::Recovery 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; @@ -139,7 +138,7 @@ class Recovery : public nav2_core::Recovery // Activate server on lifecycle transition void activate() override { - RCLCPP_INFO(logger_, "Activating %s", recovery_name_.c_str()); + RCLCPP_INFO(logger_, "Activating %s", behavior_name_.c_str()); vel_pub_->on_activate(); action_server_->activate(); @@ -157,7 +156,7 @@ class Recovery : public nav2_core::Recovery protected: rclcpp_lifecycle::LifecycleNode::WeakPtr node_; - std::string recovery_name_; + std::string behavior_name_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_pub_; std::shared_ptr action_server_; std::shared_ptr collision_checker_; @@ -173,13 +172,13 @@ class Recovery : public nav2_core::Recovery rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; // Logger - rclcpp::Logger logger_{rclcpp::get_logger("nav2_recoveries")}; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_behaviors")}; - // Main execution callbacks for the action server implementation calling the recovery's + // 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(logger_, "Attempting %s", recovery_name_.c_str()); + RCLCPP_INFO(logger_, "Attempting %s", behavior_name_.c_str()); if (!enabled_) { RCLCPP_WARN( @@ -191,24 +190,11 @@ class Recovery : public nav2_core::Recovery if (onRun(action_server_->get_current_goal()) != Status::SUCCEEDED) { RCLCPP_INFO( logger_, - "Initial checks failed for %s", recovery_name_.c_str()); + "Initial checks failed for %s", behavior_name_.c_str()); action_server_->terminate_current(); return; } - // Log a message every second - { - 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...", recovery_name_.c_str());}); - } - auto start_time = steady_clock_.now(); // Initialize the ActionT result @@ -218,19 +204,19 @@ class Recovery : public nav2_core::Recovery while (rclcpp::ok()) { if (action_server_->is_cancel_requested()) { - RCLCPP_INFO(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( 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); @@ -241,13 +227,13 @@ class Recovery : public nav2_core::Recovery case Status::SUCCEEDED: RCLCPP_INFO( logger_, - "%s completed successfully", recovery_name_.c_str()); + "%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(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; @@ -273,6 +259,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 ca8e0b0af92..5a8e8719847 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_behaviors/package.xml @@ -1,8 +1,8 @@ - nav2_recoveries - 1.0.0 + 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 00000000000..90a69a686ed --- /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 00000000000..f892c5b0e2d --- /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 00000000000..53525b3bb6e --- /dev/null +++ b/nav2_behaviors/plugins/drive_on_heading.cpp @@ -0,0 +1,20 @@ +// 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 "drive_on_heading.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 00000000000..79c8d81c719 --- /dev/null +++ b/nav2_behaviors/plugins/drive_on_heading.hpp @@ -0,0 +1,214 @@ +// 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()), + command_x_(0.0), + command_speed_(0.0), + simulate_ahead_time_(0.0) + { + } + + ~DriveOnHeading() = default; + + /** + * @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; + } + + 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_); + } + + typename ActionT::Feedback::SharedPtr feedback_; + + 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_; +}; + +} // 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 91% rename from nav2_recoveries/plugins/spin.cpp rename to nav2_behaviors/plugins/spin.cpp index 893e3b58e5d..a76fe38f525 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -13,37 +13,35 @@ // limitations under the License. #include -#include -#include #include #include #include #include #include "spin.hpp" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" #include "tf2/utils.h" -#pragma GCC diagnostic pop -#include "tf2/LinearMath/Quaternion.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) + min_rotational_vel_(0.0), + max_rotational_vel_(0.0), + rotational_acc_lim_(0.0), + cmd_yaw_(0.0), + prev_yaw_(0.0), + relative_yaw_(0.0), + simulate_ahead_time_(0.0) { } -Spin::~Spin() -{ -} +Spin::~Spin() = default; void Spin::onConfigure() { @@ -89,7 +87,7 @@ Status Spin::onRun(const std::shared_ptr command) cmd_yaw_ = command->target_yaw; RCLCPP_INFO( - logger_, "Turning %0.2f for spin recovery.", + logger_, "Turning %0.2f for spin behavior.", cmd_yaw_); command_time_allowance_ = command->time_allowance; @@ -128,7 +126,7 @@ Status Spin::onCycleUpdate() relative_yaw_ += delta_yaw; prev_yaw_ = current_yaw; - feedback_->angular_distance_traveled = relative_yaw_; + feedback_->angular_distance_traveled = static_cast(relative_yaw_); action_server_->publish_feedback(feedback_); double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_); @@ -188,7 +186,7 @@ bool Spin::isCollisionFree( 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 77% rename from nav2_recoveries/plugins/spin.hpp rename to nav2_behaviors/plugins/spin.hpp index 632f585c7fe..7174c91f467 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_behaviors/plugins/spin.hpp @@ -12,30 +12,30 @@ // 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 nav2_recoveries::Spin - * @brief An action server recovery for spinning in + * @class nav2_behaviors::Spin + * @brief An action server behavior for spinning in */ -class Spin : public Recovery +class Spin : public TimedBehavior { public: /** - * @brief A constructor for nav2_recoveries::Spin + * @brief A constructor for nav2_behaviors::Spin */ Spin(); ~Spin(); @@ -43,18 +43,18 @@ class Spin : public Recovery /** * @brief Initialization to run behavior * @param command Goal to execute - * @return Status of recovery + * @return Status of behavior */ Status onRun(const std::shared_ptr command) override; /** - * @brief Configuration of recovery action + * @brief Configuration of behavior action */ void onConfigure() override; /** * @brief Loop function to run behavior - * @return Status of recovery + * @return Status of behavior */ Status onCycleUpdate() override; @@ -84,6 +84,6 @@ class Spin : public Recovery 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 88% rename from nav2_recoveries/plugins/wait.cpp rename to nav2_behaviors/plugins/wait.cpp index 66bcabc1e91..236f7122018 100644 --- a/nav2_recoveries/plugins/wait.cpp +++ b/nav2_behaviors/plugins/wait.cpp @@ -12,24 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include "wait.hpp" -namespace nav2_recoveries +namespace nav2_behaviors { Wait::Wait() -: Recovery(), +: TimedBehavior(), feedback_(std::make_shared()) { } -Wait::~Wait() -{ -} +Wait::~Wait() = default; Status Wait::onRun(const std::shared_ptr command) { @@ -54,7 +51,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 70% rename from nav2_recoveries/plugins/wait.hpp rename to nav2_behaviors/plugins/wait.hpp index 41ade5e93b3..38e85fd14aa 100644 --- a/nav2_recoveries/plugins/wait.hpp +++ b/nav2_behaviors/plugins/wait.hpp @@ -12,29 +12,29 @@ // 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 nav2_recoveries::Wait - * @brief An action server recovery for waiting a fixed duration + * @class nav2_behaviors::Wait + * @brief An action server behavior for waiting a fixed duration */ -class Wait : public Recovery +class Wait : public TimedBehavior { public: /** - * @brief A constructor for nav2_recoveries::Wait + * @brief A constructor for nav2_behaviors::Wait */ Wait(); ~Wait(); @@ -42,13 +42,13 @@ class Wait : public Recovery /** * @brief Initialization to run behavior * @param command Goal to execute - * @return Status of recovery + * @return Status of behavior */ Status onRun(const std::shared_ptr command) override; /** * @brief Loop function to run behavior - * @return Status of recovery + * @return Status of behavior */ Status onCycleUpdate() override; @@ -57,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 65% rename from nav2_recoveries/src/recovery_server.cpp rename to nav2_behaviors/src/behavior_server.cpp index be98df7250d..d6f7568c400 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(const rclcpp::NodeOptions & options) -: LifecycleNode("recoveries_server", "", false, options), - 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,10 +38,10 @@ RecoveryServer::RecoveryServer(const rclcpp::NodeOptions & options) "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("recovery_plugins", recovery_ids_); - if (recovery_ids_ == 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]); } @@ -56,13 +59,13 @@ RecoveryServer::RecoveryServer(const rclcpp::NodeOptions & options) } -RecoveryServer::~RecoveryServer() +BehaviorServer::~BehaviorServer() { - recoveries_.clear(); + 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"); @@ -87,8 +90,8 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) collision_checker_ = std::make_shared( *costmap_sub_, *footprint_sub_, this->get_name()); - 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,11 +125,11 @@ 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(); } @@ -137,12 +140,12 @@ RecoveryServer::on_activate(const rclcpp_lifecycle::State & /*state*/) } 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(); } @@ -153,16 +156,16 @@ RecoveryServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) } 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(); @@ -173,17 +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(recovery_server::RecoveryServer) +RCLCPP_COMPONENTS_REGISTER_NODE(behavior_server::BehaviorServer) diff --git a/nav2_recoveries/src/main.cpp b/nav2_behaviors/src/main.cpp similarity index 85% rename from nav2_recoveries/src/main.cpp rename to nav2_behaviors/src/main.cpp index 23dea6b90f7..4af82831f32 100644 --- a/nav2_recoveries/src/main.cpp +++ b/nav2_behaviors/src/main.cpp @@ -13,16 +13,15 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#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 00000000000..9418db15716 --- /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 79% rename from nav2_recoveries/test/test_recoveries.cpp rename to nav2_behaviors/test/test_behaviors.cpp index 47a687538bd..4cd7527fbcf 100644 --- a/nav2_recoveries/test/test_recoveries.cpp +++ b/nav2_behaviors/test/test_behaviors.cpp @@ -16,37 +16,36 @@ #include #include #include -#include #include #include "gtest/gtest.h" #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() = default; - 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 +62,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 +91,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() = default; - void SetUp() + void SetUp() override { 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"))); @@ -131,15 +130,15 @@ class RecoveryTest : public ::testing::Test *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 +151,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 +194,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/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index b9e129e5db1..bd471472276 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -22,8 +22,8 @@ from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import PushRosNamespace from launch_ros.actions import Node +from launch_ros.actions import PushRosNamespace from nav2_common.launch import RewrittenYaml @@ -41,6 +41,7 @@ def generate_launch_description(): params_file = LaunchConfiguration('params_file') 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 @@ -102,6 +103,10 @@ def generate_launch_description(): '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( @@ -123,6 +128,7 @@ def generate_launch_description(): launch_arguments={'namespace': namespace, 'use_sim_time': use_sim_time, 'autostart': autostart, + 'use_respawn': use_respawn, 'params_file': params_file}.items()), IncludeLaunchDescription( @@ -135,6 +141,7 @@ def generate_launch_description(): 'autostart': autostart, 'params_file': params_file, 'use_composition': use_composition, + 'use_respawn': use_respawn, 'container_name': 'nav2_container'}.items()), IncludeLaunchDescription( @@ -144,6 +151,7 @@ def generate_launch_description(): 'autostart': autostart, 'params_file': params_file, 'use_composition': use_composition, + 'use_respawn': use_respawn, 'container_name': 'nav2_container'}.items()), ]) @@ -162,6 +170,7 @@ def generate_launch_description(): ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_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 index 33cf3fa5d4d..218debd9185 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -20,8 +20,8 @@ from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import Node from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml @@ -37,6 +37,7 @@ def generate_launch_description(): 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'] @@ -94,6 +95,10 @@ def generate_launch_description(): '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=[ @@ -102,6 +107,8 @@ def generate_launch_description(): executable='map_server', name='map_server', output='screen', + respawn=use_respawn, + respawn_delay=2.0, parameters=[configured_params], remappings=remappings), Node( @@ -109,6 +116,8 @@ def generate_launch_description(): executable='amcl', name='amcl', output='screen', + respawn=use_respawn, + respawn_delay=2.0, parameters=[configured_params], remappings=remappings), Node( @@ -162,6 +171,7 @@ def generate_launch_description(): 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) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index e35f88c9863..fa1be218f28 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -20,8 +20,8 @@ from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import Node from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml @@ -36,13 +36,15 @@ def generate_launch_description(): 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', - 'recoveries_server', + 'behavior_server', 'bt_navigator', - 'waypoint_follower'] + 'waypoint_follower', + 'velocity_smoother'] # 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 @@ -94,6 +96,10 @@ def generate_launch_description(): '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=[ @@ -101,13 +107,17 @@ def generate_launch_description(): package='nav2_controller', executable='controller_server', output='screen', + respawn=use_respawn, + respawn_delay=2.0, parameters=[configured_params], - remappings=remappings), + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), 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( @@ -115,13 +125,17 @@ def generate_launch_description(): executable='planner_server', name='planner_server', output='screen', + respawn=use_respawn, + respawn_delay=2.0, parameters=[configured_params], remappings=remappings), Node( - package='nav2_recoveries', - executable='recoveries_server', - name='recoveries_server', + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', output='screen', + respawn=use_respawn, + respawn_delay=2.0, parameters=[configured_params], remappings=remappings), Node( @@ -129,6 +143,8 @@ def generate_launch_description(): executable='bt_navigator', name='bt_navigator', output='screen', + respawn=use_respawn, + respawn_delay=2.0, parameters=[configured_params], remappings=remappings), Node( @@ -136,8 +152,20 @@ def generate_launch_description(): executable='waypoint_follower', name='waypoint_follower', output='screen', + respawn=use_respawn, + respawn_delay=2.0, parameters=[configured_params], remappings=remappings), + Node( + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', @@ -158,7 +186,7 @@ def generate_launch_description(): plugin='nav2_controller::ControllerServer', name='controller_server', parameters=[configured_params], - remappings=remappings), + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), ComposableNode( package='nav2_smoother', plugin='nav2_smoother::SmootherServer', @@ -172,9 +200,9 @@ def generate_launch_description(): parameters=[configured_params], remappings=remappings), ComposableNode( - package='nav2_recoveries', - plugin='recovery_server::RecoveryServer', - name='recoveries_server', + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', parameters=[configured_params], remappings=remappings), ComposableNode( @@ -189,6 +217,13 @@ def generate_launch_description(): name='waypoint_follower', parameters=[configured_params], remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), ComposableNode( package='nav2_lifecycle_manager', plugin='nav2_lifecycle_manager::LifecycleManager', @@ -212,6 +247,7 @@ def generate_launch_description(): 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) diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index f9fd723e4e0..6c3b1c1bd31 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -31,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'] @@ -70,12 +71,18 @@ def generate_launch_description(): '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_map_saver_server_cmd = Node( package='nav2_map_server', executable='map_saver_server', output='screen', + respawn=use_respawn, + respawn_delay=2.0, parameters=[configured_params]) start_lifecycle_manager_cmd = Node( @@ -111,6 +118,7 @@ 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) + ld.add_action(declare_use_respawn_cmd) # Running Map Saver Server ld.add_action(start_map_saver_server_cmd) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 116a7151ab7..b4dfd3bd167 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -40,6 +40,7 @@ def generate_launch_description(): params_file = LaunchConfiguration('params_file') 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') @@ -106,6 +107,10 @@ def generate_launch_description(): '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( @@ -129,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( @@ -154,7 +159,8 @@ def generate_launch_description(): # Specify the actions start_gazebo_server_cmd = ExecuteProcess( condition=IfCondition(use_simulator), - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.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( @@ -207,7 +213,8 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'params_file': params_file, 'autostart': autostart, - 'use_composition': use_composition}.items()) + 'use_composition': use_composition, + 'use_respawn': use_respawn}.items()) # Create the launch description and populate ld = LaunchDescription() @@ -230,6 +237,7 @@ def generate_launch_description(): 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) diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 3b8e82e5c92..685ae9a44c7 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.0.0 + 1.1.0 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index a08229f25cc..a35fe9a8702 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -39,14 +39,6 @@ amcl: 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 @@ -64,27 +56,33 @@ bt_navigator: - 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 @@ -93,8 +91,17 @@ bt_navigator: - 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_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True -bt_navigator_rclcpp_node: +bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: use_sim_time: True @@ -106,7 +113,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 @@ -160,10 +167,6 @@ controller_server: 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: @@ -195,12 +198,6 @@ local_costmap: map_subscribe_transient_local: True always_send_full_costmap: True observation_sources: scan - local_costmap_client: - ros__parameters: - use_sim_time: True - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True global_costmap: global_costmap: @@ -222,12 +219,6 @@ global_costmap: map_subscribe_transient_local: True always_send_full_costmap: True observation_sources: scan - global_costmap_client: - ros__parameters: - use_sim_time: True - global_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True map_server: ros__parameters: @@ -245,26 +236,24 @@ planner_server: use_astar: false allow_unknown: true -planner_server_rclcpp_node: - ros__parameters: - use_sim_time: True - smoother_server: ros__parameters: use_sim_time: True -recoveries_server: +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_tolerance: 0.1 @@ -282,7 +271,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false - waypoint_task_executor_plugin: "wait_at_waypoint" + waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 35a721f734b..618ba12bdfe 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -39,14 +39,6 @@ amcl: 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 @@ -64,27 +56,33 @@ bt_navigator: - 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 @@ -93,8 +91,17 @@ bt_navigator: - 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_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True -bt_navigator_rclcpp_node: +bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: use_sim_time: True @@ -106,7 +113,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 @@ -160,10 +167,6 @@ controller_server: 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: @@ -195,12 +198,6 @@ local_costmap: map_subscribe_transient_local: True always_send_full_costmap: True observation_sources: scan - local_costmap_client: - ros__parameters: - use_sim_time: True - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True global_costmap: global_costmap: @@ -222,12 +219,6 @@ global_costmap: map_subscribe_transient_local: True always_send_full_costmap: True observation_sources: scan - global_costmap_client: - ros__parameters: - use_sim_time: True - global_costmap_rclcpp_node: - ros__parameters: - use_sim_time: True map_server: ros__parameters: @@ -245,26 +236,24 @@ planner_server: use_astar: false allow_unknown: true -planner_server_rclcpp_node: - ros__parameters: - use_sim_time: True - smoother_server: ros__parameters: use_sim_time: True -recoveries_server: +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_tolerance: 0.1 @@ -282,7 +271,7 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false - waypoint_task_executor_plugin: "wait_at_waypoint" + waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 4b2fa5744ae..1b98e85c913 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -39,14 +39,6 @@ amcl: 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 @@ -64,27 +56,33 @@ bt_navigator: - 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 @@ -93,8 +91,17 @@ bt_navigator: - 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: +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: use_sim_time: True @@ -168,10 +175,6 @@ controller_server: RotateToGoal.scale: 32.0 RotateToGoal.slowing_factor: 5.0 RotateToGoal.lookahead_time: -1.0 - -controller_server_rclcpp_node: - ros__parameters: - use_sim_time: True local_costmap: local_costmap: @@ -212,14 +215,9 @@ local_costmap: obstacle_max_range: 2.5 obstacle_min_range: 0.0 static_layer: + plugin: "nav2_costmap_2d::StaticLayer" 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: @@ -255,12 +253,6 @@ global_costmap: 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: @@ -286,26 +278,30 @@ planner_server: 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 -recoveries_server: +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_tolerance: 0.1 @@ -321,10 +317,26 @@ robot_state_publisher: waypoint_follower: ros__parameters: + use_sim_time: True loop_rate: 20 stop_on_failure: false - waypoint_task_executor_plugin: "wait_at_waypoint" + waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 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 00000000000..f4ed518444d --- /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 index 28b918e2f6a..1378863cdf6 100644 --- 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 @@ -13,18 +13,12 @@ - - - - + - - - - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index d6d7f4bbe5f..2396b844aed 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -11,18 +11,12 @@ - - - - + - - - - + 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 00000000000..6b5883bfe70 --- /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 00000000000..e60a2e77513 --- /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_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 00000000000..46d18fe6548 --- /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 00000000000..e921db55578 --- /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/odometry_calibration.xml b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml new file mode 100644 index 00000000000..49000e14fa6 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + 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 00000000000..e91b283c7e8 Binary files /dev/null and b/nav2_bt_navigator/doc/parallel_w_goal_patience_and_recovery.png 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 af5b9092697..0406c8735ef 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -20,6 +20,7 @@ #include #include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/odometry_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -90,12 +91,13 @@ class BtNavigator : public nav2_util::LifecycleNode nav2_bt_navigator::NavigatorMuxer plugin_muxer_; // Odometry smoother object - std::unique_ptr odom_smoother_; + std::shared_ptr odom_smoother_; // Metrics for feedback 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_; diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp index 82c31deaeb6..e53bdce6abd 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp @@ -20,6 +20,7 @@ #include #include +#include "nav2_util/odometry_utils.hpp" #include "tf2_ros/buffer.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -135,13 +136,15 @@ class Navigator * @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) + nav2_bt_navigator::NavigatorMuxer * plugin_muxer, + std::shared_ptr odom_smoother) { auto node = parent_node.lock(); logger_ = node->get_logger(); @@ -161,7 +164,7 @@ class Navigator 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::bind(&Navigator::onCompletion, this, std::placeholders::_1, std::placeholders::_2)); bool ok = true; if (!bt_action_server_->on_configure()) { @@ -173,11 +176,11 @@ class Navigator blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT - return configure(parent_node) && ok; + return configure(parent_node, odom_smoother) && ok; } /** - * @brief Actiation of the navigator's backend BT and actions + * @brief Activation of the navigator's backend BT and actions * @return bool If successful */ bool on_activate() @@ -192,7 +195,7 @@ class Navigator } /** - * @brief Dectiation of the navigator's backend BT and actions + * @brief Deactivation of the navigator's backend BT and actions * @return bool If successful */ bool on_deactivate() @@ -252,18 +255,24 @@ class Navigator return false; } - plugin_muxer_->startNavigating(getName()); + bool goal_accepted = goalReceived(goal); - return goalReceived(goal); + if (goal_accepted) { + plugin_muxer_->startNavigating(getName()); + } + + return goal_accepted; } /** - * @brief An intermediate compution function to mux navigators + * @brief An intermediate completion function to mux navigators */ - void onCompletion(typename ActionT::Result::SharedPtr result) + void onCompletion( + typename ActionT::Result::SharedPtr result, + const nav2_behavior_tree::BtStatus final_bt_status) { plugin_muxer_->stopNavigating(getName()); - goalCompleted(result); + goalCompleted(result, final_bt_status); } /** @@ -285,15 +294,22 @@ class Navigator virtual void onPreempt(typename ActionT::Goal::ConstSharedPtr goal) = 0; /** - * @brief A callback that is called when a the action is completed, can fill in + * @brief A callback that is called when a the action is completed; Can fill in * action result message or indicate that this action is done. */ - virtual void goalCompleted(typename ActionT::Result::SharedPtr result) = 0; + 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*/) {return true;} + virtual bool configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/, + std::shared_ptr/*odom_smoother*/) + { + return true; + } /** * @brief Method to cleanup resources. @@ -301,12 +317,12 @@ class Navigator virtual bool cleanup() {return true;} /** - * @brief Method to active and any threads involved in execution. + * @brief Method to activate any threads involved in execution. */ virtual bool activate() {return true;} /** - * @brief Method to deactive and any threads involved in execution. + * @brief Method to deactivate and any threads involved in execution. */ virtual bool deactivate() {return true;} diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index d86736bad1e..d6e218d860f 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -51,9 +51,11 @@ class NavigateThroughPosesNavigator /** * @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) override; + rclcpp_lifecycle::LifecycleNode::WeakPtr node, + std::shared_ptr odom_smoother) override; /** * @brief Get action name for this navigator @@ -93,8 +95,12 @@ class NavigateThroughPosesNavigator * @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) override; + void goalCompleted( + typename ActionT::Result::SharedPtr result, + const nav2_behavior_tree::BtStatus final_bt_status) override; /** * @brief Goal pose initialization on the blackboard @@ -106,7 +112,7 @@ class NavigateThroughPosesNavigator std::string path_blackboard_id_; // Odometry smoother object - std::unique_ptr odom_smoother_; + std::shared_ptr odom_smoother_; }; } // namespace nav2_bt_navigator 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 index 9773ef65c3d..20230aa10e4 100644 --- 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 @@ -50,9 +50,11 @@ class NavigateToPoseNavigator /** * @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) override; + rclcpp_lifecycle::LifecycleNode::WeakPtr node, + std::shared_ptr odom_smoother) override; /** * @brief A cleanup state transition to remove memory allocated @@ -104,8 +106,12 @@ class NavigateToPoseNavigator * @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) override; + void goalCompleted( + typename ActionT::Result::SharedPtr result, + const nav2_behavior_tree::BtStatus final_bt_status) override; /** * @brief Goal pose initialization on the blackboard @@ -122,7 +128,7 @@ class NavigateToPoseNavigator std::string path_blackboard_id_; // Odometry smoother object - std::unique_ptr odom_smoother_; + std::shared_ptr odom_smoother_; }; } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 7cfb160c01a..e3287b118db 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.0.0 + 1.1.0 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 3ea686fb9d9..79b50992b4d 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -29,7 +29,7 @@ namespace nav2_bt_navigator { BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("bt_navigator", "", false, options) +: nav2_util::LifecycleNode("bt_navigator", "", options) { RCLCPP_INFO(get_logger(), "Creating"); @@ -38,27 +38,33 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "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_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", @@ -66,7 +72,12 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_planner_selector_bt_node", "nav2_controller_selector_bt_node", "nav2_goal_checker_selector_bt_node", - "nav2_controller_cancel_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_parameter("plugin_lib_names", plugin_libs); @@ -95,6 +106,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) 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(); // Libraries to pull plugins (BT Nodes) from auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array(); @@ -108,21 +120,21 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) feedback_utils.robot_frame = robot_frame_; feedback_utils.transform_tolerance = transform_tolerance_; + // Odometry smoother object for getting current speed + odom_smoother_ = std::make_shared(shared_from_this(), 0.3, odom_topic_); + if (!pose_navigator_->on_configure( - shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_)) + shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_)) { return nav2_util::CallbackReturn::FAILURE; } if (!poses_navigator_->on_configure( - shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_)) + shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_)) { return nav2_util::CallbackReturn::FAILURE; } - // Odometry smoother object for getting current speed - odom_smoother_ = std::make_unique(shared_from_this(), 0.3); - return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 20e7d1b139d..34a3f429a49 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -24,19 +24,26 @@ namespace nav2_bt_navigator bool NavigateThroughPosesNavigator::configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, + std::shared_ptr odom_smoother) { start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); - node->declare_parameter("goals_blackboard_id", std::string("goals")); + + 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_ = std::make_unique(node, 0.3); + odom_smoother_ = odom_smoother; return true; } @@ -47,12 +54,16 @@ NavigateThroughPosesNavigator::getDefaultBTFilepath( { std::string default_bt_xml_filename; auto node = parent_node.lock(); - 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"); + + 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; @@ -65,7 +76,7 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { RCLCPP_ERROR( - logger_, "BT file not found: %s. Navigation canceled.", + logger_, "Error loading XML file: %s. Navigation canceled.", bt_xml_filename.c_str()); return false; } @@ -76,7 +87,9 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) } void -NavigateThroughPosesNavigator::goalCompleted(typename ActionT::Result::SharedPtr /*result*/) +NavigateThroughPosesNavigator::goalCompleted( + typename ActionT::Result::SharedPtr /*result*/, + const nav2_behavior_tree::BtStatus /*final_bt_status*/) { } @@ -190,7 +203,7 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr { if (goal->poses.size() > 0) { RCLCPP_INFO( - logger_, "Begin navigating from current location through %li poses to (%.2f, %.2f)", + logger_, "Begin navigating from current location through %zu poses to (%.2f, %.2f)", goal->poses.size(), goal->poses.back().pose.position.x, goal->poses.back().pose.position.y); } diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index de0422e1034..82727af5e2d 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -24,19 +24,26 @@ namespace nav2_bt_navigator bool NavigateToPoseNavigator::configure( - rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, + std::shared_ptr odom_smoother) { start_time_ = rclcpp::Time(0); auto node = parent_node.lock(); - node->declare_parameter("goal_blackboard_id", std::string("goal")); + + 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_ = std::make_unique(node, 0.3); + odom_smoother_ = odom_smoother; self_client_ = rclcpp_action::create_client(node, getName()); @@ -53,12 +60,16 @@ NavigateToPoseNavigator::getDefaultBTFilepath( { std::string default_bt_xml_filename; auto node = parent_node.lock(); - 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"); + + 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; @@ -90,7 +101,9 @@ NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) } void -NavigateToPoseNavigator::goalCompleted(typename ActionT::Result::SharedPtr /*result*/) +NavigateToPoseNavigator::goalCompleted( + typename ActionT::Result::SharedPtr /*result*/, + const nav2_behavior_tree::BtStatus /*final_bt_status*/) { } diff --git a/nav2_common/package.xml b/nav2_common/package.xml index c2e32109889..2213c091047 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.0.0 + 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 00000000000..0d0cb497ae5 --- /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 00000000000..8359e5d3d72 --- /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 00000000000..bfd564772eb --- /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 00000000000..36c313cb092 --- /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 00000000000..b5432bf6591 --- /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 00000000000..8272b2f2aa4 --- /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 00000000000..9be4e88f0ba --- /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 00000000000..050d69077d5 --- /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 00000000000..b253b3b7757 --- /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 00000000000..4ffa4a21c3c --- /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 00000000000..cf4ecd1967c --- /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 00000000000..f50d2fb780b --- /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 00000000000..104b949c4be --- /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/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index 9b495662748..e9091db9ae8 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -61,7 +61,8 @@ class SimpleGoalChecker : public nav2_core::GoalChecker // Standard GoalChecker Interface void initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name) override; + 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, 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 7db43a4b3c7..d7586db6f31 100644 --- a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -57,7 +57,8 @@ class StoppedGoalChecker : public SimpleGoalChecker // Standard GoalChecker Interface void initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name) override; + 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; diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 7bcfb7df892..d8ed5aec78b 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.0.0 + 1.1.0 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index d0d7cc4acb1..597f843eaac 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -63,7 +63,8 @@ SimpleGoalChecker::SimpleGoalChecker() void SimpleGoalChecker::initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name) + const std::string & plugin_name, + const std::shared_ptr/*costmap_ros*/) { plugin_name_ = plugin_name; auto node = parent.lock(); diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp index 351469ab8fd..179c698bd46 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -57,10 +57,11 @@ StoppedGoalChecker::StoppedGoalChecker() void StoppedGoalChecker::initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name) + const std::string & plugin_name, + const std::shared_ptr costmap_ros) { plugin_name_ = plugin_name; - SimpleGoalChecker::initialize(parent, plugin_name); + SimpleGoalChecker::initialize(parent, plugin_name, costmap_ros); auto node = parent.lock(); diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 21b5bebdb1b..387bfcc735b 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); @@ -170,8 +172,10 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) SimpleGoalChecker gc; StoppedGoalChecker sgc; - sgc.initialize(x, "test"); - gc.initialize(x, "test2"); + 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; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 42771344fc6..3f4e71cc1cf 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -34,7 +34,7 @@ namespace nav2_controller { ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("controller_server", "", false, 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"}, @@ -77,7 +77,7 @@ ControllerServer::~ControllerServer() } nav2_util::CallbackReturn -ControllerServer::on_configure(const rclcpp_lifecycle::State & state) +ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { auto node = shared_from_this(); @@ -122,7 +122,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("speed_limit_topic", speed_limit_topic); get_parameter("failure_tolerance", failure_tolerance_); - costmap_ros_->on_configure(state); + costmap_ros_->configure(); try { progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_); @@ -146,7 +146,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_INFO( get_logger(), "Created goal checker : %s of type %s", goal_checker_ids_[i].c_str(), goal_checker_types_[i].c_str()); - goal_checker->initialize(node, goal_checker_ids_[i]); + goal_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( @@ -213,11 +213,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -ControllerServer::on_activate(const rclcpp_lifecycle::State & state) +ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - costmap_ros_->on_activate(state); + costmap_ros_->activate(); ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->activate(); @@ -237,7 +237,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) +ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); @@ -246,7 +246,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->deactivate(); } - costmap_ros_->on_deactivate(state); + costmap_ros_->deactivate(); publishZeroVelocity(); vel_publisher_->on_deactivate(); @@ -259,7 +259,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) +ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); @@ -271,7 +271,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) controllers_.clear(); goal_checkers_.clear(); - costmap_ros_->on_cleanup(state); + costmap_ros_->cleanup(); // Release any allocated resources action_server_.reset(); @@ -344,6 +344,7 @@ bool ControllerServer::findGoalCheckerId( void ControllerServer::computeControl() { std::lock_guard lock(dynamic_params_lock_); + RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { @@ -596,13 +597,28 @@ void ControllerServer::speedLimitCallback(const nav2_msgs::msg::SpeedLimit::Shar rcl_interfaces::msg::SetParametersResult ControllerServer::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 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(); @@ -616,6 +632,8 @@ ControllerServer::dynamicParametersCallback(std::vector param failure_tolerance_ = parameter.as_double(); } } + + dynamic_params_lock_.unlock(); } result.successful = true; diff --git a/nav2_core/README.md b/nav2_core/README.md index 87d052ef256..1d7e9e0bc24 100644 --- a/nav2_core/README.md +++ b/nav2_core/README.md @@ -5,7 +5,7 @@ This package hosts the abstract interface (virtual base classes) for plugins to - 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`) -- recovery behaviors (e.g. `backup`) +- 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 diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/behavior.hpp similarity index 67% rename from nav2_core/include/nav2_core/recovery.hpp rename to nav2_core/include/nav2_core/behavior.hpp index a800b2d21f5..f3510e86847 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 @@ -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 d1fb1053ea3..4e89e0b1177 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -74,8 +74,8 @@ class Controller */ virtual void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr &, - std::string name, const std::shared_ptr &, - const std::shared_ptr &) = 0; + std::string name, std::shared_ptr, + std::shared_ptr) = 0; /** * @brief Method to cleanup resources. diff --git a/nav2_core/include/nav2_core/goal_checker.hpp b/nav2_core/include/nav2_core/goal_checker.hpp index fc1d34f010c..03e11c9a3e9 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 { @@ -68,7 +71,9 @@ class GoalChecker */ virtual void initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name) = 0; + const std::string & plugin_name, + const std::shared_ptr costmap_ros) = 0; + virtual void reset() = 0; /** diff --git a/nav2_core/include/nav2_core/smoother.hpp b/nav2_core/include/nav2_core/smoother.hpp index f567e3cc785..e58b4a5ec62 100644 --- a/nav2_core/include/nav2_core/smoother.hpp +++ b/nav2_core/include/nav2_core/smoother.hpp @@ -40,7 +40,6 @@ class Smoother public: using Ptr = std::shared_ptr; - /** * @brief Virtual destructor */ @@ -48,9 +47,9 @@ class Smoother virtual void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr &, - std::string name, const std::shared_ptr &, - const std::shared_ptr &, - const std::shared_ptr &) = 0; + std::string name, std::shared_ptr, + std::shared_ptr, + std::shared_ptr) = 0; /** * @brief Method to cleanup resources. diff --git a/nav2_core/package.xml b/nav2_core/package.xml index dbd14fd7ff6..52a5390175f 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.0.0 + 1.1.0 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp index 45725757f22..dca22c9f7ff 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 ad56711c3fb..04b9f4daa43 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -92,7 +92,7 @@ class Costmap2D * @brief Constructor for a costmap from an OccupancyGrid map * @param map The OccupancyGrid map to create costmap from */ - Costmap2D(const nav_msgs::msg::OccupancyGrid & map); + explicit Costmap2D(const nav_msgs::msg::OccupancyGrid & map); /** * @brief Overloaded assignment operator @@ -433,48 +433,53 @@ class Costmap2D unsigned int y1, unsigned int max_length = UINT_MAX, unsigned int min_length = 0) { - int dx = x1 - x0; - int dy = y1 - y0; - - unsigned int abs_dx = abs(dx); - unsigned int abs_dy = abs(dy); - - int offset_dx = sign(dx); - int offset_dy = sign(dy) * size_x_; + 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, dy); + double dist = std::hypot(dx_full, dy_full); if (dist < min_length) { return; } - // Adjust starting point and offset to start from min_length distance - unsigned int min_x0 = (unsigned int)(x0 + dx / dist * min_length); - unsigned int min_y0 = (unsigned int)(y0 + dy / dist * min_length); + unsigned int 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); + + int offset_dx = sign(dx); + int offset_dy = sign(dy) * size_x_; + double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); - unsigned int length; // if x is dominant if (abs_dx >= abs_dy) { int error_y = abs_dx / 2; - // Subtract minlength from length since initial point (x0, y0)has been adjusted by min Z - length = (unsigned int)(scale * abs_dx) - min_length; - bresenham2D( - at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, length); + 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; - // Subtract minlength from total length since initial point (x0, y0) has been adjusted by min Z - length = (unsigned int)(scale * abs_dy) - min_length; bresenham2D( - at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, length); + at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy)); } private: 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 7803f69602a..a5dfc6182ac 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 @@ -294,9 +294,15 @@ 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_; @@ -305,6 +311,11 @@ class Costmap2DROS : public nav2_util::LifecycleNode 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_; @@ -321,7 +332,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode 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"}; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index ecfb9d71f8c..c71d3bfea13 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -35,8 +35,8 @@ * Author: Alexey Merzlyakov *********************************************************************/ -#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ -#define NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_ #include #include @@ -78,7 +78,7 @@ class CostmapFilter : public Layer /** * @brief Initialization process of layer on startup */ - virtual void onInitialize() final; + void onInitialize() final; /** * @brief Update the bounds of the master costmap by this layer's update dimensions @@ -90,7 +90,7 @@ class CostmapFilter : public Layer * @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( + void updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y) final; @@ -102,27 +102,27 @@ class CostmapFilter : public Layer * @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( + void updateCosts( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) final; /** * @brief Activate the layer */ - virtual void activate() final; + void activate() final; /** * @brief Deactivate the layer */ - virtual void deactivate() final; + void deactivate() final; /** * @brief Reset the layer */ - virtual void reset() final; + void reset() final; /** * @brief If clearing operations should be processed on this layer or not */ - virtual bool isClearable() {return false;} + bool isClearable() {return false;} /** CostmapFilter API **/ /** @@ -182,4 +182,4 @@ class CostmapFilter : public Layer } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ +#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 index 71d991c7e93..03a157a3e44 100644 --- 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 @@ -35,8 +35,8 @@ * Author: Alexey Merzlyakov *********************************************************************/ -#ifndef NAV2_COSTMAP_2D__FILTER_VALUES_HPP_ -#define NAV2_COSTMAP_2D__FILTER_VALUES_HPP_ +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_ /** Provides constants used in costmap filters */ @@ -59,4 +59,4 @@ static constexpr double NO_SPEED_LIMIT = 0.0; } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__FILTER_VALUES_HPP_ +#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 index 1755d57eb77..e5434571d47 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -35,14 +35,14 @@ * Author: Alexey Merzlyakov *********************************************************************/ -#ifndef NAV2_COSTMAP_2D__KEEPOUT_FILTER_HPP_ -#define NAV2_COSTMAP_2D__KEEPOUT_FILTER_HPP_ - -#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" +#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" @@ -108,4 +108,4 @@ class KeepoutFilter : public CostmapFilter } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__KEEPOUT_FILTER_HPP_ +#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 index c6b86799aa8..c4655607aa8 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp @@ -35,8 +35,11 @@ * Author: Alexey Merzlyakov *********************************************************************/ -#ifndef NAV2_COSTMAP_2D__SPEED_FILTER_HPP_ -#define NAV2_COSTMAP_2D__SPEED_FILTER_HPP_ +#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" @@ -44,8 +47,6 @@ #include "nav2_msgs/msg/costmap_filter_info.hpp" #include "nav2_msgs/msg/speed_limit.hpp" -#include - namespace nav2_costmap_2d { /** @@ -144,4 +145,4 @@ class SpeedFilter : public CostmapFilter } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__SPEED_FILTER_HPP_ +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_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 5c65b1161c7..e4ea745015d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -56,7 +56,6 @@ class CostmapSubscriber */ std::shared_ptr getCostmap(); -protected: /** * @brief Convert an occ grid message into a costmap object */ @@ -66,6 +65,7 @@ class CostmapSubscriber */ 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/footprint_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp index 9993d9190ae..d9e9ddb3c6e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp @@ -86,7 +86,6 @@ class FootprintSubscriber */ 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_; 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 66f6163b3cf..5f68dcaed12 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -238,6 +238,13 @@ class InflationLayer : public Layer 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_; @@ -257,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 5f8c152dc43..72125143bae 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -75,8 +75,7 @@ class Layer std::string name, tf2_ros::Buffer * tf, const nav2_util::LifecycleNode::WeakPtr & node, - rclcpp::Node::SharedPtr client_node, - rclcpp::Node::SharedPtr rclcpp_node); + rclcpp::CallbackGroup::SharedPtr callback_group); /** @brief Stop publishers. */ virtual void deactivate() {} /** @brief Restart publishers if they've been stopped. */ @@ -160,8 +159,7 @@ class Layer LayeredCostmap * layered_costmap_; std::string name_; tf2_ros::Buffer * tf_; - 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")}; 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 2ba47090bf6..fbced80c80d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -131,6 +131,13 @@ class ObstacleLayer : public CostmapLayer */ 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 */ @@ -224,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 @@ -234,6 +242,9 @@ 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_; 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 bf047d7f383..def79e349f4 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" @@ -152,6 +153,13 @@ class StaticLayer : public CostmapLayer */ 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 @@ -178,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 4e91c940b0e..14baefcbdd9 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,7 +51,6 @@ #include #include #include -#include #include #include @@ -220,6 +222,16 @@ class VoxelLayer : public ObstacleLayer { 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/package.xml b/nav2_costmap_2d/package.xml index 8865f88b558..e466dd98477 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.0.0 + 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 diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index daff9ccd082..796c2fd62f7 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_; } @@ -99,6 +101,11 @@ InflationLayer::onInitialize() 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; @@ -113,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_); @@ -125,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; @@ -155,13 +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( - logger_, "InflationLayer::onFootprintChanged(): num footprint points: %lu," + logger_, "InflationLayer::onFootprintChanged(): num footprint points: %zu," " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_); } @@ -407,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 0e33aafb3bc..e7836f9639e 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,7 +76,6 @@ 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("max_obstacle_height", rclcpp::ParameterValue(2.0)); @@ -94,6 +95,12 @@ void ObstacleLayer::onInitialize() node->get_parameter("transform_tolerance", transform_tolerance); node->get_parameter(name_ + "." + "observation_sources", topics_string); + 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()); @@ -112,6 +119,9 @@ void ObstacleLayer::onInitialize() 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); @@ -211,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( @@ -239,9 +250,8 @@ 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) { @@ -250,9 +260,11 @@ void ObstacleLayer::onInitialize() "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( @@ -272,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, @@ -365,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); } @@ -469,6 +514,7 @@ ObstacleLayer::updateCosts( int max_i, int max_j) { + std::lock_guard guard(*getMutex()); if (!enabled_) { return; } @@ -569,8 +615,11 @@ ObstacleLayer::raytraceFreespace( if (!worldToMap(ox, oy, x0, y0)) { RCLCPP_WARN( logger_, - "Sensor origin at (%.2f, %.2f) is out of map bounds. The costmap cannot raytrace for it.", - ox, oy); + "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; } diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 75956d64621..073109c2352 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -51,6 +51,7 @@ 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 { @@ -110,6 +111,7 @@ StaticLayer::activate() void StaticLayer::deactivate() { + dyn_params_handler_.reset(); } void @@ -162,6 +164,12 @@ 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 @@ -318,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; } @@ -372,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; @@ -435,4 +440,46 @@ StaticLayer::updateCosts( 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 5887ef467ef..149dfd6ede0 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 { @@ -102,10 +103,17 @@ void VoxelLayer::onInitialize() 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() @@ -137,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); } @@ -265,8 +275,12 @@ void VoxelLayer::raytraceFreespace( if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) { RCLCPP_WARN( logger_, - "Sensor origin: (%.2f, %.2f, %.2f), out of map bounds. The costmap can't raytrace for it.", - ox, oy, oz); + "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; } @@ -458,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/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index fde37a38150..906a9f0324b 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -126,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; diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 0ea0de0747b..12d6b2ee687 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -65,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 @@ -84,9 +84,6 @@ 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", "voxel_layer", "range_layer"}; @@ -129,6 +126,9 @@ 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_ = std::make_unique( global_frame_, rolling_window_, track_unknown_space_); @@ -140,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_); @@ -157,7 +158,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) // TODO(mjeronimo): instead of get(), use a shared ptr plugin->initialize( layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(), - shared_from_this(), client_node_, rclcpp_node_); + shared_from_this(), callback_group_); RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str()); } @@ -170,7 +171,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) filter->initialize( layered_costmap_.get(), filter_names_[i], tf_buffer_.get(), - shared_from_this(), client_node_, rclcpp_node_); + shared_from_this(), callback_group_); RCLCPP_INFO(get_logger(), "Initialized costmap filter \"%s\"", filter_names_[i].c_str()); } @@ -201,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; } @@ -238,10 +242,8 @@ 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(); @@ -264,11 +266,8 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) 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; } @@ -289,6 +288,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) costmap_publisher_.reset(); clear_costmap_service_.reset(); + executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index 2c12cd104ba..d56ac942e98 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -55,7 +55,6 @@ std::shared_ptr CostmapSubscriber::getCostmap() void CostmapSubscriber::toCostmap2D() { - auto current_costmap_msg = std::atomic_load(&costmap_msg_); if (costmap_ == nullptr) { diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 51c1fe78410..5898879e8cb 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -62,7 +62,6 @@ double FootprintCollisionChecker::footprintCost(const Footprint footpr // 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 second point if (!worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) { return static_cast(LETHAL_OBSTACLE); @@ -94,13 +93,13 @@ 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 (line_cost < point_cost) { - line_cost = point_cost; + // if in collision, no need to continue + if (point_cost == static_cast(LETHAL_OBSTACLE)) { + return point_cost; } - // if in collision, no need to continue - if (line_cost == static_cast(LETHAL_OBSTACLE)) { - return line_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 45e60db65e8..2c269fa3b06 100644 --- a/nav2_costmap_2d/src/footprint_subscriber.cpp +++ b/nav2_costmap_2d/src/footprint_subscriber.cpp @@ -31,8 +31,7 @@ FootprintSubscriber::FootprintSubscriber( tf2_ros::Buffer & tf, std::string robot_base_frame, double transform_tolerance) -: topic_name_(topic_name), - tf_(tf), +: tf_(tf), robot_base_frame_(robot_base_frame), transform_tolerance_(transform_tolerance) { @@ -48,8 +47,7 @@ FootprintSubscriber::FootprintSubscriber( tf2_ros::Buffer & tf, std::string robot_base_frame, double transform_tolerance) -: topic_name_(topic_name), - tf_(tf), +: tf_(tf), robot_base_frame_(robot_base_frame), transform_tolerance_(transform_tolerance) { diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index e76f7de7bf8..852b4b29b5a 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -50,16 +50,13 @@ Layer::initialize( std::string name, tf2_ros::Buffer * tf, const nav2_util::LifecycleNode::WeakPtr & node, - rclcpp::Node::SharedPtr client_node, - rclcpp::Node::SharedPtr rclcpp_node) + rclcpp::CallbackGroup::SharedPtr callback_group) { layered_costmap_ = parent; name_ = name; tf_ = tf; - client_node_ = client_node; - rclcpp_node_ = rclcpp_node; node_ = node; - + callback_group_ = callback_group; { auto node_shared_ptr = node_.lock(); logger_ = node_shared_ptr->get_logger(); diff --git a/nav2_costmap_2d/test/CMakeLists.txt b/nav2_costmap_2d/test/CMakeLists.txt index 44b89f86933..ad7905ad1f5 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/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index e3619c070dd..ad867128b12 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; @@ -182,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"))); @@ -600,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 643a3007fe1..707d795a34b 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; @@ -405,5 +406,151 @@ TEST_F(TestNode, testRaytracing) { 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/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 147d79c1d04..cfafb6eb4e0 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 @@ -88,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 @@ -106,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()); @@ -132,15 +135,18 @@ class TestCollisionChecker : public nav2_util::LifecycleNode 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; } @@ -165,6 +171,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode delete layers_; layers_ = nullptr; + executor_thread_.reset(); tf_buffer_.reset(); footprint_sub_.reset(); @@ -278,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_; diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt new file mode 100644 index 00000000000..c492459d2ff --- /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 00000000000..a45cb38a7f9 --- /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 957b858452a..8ab95e6d444 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 @@ -71,30 +71,33 @@ 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)); } @@ -130,13 +133,14 @@ void addObservation( 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/copy_window_test.cpp b/nav2_costmap_2d/test/unit/copy_window_test.cpp index 0f4f9ebe0eb..ce943e1cc39 100644 --- a/nav2_costmap_2d/test/unit/copy_window_test.cpp +++ b/nav2_costmap_2d/test/unit/copy_window_test.cpp @@ -35,8 +35,8 @@ TEST(CopyWindow, copyValidWindow) ASSERT_TRUE(dst.copyWindow(src, 2, 2, 6, 6, 0, 0)); // Check that both marked cells were copied to destination costmap - ASSERT_TRUE(dst.getCost(0, 0) == 100); - ASSERT_TRUE(dst.getCost(3, 3) == 200); + ASSERT_EQ(dst.getCost(0, 0), 100); + ASSERT_EQ(dst.getCost(3, 3), 200); } TEST(CopyWindow, copyInvalidWindow) diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp index 4edb6af29cd..a8a717c8f9d 100644 --- a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp +++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp @@ -44,7 +44,7 @@ TEST(DeclareParameter, useValidParameter) tf2_ros::Buffer tf(node->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); - layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); + layer.initialize(&layers, "test_layer", &tf, node, nullptr); layer.declareParameter("test1", rclcpp::ParameterValue("test_val1")); try { @@ -63,7 +63,7 @@ TEST(DeclareParameter, useInvalidParameter) tf2_ros::Buffer tf(node->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); - layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); + layer.initialize(&layers, "test_layer", &tf, node, nullptr); layer.declareParameter("test2", rclcpp::PARAMETER_STRING); try { diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index d9db1086530..304b423f5c0 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -34,9 +34,9 @@ using namespace std::chrono_literals; -static const std::string FILTER_NAME = "keepout_filter"; -static const std::string INFO_TOPIC = "costmap_filter_info"; -static const std::string MASK_TOPIC = "mask"; +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 { @@ -69,7 +69,7 @@ class InfoPublisher : public rclcpp::Node class MaskPublisher : public rclcpp::Node { public: - MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) + explicit MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) : Node("mask_pub") { publisher_ = this->create_publisher( @@ -216,16 +216,16 @@ void TestNode::createKeepoutFilter(const std::string & global_frame) nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false); node_->declare_parameter( - FILTER_NAME + ".transform_tolerance", rclcpp::ParameterValue(0.5)); + std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5)); node_->set_parameter( - rclcpp::Parameter(FILTER_NAME + ".transform_tolerance", 0.5)); + rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5)); node_->declare_parameter( - FILTER_NAME + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); + std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); node_->set_parameter( - rclcpp::Parameter(FILTER_NAME + ".filter_info_topic", INFO_TOPIC)); + rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC)); keepout_filter_ = std::make_shared(); - keepout_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr, nullptr); + 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 diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index a92bd5e156b..d419cade363 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -38,10 +38,10 @@ using namespace std::chrono_literals; -static const std::string FILTER_NAME = "speed_filter"; -static const std::string INFO_TOPIC = "costmap_filter_info"; -static const std::string MASK_TOPIC = "mask"; -static const std::string SPEED_LIMIT_TOPIC = "speed_limit"; +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; @@ -82,7 +82,7 @@ class InfoPublisher : public rclcpp::Node class MaskPublisher : public rclcpp::Node { public: - MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) + explicit MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) : Node("mask_pub") { publisher_ = this->create_publisher( @@ -104,7 +104,7 @@ class MaskPublisher : public rclcpp::Node class SpeedLimitSubscriber : public rclcpp::Node { public: - SpeedLimitSubscriber(const std::string & speed_limit_topic) + explicit SpeedLimitSubscriber(const std::string & speed_limit_topic) : Node("speed_limit_sub"), speed_limit_updated_(false) { subscriber_ = this->create_subscription( @@ -349,20 +349,20 @@ bool TestNode::createSpeedFilter(const std::string & global_frame) nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false); node_->declare_parameter( - FILTER_NAME + ".transform_tolerance", rclcpp::ParameterValue(0.5)); + std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5)); node_->set_parameter( - rclcpp::Parameter(FILTER_NAME + ".transform_tolerance", 0.5)); + rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5)); node_->declare_parameter( - FILTER_NAME + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); + std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); node_->set_parameter( - rclcpp::Parameter(FILTER_NAME + ".filter_info_topic", INFO_TOPIC)); + rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC)); node_->declare_parameter( - FILTER_NAME + ".speed_limit_topic", rclcpp::ParameterValue(SPEED_LIMIT_TOPIC)); + std::string(FILTER_NAME) + ".speed_limit_topic", rclcpp::ParameterValue(SPEED_LIMIT_TOPIC)); node_->set_parameter( - rclcpp::Parameter(FILTER_NAME + ".speed_limit_topic", SPEED_LIMIT_TOPIC)); + 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, nullptr); + 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); diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 92081c71267..c4d1f04f28f 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.0.0 + 1.1.0 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 81c39aee268..ba2c87d67b4 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 @@ -68,8 +68,8 @@ class DWBLocalPlanner : public nav2_core::Controller void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) override; + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; virtual ~DWBLocalPlanner() {} diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 08e005440fd..a3f64e07a14 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.0.0 + 1.1.0 TODO Carl Delsey BSD-3-Clause 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 ab7baed0462..0be00388cac 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 { @@ -65,8 +67,8 @@ DWBLocalPlanner::DWBLocalPlanner() void DWBLocalPlanner::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) { node_ = parent; auto node = node_.lock(); @@ -87,7 +89,7 @@ void DWBLocalPlanner::configure( rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".prune_distance", - rclcpp::ParameterValue(1.0)); + rclcpp::ParameterValue(2.0)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".debug_trajectory_details", rclcpp::ParameterValue(false)); @@ -434,17 +436,6 @@ 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) @@ -467,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. diff --git a/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp b/nav2_dwb_controller/dwb_core/src/trajectory_utils.cpp index 23dad70fdfc..a7764d4b7a8 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/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 1d6a1668dc4..a213c6e200c 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.0.0 + 1.1.0 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 5b146ed08fc..9c84c226fe1 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.0.0 + 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/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index e9535c72709..2236fc8274b 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.0.0 + 1.1.0 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 52d09579a53..cab00606dc5 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.0.0 + 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 1cb957c74a6..f9ca6fba949 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.0.0 + 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/include/nav_2d_utils/parameters.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp index 5951bd09efa..c3c30428feb 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)); diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index e93a6f01f7a..90da3fc6286 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.0.0 + 1.1.0 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp index 85cae2cb699..077707b3947 100644 --- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp +++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp @@ -33,17 +33,20 @@ */ #include "nav_2d_utils/conversions.hpp" -#include -#include -#include -#include -#include + #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_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt index e0980df7b48..5f4b3f3c232 100644 --- a/nav2_lifecycle_manager/CMakeLists.txt +++ b/nav2_lifecycle_manager/CMakeLists.txt @@ -15,6 +15,7 @@ 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() @@ -42,6 +43,7 @@ set(dependencies std_srvs tf2_geometry_msgs bondcpp + diagnostic_updater ) ament_target_dependencies(${library_name} 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 e218871c04c..edd3454ffe6 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. @@ -29,9 +30,12 @@ #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; /** @@ -98,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 @@ -151,6 +155,13 @@ class LifecycleManager : public rclcpp::Node */ 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 */ @@ -161,7 +172,7 @@ class LifecycleManager : public rclcpp::Node /** * @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 /** @@ -169,9 +180,16 @@ 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 @@ -190,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/package.xml b/nav2_lifecycle_manager/package.xml index e349e3af490..d258e30154c 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.0.0 + 1.1.0 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 @@ -20,6 +20,7 @@ tf2_geometry_msgs bondcpp nav2_common + diagnostic_updater geometry_msgs lifecycle_msgs @@ -31,6 +32,7 @@ std_srvs bondcpp tf2_geometry_msgs + diagnostic_updater ament_lint_auto ament_lint_common diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 8474969f8a4..f18436f21f1 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. @@ -32,7 +33,7 @@ namespace nav2_lifecycle_manager { LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) -: Node("lifecycle_manager", options) +: Node("lifecycle_manager", options), diagnostics_updater_(this) { RCLCPP_INFO(get_logger(), "Creating"); @@ -41,6 +42,8 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) declare_parameter("node_names", rclcpp::PARAMETER_STRING_ARRAY); declare_parameter("autostart", rclcpp::ParameterValue(false)); declare_parameter("bond_timeout", 4.0); + declare_parameter("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_); @@ -49,6 +52,12 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) 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"), @@ -90,10 +99,12 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) }, callback_group_); } + auto executor = std::make_shared(); + executor->add_callback_group(callback_group_, get_node_base_interface()); + service_thread_ = std::make_unique(executor); }); - 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() @@ -136,6 +147,16 @@ 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() { @@ -207,20 +228,35 @@ LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t } 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; } } @@ -267,22 +303,23 @@ LifecycleManager::shutdown() } 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"); - return true; } @@ -297,8 +334,8 @@ LifecycleManager::pause() RCLCPP_ERROR(get_logger(), "Failed to pause nodes: aborting pause"); return false; } - message("Managed nodes have been paused"); + message("Managed nodes have been paused"); return true; } @@ -310,6 +347,7 @@ LifecycleManager::resume() RCLCPP_ERROR(get_logger(), "Failed to resume nodes: aborting resume"); return false; } + message("Managed nodes are active"); system_active_ = true; createBondTimer(); @@ -324,7 +362,6 @@ LifecycleManager::createBondTimer() } message("Creating bond timer..."); - bond_timer_ = this->create_wall_timer( 200ms, std::bind(&LifecycleManager::checkBondConnections, this), @@ -364,9 +401,66 @@ LifecycleManager::checkBondConnections() "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(); + 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(); } } @@ -382,8 +476,4 @@ LifecycleManager::message(const std::string & msg) } // namespace nav2_lifecycle_manager #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_lifecycle_manager::LifecycleManager) 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 6863844c7b5..bfeb986c624 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -56,7 +56,6 @@ class MapSaver : public nav2_util::LifecycleNode const std::string & map_topic, const SaveParameters & save_parameters); -protected: /** * @brief Sets up map saving service * @param state Lifecycle Node's state @@ -88,6 +87,7 @@ class MapSaver : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; +protected: /** * @brief Map saving service callback * @param request_header Service request header diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index d89039889ce..b719f972a39 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.0.0 + 1.1.0 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/src/map_saver/main_cli.cpp b/nav2_map_server/src/map_saver/main_cli.cpp index 09cf039f441..48d53450606 100644 --- a/nav2_map_server/src/map_saver/main_cli.cpp +++ b/nav2_map_server/src/map_saver/main_cli.cpp @@ -164,6 +164,7 @@ int main(int argc, char ** argv) int retcode; try { auto map_saver = std::make_shared(); + map_saver->on_configure(rclcpp_lifecycle::State()); if (map_saver->saveMapTopicToFile(map_topic, save_parameters)) { retcode = 0; } else { diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 580825f8ed4..1517e787526 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -42,16 +42,15 @@ using namespace std::placeholders; namespace nav2_map_server { MapSaver::MapSaver(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("map_saver", "", false, options) +: nav2_util::LifecycleNode("map_saver", "", options) { RCLCPP_INFO(get_logger(), "Creating"); - save_map_timeout_ = std::make_shared( - 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); - map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", true); + // Declare the node parameters + declare_parameter("save_map_timeout", 2.0); + declare_parameter("free_thresh_default", 0.25); + declare_parameter("occupied_thresh_default", 0.65); + declare_parameter("map_subscribe_transient_local", true); } MapSaver::~MapSaver() @@ -66,6 +65,12 @@ MapSaver::on_configure(const rclcpp_lifecycle::State & /*state*/) // Make name prefix for services const std::string service_prefix = get_name() + std::string("/"); + save_map_timeout_ = std::make_shared( + rclcpp::Duration::from_seconds(get_parameter("save_map_timeout").as_double())); + free_thresh_default_ = get_parameter("free_thresh_default").as_double(); + occupied_thresh_default_ = get_parameter("occupied_thresh_default").as_double(); + map_subscribe_transient_local_ = get_parameter("map_subscribe_transient_local").as_bool(); + // Create a service that saves the occupancy grid from map topic to a file save_map_service_ = create_service( service_prefix + save_map_service_name_, diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index 332c67ee5eb..5920e095be0 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -63,7 +63,7 @@ namespace nav2_map_server { MapServer::MapServer(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("map_server", "", false, options) +: nav2_util::LifecycleNode("map_server", "", options) { RCLCPP_INFO(get_logger(), "Creating"); 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 1f57fa20c5d..909675ae8a5 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" 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 e381afec87a..d034ca3c68c 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" diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index e85d2bd8efa..ce7376cebb1 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Particle.msg" "msg/ParticleCloud.msg" "srv/GetCostmap.srv" + "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" "srv/ClearCostmapAroundRobot.srv" "srv/ClearEntireCostmap.srv" @@ -32,13 +33,14 @@ rosidl_generate_interfaces(${PROJECT_NAME} "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" DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs ) diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action index 4d2ca6b42d0..9937302578a 100644 --- a/nav2_msgs/action/BackUp.action +++ b/nav2_msgs/action/BackUp.action @@ -6,4 +6,5 @@ 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 index d149f7d3814..a1f609a9e0b 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -8,4 +8,4 @@ bool use_start # If false, use current robot pose as path start, if true, use st nav_msgs/Path path builtin_interfaces/Duration planning_time --- -#feedback +#feedback definition diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 0eddc790753..a052c552abc 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -8,4 +8,4 @@ bool use_start # If false, use current robot pose as path start, if true, use st 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 00000000000..9937302578a --- /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 78fabe0b81e..4b8db815d2b 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 0e935962844..5462faa2398 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -6,6 +6,6 @@ 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 dfa7517a5bc..c4b47648219 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 index 2a0d29e5a75..e2a57f25a94 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -5,6 +5,7 @@ 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 diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index a7b778f4d6a..b7077922418 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -5,6 +5,7 @@ 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 diff --git a/nav2_msgs/action/SmoothPath.action b/nav2_msgs/action/SmoothPath.action index 66b809485b0..18560d70b97 100644 --- a/nav2_msgs/action/SmoothPath.action +++ b/nav2_msgs/action/SmoothPath.action @@ -9,4 +9,4 @@ nav_msgs/Path path builtin_interfaces/Duration smoothing_duration bool was_completed --- -#feedback +#feedback definition diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action index e2db32d5912..3e1c0aadef0 100644 --- a/nav2_msgs/action/Spin.action +++ b/nav2_msgs/action/Spin.action @@ -5,4 +5,5 @@ 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 ed651226c61..8cf7119431d 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/package.xml b/nav2_msgs/package.xml index c9e90e6fbaf..01db2506e54 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.0.0 + 1.1.0 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_msgs/srv/IsPathValid.srv b/nav2_msgs/srv/IsPathValid.srv new file mode 100644 index 00000000000..8d6dc3b38dd --- /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/package.xml b/nav2_navfn_planner/package.xml index 103143f337e..6aa6dd8f1fd 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.0.0 + 1.1.0 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index ec599e5070a..54e740e5280 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -38,6 +38,7 @@ #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 { @@ -200,6 +201,15 @@ class PlannerServer : public nav2_util::LifecycleNode */ 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 Reference to Global Path @@ -240,6 +250,9 @@ class PlannerServer : public nav2_util::LifecycleNode // 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 e75e383729b..d9902074df1 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.0.0 + 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 ce1c1091e31..9abc461d305 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -27,6 +27,7 @@ #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" @@ -39,7 +40,7 @@ namespace nav2_planner { PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("planner_server", "", false, options), +: nav2_util::LifecycleNode("planner_server", "", options), gp_loader_("nav2_core", "nav2_core::GlobalPlanner"), default_ids_{"GridBased"}, default_types_{"nav2_navfn_planner/NavfnPlanner"}, @@ -73,11 +74,11 @@ PlannerServer::~PlannerServer() } nav2_util::CallbackReturn -PlannerServer::on_configure(const rclcpp_lifecycle::State & state) +PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); - costmap_ros_->on_configure(state); + costmap_ros_->configure(); costmap_ = costmap_ros_->getCostmap(); RCLCPP_DEBUG( @@ -153,14 +154,14 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -PlannerServer::on_activate(const rclcpp_lifecycle::State & state) +PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); plan_publisher_->on_activate(); action_server_pose_->activate(); action_server_poses_->activate(); - costmap_ros_->on_activate(state); + costmap_ros_->activate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -168,6 +169,13 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & state) } auto node = shared_from_this(); + + 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)); @@ -179,14 +187,14 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -PlannerServer::on_deactivate(const rclcpp_lifecycle::State & state) +PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); action_server_pose_->deactivate(); action_server_poses_->deactivate(); plan_publisher_->on_deactivate(); - costmap_ros_->on_deactivate(state); + costmap_ros_->deactivate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -202,7 +210,7 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) +PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); @@ -210,7 +218,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) action_server_poses_.reset(); plan_publisher_.reset(); tf_.reset(); - costmap_ros_->on_cleanup(state); + costmap_ros_->cleanup(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -324,7 +332,7 @@ bool PlannerServer::validatePath( RCLCPP_DEBUG( get_logger(), - "Found valid path of size %lu to (%.2f, %.2f)", + "Found valid path of size %zu to (%.2f, %.2f)", path.poses.size(), goal.pose.position.x, goal.pose.position.y); @@ -414,7 +422,7 @@ PlannerServer::computePlanThroughPoses() } catch (std::exception & ex) { RCLCPP_WARN( get_logger(), - "%s plugin failed to plan through %li points with final goal (%.2f, %.2f): \"%s\"", + "%s plugin failed to plan through %zu points with final goal (%.2f, %.2f): \"%s\"", goal->planner_id.c_str(), goal->goals.size(), goal->goals.back().pose.position.x, goal->goals.back().pose.position.y, ex.what()); action_server_poses_->terminate_current(); @@ -522,6 +530,57 @@ PlannerServer::publishPlan(const nav_msgs::msg::Path & path) } } +void PlannerServer::isPathValid( + const std::shared_ptr request, + std::shared_ptr response) +{ + response->is_valid = true; + + 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; + } + } + + /** + * 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) + { + response->is_valid = false; + } + } + } +} + rcl_interfaces::msg::SetParametersResult PlannerServer::dynamicParametersCallback(std::vector parameters) { diff --git a/nav2_recoveries/README.md b/nav2_recoveries/README.md deleted file mode 100644 index beecd7cdac6..00000000000 --- a/nav2_recoveries/README.md +++ /dev/null @@ -1,15 +0,0 @@ -# Recoveries - -The `nav2_recoveries` package implements a task server for executing simple controlled robot movements such as rotating on its own axis, assisted teleop, or moving linearly. - -The package defines: -- A `Recovery` template which is used as a base class to implement specific recovery timed action server - but not required. -- The `BackUp`, `Spin` and `Wait` recoveries. - -The only required class a recovery must derive from is the `nav2_core/recovery.hpp` class, which implements the pluginlib interface the recovery server will use to dynamically load your behavior. The `nav2_recoveries/recovery.hpp` derives from this class and implements a generic action server for a timed recovery 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 `recovery.hpp` helps in managing the complexity to simplify new behavior development, described more below. - -The value of the centralized recovery 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-recovery-server.html) for additional parameter descriptions and a [tutorial about writing recovery behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_recovery_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_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp deleted file mode 100644 index d078b6c8b8b..00000000000 --- a/nav2_recoveries/plugins/back_up.cpp +++ /dev/null @@ -1,165 +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() -{ - auto node = 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_); -} - -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."); - } - - // Silently ensure that both the speed and direction are positive. - 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; -} - -Status BackUp::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 BackUp goal - Exiting BackUp"); - 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(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 >= 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; - 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(logger_, "Collision Ahead - Exiting BackUp"); - return Status::FAILED; - } - - 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; - bool fetch_data = true; - - 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, fetch_data)) { - return false; - } - fetch_data = 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 7c979d153a2..00000000000 --- a/nav2_recoveries/plugins/back_up.hpp +++ /dev/null @@ -1,88 +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 nav2_recoveries::BackUp - * @brief An action server recovery for spinning in - */ -class BackUp : public Recovery -{ -public: - /** - * @brief A constructor for nav2_recoveries::BackUp - */ - BackUp(); - ~BackUp(); - - /** - * @brief Initialization to run behavior - * @param command Goal to execute - * @return Status of recovery - */ - Status onRun(const std::shared_ptr command) override; - - /** - * @brief Loop function to run behavior - * @return Status of recovery - */ - Status onCycleUpdate() override; - -protected: - /** - * @brief Check if pose is collision free - * @param distance Distance to check forward - * @param cmd_vel current commanded velocity - * @param pose2d Current pose - * @return is collision free or not - */ - bool isCollisionFree( - const double & distance, - geometry_msgs::msg::Twist * cmd_vel, - geometry_msgs::msg::Pose2D & pose2d); - - /** - * @brief Configuration of recovery action - */ - void onConfigure() override; - - double min_linear_vel_; - 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_; - - 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 b8ccaca0ad5..00000000000 --- 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 98cb9a6ec04..00000000000 --- 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 d64353d94a1..6c17481925c 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -11,6 +11,7 @@ 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) @@ -28,6 +29,7 @@ set(dependencies nav2_util nav2_core tf2 + tf2_geometry_msgs ) set(library_name nav2_regulated_pure_pursuit_controller) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index cf1d115c92b..839c0b0925c 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -30,14 +30,12 @@ Note that a pure pursuit controller is that, it "purely" pursues the path withou ## 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. @@ -55,8 +53,6 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | 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 | @@ -77,6 +73,8 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | +| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | +| `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: @@ -89,7 +87,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: @@ -104,8 +102,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 @@ -123,6 +119,8 @@ controller_server: use_rotate_to_heading: true rotate_to_heading_min_angle: 0.785 max_angular_accel: 3.2 + 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 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 00000000000..550238704e6 --- /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 b69bd0e63c3..9224921de2f 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 @@ -60,8 +60,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) override; + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; /** * @brief Cleanup controller state machine @@ -112,11 +112,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller 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. @@ -219,6 +222,20 @@ class RegulatedPurePursuitController : public nav2_core::Controller const double & curvature, const geometry_msgs::msg::Twist & speed, 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 * @param lookahead_dist Optimal lookahead distance @@ -232,7 +249,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param pose Pose input to determine the cusp position * @return robot distance from the cusp */ - double findDirectionChange(const geometry_msgs::msg::PoseStamped & pose); + 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 @@ -272,6 +295,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller 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_; diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 0420ba04d22..cffd2c1f499 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.0.0 + 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 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 2c3e7c63fbb..3428faae854 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 @@ -40,8 +40,8 @@ namespace nav2_regulated_pure_pursuit_controller void RegulatedPurePursuitController::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) { auto node = parent.lock(); node_ = parent; @@ -105,6 +105,12 @@ void RegulatedPurePursuitController::configure( node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( 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_; @@ -147,6 +153,12 @@ void RegulatedPurePursuitController::configure( node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); 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; @@ -232,7 +244,8 @@ 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 @@ -270,11 +283,11 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Check for reverse driving if (allow_reversing_) { // Cusp check - double dist_to_direction_change = findDirectionChange(pose); + double dist_to_cusp = findVelocitySignChange(transformed_plan); // if the lookahead distance is further than the cusp, use the cusp distance instead - if (dist_to_direction_change < lookahead_dist) { - lookahead_dist = dist_to_direction_change; + if (dist_to_cusp < lookahead_dist) { + lookahead_dist = dist_to_cusp; } } @@ -317,7 +330,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity 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; + angular_vel = sign * linear_vel * curvature; } // Collision checking on this velocity heading @@ -365,6 +378,41 @@ void RegulatedPurePursuitController::rotateToHeading( 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( const double & lookahead_dist, const nav_msgs::msg::Path & transformed_plan) @@ -378,6 +426,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; @@ -602,23 +665,27 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( } // 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 = nav2_util::geometry_utils::min_by( - global_plan_.poses.begin(), global_plan_.poses.end(), + 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 @@ -652,28 +719,30 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( return transformed_plan; } -double RegulatedPurePursuitController::findDirectionChange( - const geometry_msgs::msg::PoseStamped & pose) +double RegulatedPurePursuitController::findVelocitySignChange( + const nav_msgs::msg::Path & transformed_plan) { - // Iterating through the global path to determine the position of the cusp - for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size() - 1; ++pose_id) { + // 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 = global_plan_.poses[pose_id].pose.position.x - - global_plan_.poses[pose_id - 1].pose.position.x; - double oa_y = global_plan_.poses[pose_id].pose.position.y - - global_plan_.poses[pose_id - 1].pose.position.y; - double ab_x = global_plan_.poses[pose_id + 1].pose.position.x - - global_plan_.poses[pose_id].pose.position.x; - double ab_y = global_plan_.poses[pose_id + 1].pose.position.y - - global_plan_.poses[pose_id].pose.position.y; + 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) { - auto x = global_plan_.poses[pose_id].pose.position.x - pose.pose.position.x; - auto y = global_plan_.poses[pose_id].pose.position.y - pose.pose.position.y; - return hypot(x, y); // returning the distance if there is a cusp + // 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); } } @@ -700,6 +769,13 @@ bool RegulatedPurePursuitController::transformPose( return false; } +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( diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt index aee6297fcbb..3e8d4c5b0a0 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 00000000000..4b516ec1f3b --- /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 00000000000..c079e21614f --- /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 00000000000..795555521a7 --- /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 9139097890e..9f65a8554fe 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -22,8 +22,10 @@ #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 { @@ -58,6 +60,14 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure 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) { @@ -92,10 +102,16 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure linear_vel, sign); } - double findDirectionChangeWrapper( + 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 findDirectionChange(pose); + return transformGlobalPlan(pose); } }; @@ -108,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(); @@ -150,15 +167,21 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) EXPECT_EQ(rtn->point.z, 0.01); } -TEST(RegulatedPurePursuitTest, findDirectionChange) +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; @@ -166,16 +189,140 @@ TEST(RegulatedPurePursuitTest, findDirectionChange) path.poses[2].pose.position.x = -1.0; path.poses[2].pose.position.y = -1.0; ctrl->setPlan(path); - auto rtn = ctrl->findDirectionChangeWrapper(pose); - EXPECT_EQ(rtn, sqrt(5.0)); + 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->findDirectionChangeWrapper(pose); + 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(); @@ -225,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); @@ -234,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) @@ -452,3 +614,356 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) 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/README.md b/nav2_rotation_shim_controller/README.md index 3c03186452a..15185bfa963 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -47,7 +47,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: 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 index b185cb28802..d3f96dd25ef 100644 --- 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 @@ -61,8 +61,8 @@ class RotationShimController : public nav2_core::Controller */ void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) override; + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; /** * @brief Cleanup controller state machine diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index e180a944d81..a92ba01bb06 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.0.0 + 1.1.0 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index ebae7f19715..bc86900f640 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -35,8 +35,8 @@ RotationShimController::RotationShimController() void RotationShimController::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) { plugin_name_ = name; node_ = parent; diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index b6232d003c6..e1bc30f80ec 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -263,7 +263,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) pose.header.frame_id = "base_link"; geometry_msgs::msg::Twist velocity; nav2_controller::SimpleGoalChecker checker; - checker.initialize(node, "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 diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index cfbed8a70e3..39bdbabae49 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -33,8 +33,8 @@ 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 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index e40a5c9058a..5e090d7496f 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.0.0 + 1.1.0 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 1eb5b54f77a..c4f398f77d3 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -17,24 +17,29 @@ The methods provided by the basic navigator are shown below, with inputs and exp | Robot Navigator Method | Description | | --------------------------------- | -------------------------------------------------------------------------- | | setInitialPose(initial_pose) | Sets the initial pose (`PoseStamped`) of the robot to localization. | -| goThroughPoses(poses) | Requests the robot to drive through a set of poses (list of `PoseStamped`).| -| goToPose(pose) | Requests the robot to drive to a pose (`PoseStamped`). | +| 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. | -| cancelNav() | Cancel an ongoing `goThroughPoses` `goToPose` or `followWaypoints` request.| -| isNavComplete() | Checks if navigation is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | -| getFeedback() | Gets feedback from navigation task, returns action server feedback object. | -| getResult() | Gets final result of navigation task, to be called after `isNavComplete` returns `True`. Returns action server result object. | -| getPath(start, goal) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | -| getPathThroughPoses(start, goals) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. | +| 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() | 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. | +| 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: @@ -50,18 +55,21 @@ 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.isNavComplete(): +while not nav.isTaskComplete(): feedback = nav.getFeedback() if feedback.navigation_duration > 600: - nav.cancelNav() + nav.cancelTask() ... result = nav.getResult() -if result == NavigationResult.SUCCEEDED: +if result == TaskResult.SUCCEEDED: print('Goal succeeded!') -elif result == NavigationResult.CANCELED: +elif result == TaskResult.CANCELED: print('Goal was canceled!') -elif result == NavigationResult.FAILED: +elif result == TaskResult.FAILED: print('Goal failed!') ``` @@ -102,8 +110,8 @@ The `nav2_simple_commander` has a few examples to highlight the API functions av - `example_nav_to_pose.py` - Demonstrates the navigate to pose capabilities of the navigator, as well as a number of auxiliary methods. - `example_nav_through_poses.py` - Demonstrates the navigate through poses capabilities of the navigator, as well as a number of auxiliary methods. -- `example_waypoint_follower.py` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods required. - +- `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: 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 00000000000..41624dff6fd --- /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/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py new file mode 100644 index 00000000000..0d505000349 --- /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/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index 111e54dc9d3..429e11b68f8 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -16,7 +16,7 @@ from copy import deepcopy from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy """ @@ -72,7 +72,7 @@ def main(): # Do something during our route (e.x. AI to analyze stock information or upload to the cloud) # Simply the current waypoint ID for the demonstation i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: @@ -80,18 +80,17 @@ def main(): str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points))) result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Inspection of shelves complete! Returning to start...') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Inspection of shelving was canceled. Returning to start...') - exit(1) - elif result == NavigationResult.FAILED: + 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.isNavComplete(): + while not navigator.isTaskComplete(): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index 8cfcb2aec34..6a7a177acb1 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -82,7 +82,7 @@ def main(): # (e.x. queue up future tasks or detect person for fine-tuned positioning) # Simply print information for workers on the robot's ETA for the demonstation i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: @@ -92,7 +92,7 @@ def main(): + ' seconds.') result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Got product from ' + request_item_location + '! Bringing product to shipping destination (' + request_destination + ')...') shipping_destination = PoseStamped() @@ -104,16 +104,16 @@ def main(): shipping_destination.pose.orientation.w = 0.0 navigator.goToPose(shipping_destination) - elif result == NavigationResult.CANCELED: + 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 == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print(f'Task at {request_item_location} failed!') exit(-1) - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py new file mode 100644 index 00000000000..0313f956fff --- /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 index 8180bc36b4d..f727cc8d722 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -16,7 +16,7 @@ from copy import deepcopy from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -75,7 +75,7 @@ def main(): # Do something during our route (e.x. AI detection on camera images for anomalies) # Simply print ETA for the demonstation i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): i += 1 feedback = navigator.getFeedback() if feedback and i % 5 == 0: @@ -86,18 +86,18 @@ def main(): # Some failure mode, must stop since the robot is clearly stuck if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0): print('Navigation has exceeded timeout of 180s, canceling request.') - navigator.cancelNav() + navigator.cancelTask() # If at end of route, reverse the route to restart security_route.reverse() result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Route complete! Restarting...') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Security route was canceled, exiting.') exit(1) - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Security route failed! Restarting from other side...') exit(0) 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 00000000000..963dacf9428 --- /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 index f4523b8974f..58c47c75e1d 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -89,7 +89,7 @@ def main(): navigator.goThroughPoses(goal_poses) i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! @@ -106,7 +106,7 @@ def main(): # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): - navigator.cancelNav() + navigator.cancelTask() # Some navigation request change to demo preemption if Duration.from_msg(feedback.navigation_time) > Duration(seconds=35.0): @@ -121,11 +121,11 @@ def main(): # Do something depending on the return code result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Goal succeeded!') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Goal was canceled!') - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py index cd302e418c9..58dee813c9b 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -68,7 +68,7 @@ def main(): navigator.goToPose(goal_pose) i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! @@ -85,7 +85,7 @@ def main(): # Some navigation timeout to demo cancellation if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): - navigator.cancelNav() + navigator.cancelTask() # Some navigation request change to demo preemption if Duration.from_msg(feedback.navigation_time) > Duration(seconds=18.0): @@ -94,11 +94,11 @@ def main(): # Do something depending on the return code result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Goal succeeded!') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Goal was canceled!') - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') diff --git a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py index 8ed0fc3d16b..9d02341a669 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -14,7 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration @@ -90,7 +90,7 @@ def main(): navigator.followWaypoints(goal_poses) i = 0 - while not navigator.isNavComplete(): + while not navigator.isTaskComplete(): ################################################ # # Implement some code here for your application! @@ -107,7 +107,7 @@ def main(): # Some navigation timeout to demo cancellation if now - nav_start > Duration(seconds=600.0): - navigator.cancelNav() + navigator.cancelTask() # Some follow waypoints request change to demo preemption if now - nav_start > Duration(seconds=35.0): @@ -124,11 +124,11 @@ def main(): # Do something depending on the return code result = navigator.getResult() - if result == NavigationResult.SUCCEEDED: + if result == TaskResult.SUCCEEDED: print('Goal succeeded!') - elif result == NavigationResult.CANCELED: + elif result == TaskResult.CANCELED: print('Goal was canceled!') - elif result == NavigationResult.FAILED: + elif result == TaskResult.FAILED: print('Goal failed!') else: print('Goal has an invalid return status!') diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 80c21cfb580..1026f8f75df 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -18,22 +18,27 @@ 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 FollowWaypoints, NavigateThroughPoses, NavigateToPose +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 NavigationResult(Enum): - UKNOWN = 0 +class TaskResult(Enum): + UNKNOWN = 0 SUCCEEDED = 1 CANCELED = 2 FAILED = 3 @@ -62,10 +67,14 @@ def __init__(self): '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, @@ -81,13 +90,28 @@ def __init__(self): 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): + 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): @@ -95,6 +119,7 @@ def goThroughPoses(self, poses): 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, @@ -109,7 +134,7 @@ def goThroughPoses(self, poses): self.result_future = self.goal_handle.get_result_async() return True - def goToPose(self, pose): + 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): @@ -117,6 +142,7 @@ def goToPose(self, pose): 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) + '...') @@ -155,16 +181,81 @@ def followWaypoints(self, poses): self.result_future = self.goal_handle.get_result_async() return True - def cancelNav(self): - """Cancel pending navigation request of any type.""" - self.info('Canceling current goal.') + 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 isNavComplete(self): - """Check if the navigation request of any type is complete yet.""" + 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 @@ -172,13 +263,13 @@ def isNavComplete(self): if self.result_future.result(): self.status = self.result_future.result().status if self.status != GoalStatus.STATUS_SUCCEEDED: - self.debug(f'Goal with failed with status code: {self.status}') + 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('Goal succeeded!') + self.debug('Task succeeded!') return True def getFeedback(self): @@ -188,31 +279,34 @@ def getFeedback(self): def getResult(self): """Get the pending action result message.""" if self.status == GoalStatus.STATUS_SUCCEEDED: - return NavigationResult.SUCCEEDED + return TaskResult.SUCCEEDED elif self.status == GoalStatus.STATUS_ABORTED: - return NavigationResult.FAILED + return TaskResult.FAILED elif self.status == GoalStatus.STATUS_CANCELED: - return NavigationResult.CANCELED + return TaskResult.CANCELED else: - return NavigationResult.UNKNOWN + return TaskResult.UNKNOWN - def waitUntilNav2Active(self): + def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): """Block until the full navigation system is up and running.""" - self._waitForNodeToActivate('amcl') - self._waitForInitialPose() - self._waitForNodeToActivate('bt_navigator') + self._waitForNodeToActivate(localizer) + if localizer == 'amcl': + self._waitForInitialPose() + self._waitForNodeToActivate(navigator) self.info('Nav2 is ready for use!') return - def getPath(self, start, goal): + 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.goal = 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) @@ -232,15 +326,17 @@ def getPath(self, start, goal): return self.result_future.result().result.path - def getPathThroughPoses(self, start, goals): + 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.goals = goals 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) @@ -260,6 +356,36 @@ def getPathThroughPoses(self, start, goals): 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): diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index a30175bf7d9..c4e7c941586 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,12 +2,11 @@ nav2_simple_commander - 1.0.0 + 1.1.0 An importable library for writing mobile robot applications in python3 steve Apache-2.0 - python-enum34 rclpy geometry_msgs nav2_msgs diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index ca44856c5ad..ab19a26e473 100644 --- a/nav2_simple_commander/setup.py +++ b/nav2_simple_commander/setup.py @@ -28,9 +28,11 @@ '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_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index 5e855b3b3b4..c21e856da8e 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -148,6 +148,8 @@ 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() diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index 7057cac5014..e6520f4bd26 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -100,6 +100,16 @@ class GridCollisionChecker 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_; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 10bb826887d..b40624efd6a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -15,9 +15,8 @@ #ifndef NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_ #define NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_ -#include - #include + #include #include #include @@ -28,6 +27,7 @@ #include #include +#include "nlohmann/json.hpp" #include "ompl/base/StateSpace.h" #include "angles/angles.h" diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 4395db12e20..51f6608b5ff 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -114,6 +114,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner 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; diff --git a/nav2_smac_planner/lattice_primitives/helper.py b/nav2_smac_planner/lattice_primitives/helper.py index 82da9125872..24d3cf91402 100644 --- a/nav2_smac_planner/lattice_primitives/helper.py +++ b/nav2_smac_planner/lattice_primitives/helper.py @@ -17,7 +17,7 @@ def normalize_angle(angle): """ - Normalize the angle to between [-pi, pi). + Normalize the angle to between [0, 2pi). Args: angle: float @@ -25,10 +25,16 @@ def normalize_angle(angle): Returns ------- - The normalized angle in the range [-pi,pi) + The normalized angle in the range [0,2pi) """ - return (angle + np.pi) % (2 * np.pi) - np.pi + 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): 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 similarity index 88% rename from nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m _turning_radius/ackermann/output.json rename to nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json index 874dd2f7cef..0df406dfdc1 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m _turning_radius/ackermann/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json @@ -1,6 +1,6 @@ { "version": 1.0, - "date_generated": "2022-01-19", + "date_generated": "2022-03-17", "lattice_metadata": { "motion_model": "ackermann", "turning_radius": 0.5, @@ -41,67 +41,67 @@ [ 0.04981, -0.00236, - -0.0948654249597968 + 6.18831988221979 ], [ 0.09917, -0.00944, - -0.1897308499195936 + 6.093454457259993 ], [ 0.14765, -0.02115, - -0.2845962748793904 + 5.998589032300196 ], [ 0.19479, -0.03741, - -0.3794616998391872 + 5.903723607340399 ], [ 0.24018, -0.05805, - -0.4743271247989839 + 5.808858182380602 ], [ 0.28341, -0.08291, - -0.5691925497587808 + 5.713992757420805 ], [ 0.3241, -0.11175, - -0.6640579747185776 + 5.619127332461009 ], [ 0.36187, -0.14431, - -0.7589233996783744 + 5.524261907501212 ], [ 0.39638, -0.1803, - -0.8537888246381711 + 5.429396482541415 ], [ 0.42733, -0.2194, - -0.9486542495979677 + 5.334531057581619 ], [ 0.45444, -0.26126, - -1.0435196745577644 + 5.239665632621822 ], [ 0.47769, -0.30538, - -1.1071487177940904 + 5.176036589385496 ], [ 0.5, -0.35, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -118,37 +118,37 @@ [ 0.05254, -0.00218, - -0.0827841472401804 + 6.200401159939406 ], [ 0.10472, -0.00869, - -0.1655682944803608 + 6.117617012699226 ], [ 0.15619, -0.0195, - -0.2483524417205412 + 6.034832865459045 ], [ 0.20658, -0.03452, - -0.3311365889607216 + 5.952048718218864 ], [ 0.25556, -0.05366, - -0.413920736200902 + 5.869264570978684 ], [ 0.30295, -0.07648, - -0.4636476090008061 + 5.81953769817878 ], [ 0.35, -0.1, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -336,27 +336,27 @@ [ 0.25091, 0.05245, - -0.05151640100008953 + 6.231668906179497 ], [ 0.30245, 0.04712, - -0.1545492030002687 + 6.128636104179318 ], [ 0.35317, 0.03652, - -0.25758200500044787 + 6.025603302179138 ], [ 0.40253, 0.02076, - -0.36061480700062715 + 5.922570500178959 ], [ 0.45, 0.0, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -398,32 +398,32 @@ [ 0.30091, 0.06437, - -0.04214978263643693 + 6.2410355245431495 ], [ 0.35255, 0.06001, - -0.1264493479093109 + 6.156735959270275 ], [ 0.40364, 0.05131, - -0.21074891318218475 + 6.0724363939974015 ], [ 0.45381, 0.03835, - -0.2950484784550586 + 5.988136828724528 ], [ 0.50271, 0.0212, - -0.37934804372793235 + 5.903837263451654 ], [ 0.55, 0.0, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -1791,7 +1791,7 @@ [ -0.45, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -1858,7 +1858,7 @@ [ -0.55, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2016,12 +2016,12 @@ [ -0.30295, -0.07648, - -2.677945044588987 + 3.6052402625905993 ], [ -0.35, -0.1, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2093,12 +2093,12 @@ [ -0.47769, -0.30538, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.5, -0.35, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -2266,22 +2266,22 @@ [ -0.05, -0.025, - -2.677945044588987 + 3.6052402625905993 ], [ -0.1, -0.05, - -2.677945044588987 + 3.6052402625905993 ], [ -0.15, -0.075, - -2.677945044588987 + 3.6052402625905993 ], [ -0.2, -0.1, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2298,27 +2298,27 @@ [ -0.04409, -0.0241, - -2.604995947937887 + 3.678189359241699 ], [ -0.08631, -0.05134, - -2.532046851286787 + 3.751138455892799 ], [ -0.12643, -0.08159, - -2.4590977546356876 + 3.8240875525438986 ], [ -0.16424, -0.11469, - -2.3861486579845876 + 3.8970366491949986 ], [ -0.2, -0.15, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -2335,42 +2335,42 @@ [ -0.04747, -0.02076, - -2.780977846589166 + 3.5022074605904203 ], [ -0.09683, -0.03652, - -2.8840106485893453 + 3.399174658590241 ], [ -0.14755, -0.04712, - -2.9870434505895243 + 3.296141856590062 ], [ -0.19909, -0.05245, - -3.0900762525897036 + 3.1931090545898826 ], [ -0.25091, -0.05245, - -3.1931090545898826 + 3.0900762525897036 ], [ -0.30245, -0.04712, - -3.296141856590062 + 2.9870434505895243 ], [ -0.35317, -0.03652, - -3.399174658590241 + 2.8840106485893453 ], [ -0.40253, -0.02076, - -3.5022074605904203 + 2.780977846589166 ], [ -0.45, @@ -2392,52 +2392,52 @@ [ -0.04729, -0.0212, - -2.762244609861861 + 3.5209406973177253 ], [ -0.09619, -0.03835, - -2.8465441751347345 + 3.4366411320448518 ], [ -0.14636, -0.05131, - -2.9308437404076084 + 3.352341566771978 ], [ -0.19745, -0.06001, - -3.0151433056804824 + 3.268042001499104 ], [ -0.24909, -0.06437, - -3.0994428709533564 + 3.18374243622623 ], [ -0.30091, -0.06437, - -3.18374243622623 + 3.0994428709533564 ], [ -0.35255, -0.06001, - -3.268042001499104 + 3.0151433056804824 ], [ -0.40364, -0.05131, - -3.352341566771978 + 2.9308437404076084 ], [ -0.45381, -0.03835, - -3.4366411320448518 + 2.8465441751347345 ], [ -0.50271, -0.0212, - -3.5209406973177253 + 2.762244609861861 ], [ -0.55, @@ -2459,32 +2459,32 @@ [ -0.04705, -0.02352, - -2.677945044588987 + 3.6052402625905993 ], [ -0.09444, -0.04634, - -2.727671917388891 + 3.555513389790695 ], [ -0.14342, -0.06548, - -2.8104560646290713 + 3.472729242550515 ], [ -0.19381, -0.0805, - -2.893240211869252 + 3.3899450953103343 ], [ -0.24528, -0.09131, - -2.976024359109432 + 3.307160948070154 ], [ -0.29746, -0.09782, - -3.058808506349613 + 3.2243768008299734 ], [ -0.35, @@ -2506,27 +2506,27 @@ [ -0.03576, -0.03531, - -2.3861486579845876 + 3.8970366491949986 ], [ -0.07357, -0.06841, - -2.4590977546356876 + 3.8240875525438986 ], [ -0.11369, -0.09866, - -2.532046851286787 + 3.751138455892799 ], [ -0.15591, -0.1259, - -2.604995947937887 + 3.678189359241699 ], [ -0.2, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2543,22 +2543,22 @@ [ -0.0375, -0.0375, - -2.356194490192345 + 3.9269908169872414 ], [ -0.075, -0.075, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1125, -0.1125, - -2.356194490192345 + 3.9269908169872414 ], [ -0.15, -0.15, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -2575,27 +2575,27 @@ [ -0.03531, -0.03576, - -2.326240322400102 + 3.956944984779484 ], [ -0.06841, -0.07357, - -2.253291225749002 + 4.029894081430584 ], [ -0.09866, -0.11369, - -2.1803421290979026 + 4.102843178081684 ], [ -0.1259, -0.15591, - -2.1073930324468026 + 4.175792274732784 ], [ -0.15, -0.2, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -2612,47 +2612,47 @@ [ -0.03288, -0.03617, - -2.260711797772509 + 4.022473509407077 ], [ -0.06215, -0.07532, - -2.165229105352673 + 4.117956201826914 ], [ -0.08756, -0.11707, - -2.069746412932837 + 4.2134388942467496 ], [ -0.10888, -0.16106, - -1.9742637205130011 + 4.308921586666585 ], [ -0.1259, -0.20688, - -1.8787810280931652 + 4.404404279086421 ], [ -0.13848, -0.25412, - -1.7832983356733292 + 4.499886971506257 ], [ -0.1465, -0.30234, - -1.6878156432534932 + 4.595369663926093 ], [ -0.14988, -0.3511, - -1.5923329508336572 + 4.690852356345929 ], [ -0.15, -0.4, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -2669,42 +2669,42 @@ [ -0.03617, -0.03288, - -2.4516771826121806 + 3.8315081245674056 ], [ -0.07532, -0.06215, - -2.547159875032017 + 3.7360254321475694 ], [ -0.11707, -0.08756, - -2.6426425674518526 + 3.6405427397277337 ], [ -0.16106, -0.10888, - -2.7381252598716888 + 3.5450600473078975 ], [ -0.20688, -0.1259, - -2.8336079522915245 + 3.4495773548880617 ], [ -0.25412, -0.13848, - -2.9290906447113603 + 3.354094662468226 ], [ -0.30234, -0.1465, - -3.0245733371311965 + 3.2586119700483898 ], [ -0.3511, -0.14988, - -3.1200560295510327 + 3.1631292776285536 ], [ -0.4, @@ -2726,27 +2726,27 @@ [ -0.0241, -0.04409, - -2.1073930324468026 + 4.175792274732784 ], [ -0.05134, -0.08631, - -2.1803421290979026 + 4.102843178081684 ], [ -0.08159, -0.12643, - -2.253291225749002 + 4.029894081430584 ], [ -0.11469, -0.16424, - -2.326240322400102 + 3.956944984779484 ], [ -0.15, -0.2, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -2763,22 +2763,22 @@ [ -0.025, -0.05, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.05, -0.1, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.075, -0.15, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.1, -0.2, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -2795,37 +2795,37 @@ [ -0.02352, -0.04705, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.04634, -0.09444, - -1.9847170629957986 + 4.298468244183788 ], [ -0.06548, -0.14342, - -1.9019329157556182 + 4.381252391423968 ], [ -0.0805, -0.19381, - -1.8191487685154377 + 4.464036538664148 ], [ -0.09131, -0.24528, - -1.7363646212752575 + 4.546820685904329 ], [ -0.09782, -0.29746, - -1.653580474035077 + 4.629604833144509 ], [ -0.1, -0.35, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -2842,47 +2842,47 @@ [ -0.02076, -0.04747, - -1.9314111337955235 + 4.3517741733840625 ], [ -0.03652, -0.09683, - -1.8283783317953444 + 4.454806975384242 ], [ -0.04712, -0.14755, - -1.7253455297951654 + 4.557839777384421 ], [ -0.05245, -0.19909, - -1.622312727794986 + 4.6608725793846 ], [ -0.05245, -0.25091, - -1.519279925794807 + 4.763905381384779 ], [ -0.04712, -0.30245, - -1.4162471237946277 + 4.866938183384958 ], [ -0.03652, -0.35317, - -1.3132143217944487 + 4.969970985385137 ], [ -0.02076, -0.40253, - -1.2101815197942694 + 5.073003787385317 ], [ 0.0, -0.45, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -2899,57 +2899,57 @@ [ -0.0212, -0.04729, - -1.950144370522829 + 4.333040936656757 ], [ -0.03835, -0.09619, - -1.865844805249955 + 4.4173405019296315 ], [ -0.05131, -0.14636, - -1.7815452399770813 + 4.501640067202505 ], [ -0.06001, -0.19745, - -1.6972456747042073 + 4.5859396324753785 ], [ -0.06437, -0.24909, - -1.6129461094313335 + 4.670239197748253 ], [ -0.06437, -0.30091, - -1.5286465441584598 + 4.754538763021126 ], [ -0.06001, -0.35255, - -1.4443469788855858 + 4.838838328294001 ], [ -0.05131, -0.40364, - -1.3600474136127119 + 4.923137893566874 ], [ -0.03835, -0.45381, - -1.275747848339838 + 5.007437458839748 ], [ -0.0212, -0.50271, - -1.1914482830669644 + 5.091737024112621 ], [ 0.0, -0.55, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -2966,67 +2966,67 @@ [ -0.00236, -0.04981, - -1.6656617517546934 + 4.617523555424893 ], [ -0.00944, -0.09917, - -1.76052717671449 + 4.522658130465096 ], [ -0.02115, -0.14765, - -1.855392601674287 + 4.427792705505299 ], [ -0.03741, -0.19479, - -1.9502580266340837 + 4.332927280545503 ], [ -0.05805, -0.24018, - -2.0451234515938803 + 4.2380618555857055 ], [ -0.08291, -0.28341, - -2.1399888765536774 + 4.143196430625909 ], [ -0.11175, -0.3241, - -2.234854301513474 + 4.048331005666112 ], [ -0.14431, -0.36187, - -2.3297197264732707 + 3.9534655807063155 ], [ -0.1803, -0.39638, - -2.4245851514330674 + 3.858600155746519 ], [ -0.2194, -0.42733, - -2.519450576392864 + 3.763734730786722 ], [ -0.26126, -0.45444, - -2.6143160013526607 + 3.6688693058269255 ], [ -0.30538, -0.47769, - -2.677945044588987 + 3.6052402625905993 ], [ -0.35, -0.5, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3043,37 +3043,37 @@ [ -0.00218, -0.05254, - -1.653580474035077 + 4.629604833144509 ], [ -0.00869, -0.10472, - -1.7363646212752575 + 4.546820685904329 ], [ -0.0195, -0.15619, - -1.8191487685154377 + 4.464036538664148 ], [ -0.03452, -0.20658, - -1.9019329157556182 + 4.381252391423968 ], [ -0.05366, -0.25556, - -1.9847170629957986 + 4.298468244183788 ], [ -0.07648, -0.30295, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.1, -0.35, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3090,17 +3090,17 @@ [ 0.0, -0.05, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.1, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.15, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3117,37 +3117,37 @@ [ 0.00218, -0.05254, - -1.488012179554716 + 4.79517312762487 ], [ 0.00869, -0.10472, - -1.4052280323145356 + 4.87795727486505 ], [ 0.0195, -0.15619, - -1.3224438850743554 + 4.960741422105231 ], [ 0.03452, -0.20658, - -1.239659737834175 + 5.0435255693454115 ], [ 0.05366, -0.25556, - -1.1568755905939945 + 5.126309716585592 ], [ 0.07648, -0.30295, - -1.1071487177940904 + 5.176036589385496 ], [ 0.1, -0.35, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3164,67 +3164,67 @@ [ 0.00236, -0.04981, - -1.4759309018350997 + 4.807254405344486 ], [ 0.00944, -0.09917, - -1.381065476875303 + 4.902119830304283 ], [ 0.02115, -0.14765, - -1.2862000519155061 + 4.9969852552640805 ], [ 0.03741, -0.19479, - -1.1913346269557095 + 5.091850680223876 ], [ 0.05805, -0.24018, - -1.0964692019959126 + 5.186716105183674 ], [ 0.08291, -0.28341, - -1.0016037770361157 + 5.2815815301434705 ], [ 0.11175, -0.3241, - -0.9067383520763189 + 5.376446955103267 ], [ 0.14431, -0.36187, - -0.8118729271165221 + 5.471312380063064 ], [ 0.1803, -0.39638, - -0.7170075021567255 + 5.5661778050228605 ], [ 0.2194, -0.42733, - -0.6221420771969288 + 5.661043229982657 ], [ 0.26126, -0.45444, - -0.5272766522371322 + 5.755908654942454 ], [ 0.30538, -0.47769, - -0.4636476090008061 + 5.81953769817878 ], [ 0.35, -0.5, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -3241,47 +3241,47 @@ [ 0.02076, -0.04747, - -1.2101815197942696 + 5.073003787385317 ], [ 0.03652, -0.09683, - -1.3132143217944487 + 4.969970985385137 ], [ 0.04712, -0.14755, - -1.4162471237946277 + 4.866938183384958 ], [ 0.05245, -0.19909, - -1.519279925794807 + 4.763905381384779 ], [ 0.05245, -0.25091, - -1.622312727794986 + 4.6608725793846 ], [ 0.04712, -0.30245, - -1.7253455297951654 + 4.557839777384421 ], [ 0.03652, -0.35317, - -1.8283783317953444 + 4.454806975384242 ], [ 0.02076, -0.40253, - -1.9314111337955238 + 4.3517741733840625 ], [ 0.0, -0.45, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3298,57 +3298,57 @@ [ 0.0212, -0.04729, - -1.1914482830669642 + 5.091737024112622 ], [ 0.03835, -0.09619, - -1.2757478483398381 + 5.007437458839748 ], [ 0.05131, -0.14636, - -1.3600474136127119 + 4.923137893566874 ], [ 0.06001, -0.19745, - -1.4443469788855858 + 4.838838328294001 ], [ 0.06437, -0.24909, - -1.5286465441584596 + 4.754538763021126 ], [ 0.06437, -0.30091, - -1.6129461094313333 + 4.670239197748253 ], [ 0.06001, -0.35255, - -1.6972456747042073 + 4.5859396324753785 ], [ 0.05131, -0.40364, - -1.7815452399770813 + 4.501640067202505 ], [ 0.03835, -0.45381, - -1.8658448052499552 + 4.4173405019296315 ], [ 0.0212, -0.50271, - -1.9501443705228287 + 4.333040936656758 ], [ 0.0, -0.55, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3365,37 +3365,37 @@ [ 0.02352, -0.04705, - -1.1071487177940904 + 5.176036589385496 ], [ 0.04634, -0.09444, - -1.1568755905939945 + 5.126309716585592 ], [ 0.06548, -0.14342, - -1.239659737834175 + 5.0435255693454115 ], [ 0.0805, -0.19381, - -1.3224438850743554 + 4.960741422105231 ], [ 0.09131, -0.24528, - -1.4052280323145356 + 4.87795727486505 ], [ 0.09782, -0.29746, - -1.488012179554716 + 4.79517312762487 ], [ 0.1, -0.35, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3412,22 +3412,22 @@ [ 0.025, -0.05, - -1.1071487177940904 + 5.176036589385496 ], [ 0.05, -0.1, - -1.1071487177940904 + 5.176036589385496 ], [ 0.075, -0.15, - -1.1071487177940904 + 5.176036589385496 ], [ 0.1, -0.2, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3444,27 +3444,27 @@ [ 0.0241, -0.04409, - -1.0341996211429905 + 5.248985686036596 ], [ 0.05134, -0.08631, - -0.9612505244918907 + 5.321934782687696 ], [ 0.08159, -0.12643, - -0.8883014278407909 + 5.394883879338796 ], [ 0.11469, -0.16424, - -0.8153523311896911 + 5.467832975989895 ], [ 0.15, -0.2, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -3481,47 +3481,47 @@ [ 0.03288, -0.03617, - -0.8808808558172843 + 5.402304451362302 ], [ 0.06215, -0.07532, - -0.9763635482371201 + 5.306821758942466 ], [ 0.08756, -0.11707, - -1.071846240656956 + 5.21133906652263 ], [ 0.10888, -0.16106, - -1.167328933076792 + 5.115856374102794 ], [ 0.1259, -0.20688, - -1.262811625496628 + 5.020373681682958 ], [ 0.13848, -0.25412, - -1.358294317916464 + 4.9248909892631225 ], [ 0.1465, -0.30234, - -1.4537770103363 + 4.829408296843287 ], [ 0.14988, -0.3511, - -1.549259702756136 + 4.73392560442345 ], [ 0.15, -0.4, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3538,27 +3538,27 @@ [ 0.03531, -0.03576, - -0.8153523311896911 + 5.467832975989895 ], [ 0.06841, -0.07357, - -0.8883014278407909 + 5.394883879338796 ], [ 0.09866, -0.11369, - -0.9612505244918907 + 5.321934782687696 ], [ 0.1259, -0.15591, - -1.0341996211429907 + 5.248985686036596 ], [ 0.15, -0.2, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3575,22 +3575,22 @@ [ 0.0375, -0.0375, - -0.7853981633974483 + 5.497787143782138 ], [ 0.075, -0.075, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1125, -0.1125, - -0.7853981633974483 + 5.497787143782138 ], [ 0.15, -0.15, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -3607,27 +3607,27 @@ [ 0.03576, -0.03531, - -0.7554439956052055 + 5.527741311574381 ], [ 0.07357, -0.06841, - -0.6824948989541056 + 5.60069040822548 ], [ 0.11369, -0.09866, - -0.6095458023030058 + 5.67363950487658 ], [ 0.15591, -0.1259, - -0.5365967056519059 + 5.74658860152768 ], [ 0.2, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -3644,42 +3644,42 @@ [ 0.03617, -0.03288, - -0.6899154709776123 + 5.593269836201974 ], [ 0.07532, -0.06215, - -0.5944327785577764 + 5.6887525286218095 ], [ 0.11707, -0.08756, - -0.4989500861379405 + 5.784235221041646 ], [ 0.16106, -0.10888, - -0.4034673937181046 + 5.879717913461482 ], [ 0.20688, -0.1259, - -0.30798470129826866 + 5.975200605881318 ], [ 0.25412, -0.13848, - -0.21250200887843262 + 6.070683298301153 ], [ 0.30234, -0.1465, - -0.11701931645859664 + 6.166165990720989 ], [ 0.3511, -0.14988, - -0.021536624038760666 + 6.261648683140826 ], [ 0.4, @@ -3701,27 +3701,27 @@ [ 0.04409, -0.0241, - -0.536596705651906 + 5.74658860152768 ], [ 0.08631, -0.05134, - -0.6095458023030058 + 5.67363950487658 ], [ 0.12643, -0.08159, - -0.6824948989541056 + 5.60069040822548 ], [ 0.16424, -0.11469, - -0.7554439956052055 + 5.527741311574381 ], [ 0.2, -0.15, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -3738,22 +3738,22 @@ [ 0.05, -0.025, - -0.4636476090008061 + 5.81953769817878 ], [ 0.1, -0.05, - -0.4636476090008061 + 5.81953769817878 ], [ 0.15, -0.075, - -0.4636476090008061 + 5.81953769817878 ], [ 0.2, -0.1, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -3770,32 +3770,32 @@ [ 0.04705, -0.02352, - -0.4636476090008061 + 5.81953769817878 ], [ 0.09444, -0.04634, - -0.413920736200902 + 5.869264570978684 ], [ 0.14342, -0.06548, - -0.3311365889607216 + 5.952048718218864 ], [ 0.19381, -0.0805, - -0.24835244172054122 + 6.034832865459045 ], [ 0.24528, -0.09131, - -0.16556829448036087 + 6.117617012699226 ], [ 0.29746, -0.09782, - -0.08278414724018052 + 6.200401159939406 ], [ 0.35, @@ -3817,22 +3817,22 @@ [ 0.04747, -0.02076, - -0.360614807000627 + 5.922570500178959 ], [ 0.09683, -0.03652, - -0.2575820050004478 + 6.025603302179139 ], [ 0.14755, -0.04712, - -0.15454920300026875 + 6.128636104179318 ], [ 0.19909, -0.05245, - -0.051516401000089584 + 6.231668906179497 ], [ 0.25091, @@ -3874,27 +3874,27 @@ [ 0.04729, -0.0212, - -0.37934804372793224 + 5.903837263451654 ], [ 0.09619, -0.03835, - -0.29504847845505844 + 5.988136828724528 ], [ 0.14636, -0.05131, - -0.21074891318218458 + 6.0724363939974015 ], [ 0.19745, -0.06001, - -0.12644934790931073 + 6.156735959270275 ], [ 0.24909, -0.06437, - -0.04214978263643687 + 6.2410355245431495 ], [ 0.30091, 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 similarity index 100% rename from nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m _turning_radius/all_trajectories.png rename to nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/all_trajectories.png 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 similarity index 88% rename from nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m _turning_radius/diff/output.json rename to nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json index 90b5d2cd660..984bbb62d9e 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m _turning_radius/diff/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json @@ -1,6 +1,6 @@ { "version": 1.0, - "date_generated": "2022-01-19", + "date_generated": "2022-03-17", "lattice_metadata": { "motion_model": "diff", "turning_radius": 0.5, @@ -41,67 +41,67 @@ [ 0.04981, -0.00236, - -0.0948654249597968 + 6.18831988221979 ], [ 0.09917, -0.00944, - -0.1897308499195936 + 6.093454457259993 ], [ 0.14765, -0.02115, - -0.2845962748793904 + 5.998589032300196 ], [ 0.19479, -0.03741, - -0.3794616998391872 + 5.903723607340399 ], [ 0.24018, -0.05805, - -0.4743271247989839 + 5.808858182380602 ], [ 0.28341, -0.08291, - -0.5691925497587808 + 5.713992757420805 ], [ 0.3241, -0.11175, - -0.6640579747185776 + 5.619127332461009 ], [ 0.36187, -0.14431, - -0.7589233996783744 + 5.524261907501212 ], [ 0.39638, -0.1803, - -0.8537888246381711 + 5.429396482541415 ], [ 0.42733, -0.2194, - -0.9486542495979677 + 5.334531057581619 ], [ 0.45444, -0.26126, - -1.0435196745577644 + 5.239665632621822 ], [ 0.47769, -0.30538, - -1.1071487177940904 + 5.176036589385496 ], [ 0.5, -0.35, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -118,37 +118,37 @@ [ 0.05254, -0.00218, - -0.0827841472401804 + 6.200401159939406 ], [ 0.10472, -0.00869, - -0.1655682944803608 + 6.117617012699226 ], [ 0.15619, -0.0195, - -0.2483524417205412 + 6.034832865459045 ], [ 0.20658, -0.03452, - -0.3311365889607216 + 5.952048718218864 ], [ 0.25556, -0.05366, - -0.413920736200902 + 5.869264570978684 ], [ 0.30295, -0.07648, - -0.4636476090008061 + 5.81953769817878 ], [ 0.35, -0.1, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -170,17 +170,17 @@ [ 0.0, 0.0, - -0.15454920300026886 + 6.128636104179318 ], [ 0.0, 0.0, - -0.3090984060005373 + 5.9740869011790485 ], [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ] ] }, @@ -276,17 +276,17 @@ [ 0.0, 0.0, - 0.15454920300026886 + 0.1545492030002687 ], [ 0.0, 0.0, - 0.3090984060005373 + 0.3090984060005374 ], [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ] ] }, @@ -400,27 +400,27 @@ [ 0.25091, 0.05245, - -0.05151640100008953 + 6.231668906179497 ], [ 0.30245, 0.04712, - -0.1545492030002687 + 6.128636104179318 ], [ 0.35317, 0.03652, - -0.25758200500044787 + 6.025603302179138 ], [ 0.40253, 0.02076, - -0.36061480700062715 + 5.922570500178959 ], [ 0.45, 0.0, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -462,32 +462,32 @@ [ 0.30091, 0.06437, - -0.04214978263643693 + 6.2410355245431495 ], [ 0.35255, 0.06001, - -0.1264493479093109 + 6.156735959270275 ], [ 0.40364, 0.05131, - -0.21074891318218475 + 6.0724363939974015 ], [ 0.45381, 0.03835, - -0.2950484784550586 + 5.988136828724528 ], [ 0.50271, 0.0212, - -0.37934804372793235 + 5.903837263451654 ], [ 0.55, 0.0, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -551,12 +551,12 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ], [ 0.0, 0.0, - 0.23182380450040307 + 0.23182380450040305 ], [ 0.0, @@ -647,7 +647,7 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ], [ 0.0, @@ -778,7 +778,7 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ] ] }, @@ -1101,12 +1101,12 @@ [ 0.0, 0.0, - 1.2616979207943588 + 1.261697920794359 ], [ 0.0, 0.0, - 1.4162471237946281 + 1.416247123794628 ], [ 0.0, @@ -1381,12 +1381,12 @@ [ 0.0, 0.0, - 1.4162471237946281 + 1.416247123794628 ], [ 0.0, 0.0, - 1.2616979207943588 + 1.261697920794359 ], [ 0.0, @@ -1487,12 +1487,12 @@ [ 0.0, 0.0, - 1.725345529795165 + 1.7253455297951652 ], [ 0.0, 0.0, - 1.8798947327954343 + 1.879894732795434 ], [ 0.0, @@ -1767,7 +1767,7 @@ [ 0.0, 0.0, - 1.8026201312952992 + 1.8026201312952996 ], [ 0.0, @@ -2199,7 +2199,7 @@ [ -0.45, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2266,7 +2266,7 @@ [ -0.55, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2436,17 +2436,17 @@ [ 0.0, 0.0, - 2.8324942475892563 + 2.832494247589256 ], [ 0.0, 0.0, - 2.9870434505895247 + 2.9870434505895243 ], [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ] ] }, @@ -2488,12 +2488,12 @@ [ -0.30295, -0.07648, - -2.677945044588987 + 3.6052402625905993 ], [ -0.35, -0.1, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2510,22 +2510,22 @@ [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ], [ 0.0, 0.0, - -2.9870434505895247 + 3.296141856590062 ], [ 0.0, 0.0, - -2.8324942475892563 + 3.4506910595903304 ], [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2597,12 +2597,12 @@ [ -0.47769, -0.30538, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.5, -0.35, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -2743,17 +2743,17 @@ [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ], [ 0.0, 0.0, - 2.9870434505895247 + 2.9870434505895243 ], [ 0.0, 0.0, - 2.8324942475892563 + 2.832494247589256 ], [ 0.0, @@ -2802,22 +2802,22 @@ [ -0.05, -0.025, - -2.677945044588987 + 3.6052402625905993 ], [ -0.1, -0.05, - -2.677945044588987 + 3.6052402625905993 ], [ -0.15, -0.075, - -2.677945044588987 + 3.6052402625905993 ], [ -0.2, -0.1, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2834,27 +2834,27 @@ [ -0.04409, -0.0241, - -2.604995947937887 + 3.678189359241699 ], [ -0.08631, -0.05134, - -2.532046851286787 + 3.751138455892799 ], [ -0.12643, -0.08159, - -2.4590977546356876 + 3.8240875525438986 ], [ -0.16424, -0.11469, - -2.3861486579845876 + 3.8970366491949986 ], [ -0.2, -0.15, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -2871,17 +2871,17 @@ [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ], [ 0.0, 0.0, - -2.517069767390666 + 3.7661155397889203 ], [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -2898,42 +2898,42 @@ [ -0.04747, -0.02076, - -2.780977846589166 + 3.5022074605904203 ], [ -0.09683, -0.03652, - -2.8840106485893453 + 3.399174658590241 ], [ -0.14755, -0.04712, - -2.9870434505895243 + 3.296141856590062 ], [ -0.19909, -0.05245, - -3.0900762525897036 + 3.1931090545898826 ], [ -0.25091, -0.05245, - -3.1931090545898826 + 3.0900762525897036 ], [ -0.30245, -0.04712, - -3.296141856590062 + 2.9870434505895243 ], [ -0.35317, -0.03652, - -3.399174658590241 + 2.8840106485893453 ], [ -0.40253, -0.02076, - -3.5022074605904203 + 2.780977846589166 ], [ -0.45, @@ -2955,52 +2955,52 @@ [ -0.04729, -0.0212, - -2.762244609861861 + 3.5209406973177253 ], [ -0.09619, -0.03835, - -2.8465441751347345 + 3.4366411320448518 ], [ -0.14636, -0.05131, - -2.9308437404076084 + 3.352341566771978 ], [ -0.19745, -0.06001, - -3.0151433056804824 + 3.268042001499104 ], [ -0.24909, -0.06437, - -3.0994428709533564 + 3.18374243622623 ], [ -0.30091, -0.06437, - -3.18374243622623 + 3.0994428709533564 ], [ -0.35255, -0.06001, - -3.268042001499104 + 3.0151433056804824 ], [ -0.40364, -0.05131, - -3.352341566771978 + 2.9308437404076084 ], [ -0.45381, -0.03835, - -3.4366411320448518 + 2.8465441751347345 ], [ -0.50271, -0.0212, - -3.5209406973177253 + 2.762244609861861 ], [ -0.55, @@ -3022,32 +3022,32 @@ [ -0.04705, -0.02352, - -2.677945044588987 + 3.6052402625905993 ], [ -0.09444, -0.04634, - -2.727671917388891 + 3.555513389790695 ], [ -0.14342, -0.06548, - -2.8104560646290713 + 3.472729242550515 ], [ -0.19381, -0.0805, - -2.893240211869252 + 3.3899450953103343 ], [ -0.24528, -0.09131, - -2.976024359109432 + 3.307160948070154 ], [ -0.29746, -0.09782, - -3.058808506349613 + 3.2243768008299734 ], [ -0.35, @@ -3069,17 +3069,17 @@ [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ], [ 0.0, 0.0, - -2.90976884908939 + 3.373416458090196 ], [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ] ] }, @@ -3096,27 +3096,27 @@ [ -0.03576, -0.03531, - -2.3861486579845876 + 3.8970366491949986 ], [ -0.07357, -0.06841, - -2.4590977546356876 + 3.8240875525438986 ], [ -0.11369, -0.09866, - -2.532046851286787 + 3.751138455892799 ], [ -0.15591, -0.1259, - -2.604995947937887 + 3.678189359241699 ], [ -0.2, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3133,17 +3133,17 @@ [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ], [ 0.0, 0.0, - -2.517069767390666 + 3.7661155397889203 ], [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3160,22 +3160,22 @@ [ -0.0375, -0.0375, - -2.356194490192345 + 3.9269908169872414 ], [ -0.075, -0.075, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1125, -0.1125, - -2.356194490192345 + 3.9269908169872414 ], [ -0.15, -0.15, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3192,27 +3192,27 @@ [ -0.03531, -0.03576, - -2.326240322400102 + 3.956944984779484 ], [ -0.06841, -0.07357, - -2.253291225749002 + 4.029894081430584 ], [ -0.09866, -0.11369, - -2.1803421290979026 + 4.102843178081684 ], [ -0.1259, -0.15591, - -2.1073930324468026 + 4.175792274732784 ], [ -0.15, -0.2, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3229,17 +3229,17 @@ [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ], [ 0.0, 0.0, - -2.1953192129940238 + 4.0878660941855625 ], [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3256,47 +3256,47 @@ [ -0.03288, -0.03617, - -2.260711797772509 + 4.022473509407077 ], [ -0.06215, -0.07532, - -2.165229105352673 + 4.117956201826914 ], [ -0.08756, -0.11707, - -2.069746412932837 + 4.2134388942467496 ], [ -0.10888, -0.16106, - -1.9742637205130011 + 4.308921586666585 ], [ -0.1259, -0.20688, - -1.8787810280931652 + 4.404404279086421 ], [ -0.13848, -0.25412, - -1.7832983356733292 + 4.499886971506257 ], [ -0.1465, -0.30234, - -1.6878156432534932 + 4.595369663926093 ], [ -0.14988, -0.3511, - -1.5923329508336572 + 4.690852356345929 ], [ -0.15, -0.4, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3313,42 +3313,42 @@ [ -0.03617, -0.03288, - -2.4516771826121806 + 3.8315081245674056 ], [ -0.07532, -0.06215, - -2.547159875032017 + 3.7360254321475694 ], [ -0.11707, -0.08756, - -2.6426425674518526 + 3.6405427397277337 ], [ -0.16106, -0.10888, - -2.7381252598716888 + 3.5450600473078975 ], [ -0.20688, -0.1259, - -2.8336079522915245 + 3.4495773548880617 ], [ -0.25412, -0.13848, - -2.9290906447113603 + 3.354094662468226 ], [ -0.30234, -0.1465, - -3.0245733371311965 + 3.2586119700483898 ], [ -0.3511, -0.14988, - -3.1200560295510327 + 3.1631292776285536 ], [ -0.4, @@ -3370,27 +3370,27 @@ [ -0.0241, -0.04409, - -2.1073930324468026 + 4.175792274732784 ], [ -0.05134, -0.08631, - -2.1803421290979026 + 4.102843178081684 ], [ -0.08159, -0.12643, - -2.253291225749002 + 4.029894081430584 ], [ -0.11469, -0.16424, - -2.326240322400102 + 3.956944984779484 ], [ -0.15, -0.2, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3407,22 +3407,22 @@ [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.0, 0.0, - -2.1416941205945834 + 4.141491186585003 ], [ 0.0, 0.0, - -2.248944305393464 + 4.034241001786122 ], [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3439,22 +3439,22 @@ [ -0.025, -0.05, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.05, -0.1, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.075, -0.15, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.1, -0.2, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3471,37 +3471,37 @@ [ -0.02352, -0.04705, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.04634, -0.09444, - -1.9847170629957986 + 4.298468244183788 ], [ -0.06548, -0.14342, - -1.9019329157556182 + 4.381252391423968 ], [ -0.0805, -0.19381, - -1.8191487685154377 + 4.464036538664148 ], [ -0.09131, -0.24528, - -1.7363646212752575 + 4.546820685904329 ], [ -0.09782, -0.29746, - -1.653580474035077 + 4.629604833144509 ], [ -0.1, -0.35, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3518,22 +3518,22 @@ [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.0, 0.0, - -1.879894732795434 + 4.403290574384152 ], [ 0.0, 0.0, - -1.7253455297951652 + 4.557839777384421 ], [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3550,47 +3550,47 @@ [ -0.02076, -0.04747, - -1.9314111337955235 + 4.3517741733840625 ], [ -0.03652, -0.09683, - -1.8283783317953444 + 4.454806975384242 ], [ -0.04712, -0.14755, - -1.7253455297951654 + 4.557839777384421 ], [ -0.05245, -0.19909, - -1.622312727794986 + 4.6608725793846 ], [ -0.05245, -0.25091, - -1.519279925794807 + 4.763905381384779 ], [ -0.04712, -0.30245, - -1.4162471237946277 + 4.866938183384958 ], [ -0.03652, -0.35317, - -1.3132143217944487 + 4.969970985385137 ], [ -0.02076, -0.40253, - -1.2101815197942694 + 5.073003787385317 ], [ 0.0, -0.45, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3607,57 +3607,57 @@ [ -0.0212, -0.04729, - -1.950144370522829 + 4.333040936656757 ], [ -0.03835, -0.09619, - -1.865844805249955 + 4.4173405019296315 ], [ -0.05131, -0.14636, - -1.7815452399770813 + 4.501640067202505 ], [ -0.06001, -0.19745, - -1.6972456747042073 + 4.5859396324753785 ], [ -0.06437, -0.24909, - -1.6129461094313335 + 4.670239197748253 ], [ -0.06437, -0.30091, - -1.5286465441584598 + 4.754538763021126 ], [ -0.06001, -0.35255, - -1.4443469788855858 + 4.838838328294001 ], [ -0.05131, -0.40364, - -1.3600474136127119 + 4.923137893566874 ], [ -0.03835, -0.45381, - -1.275747848339838 + 5.007437458839748 ], [ -0.0212, -0.50271, - -1.1914482830669644 + 5.091737024112621 ], [ 0.0, -0.55, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3674,67 +3674,67 @@ [ -0.00236, -0.04981, - -1.6656617517546934 + 4.617523555424893 ], [ -0.00944, -0.09917, - -1.76052717671449 + 4.522658130465096 ], [ -0.02115, -0.14765, - -1.855392601674287 + 4.427792705505299 ], [ -0.03741, -0.19479, - -1.9502580266340837 + 4.332927280545503 ], [ -0.05805, -0.24018, - -2.0451234515938803 + 4.2380618555857055 ], [ -0.08291, -0.28341, - -2.1399888765536774 + 4.143196430625909 ], [ -0.11175, -0.3241, - -2.234854301513474 + 4.048331005666112 ], [ -0.14431, -0.36187, - -2.3297197264732707 + 3.9534655807063155 ], [ -0.1803, -0.39638, - -2.4245851514330674 + 3.858600155746519 ], [ -0.2194, -0.42733, - -2.519450576392864 + 3.763734730786722 ], [ -0.26126, -0.45444, - -2.6143160013526607 + 3.6688693058269255 ], [ -0.30538, -0.47769, - -2.677945044588987 + 3.6052402625905993 ], [ -0.35, -0.5, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3751,37 +3751,37 @@ [ -0.00218, -0.05254, - -1.653580474035077 + 4.629604833144509 ], [ -0.00869, -0.10472, - -1.7363646212752575 + 4.546820685904329 ], [ -0.0195, -0.15619, - -1.8191487685154377 + 4.464036538664148 ], [ -0.03452, -0.20658, - -1.9019329157556182 + 4.381252391423968 ], [ -0.05366, -0.25556, - -1.9847170629957986 + 4.298468244183788 ], [ -0.07648, -0.30295, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.1, -0.35, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3798,22 +3798,22 @@ [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, 0.0, - -1.7253455297951652 + 4.557839777384421 ], [ 0.0, 0.0, - -1.879894732795434 + 4.403290574384152 ], [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3830,17 +3830,17 @@ [ 0.0, -0.05, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.1, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.15, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3857,37 +3857,37 @@ [ 0.00218, -0.05254, - -1.488012179554716 + 4.79517312762487 ], [ 0.00869, -0.10472, - -1.4052280323145356 + 4.87795727486505 ], [ 0.0195, -0.15619, - -1.3224438850743554 + 4.960741422105231 ], [ 0.03452, -0.20658, - -1.239659737834175 + 5.0435255693454115 ], [ 0.05366, -0.25556, - -1.1568755905939945 + 5.126309716585592 ], [ 0.07648, -0.30295, - -1.1071487177940904 + 5.176036589385496 ], [ 0.1, -0.35, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3904,22 +3904,22 @@ [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, 0.0, - -1.416247123794628 + 4.866938183384958 ], [ 0.0, 0.0, - -1.261697920794359 + 5.021487386385227 ], [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3936,67 +3936,67 @@ [ 0.00236, -0.04981, - -1.4759309018350997 + 4.807254405344486 ], [ 0.00944, -0.09917, - -1.381065476875303 + 4.902119830304283 ], [ 0.02115, -0.14765, - -1.2862000519155061 + 4.9969852552640805 ], [ 0.03741, -0.19479, - -1.1913346269557095 + 5.091850680223876 ], [ 0.05805, -0.24018, - -1.0964692019959126 + 5.186716105183674 ], [ 0.08291, -0.28341, - -1.0016037770361157 + 5.2815815301434705 ], [ 0.11175, -0.3241, - -0.9067383520763189 + 5.376446955103267 ], [ 0.14431, -0.36187, - -0.8118729271165221 + 5.471312380063064 ], [ 0.1803, -0.39638, - -0.7170075021567255 + 5.5661778050228605 ], [ 0.2194, -0.42733, - -0.6221420771969288 + 5.661043229982657 ], [ 0.26126, -0.45444, - -0.5272766522371322 + 5.755908654942454 ], [ 0.30538, -0.47769, - -0.4636476090008061 + 5.81953769817878 ], [ 0.35, -0.5, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -4013,47 +4013,47 @@ [ 0.02076, -0.04747, - -1.2101815197942696 + 5.073003787385317 ], [ 0.03652, -0.09683, - -1.3132143217944487 + 4.969970985385137 ], [ 0.04712, -0.14755, - -1.4162471237946277 + 4.866938183384958 ], [ 0.05245, -0.19909, - -1.519279925794807 + 4.763905381384779 ], [ 0.05245, -0.25091, - -1.622312727794986 + 4.6608725793846 ], [ 0.04712, -0.30245, - -1.7253455297951654 + 4.557839777384421 ], [ 0.03652, -0.35317, - -1.8283783317953444 + 4.454806975384242 ], [ 0.02076, -0.40253, - -1.9314111337955238 + 4.3517741733840625 ], [ 0.0, -0.45, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4070,57 +4070,57 @@ [ 0.0212, -0.04729, - -1.1914482830669642 + 5.091737024112622 ], [ 0.03835, -0.09619, - -1.2757478483398381 + 5.007437458839748 ], [ 0.05131, -0.14636, - -1.3600474136127119 + 4.923137893566874 ], [ 0.06001, -0.19745, - -1.4443469788855858 + 4.838838328294001 ], [ 0.06437, -0.24909, - -1.5286465441584596 + 4.754538763021126 ], [ 0.06437, -0.30091, - -1.6129461094313333 + 4.670239197748253 ], [ 0.06001, -0.35255, - -1.6972456747042073 + 4.5859396324753785 ], [ 0.05131, -0.40364, - -1.7815452399770813 + 4.501640067202505 ], [ 0.03835, -0.45381, - -1.8658448052499552 + 4.4173405019296315 ], [ 0.0212, -0.50271, - -1.9501443705228287 + 4.333040936656758 ], [ 0.0, -0.55, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4137,37 +4137,37 @@ [ 0.02352, -0.04705, - -1.1071487177940904 + 5.176036589385496 ], [ 0.04634, -0.09444, - -1.1568755905939945 + 5.126309716585592 ], [ 0.06548, -0.14342, - -1.239659737834175 + 5.0435255693454115 ], [ 0.0805, -0.19381, - -1.3224438850743554 + 4.960741422105231 ], [ 0.09131, -0.24528, - -1.4052280323145356 + 4.87795727486505 ], [ 0.09782, -0.29746, - -1.488012179554716 + 4.79517312762487 ], [ 0.1, -0.35, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4184,17 +4184,17 @@ [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ], [ 0.0, 0.0, - -1.3389725222944935 + 4.944212784885092 ], [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4211,22 +4211,22 @@ [ 0.025, -0.05, - -1.1071487177940904 + 5.176036589385496 ], [ 0.05, -0.1, - -1.1071487177940904 + 5.176036589385496 ], [ 0.075, -0.15, - -1.1071487177940904 + 5.176036589385496 ], [ 0.1, -0.2, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4243,27 +4243,27 @@ [ 0.0241, -0.04409, - -1.0341996211429905 + 5.248985686036596 ], [ 0.05134, -0.08631, - -0.9612505244918907 + 5.321934782687696 ], [ 0.08159, -0.12643, - -0.8883014278407909 + 5.394883879338796 ], [ 0.11469, -0.16424, - -0.8153523311896911 + 5.467832975989895 ], [ 0.15, -0.2, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -4280,17 +4280,17 @@ [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ], [ 0.0, 0.0, - -0.9462734405957693 + 5.336911866583817 ], [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -4307,47 +4307,47 @@ [ 0.03288, -0.03617, - -0.8808808558172843 + 5.402304451362302 ], [ 0.06215, -0.07532, - -0.9763635482371201 + 5.306821758942466 ], [ 0.08756, -0.11707, - -1.071846240656956 + 5.21133906652263 ], [ 0.10888, -0.16106, - -1.167328933076792 + 5.115856374102794 ], [ 0.1259, -0.20688, - -1.262811625496628 + 5.020373681682958 ], [ 0.13848, -0.25412, - -1.358294317916464 + 4.9248909892631225 ], [ 0.1465, -0.30234, - -1.4537770103363 + 4.829408296843287 ], [ 0.14988, -0.3511, - -1.549259702756136 + 4.73392560442345 ], [ 0.15, -0.4, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4364,27 +4364,27 @@ [ 0.03531, -0.03576, - -0.8153523311896911 + 5.467832975989895 ], [ 0.06841, -0.07357, - -0.8883014278407909 + 5.394883879338796 ], [ 0.09866, -0.11369, - -0.9612505244918907 + 5.321934782687696 ], [ 0.1259, -0.15591, - -1.0341996211429907 + 5.248985686036596 ], [ 0.15, -0.2, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4401,17 +4401,17 @@ [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ], [ 0.0, 0.0, - -0.9462734405957693 + 5.336911866583817 ], [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4428,22 +4428,22 @@ [ 0.0375, -0.0375, - -0.7853981633974483 + 5.497787143782138 ], [ 0.075, -0.075, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1125, -0.1125, - -0.7853981633974483 + 5.497787143782138 ], [ 0.15, -0.15, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -4460,27 +4460,27 @@ [ 0.03576, -0.03531, - -0.7554439956052055 + 5.527741311574381 ], [ 0.07357, -0.06841, - -0.6824948989541056 + 5.60069040822548 ], [ 0.11369, -0.09866, - -0.6095458023030058 + 5.67363950487658 ], [ 0.15591, -0.1259, - -0.5365967056519059 + 5.74658860152768 ], [ 0.2, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -4497,17 +4497,17 @@ [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ], [ 0.0, 0.0, - -0.6245228861991272 + 5.658662420980459 ], [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ] ] }, @@ -4524,42 +4524,42 @@ [ 0.03617, -0.03288, - -0.6899154709776123 + 5.593269836201974 ], [ 0.07532, -0.06215, - -0.5944327785577764 + 5.6887525286218095 ], [ 0.11707, -0.08756, - -0.4989500861379405 + 5.784235221041646 ], [ 0.16106, -0.10888, - -0.4034673937181046 + 5.879717913461482 ], [ 0.20688, -0.1259, - -0.30798470129826866 + 5.975200605881318 ], [ 0.25412, -0.13848, - -0.21250200887843262 + 6.070683298301153 ], [ 0.30234, -0.1465, - -0.11701931645859664 + 6.166165990720989 ], [ 0.3511, -0.14988, - -0.021536624038760666 + 6.261648683140826 ], [ 0.4, @@ -4581,27 +4581,27 @@ [ 0.04409, -0.0241, - -0.536596705651906 + 5.74658860152768 ], [ 0.08631, -0.05134, - -0.6095458023030058 + 5.67363950487658 ], [ 0.12643, -0.08159, - -0.6824948989541056 + 5.60069040822548 ], [ 0.16424, -0.11469, - -0.7554439956052055 + 5.527741311574381 ], [ 0.2, -0.15, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -4618,22 +4618,22 @@ [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ], [ 0.0, 0.0, - -0.5708977937996869 + 5.712287513379899 ], [ 0.0, 0.0, - -0.6781479785985676 + 5.605037328581019 ], [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -4650,22 +4650,22 @@ [ 0.05, -0.025, - -0.4636476090008061 + 5.81953769817878 ], [ 0.1, -0.05, - -0.4636476090008061 + 5.81953769817878 ], [ 0.15, -0.075, - -0.4636476090008061 + 5.81953769817878 ], [ 0.2, -0.1, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -4682,32 +4682,32 @@ [ 0.04705, -0.02352, - -0.4636476090008061 + 5.81953769817878 ], [ 0.09444, -0.04634, - -0.413920736200902 + 5.869264570978684 ], [ 0.14342, -0.06548, - -0.3311365889607216 + 5.952048718218864 ], [ 0.19381, -0.0805, - -0.24835244172054122 + 6.034832865459045 ], [ 0.24528, -0.09131, - -0.16556829448036087 + 6.117617012699226 ], [ 0.29746, -0.09782, - -0.08278414724018052 + 6.200401159939406 ], [ 0.35, @@ -4729,17 +4729,17 @@ [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ], [ 0.0, 0.0, - -0.3090984060005373 + 5.9740869011790485 ], [ 0.0, 0.0, - -0.15454920300026886 + 6.128636104179318 ], [ 0.0, @@ -4761,22 +4761,22 @@ [ 0.04747, -0.02076, - -0.360614807000627 + 5.922570500178959 ], [ 0.09683, -0.03652, - -0.2575820050004478 + 6.025603302179139 ], [ 0.14755, -0.04712, - -0.15454920300026875 + 6.128636104179318 ], [ 0.19909, -0.05245, - -0.051516401000089584 + 6.231668906179497 ], [ 0.25091, @@ -4818,27 +4818,27 @@ [ 0.04729, -0.0212, - -0.37934804372793224 + 5.903837263451654 ], [ 0.09619, -0.03835, - -0.29504847845505844 + 5.988136828724528 ], [ 0.14636, -0.05131, - -0.21074891318218458 + 6.0724363939974015 ], [ 0.19745, -0.06001, - -0.12644934790931073 + 6.156735959270275 ], [ 0.24909, -0.06437, - -0.04214978263643687 + 6.2410355245431495 ], [ 0.30091, 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 similarity index 88% rename from nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m _turning_radius/omni/output.json rename to nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json index 4212283ee8b..8ea504f6c38 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m _turning_radius/omni/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json @@ -1,6 +1,6 @@ { "version": 1.0, - "date_generated": "2022-01-19", + "date_generated": "2022-03-17", "lattice_metadata": { "motion_model": "omni", "turning_radius": 0.5, @@ -41,67 +41,67 @@ [ 0.04981, -0.00236, - -0.0948654249597968 + 6.18831988221979 ], [ 0.09917, -0.00944, - -0.1897308499195936 + 6.093454457259993 ], [ 0.14765, -0.02115, - -0.2845962748793904 + 5.998589032300196 ], [ 0.19479, -0.03741, - -0.3794616998391872 + 5.903723607340399 ], [ 0.24018, -0.05805, - -0.4743271247989839 + 5.808858182380602 ], [ 0.28341, -0.08291, - -0.5691925497587808 + 5.713992757420805 ], [ 0.3241, -0.11175, - -0.6640579747185776 + 5.619127332461009 ], [ 0.36187, -0.14431, - -0.7589233996783744 + 5.524261907501212 ], [ 0.39638, -0.1803, - -0.8537888246381711 + 5.429396482541415 ], [ 0.42733, -0.2194, - -0.9486542495979677 + 5.334531057581619 ], [ 0.45444, -0.26126, - -1.0435196745577644 + 5.239665632621822 ], [ 0.47769, -0.30538, - -1.1071487177940904 + 5.176036589385496 ], [ 0.5, -0.35, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -118,37 +118,37 @@ [ 0.05254, -0.00218, - -0.0827841472401804 + 6.200401159939406 ], [ 0.10472, -0.00869, - -0.1655682944803608 + 6.117617012699226 ], [ 0.15619, -0.0195, - -0.2483524417205412 + 6.034832865459045 ], [ 0.20658, -0.03452, - -0.3311365889607216 + 5.952048718218864 ], [ 0.25556, -0.05366, - -0.413920736200902 + 5.869264570978684 ], [ 0.30295, -0.07648, - -0.4636476090008061 + 5.81953769817878 ], [ 0.35, -0.1, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -170,17 +170,17 @@ [ 0.0, 0.0, - -0.15454920300026886 + 6.128636104179318 ], [ 0.0, 0.0, - -0.3090984060005373 + 5.9740869011790485 ], [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ] ] }, @@ -330,17 +330,17 @@ [ 0.0, 0.0, - 0.15454920300026886 + 0.1545492030002687 ], [ 0.0, 0.0, - 0.3090984060005373 + 0.3090984060005374 ], [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ] ] }, @@ -454,27 +454,27 @@ [ 0.25091, 0.05245, - -0.05151640100008953 + 6.231668906179497 ], [ 0.30245, 0.04712, - -0.1545492030002687 + 6.128636104179318 ], [ 0.35317, 0.03652, - -0.25758200500044787 + 6.025603302179138 ], [ 0.40253, 0.02076, - -0.36061480700062715 + 5.922570500178959 ], [ 0.45, 0.0, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -516,32 +516,32 @@ [ 0.30091, 0.06437, - -0.04214978263643693 + 6.2410355245431495 ], [ 0.35255, 0.06001, - -0.1264493479093109 + 6.156735959270275 ], [ 0.40364, 0.05131, - -0.21074891318218475 + 6.0724363939974015 ], [ 0.45381, 0.03835, - -0.2950484784550586 + 5.988136828724528 ], [ 0.50271, 0.0212, - -0.37934804372793235 + 5.903837263451654 ], [ 0.55, 0.0, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -605,12 +605,12 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ], [ 0.0, 0.0, - 0.23182380450040307 + 0.23182380450040305 ], [ 0.0, @@ -765,7 +765,7 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ], [ 0.0, @@ -896,7 +896,7 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ] ] }, @@ -1347,12 +1347,12 @@ [ 0.0, 0.0, - 1.2616979207943588 + 1.261697920794359 ], [ 0.0, 0.0, - 1.4162471237946281 + 1.416247123794628 ], [ 0.0, @@ -1627,12 +1627,12 @@ [ 0.0, 0.0, - 1.4162471237946281 + 1.416247123794628 ], [ 0.0, 0.0, - 1.2616979207943588 + 1.261697920794359 ], [ 0.0, @@ -1787,12 +1787,12 @@ [ 0.0, 0.0, - 1.725345529795165 + 1.7253455297951652 ], [ 0.0, 0.0, - 1.8798947327954343 + 1.879894732795434 ], [ 0.0, @@ -2067,7 +2067,7 @@ [ 0.0, 0.0, - 1.8026201312952992 + 1.8026201312952996 ], [ 0.0, @@ -2627,7 +2627,7 @@ [ -0.45, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2694,7 +2694,7 @@ [ -0.55, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2928,17 +2928,17 @@ [ 0.0, 0.0, - 2.8324942475892563 + 2.832494247589256 ], [ 0.0, 0.0, - 2.9870434505895247 + 2.9870434505895243 ], [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ] ] }, @@ -2980,12 +2980,12 @@ [ -0.30295, -0.07648, - -2.677945044588987 + 3.6052402625905993 ], [ -0.35, -0.1, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3002,22 +3002,22 @@ [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ], [ 0.0, 0.0, - -2.9870434505895247 + 3.296141856590062 ], [ 0.0, 0.0, - -2.8324942475892563 + 3.4506910595903304 ], [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3089,12 +3089,12 @@ [ -0.47769, -0.30538, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.5, -0.35, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3235,17 +3235,17 @@ [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ], [ 0.0, 0.0, - 2.9870434505895247 + 2.9870434505895243 ], [ 0.0, 0.0, - 2.8324942475892563 + 2.832494247589256 ], [ 0.0, @@ -3348,22 +3348,22 @@ [ -0.05, -0.025, - -2.677945044588987 + 3.6052402625905993 ], [ -0.1, -0.05, - -2.677945044588987 + 3.6052402625905993 ], [ -0.15, -0.075, - -2.677945044588987 + 3.6052402625905993 ], [ -0.2, -0.1, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3380,22 +3380,22 @@ [ 0.025, -0.05, - -2.677945044588987 + 3.6052402625905993 ], [ 0.05, -0.1, - -2.677945044588987 + 3.6052402625905993 ], [ 0.075, -0.15, - -2.677945044588987 + 3.6052402625905993 ], [ 0.1, -0.2, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3412,22 +3412,22 @@ [ -0.025, 0.05, - -2.677945044588987 + 3.6052402625905993 ], [ -0.05, 0.1, - -2.677945044588987 + 3.6052402625905993 ], [ -0.075, 0.15, - -2.677945044588987 + 3.6052402625905993 ], [ -0.1, 0.2, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3444,27 +3444,27 @@ [ -0.04409, -0.0241, - -2.604995947937887 + 3.678189359241699 ], [ -0.08631, -0.05134, - -2.532046851286787 + 3.751138455892799 ], [ -0.12643, -0.08159, - -2.4590977546356876 + 3.8240875525438986 ], [ -0.16424, -0.11469, - -2.3861486579845876 + 3.8970366491949986 ], [ -0.2, -0.15, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3481,17 +3481,17 @@ [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ], [ 0.0, 0.0, - -2.517069767390666 + 3.7661155397889203 ], [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3508,42 +3508,42 @@ [ -0.04747, -0.02076, - -2.780977846589166 + 3.5022074605904203 ], [ -0.09683, -0.03652, - -2.8840106485893453 + 3.399174658590241 ], [ -0.14755, -0.04712, - -2.9870434505895243 + 3.296141856590062 ], [ -0.19909, -0.05245, - -3.0900762525897036 + 3.1931090545898826 ], [ -0.25091, -0.05245, - -3.1931090545898826 + 3.0900762525897036 ], [ -0.30245, -0.04712, - -3.296141856590062 + 2.9870434505895243 ], [ -0.35317, -0.03652, - -3.399174658590241 + 2.8840106485893453 ], [ -0.40253, -0.02076, - -3.5022074605904203 + 2.780977846589166 ], [ -0.45, @@ -3565,52 +3565,52 @@ [ -0.04729, -0.0212, - -2.762244609861861 + 3.5209406973177253 ], [ -0.09619, -0.03835, - -2.8465441751347345 + 3.4366411320448518 ], [ -0.14636, -0.05131, - -2.9308437404076084 + 3.352341566771978 ], [ -0.19745, -0.06001, - -3.0151433056804824 + 3.268042001499104 ], [ -0.24909, -0.06437, - -3.0994428709533564 + 3.18374243622623 ], [ -0.30091, -0.06437, - -3.18374243622623 + 3.0994428709533564 ], [ -0.35255, -0.06001, - -3.268042001499104 + 3.0151433056804824 ], [ -0.40364, -0.05131, - -3.352341566771978 + 2.9308437404076084 ], [ -0.45381, -0.03835, - -3.4366411320448518 + 2.8465441751347345 ], [ -0.50271, -0.0212, - -3.5209406973177253 + 2.762244609861861 ], [ -0.55, @@ -3632,32 +3632,32 @@ [ -0.04705, -0.02352, - -2.677945044588987 + 3.6052402625905993 ], [ -0.09444, -0.04634, - -2.727671917388891 + 3.555513389790695 ], [ -0.14342, -0.06548, - -2.8104560646290713 + 3.472729242550515 ], [ -0.19381, -0.0805, - -2.893240211869252 + 3.3899450953103343 ], [ -0.24528, -0.09131, - -2.976024359109432 + 3.307160948070154 ], [ -0.29746, -0.09782, - -3.058808506349613 + 3.2243768008299734 ], [ -0.35, @@ -3679,17 +3679,17 @@ [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ], [ 0.0, 0.0, - -2.90976884908939 + 3.373416458090196 ], [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ] ] }, @@ -3706,27 +3706,27 @@ [ -0.03576, -0.03531, - -2.3861486579845876 + 3.8970366491949986 ], [ -0.07357, -0.06841, - -2.4590977546356876 + 3.8240875525438986 ], [ -0.11369, -0.09866, - -2.532046851286787 + 3.751138455892799 ], [ -0.15591, -0.1259, - -2.604995947937887 + 3.678189359241699 ], [ -0.2, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3743,17 +3743,17 @@ [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ], [ 0.0, 0.0, - -2.517069767390666 + 3.7661155397889203 ], [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3770,22 +3770,22 @@ [ -0.0375, -0.0375, - -2.356194490192345 + 3.9269908169872414 ], [ -0.075, -0.075, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1125, -0.1125, - -2.356194490192345 + 3.9269908169872414 ], [ -0.15, -0.15, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3802,22 +3802,22 @@ [ 0.0375, -0.0375, - -2.356194490192345 + 3.9269908169872414 ], [ 0.075, -0.075, - -2.356194490192345 + 3.9269908169872414 ], [ 0.1125, -0.1125, - -2.356194490192345 + 3.9269908169872414 ], [ 0.15, -0.15, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3834,22 +3834,22 @@ [ -0.0375, 0.0375, - -2.356194490192345 + 3.9269908169872414 ], [ -0.075, 0.075, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1125, 0.1125, - -2.356194490192345 + 3.9269908169872414 ], [ -0.15, 0.15, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3866,27 +3866,27 @@ [ -0.03531, -0.03576, - -2.326240322400102 + 3.956944984779484 ], [ -0.06841, -0.07357, - -2.253291225749002 + 4.029894081430584 ], [ -0.09866, -0.11369, - -2.1803421290979026 + 4.102843178081684 ], [ -0.1259, -0.15591, - -2.1073930324468026 + 4.175792274732784 ], [ -0.15, -0.2, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3903,17 +3903,17 @@ [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ], [ 0.0, 0.0, - -2.1953192129940238 + 4.0878660941855625 ], [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3930,47 +3930,47 @@ [ -0.03288, -0.03617, - -2.260711797772509 + 4.022473509407077 ], [ -0.06215, -0.07532, - -2.165229105352673 + 4.117956201826914 ], [ -0.08756, -0.11707, - -2.069746412932837 + 4.2134388942467496 ], [ -0.10888, -0.16106, - -1.9742637205130011 + 4.308921586666585 ], [ -0.1259, -0.20688, - -1.8787810280931652 + 4.404404279086421 ], [ -0.13848, -0.25412, - -1.7832983356733292 + 4.499886971506257 ], [ -0.1465, -0.30234, - -1.6878156432534932 + 4.595369663926093 ], [ -0.14988, -0.3511, - -1.5923329508336572 + 4.690852356345929 ], [ -0.15, -0.4, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3987,42 +3987,42 @@ [ -0.03617, -0.03288, - -2.4516771826121806 + 3.8315081245674056 ], [ -0.07532, -0.06215, - -2.547159875032017 + 3.7360254321475694 ], [ -0.11707, -0.08756, - -2.6426425674518526 + 3.6405427397277337 ], [ -0.16106, -0.10888, - -2.7381252598716888 + 3.5450600473078975 ], [ -0.20688, -0.1259, - -2.8336079522915245 + 3.4495773548880617 ], [ -0.25412, -0.13848, - -2.9290906447113603 + 3.354094662468226 ], [ -0.30234, -0.1465, - -3.0245733371311965 + 3.2586119700483898 ], [ -0.3511, -0.14988, - -3.1200560295510327 + 3.1631292776285536 ], [ -0.4, @@ -4044,27 +4044,27 @@ [ -0.0241, -0.04409, - -2.1073930324468026 + 4.175792274732784 ], [ -0.05134, -0.08631, - -2.1803421290979026 + 4.102843178081684 ], [ -0.08159, -0.12643, - -2.253291225749002 + 4.029894081430584 ], [ -0.11469, -0.16424, - -2.326240322400102 + 3.956944984779484 ], [ -0.15, -0.2, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -4081,22 +4081,22 @@ [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.0, 0.0, - -2.1416941205945834 + 4.141491186585003 ], [ 0.0, 0.0, - -2.248944305393464 + 4.034241001786122 ], [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -4113,22 +4113,22 @@ [ -0.025, -0.05, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.05, -0.1, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.075, -0.15, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.1, -0.2, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4145,22 +4145,22 @@ [ 0.05, -0.025, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.1, -0.05, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.15, -0.075, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.2, -0.1, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4177,22 +4177,22 @@ [ -0.05, 0.025, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.1, 0.05, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.15, 0.075, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.2, 0.1, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4209,37 +4209,37 @@ [ -0.02352, -0.04705, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.04634, -0.09444, - -1.9847170629957986 + 4.298468244183788 ], [ -0.06548, -0.14342, - -1.9019329157556182 + 4.381252391423968 ], [ -0.0805, -0.19381, - -1.8191487685154377 + 4.464036538664148 ], [ -0.09131, -0.24528, - -1.7363646212752575 + 4.546820685904329 ], [ -0.09782, -0.29746, - -1.653580474035077 + 4.629604833144509 ], [ -0.1, -0.35, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4256,22 +4256,22 @@ [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.0, 0.0, - -1.879894732795434 + 4.403290574384152 ], [ 0.0, 0.0, - -1.7253455297951652 + 4.557839777384421 ], [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4288,47 +4288,47 @@ [ -0.02076, -0.04747, - -1.9314111337955235 + 4.3517741733840625 ], [ -0.03652, -0.09683, - -1.8283783317953444 + 4.454806975384242 ], [ -0.04712, -0.14755, - -1.7253455297951654 + 4.557839777384421 ], [ -0.05245, -0.19909, - -1.622312727794986 + 4.6608725793846 ], [ -0.05245, -0.25091, - -1.519279925794807 + 4.763905381384779 ], [ -0.04712, -0.30245, - -1.4162471237946277 + 4.866938183384958 ], [ -0.03652, -0.35317, - -1.3132143217944487 + 4.969970985385137 ], [ -0.02076, -0.40253, - -1.2101815197942694 + 5.073003787385317 ], [ 0.0, -0.45, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4345,57 +4345,57 @@ [ -0.0212, -0.04729, - -1.950144370522829 + 4.333040936656757 ], [ -0.03835, -0.09619, - -1.865844805249955 + 4.4173405019296315 ], [ -0.05131, -0.14636, - -1.7815452399770813 + 4.501640067202505 ], [ -0.06001, -0.19745, - -1.6972456747042073 + 4.5859396324753785 ], [ -0.06437, -0.24909, - -1.6129461094313335 + 4.670239197748253 ], [ -0.06437, -0.30091, - -1.5286465441584598 + 4.754538763021126 ], [ -0.06001, -0.35255, - -1.4443469788855858 + 4.838838328294001 ], [ -0.05131, -0.40364, - -1.3600474136127119 + 4.923137893566874 ], [ -0.03835, -0.45381, - -1.275747848339838 + 5.007437458839748 ], [ -0.0212, -0.50271, - -1.1914482830669644 + 5.091737024112621 ], [ 0.0, -0.55, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4412,67 +4412,67 @@ [ -0.00236, -0.04981, - -1.6656617517546934 + 4.617523555424893 ], [ -0.00944, -0.09917, - -1.76052717671449 + 4.522658130465096 ], [ -0.02115, -0.14765, - -1.855392601674287 + 4.427792705505299 ], [ -0.03741, -0.19479, - -1.9502580266340837 + 4.332927280545503 ], [ -0.05805, -0.24018, - -2.0451234515938803 + 4.2380618555857055 ], [ -0.08291, -0.28341, - -2.1399888765536774 + 4.143196430625909 ], [ -0.11175, -0.3241, - -2.234854301513474 + 4.048331005666112 ], [ -0.14431, -0.36187, - -2.3297197264732707 + 3.9534655807063155 ], [ -0.1803, -0.39638, - -2.4245851514330674 + 3.858600155746519 ], [ -0.2194, -0.42733, - -2.519450576392864 + 3.763734730786722 ], [ -0.26126, -0.45444, - -2.6143160013526607 + 3.6688693058269255 ], [ -0.30538, -0.47769, - -2.677945044588987 + 3.6052402625905993 ], [ -0.35, -0.5, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -4489,37 +4489,37 @@ [ -0.00218, -0.05254, - -1.653580474035077 + 4.629604833144509 ], [ -0.00869, -0.10472, - -1.7363646212752575 + 4.546820685904329 ], [ -0.0195, -0.15619, - -1.8191487685154377 + 4.464036538664148 ], [ -0.03452, -0.20658, - -1.9019329157556182 + 4.381252391423968 ], [ -0.05366, -0.25556, - -1.9847170629957986 + 4.298468244183788 ], [ -0.07648, -0.30295, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.1, -0.35, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4536,22 +4536,22 @@ [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, 0.0, - -1.7253455297951652 + 4.557839777384421 ], [ 0.0, 0.0, - -1.879894732795434 + 4.403290574384152 ], [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4568,17 +4568,17 @@ [ 0.0, -0.05, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.1, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.15, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4595,17 +4595,17 @@ [ 0.05, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.1, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.15, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4622,17 +4622,17 @@ [ -0.05, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ -0.1, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ -0.15, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4649,37 +4649,37 @@ [ 0.00218, -0.05254, - -1.488012179554716 + 4.79517312762487 ], [ 0.00869, -0.10472, - -1.4052280323145356 + 4.87795727486505 ], [ 0.0195, -0.15619, - -1.3224438850743554 + 4.960741422105231 ], [ 0.03452, -0.20658, - -1.239659737834175 + 5.0435255693454115 ], [ 0.05366, -0.25556, - -1.1568755905939945 + 5.126309716585592 ], [ 0.07648, -0.30295, - -1.1071487177940904 + 5.176036589385496 ], [ 0.1, -0.35, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4696,22 +4696,22 @@ [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, 0.0, - -1.416247123794628 + 4.866938183384958 ], [ 0.0, 0.0, - -1.261697920794359 + 5.021487386385227 ], [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4728,67 +4728,67 @@ [ 0.00236, -0.04981, - -1.4759309018350997 + 4.807254405344486 ], [ 0.00944, -0.09917, - -1.381065476875303 + 4.902119830304283 ], [ 0.02115, -0.14765, - -1.2862000519155061 + 4.9969852552640805 ], [ 0.03741, -0.19479, - -1.1913346269557095 + 5.091850680223876 ], [ 0.05805, -0.24018, - -1.0964692019959126 + 5.186716105183674 ], [ 0.08291, -0.28341, - -1.0016037770361157 + 5.2815815301434705 ], [ 0.11175, -0.3241, - -0.9067383520763189 + 5.376446955103267 ], [ 0.14431, -0.36187, - -0.8118729271165221 + 5.471312380063064 ], [ 0.1803, -0.39638, - -0.7170075021567255 + 5.5661778050228605 ], [ 0.2194, -0.42733, - -0.6221420771969288 + 5.661043229982657 ], [ 0.26126, -0.45444, - -0.5272766522371322 + 5.755908654942454 ], [ 0.30538, -0.47769, - -0.4636476090008061 + 5.81953769817878 ], [ 0.35, -0.5, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -4805,47 +4805,47 @@ [ 0.02076, -0.04747, - -1.2101815197942696 + 5.073003787385317 ], [ 0.03652, -0.09683, - -1.3132143217944487 + 4.969970985385137 ], [ 0.04712, -0.14755, - -1.4162471237946277 + 4.866938183384958 ], [ 0.05245, -0.19909, - -1.519279925794807 + 4.763905381384779 ], [ 0.05245, -0.25091, - -1.622312727794986 + 4.6608725793846 ], [ 0.04712, -0.30245, - -1.7253455297951654 + 4.557839777384421 ], [ 0.03652, -0.35317, - -1.8283783317953444 + 4.454806975384242 ], [ 0.02076, -0.40253, - -1.9314111337955238 + 4.3517741733840625 ], [ 0.0, -0.45, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4862,57 +4862,57 @@ [ 0.0212, -0.04729, - -1.1914482830669642 + 5.091737024112622 ], [ 0.03835, -0.09619, - -1.2757478483398381 + 5.007437458839748 ], [ 0.05131, -0.14636, - -1.3600474136127119 + 4.923137893566874 ], [ 0.06001, -0.19745, - -1.4443469788855858 + 4.838838328294001 ], [ 0.06437, -0.24909, - -1.5286465441584596 + 4.754538763021126 ], [ 0.06437, -0.30091, - -1.6129461094313333 + 4.670239197748253 ], [ 0.06001, -0.35255, - -1.6972456747042073 + 4.5859396324753785 ], [ 0.05131, -0.40364, - -1.7815452399770813 + 4.501640067202505 ], [ 0.03835, -0.45381, - -1.8658448052499552 + 4.4173405019296315 ], [ 0.0212, -0.50271, - -1.9501443705228287 + 4.333040936656758 ], [ 0.0, -0.55, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4929,37 +4929,37 @@ [ 0.02352, -0.04705, - -1.1071487177940904 + 5.176036589385496 ], [ 0.04634, -0.09444, - -1.1568755905939945 + 5.126309716585592 ], [ 0.06548, -0.14342, - -1.239659737834175 + 5.0435255693454115 ], [ 0.0805, -0.19381, - -1.3224438850743554 + 4.960741422105231 ], [ 0.09131, -0.24528, - -1.4052280323145356 + 4.87795727486505 ], [ 0.09782, -0.29746, - -1.488012179554716 + 4.79517312762487 ], [ 0.1, -0.35, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4976,17 +4976,17 @@ [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ], [ 0.0, 0.0, - -1.3389725222944935 + 4.944212784885092 ], [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -5003,22 +5003,22 @@ [ 0.025, -0.05, - -1.1071487177940904 + 5.176036589385496 ], [ 0.05, -0.1, - -1.1071487177940904 + 5.176036589385496 ], [ 0.075, -0.15, - -1.1071487177940904 + 5.176036589385496 ], [ 0.1, -0.2, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5035,22 +5035,22 @@ [ 0.05, 0.025, - -1.1071487177940904 + 5.176036589385496 ], [ 0.1, 0.05, - -1.1071487177940904 + 5.176036589385496 ], [ 0.15, 0.075, - -1.1071487177940904 + 5.176036589385496 ], [ 0.2, 0.1, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5067,22 +5067,22 @@ [ -0.05, -0.025, - -1.1071487177940904 + 5.176036589385496 ], [ -0.1, -0.05, - -1.1071487177940904 + 5.176036589385496 ], [ -0.15, -0.075, - -1.1071487177940904 + 5.176036589385496 ], [ -0.2, -0.1, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5099,27 +5099,27 @@ [ 0.0241, -0.04409, - -1.0341996211429905 + 5.248985686036596 ], [ 0.05134, -0.08631, - -0.9612505244918907 + 5.321934782687696 ], [ 0.08159, -0.12643, - -0.8883014278407909 + 5.394883879338796 ], [ 0.11469, -0.16424, - -0.8153523311896911 + 5.467832975989895 ], [ 0.15, -0.2, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5136,17 +5136,17 @@ [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ], [ 0.0, 0.0, - -0.9462734405957693 + 5.336911866583817 ], [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5163,47 +5163,47 @@ [ 0.03288, -0.03617, - -0.8808808558172843 + 5.402304451362302 ], [ 0.06215, -0.07532, - -0.9763635482371201 + 5.306821758942466 ], [ 0.08756, -0.11707, - -1.071846240656956 + 5.21133906652263 ], [ 0.10888, -0.16106, - -1.167328933076792 + 5.115856374102794 ], [ 0.1259, -0.20688, - -1.262811625496628 + 5.020373681682958 ], [ 0.13848, -0.25412, - -1.358294317916464 + 4.9248909892631225 ], [ 0.1465, -0.30234, - -1.4537770103363 + 4.829408296843287 ], [ 0.14988, -0.3511, - -1.549259702756136 + 4.73392560442345 ], [ 0.15, -0.4, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -5220,27 +5220,27 @@ [ 0.03531, -0.03576, - -0.8153523311896911 + 5.467832975989895 ], [ 0.06841, -0.07357, - -0.8883014278407909 + 5.394883879338796 ], [ 0.09866, -0.11369, - -0.9612505244918907 + 5.321934782687696 ], [ 0.1259, -0.15591, - -1.0341996211429907 + 5.248985686036596 ], [ 0.15, -0.2, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5257,17 +5257,17 @@ [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ], [ 0.0, 0.0, - -0.9462734405957693 + 5.336911866583817 ], [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5284,22 +5284,22 @@ [ 0.0375, -0.0375, - -0.7853981633974483 + 5.497787143782138 ], [ 0.075, -0.075, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1125, -0.1125, - -0.7853981633974483 + 5.497787143782138 ], [ 0.15, -0.15, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5316,22 +5316,22 @@ [ 0.0375, 0.0375, - -0.7853981633974483 + 5.497787143782138 ], [ 0.075, 0.075, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1125, 0.1125, - -0.7853981633974483 + 5.497787143782138 ], [ 0.15, 0.15, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5348,22 +5348,22 @@ [ -0.0375, -0.0375, - -0.7853981633974483 + 5.497787143782138 ], [ -0.075, -0.075, - -0.7853981633974483 + 5.497787143782138 ], [ -0.1125, -0.1125, - -0.7853981633974483 + 5.497787143782138 ], [ -0.15, -0.15, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5380,27 +5380,27 @@ [ 0.03576, -0.03531, - -0.7554439956052055 + 5.527741311574381 ], [ 0.07357, -0.06841, - -0.6824948989541056 + 5.60069040822548 ], [ 0.11369, -0.09866, - -0.6095458023030058 + 5.67363950487658 ], [ 0.15591, -0.1259, - -0.5365967056519059 + 5.74658860152768 ], [ 0.2, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -5417,17 +5417,17 @@ [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ], [ 0.0, 0.0, - -0.6245228861991272 + 5.658662420980459 ], [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ] ] }, @@ -5444,42 +5444,42 @@ [ 0.03617, -0.03288, - -0.6899154709776123 + 5.593269836201974 ], [ 0.07532, -0.06215, - -0.5944327785577764 + 5.6887525286218095 ], [ 0.11707, -0.08756, - -0.4989500861379405 + 5.784235221041646 ], [ 0.16106, -0.10888, - -0.4034673937181046 + 5.879717913461482 ], [ 0.20688, -0.1259, - -0.30798470129826866 + 5.975200605881318 ], [ 0.25412, -0.13848, - -0.21250200887843262 + 6.070683298301153 ], [ 0.30234, -0.1465, - -0.11701931645859664 + 6.166165990720989 ], [ 0.3511, -0.14988, - -0.021536624038760666 + 6.261648683140826 ], [ 0.4, @@ -5501,27 +5501,27 @@ [ 0.04409, -0.0241, - -0.536596705651906 + 5.74658860152768 ], [ 0.08631, -0.05134, - -0.6095458023030058 + 5.67363950487658 ], [ 0.12643, -0.08159, - -0.6824948989541056 + 5.60069040822548 ], [ 0.16424, -0.11469, - -0.7554439956052055 + 5.527741311574381 ], [ 0.2, -0.15, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5538,22 +5538,22 @@ [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ], [ 0.0, 0.0, - -0.5708977937996869 + 5.712287513379899 ], [ 0.0, 0.0, - -0.6781479785985676 + 5.605037328581019 ], [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5570,22 +5570,22 @@ [ 0.05, -0.025, - -0.4636476090008061 + 5.81953769817878 ], [ 0.1, -0.05, - -0.4636476090008061 + 5.81953769817878 ], [ 0.15, -0.075, - -0.4636476090008061 + 5.81953769817878 ], [ 0.2, -0.1, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -5602,22 +5602,22 @@ [ 0.025, 0.05, - -0.4636476090008061 + 5.81953769817878 ], [ 0.05, 0.1, - -0.4636476090008061 + 5.81953769817878 ], [ 0.075, 0.15, - -0.4636476090008061 + 5.81953769817878 ], [ 0.1, 0.2, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -5634,22 +5634,22 @@ [ -0.025, -0.05, - -0.4636476090008061 + 5.81953769817878 ], [ -0.05, -0.1, - -0.4636476090008061 + 5.81953769817878 ], [ -0.075, -0.15, - -0.4636476090008061 + 5.81953769817878 ], [ -0.1, -0.2, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -5666,32 +5666,32 @@ [ 0.04705, -0.02352, - -0.4636476090008061 + 5.81953769817878 ], [ 0.09444, -0.04634, - -0.413920736200902 + 5.869264570978684 ], [ 0.14342, -0.06548, - -0.3311365889607216 + 5.952048718218864 ], [ 0.19381, -0.0805, - -0.24835244172054122 + 6.034832865459045 ], [ 0.24528, -0.09131, - -0.16556829448036087 + 6.117617012699226 ], [ 0.29746, -0.09782, - -0.08278414724018052 + 6.200401159939406 ], [ 0.35, @@ -5713,17 +5713,17 @@ [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ], [ 0.0, 0.0, - -0.3090984060005373 + 5.9740869011790485 ], [ 0.0, 0.0, - -0.15454920300026886 + 6.128636104179318 ], [ 0.0, @@ -5745,22 +5745,22 @@ [ 0.04747, -0.02076, - -0.360614807000627 + 5.922570500178959 ], [ 0.09683, -0.03652, - -0.2575820050004478 + 6.025603302179139 ], [ 0.14755, -0.04712, - -0.15454920300026875 + 6.128636104179318 ], [ 0.19909, -0.05245, - -0.051516401000089584 + 6.231668906179497 ], [ 0.25091, @@ -5802,27 +5802,27 @@ [ 0.04729, -0.0212, - -0.37934804372793224 + 5.903837263451654 ], [ 0.09619, -0.03835, - -0.29504847845505844 + 5.988136828724528 ], [ 0.14636, -0.05131, - -0.21074891318218458 + 6.0724363939974015 ], [ 0.19745, -0.06001, - -0.12644934790931073 + 6.156735959270275 ], [ 0.24909, -0.06437, - -0.04214978263643687 + 6.2410355245431495 ], [ 0.30091, diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json index c3018ae1bb0..646513c0aa0 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json @@ -1,6 +1,6 @@ { "version": 1.0, - "date_generated": "2022-01-19", + "date_generated": "2022-03-17", "lattice_metadata": { "motion_model": "ackermann", "turning_radius": 1, @@ -41,62 +41,62 @@ [ 0.04802, -0.00109, - -0.045358152467139604 + 6.237827154712447 ], [ 0.09594, -0.00435, - -0.09071630493427921 + 6.192469002245307 ], [ 0.14366, -0.00979, - -0.13607445740141882 + 6.1471108497781675 ], [ 0.19109, -0.01738, - -0.18143260986855841 + 6.101752697311028 ], [ 0.23812, -0.02712, - -0.226790762335698 + 6.056394544843888 ], [ 0.28467, -0.03898, - -0.2721489148028376 + 6.011036392376749 ], [ 0.33063, -0.05293, - -0.3175070672699772 + 5.965678239909609 ], [ 0.3759, -0.06896, - -0.36286521973711683 + 5.9203200874424695 ], [ 0.42041, -0.08702, - -0.4082233722042565 + 5.87496193497533 ], [ 0.46405, -0.10708, - -0.45358152467139606 + 5.82960378250819 ], [ 0.50704, -0.12852, - -0.4636476090008061 + 5.81953769817878 ], [ 0.55, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -113,67 +113,67 @@ [ 0.05179, -0.00049, - -0.026186384017708383 + 6.256998923161878 ], [ 0.10353, -0.00279, - -0.0626414860996332 + 6.220543821079953 ], [ 0.15516, -0.00697, - -0.09909658818155803 + 6.184088718998028 ], [ 0.2066, -0.01303, - -0.13555169026348282 + 6.147633616916103 ], [ 0.25778, -0.02097, - -0.17200679234540764 + 6.111178514834179 ], [ 0.30864, -0.03076, - -0.20846189442733246 + 6.074723412752253 ], [ 0.3591, -0.0424, - -0.24491699650925727 + 6.038268310670329 ], [ 0.40911, -0.05587, - -0.28137209859118206 + 6.001813208588404 ], [ 0.4586, -0.07116, - -0.3178272006731068 + 5.965358106506479 ], [ 0.5075, -0.08824, - -0.35428230275503164 + 5.928903004424555 ], [ 0.55574, -0.10709, - -0.3907374048369565 + 5.89244790234263 ], [ 0.60326, -0.12769, - -0.4271925069188813 + 5.855992800260705 ], [ 0.65, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -2075,12 +2075,12 @@ [ -0.50704, -0.12852, - -2.677945044588987 + 3.6052402625905993 ], [ -0.55, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2157,7 +2157,7 @@ [ -0.65, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2365,37 +2365,37 @@ [ -0.04286, -0.02143, - -2.677945044588987 + 3.6052402625905993 ], [ -0.08571, -0.04286, - -2.677945044588987 + 3.6052402625905993 ], [ -0.12857, -0.06429, - -2.677945044588987 + 3.6052402625905993 ], [ -0.17143, -0.08571, - -2.677945044588987 + 3.6052402625905993 ], [ -0.21429, -0.10714, - -2.677945044588987 + 3.6052402625905993 ], [ -0.25714, -0.12857, - -2.677945044588987 + 3.6052402625905993 ], [ -0.3, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2412,47 +2412,47 @@ [ -0.0427, -0.02193, - -2.6499990833251754 + 3.633186223854411 ], [ -0.0846, -0.04536, - -2.6132735091835717 + 3.6699117979960145 ], [ -0.12561, -0.07032, - -2.576547935041968 + 3.7066373721376182 ], [ -0.16568, -0.09676, - -2.539822360900364 + 3.7433629462792224 ], [ -0.20474, -0.12466, - -2.50309678675876 + 3.780088520420826 ], [ -0.24276, -0.15397, - -2.4663712126171564 + 3.81681409456243 ], [ -0.27967, -0.18466, - -2.4296456384755523 + 3.853539668704034 ], [ -0.31543, -0.21669, - -2.3929200643339485 + 3.8902652428456377 ], [ -0.35, -0.25, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -2469,57 +2469,57 @@ [ -0.04296, -0.02148, - -2.677945044588987 + 3.6052402625905993 ], [ -0.08595, -0.04292, - -2.688011128918397 + 3.595174178261189 ], [ -0.12959, -0.06298, - -2.733369281385537 + 3.5498160257940494 ], [ -0.1741, -0.08104, - -2.7787274338526764 + 3.50445787332691 ], [ -0.21937, -0.09707, - -2.824085586319816 + 3.4590997208597702 ], [ -0.26533, -0.11102, - -2.8694437387869556 + 3.4137415683926307 ], [ -0.31188, -0.12288, - -2.914801891254095 + 3.368383415925491 ], [ -0.35891, -0.13262, - -2.9601600437212348 + 3.3230252634583515 ], [ -0.40634, -0.14021, - -3.0055181961883743 + 3.277667110991212 ], [ -0.45406, -0.14565, - -3.050876348655514 + 3.2323089585240723 ], [ -0.50198, -0.14891, - -3.0962345011226535 + 3.1869508060569327 ], [ -0.55, @@ -2541,47 +2541,47 @@ [ -0.03457, -0.03331, - -2.3929200643339485 + 3.8902652428456377 ], [ -0.07033, -0.06534, - -2.4296456384755527 + 3.8535396687040335 ], [ -0.10724, -0.09603, - -2.4663712126171564 + 3.81681409456243 ], [ -0.14526, -0.12534, - -2.50309678675876 + 3.780088520420826 ], [ -0.18432, -0.15324, - -2.539822360900364 + 3.7433629462792224 ], [ -0.22439, -0.17968, - -2.576547935041968 + 3.7066373721376182 ], [ -0.2654, -0.20464, - -2.6132735091835717 + 3.6699117979960145 ], [ -0.3073, -0.22807, - -2.6499990833251754 + 3.633186223854411 ], [ -0.35, -0.25, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2598,57 +2598,57 @@ [ -0.03685, -0.03685, - -2.356194490192345 + 3.9269908169872414 ], [ -0.07371, -0.07371, - -2.356194490192345 + 3.9269908169872414 ], [ -0.11073, -0.11039, - -2.3753608974926848 + 3.9078244096869015 ], [ -0.14896, -0.14581, - -2.413183915879723 + 3.8700013912998634 ], [ -0.1885, -0.17976, - -2.4510069342667604 + 3.832178372912826 ], [ -0.22929, -0.21219, - -2.488829952653798 + 3.794355354525788 ], [ -0.27128, -0.24305, - -2.526652971040836 + 3.75653233613875 ], [ -0.31441, -0.27231, - -2.5644759894278737 + 3.7187093177517125 ], [ -0.35862, -0.29991, - -2.6022990078149117 + 3.6808862993646745 ], [ -0.40383, -0.32582, - -2.6401220262019494 + 3.643063280977637 ], [ -0.45, -0.35, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2665,42 +2665,42 @@ [ -0.0375, -0.0375, - -2.356194490192345 + 3.9269908169872414 ], [ -0.075, -0.075, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1125, -0.1125, - -2.356194490192345 + 3.9269908169872414 ], [ -0.15, -0.15, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1875, -0.1875, - -2.356194490192345 + 3.9269908169872414 ], [ -0.225, -0.225, - -2.356194490192345 + 3.9269908169872414 ], [ -0.2625, -0.2625, - -2.356194490192345 + 3.9269908169872414 ], [ -0.3, -0.3, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -2717,47 +2717,47 @@ [ -0.03331, -0.03457, - -2.319468916050741 + 3.963716391128845 ], [ -0.06534, -0.07033, - -2.282743341909137 + 4.000441965270449 ], [ -0.09603, -0.10724, - -2.2460177677675333 + 4.037167539412053 ], [ -0.12534, -0.14526, - -2.2092921936259295 + 4.073893113553657 ], [ -0.15324, -0.18432, - -2.172566619484326 + 4.11061868769526 ], [ -0.17968, -0.22439, - -2.1358410453427217 + 4.1473442618368646 ], [ -0.20464, -0.2654, - -2.099115471201118 + 4.184069835978468 ], [ -0.22807, -0.3073, - -2.0623898970595143 + 4.220795410120072 ], [ -0.25, -0.35, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -2774,57 +2774,57 @@ [ -0.03685, -0.03685, - -2.356194490192345 + 3.9269908169872414 ], [ -0.07371, -0.07371, - -2.356194490192345 + 3.9269908169872414 ], [ -0.11039, -0.11073, - -2.337028082892005 + 3.9461572242875813 ], [ -0.14581, -0.14896, - -2.299205064504967 + 3.9839802426746194 ], [ -0.17976, -0.1885, - -2.2613820461179293 + 4.021803261061657 ], [ -0.21219, -0.22929, - -2.2235590277308916 + 4.059626279448695 ], [ -0.24305, -0.27128, - -2.1857360093438536 + 4.097449297835732 ], [ -0.27231, -0.31441, - -2.147912990956816 + 4.13527231622277 ], [ -0.29991, -0.35862, - -2.110089972569778 + 4.173095334609808 ], [ -0.32582, -0.40383, - -2.0722669541827403 + 4.2109183529968455 ], [ -0.35, -0.45, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -2841,47 +2841,47 @@ [ -0.02193, -0.0427, - -2.0623898970595143 + 4.220795410120072 ], [ -0.04536, -0.0846, - -2.099115471201118 + 4.184069835978468 ], [ -0.07032, -0.12561, - -2.1358410453427217 + 4.1473442618368646 ], [ -0.09676, -0.16568, - -2.172566619484326 + 4.11061868769526 ], [ -0.12466, -0.20474, - -2.2092921936259295 + 4.073893113553657 ], [ -0.15397, -0.24276, - -2.2460177677675333 + 4.037167539412053 ], [ -0.18466, -0.27967, - -2.2827433419091374 + 4.000441965270449 ], [ -0.21669, -0.31543, - -2.319468916050741 + 3.963716391128845 ], [ -0.25, -0.35, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -2898,37 +2898,37 @@ [ -0.02143, -0.04286, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.04286, -0.08571, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.06429, -0.12857, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.08571, -0.17143, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.10714, -0.21429, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.12857, -0.25714, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.15, -0.3, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -2945,62 +2945,62 @@ [ -0.02148, -0.04296, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.04292, -0.08595, - -2.0243778514662925 + 4.258807455713294 ], [ -0.06298, -0.12959, - -1.979019698999153 + 4.304165608180433 ], [ -0.08104, -0.1741, - -1.9336615465320135 + 4.349523760647573 ], [ -0.09707, -0.21937, - -1.888303394064874 + 4.3948819131147125 ], [ -0.11102, -0.26533, - -1.8429452415977343 + 4.440240065581852 ], [ -0.12288, -0.31188, - -1.7975870891305947 + 4.485598218048992 ], [ -0.13262, -0.35891, - -1.752228936663455 + 4.530956370516131 ], [ -0.14021, -0.40634, - -1.7068707841963153 + 4.576314522983271 ], [ -0.14565, -0.45406, - -1.6615126317291757 + 4.6216726754504105 ], [ -0.14891, -0.50198, - -1.6161544792620361 + 4.66703082791755 ], [ -0.15, -0.55, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3017,62 +3017,62 @@ [ -0.00109, -0.04802, - -1.6161544792620361 + 4.66703082791755 ], [ -0.00435, -0.09594, - -1.6615126317291757 + 4.6216726754504105 ], [ -0.00979, -0.14366, - -1.7068707841963153 + 4.576314522983271 ], [ -0.01738, -0.19109, - -1.752228936663455 + 4.530956370516131 ], [ -0.02712, -0.23812, - -1.7975870891305945 + 4.485598218048992 ], [ -0.03898, -0.28467, - -1.842945241597734 + 4.440240065581852 ], [ -0.05293, -0.33063, - -1.8883033940648737 + 4.3948819131147125 ], [ -0.06896, -0.3759, - -1.9336615465320133 + 4.349523760647573 ], [ -0.08702, -0.42041, - -1.979019698999153 + 4.304165608180433 ], [ -0.10708, -0.46405, - -2.0243778514662925 + 4.258807455713294 ], [ -0.12852, -0.50704, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.15, -0.55, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3089,67 +3089,67 @@ [ -0.00049, -0.05179, - -1.596982710812605 + 4.686202596366981 ], [ -0.00279, -0.10353, - -1.6334378128945297 + 4.649747494285057 ], [ -0.00697, -0.15516, - -1.6698929149764545 + 4.613292392203132 ], [ -0.01303, -0.2066, - -1.7063480170583794 + 4.576837290121206 ], [ -0.02097, -0.25778, - -1.7428031191403042 + 4.540382188039282 ], [ -0.03076, -0.30864, - -1.779258221222229 + 4.503927085957358 ], [ -0.0424, -0.3591, - -1.8157133233041538 + 4.467471983875432 ], [ -0.05587, -0.40911, - -1.8521684253860786 + 4.431016881793507 ], [ -0.07116, -0.4586, - -1.8886235274680034 + 4.394561779711583 ], [ -0.08824, -0.5075, - -1.9250786295499283 + 4.358106677629658 ], [ -0.10709, -0.55574, - -1.961533731631853 + 4.321651575547733 ], [ -0.12769, -0.60326, - -1.997988833713778 + 4.285196473465808 ], [ -0.15, -0.65, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3166,32 +3166,32 @@ [ 0.0, -0.05, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.1, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.15, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.2, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.25, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.3, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3208,62 +3208,62 @@ [ 0.00109, -0.04802, - -1.525438174327757 + 4.757747132851829 ], [ 0.00435, -0.09594, - -1.4800800218606174 + 4.803105285318969 ], [ 0.00979, -0.14366, - -1.4347218693934778 + 4.848463437786108 ], [ 0.01738, -0.19109, - -1.3893637169263382 + 4.893821590253248 ], [ 0.02712, -0.23812, - -1.3440055644591986 + 4.939179742720388 ], [ 0.03898, -0.28467, - -1.298647411992059 + 4.984537895187527 ], [ 0.05293, -0.33063, - -1.2532892595249194 + 5.029896047654667 ], [ 0.06896, -0.3759, - -1.2079311070577798 + 5.075254200121806 ], [ 0.08702, -0.42041, - -1.16257295459064 + 5.120612352588946 ], [ 0.10708, -0.46405, - -1.1172148021235004 + 5.165970505056086 ], [ 0.12852, -0.50704, - -1.1071487177940904 + 5.176036589385496 ], [ 0.15, -0.55, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3280,67 +3280,67 @@ [ 0.00049, -0.05179, - -1.5446099427771882 + 4.738575364402398 ], [ 0.00279, -0.10353, - -1.5081548406952634 + 4.775030466484322 ], [ 0.00697, -0.15516, - -1.4716997386133386 + 4.811485568566248 ], [ 0.01303, -0.2066, - -1.4352446365314138 + 4.847940670648173 ], [ 0.02097, -0.25778, - -1.398789534449489 + 4.884395772730097 ], [ 0.03076, -0.30864, - -1.3623344323675641 + 4.920850874812022 ], [ 0.0424, -0.3591, - -1.3258793302856393 + 4.957305976893947 ], [ 0.05587, -0.40911, - -1.2894242282037145 + 4.993761078975872 ], [ 0.07116, -0.4586, - -1.2529691261217897 + 5.0302161810577966 ], [ 0.08824, -0.5075, - -1.2165140240398649 + 5.066671283139721 ], [ 0.10709, -0.55574, - -1.18005892195794 + 5.103126385221646 ], [ 0.12769, -0.60326, - -1.1436038198760152 + 5.1395814873035714 ], [ 0.15, -0.65, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3357,62 +3357,62 @@ [ 0.02148, -0.04296, - -1.1071487177940904 + 5.176036589385496 ], [ 0.04292, -0.08595, - -1.1172148021235004 + 5.165970505056086 ], [ 0.06298, -0.12959, - -1.16257295459064 + 5.120612352588946 ], [ 0.08104, -0.1741, - -1.2079311070577796 + 5.075254200121806 ], [ 0.09707, -0.21937, - -1.2532892595249192 + 5.029896047654667 ], [ 0.11102, -0.26533, - -1.298647411992059 + 4.984537895187527 ], [ 0.12288, -0.31188, - -1.3440055644591986 + 4.939179742720388 ], [ 0.13262, -0.35891, - -1.3893637169263382 + 4.893821590253248 ], [ 0.14021, -0.40634, - -1.4347218693934778 + 4.848463437786108 ], [ 0.14565, -0.45406, - -1.4800800218606174 + 4.803105285318969 ], [ 0.14891, -0.50198, - -1.525438174327757 + 4.757747132851829 ], [ 0.15, -0.55, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3429,37 +3429,37 @@ [ 0.02143, -0.04286, - -1.1071487177940904 + 5.176036589385496 ], [ 0.04286, -0.08571, - -1.1071487177940904 + 5.176036589385496 ], [ 0.06429, -0.12857, - -1.1071487177940904 + 5.176036589385496 ], [ 0.08571, -0.17143, - -1.1071487177940904 + 5.176036589385496 ], [ 0.10714, -0.21429, - -1.1071487177940904 + 5.176036589385496 ], [ 0.12857, -0.25714, - -1.1071487177940904 + 5.176036589385496 ], [ 0.15, -0.3, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3476,47 +3476,47 @@ [ 0.02193, -0.0427, - -1.079202756530279 + 5.203982550649307 ], [ 0.04536, -0.0846, - -1.0424771823886751 + 5.2407081247909115 ], [ 0.07032, -0.12561, - -1.0057516082470712 + 5.277433698932515 ], [ 0.09676, -0.16568, - -0.9690260341054675 + 5.314159273074119 ], [ 0.12466, -0.20474, - -0.9323004599638636 + 5.350884847215722 ], [ 0.15397, -0.24276, - -0.8955748858222597 + 5.387610421357326 ], [ 0.18466, -0.27967, - -0.8588493116806559 + 5.4243359954989305 ], [ 0.21669, -0.31543, - -0.8221237375390521 + 5.461061569640534 ], [ 0.25, -0.35, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -3533,47 +3533,47 @@ [ 0.03331, -0.03457, - -0.8221237375390521 + 5.461061569640534 ], [ 0.06534, -0.07033, - -0.8588493116806559 + 5.4243359954989305 ], [ 0.09603, -0.10724, - -0.8955748858222599 + 5.387610421357326 ], [ 0.12534, -0.14526, - -0.9323004599638637 + 5.350884847215722 ], [ 0.15324, -0.18432, - -0.9690260341054675 + 5.314159273074119 ], [ 0.17968, -0.22439, - -1.0057516082470714 + 5.277433698932515 ], [ 0.20464, -0.2654, - -1.0424771823886751 + 5.2407081247909115 ], [ 0.22807, -0.3073, - -1.079202756530279 + 5.203982550649307 ], [ 0.25, -0.35, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3590,57 +3590,57 @@ [ 0.03685, -0.03685, - -0.7853981633974483 + 5.497787143782138 ], [ 0.07371, -0.07371, - -0.7853981633974483 + 5.497787143782138 ], [ 0.11039, -0.11073, - -0.8045645706977883 + 5.478620736481798 ], [ 0.14581, -0.14896, - -0.8423875890848261 + 5.44079771809476 ], [ 0.17976, -0.1885, - -0.8802106074718639 + 5.402974699707722 ], [ 0.21219, -0.22929, - -0.9180336258589017 + 5.365151681320684 ], [ 0.24305, -0.27128, - -0.9558566442459394 + 5.327328662933647 ], [ 0.27231, -0.31441, - -0.9936796626329771 + 5.289505644546609 ], [ 0.29991, -0.35862, - -1.031502681020015 + 5.251682626159571 ], [ 0.32582, -0.40383, - -1.0693256994070528 + 5.213859607772534 ], [ 0.35, -0.45, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -3657,42 +3657,42 @@ [ 0.0375, -0.0375, - -0.7853981633974483 + 5.497787143782138 ], [ 0.075, -0.075, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1125, -0.1125, - -0.7853981633974483 + 5.497787143782138 ], [ 0.15, -0.15, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1875, -0.1875, - -0.7853981633974483 + 5.497787143782138 ], [ 0.225, -0.225, - -0.7853981633974483 + 5.497787143782138 ], [ 0.2625, -0.2625, - -0.7853981633974483 + 5.497787143782138 ], [ 0.3, -0.3, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -3709,47 +3709,47 @@ [ 0.03457, -0.03331, - -0.7486725892558445 + 5.534512717923742 ], [ 0.07033, -0.06534, - -0.7119470151142406 + 5.571238292065345 ], [ 0.10724, -0.09603, - -0.6752214409726367 + 5.6079638662069495 ], [ 0.14526, -0.12534, - -0.6384958668310329 + 5.644689440348554 ], [ 0.18432, -0.15324, - -0.601770292689429 + 5.681415014490157 ], [ 0.22439, -0.17968, - -0.5650447185478252 + 5.718140588631761 ], [ 0.2654, -0.20464, - -0.5283191444062213 + 5.754866162773365 ], [ 0.3073, -0.22807, - -0.4915935702646175 + 5.7915917369149685 ], [ 0.35, -0.25, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -3766,57 +3766,57 @@ [ 0.03685, -0.03685, - -0.7853981633974483 + 5.497787143782138 ], [ 0.07371, -0.07371, - -0.7853981633974483 + 5.497787143782138 ], [ 0.11073, -0.11039, - -0.7662317560971083 + 5.516953551082478 ], [ 0.14896, -0.14581, - -0.7284087377100704 + 5.5547765694695155 ], [ 0.1885, -0.17976, - -0.6905857193230327 + 5.5925995878565535 ], [ 0.22929, -0.21219, - -0.6527627009359949 + 5.630422606243592 ], [ 0.27128, -0.24305, - -0.614939682548957 + 5.668245624630629 ], [ 0.31441, -0.27231, - -0.5771166641619194 + 5.706068643017667 ], [ 0.35862, -0.29991, - -0.5392936457748816 + 5.743891661404705 ], [ 0.40383, -0.32582, - -0.5014706273878439 + 5.781714679791742 ], [ 0.45, -0.35, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -3833,47 +3833,47 @@ [ 0.0427, -0.02193, - -0.4915935702646175 + 5.7915917369149685 ], [ 0.0846, -0.04536, - -0.5283191444062214 + 5.754866162773364 ], [ 0.12561, -0.07032, - -0.5650447185478252 + 5.718140588631761 ], [ 0.16568, -0.09676, - -0.601770292689429 + 5.681415014490157 ], [ 0.20474, -0.12466, - -0.6384958668310329 + 5.644689440348554 ], [ 0.24276, -0.15397, - -0.6752214409726367 + 5.6079638662069495 ], [ 0.27967, -0.18466, - -0.7119470151142406 + 5.571238292065345 ], [ 0.31543, -0.21669, - -0.7486725892558445 + 5.534512717923742 ], [ 0.35, -0.25, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -3890,37 +3890,37 @@ [ 0.04286, -0.02143, - -0.4636476090008061 + 5.81953769817878 ], [ 0.08571, -0.04286, - -0.4636476090008061 + 5.81953769817878 ], [ 0.12857, -0.06429, - -0.4636476090008061 + 5.81953769817878 ], [ 0.17143, -0.08571, - -0.4636476090008061 + 5.81953769817878 ], [ 0.21429, -0.10714, - -0.4636476090008061 + 5.81953769817878 ], [ 0.25714, -0.12857, - -0.4636476090008061 + 5.81953769817878 ], [ 0.3, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -3937,57 +3937,57 @@ [ 0.04296, -0.02148, - -0.4636476090008061 + 5.81953769817878 ], [ 0.08595, -0.04292, - -0.453581524671396 + 5.82960378250819 ], [ 0.12959, -0.06298, - -0.4082233722042564 + 5.87496193497533 ], [ 0.1741, -0.08104, - -0.36286521973711683 + 5.9203200874424695 ], [ 0.21937, -0.09707, - -0.31750706726997724 + 5.965678239909609 ], [ 0.26533, -0.11102, - -0.27214891480283765 + 6.011036392376749 ], [ 0.31188, -0.12288, - -0.22679076233569806 + 6.056394544843888 ], [ 0.35891, -0.13262, - -0.18143260986855847 + 6.101752697311028 ], [ 0.40634, -0.14021, - -0.13607445740141882 + 6.1471108497781675 ], [ 0.45406, -0.14565, - -0.09071630493427918 + 6.192469002245307 ], [ 0.50198, -0.14891, - -0.04535815246713959 + 6.237827154712447 ], [ 0.55, diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json index 89e948f576c..9146355201c 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json @@ -1,6 +1,6 @@ { "version": 1.0, - "date_generated": "2022-01-19", + "date_generated": "2022-03-17", "lattice_metadata": { "motion_model": "diff", "turning_radius": 1, @@ -41,62 +41,62 @@ [ 0.04802, -0.00109, - -0.045358152467139604 + 6.237827154712447 ], [ 0.09594, -0.00435, - -0.09071630493427921 + 6.192469002245307 ], [ 0.14366, -0.00979, - -0.13607445740141882 + 6.1471108497781675 ], [ 0.19109, -0.01738, - -0.18143260986855841 + 6.101752697311028 ], [ 0.23812, -0.02712, - -0.226790762335698 + 6.056394544843888 ], [ 0.28467, -0.03898, - -0.2721489148028376 + 6.011036392376749 ], [ 0.33063, -0.05293, - -0.3175070672699772 + 5.965678239909609 ], [ 0.3759, -0.06896, - -0.36286521973711683 + 5.9203200874424695 ], [ 0.42041, -0.08702, - -0.4082233722042565 + 5.87496193497533 ], [ 0.46405, -0.10708, - -0.45358152467139606 + 5.82960378250819 ], [ 0.50704, -0.12852, - -0.4636476090008061 + 5.81953769817878 ], [ 0.55, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -113,67 +113,67 @@ [ 0.05179, -0.00049, - -0.026186384017708383 + 6.256998923161878 ], [ 0.10353, -0.00279, - -0.0626414860996332 + 6.220543821079953 ], [ 0.15516, -0.00697, - -0.09909658818155803 + 6.184088718998028 ], [ 0.2066, -0.01303, - -0.13555169026348282 + 6.147633616916103 ], [ 0.25778, -0.02097, - -0.17200679234540764 + 6.111178514834179 ], [ 0.30864, -0.03076, - -0.20846189442733246 + 6.074723412752253 ], [ 0.3591, -0.0424, - -0.24491699650925727 + 6.038268310670329 ], [ 0.40911, -0.05587, - -0.28137209859118206 + 6.001813208588404 ], [ 0.4586, -0.07116, - -0.3178272006731068 + 5.965358106506479 ], [ 0.5075, -0.08824, - -0.35428230275503164 + 5.928903004424555 ], [ 0.55574, -0.10709, - -0.3907374048369565 + 5.89244790234263 ], [ 0.60326, -0.12769, - -0.4271925069188813 + 5.855992800260705 ], [ 0.65, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -195,17 +195,17 @@ [ 0.0, 0.0, - -0.15454920300026886 + 6.128636104179318 ], [ 0.0, 0.0, - -0.3090984060005373 + 5.9740869011790485 ], [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ] ] }, @@ -418,17 +418,17 @@ [ 0.0, 0.0, - 0.15454920300026886 + 0.1545492030002687 ], [ 0.0, 0.0, - 0.3090984060005373 + 0.3090984060005374 ], [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ] ] }, @@ -517,12 +517,12 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ], [ 0.0, 0.0, - 0.23182380450040307 + 0.23182380450040305 ], [ 0.0, @@ -648,7 +648,7 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ], [ 0.0, @@ -809,7 +809,7 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ] ] }, @@ -1242,12 +1242,12 @@ [ 0.0, 0.0, - 1.2616979207943588 + 1.261697920794359 ], [ 0.0, 0.0, - 1.4162471237946281 + 1.416247123794628 ], [ 0.0, @@ -1423,12 +1423,12 @@ [ 0.0, 0.0, - 1.4162471237946281 + 1.416247123794628 ], [ 0.0, 0.0, - 1.2616979207943588 + 1.261697920794359 ], [ 0.0, @@ -1646,12 +1646,12 @@ [ 0.0, 0.0, - 1.725345529795165 + 1.7253455297951652 ], [ 0.0, 0.0, - 1.8798947327954343 + 1.879894732795434 ], [ 0.0, @@ -1750,7 +1750,7 @@ [ 0.0, 0.0, - 1.8026201312952992 + 1.8026201312952996 ], [ 0.0, @@ -2470,17 +2470,17 @@ [ 0.0, 0.0, - 2.8324942475892563 + 2.832494247589256 ], [ 0.0, 0.0, - 2.9870434505895247 + 2.9870434505895243 ], [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ] ] }, @@ -2547,12 +2547,12 @@ [ -0.50704, -0.12852, - -2.677945044588987 + 3.6052402625905993 ], [ -0.55, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2629,7 +2629,7 @@ [ -0.65, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2646,22 +2646,22 @@ [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ], [ 0.0, 0.0, - -2.9870434505895247 + 3.296141856590062 ], [ 0.0, 0.0, - -2.8324942475892563 + 3.4506910595903304 ], [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2827,17 +2827,17 @@ [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ], [ 0.0, 0.0, - 2.9870434505895247 + 2.9870434505895243 ], [ 0.0, 0.0, - 2.8324942475892563 + 2.832494247589256 ], [ 0.0, @@ -2901,37 +2901,37 @@ [ -0.04286, -0.02143, - -2.677945044588987 + 3.6052402625905993 ], [ -0.08571, -0.04286, - -2.677945044588987 + 3.6052402625905993 ], [ -0.12857, -0.06429, - -2.677945044588987 + 3.6052402625905993 ], [ -0.17143, -0.08571, - -2.677945044588987 + 3.6052402625905993 ], [ -0.21429, -0.10714, - -2.677945044588987 + 3.6052402625905993 ], [ -0.25714, -0.12857, - -2.677945044588987 + 3.6052402625905993 ], [ -0.3, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -2948,47 +2948,47 @@ [ -0.0427, -0.02193, - -2.6499990833251754 + 3.633186223854411 ], [ -0.0846, -0.04536, - -2.6132735091835717 + 3.6699117979960145 ], [ -0.12561, -0.07032, - -2.576547935041968 + 3.7066373721376182 ], [ -0.16568, -0.09676, - -2.539822360900364 + 3.7433629462792224 ], [ -0.20474, -0.12466, - -2.50309678675876 + 3.780088520420826 ], [ -0.24276, -0.15397, - -2.4663712126171564 + 3.81681409456243 ], [ -0.27967, -0.18466, - -2.4296456384755523 + 3.853539668704034 ], [ -0.31543, -0.21669, - -2.3929200643339485 + 3.8902652428456377 ], [ -0.35, -0.25, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3005,17 +3005,17 @@ [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ], [ 0.0, 0.0, - -2.517069767390666 + 3.7661155397889203 ], [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3032,57 +3032,57 @@ [ -0.04296, -0.02148, - -2.677945044588987 + 3.6052402625905993 ], [ -0.08595, -0.04292, - -2.688011128918397 + 3.595174178261189 ], [ -0.12959, -0.06298, - -2.733369281385537 + 3.5498160257940494 ], [ -0.1741, -0.08104, - -2.7787274338526764 + 3.50445787332691 ], [ -0.21937, -0.09707, - -2.824085586319816 + 3.4590997208597702 ], [ -0.26533, -0.11102, - -2.8694437387869556 + 3.4137415683926307 ], [ -0.31188, -0.12288, - -2.914801891254095 + 3.368383415925491 ], [ -0.35891, -0.13262, - -2.9601600437212348 + 3.3230252634583515 ], [ -0.40634, -0.14021, - -3.0055181961883743 + 3.277667110991212 ], [ -0.45406, -0.14565, - -3.050876348655514 + 3.2323089585240723 ], [ -0.50198, -0.14891, - -3.0962345011226535 + 3.1869508060569327 ], [ -0.55, @@ -3104,17 +3104,17 @@ [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ], [ 0.0, 0.0, - -2.90976884908939 + 3.373416458090196 ], [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ] ] }, @@ -3131,47 +3131,47 @@ [ -0.03457, -0.03331, - -2.3929200643339485 + 3.8902652428456377 ], [ -0.07033, -0.06534, - -2.4296456384755527 + 3.8535396687040335 ], [ -0.10724, -0.09603, - -2.4663712126171564 + 3.81681409456243 ], [ -0.14526, -0.12534, - -2.50309678675876 + 3.780088520420826 ], [ -0.18432, -0.15324, - -2.539822360900364 + 3.7433629462792224 ], [ -0.22439, -0.17968, - -2.576547935041968 + 3.7066373721376182 ], [ -0.2654, -0.20464, - -2.6132735091835717 + 3.6699117979960145 ], [ -0.3073, -0.22807, - -2.6499990833251754 + 3.633186223854411 ], [ -0.35, -0.25, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3188,57 +3188,57 @@ [ -0.03685, -0.03685, - -2.356194490192345 + 3.9269908169872414 ], [ -0.07371, -0.07371, - -2.356194490192345 + 3.9269908169872414 ], [ -0.11073, -0.11039, - -2.3753608974926848 + 3.9078244096869015 ], [ -0.14896, -0.14581, - -2.413183915879723 + 3.8700013912998634 ], [ -0.1885, -0.17976, - -2.4510069342667604 + 3.832178372912826 ], [ -0.22929, -0.21219, - -2.488829952653798 + 3.794355354525788 ], [ -0.27128, -0.24305, - -2.526652971040836 + 3.75653233613875 ], [ -0.31441, -0.27231, - -2.5644759894278737 + 3.7187093177517125 ], [ -0.35862, -0.29991, - -2.6022990078149117 + 3.6808862993646745 ], [ -0.40383, -0.32582, - -2.6401220262019494 + 3.643063280977637 ], [ -0.45, -0.35, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3255,17 +3255,17 @@ [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ], [ 0.0, 0.0, - -2.517069767390666 + 3.7661155397889203 ], [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3282,42 +3282,42 @@ [ -0.0375, -0.0375, - -2.356194490192345 + 3.9269908169872414 ], [ -0.075, -0.075, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1125, -0.1125, - -2.356194490192345 + 3.9269908169872414 ], [ -0.15, -0.15, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1875, -0.1875, - -2.356194490192345 + 3.9269908169872414 ], [ -0.225, -0.225, - -2.356194490192345 + 3.9269908169872414 ], [ -0.2625, -0.2625, - -2.356194490192345 + 3.9269908169872414 ], [ -0.3, -0.3, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3334,47 +3334,47 @@ [ -0.03331, -0.03457, - -2.319468916050741 + 3.963716391128845 ], [ -0.06534, -0.07033, - -2.282743341909137 + 4.000441965270449 ], [ -0.09603, -0.10724, - -2.2460177677675333 + 4.037167539412053 ], [ -0.12534, -0.14526, - -2.2092921936259295 + 4.073893113553657 ], [ -0.15324, -0.18432, - -2.172566619484326 + 4.11061868769526 ], [ -0.17968, -0.22439, - -2.1358410453427217 + 4.1473442618368646 ], [ -0.20464, -0.2654, - -2.099115471201118 + 4.184069835978468 ], [ -0.22807, -0.3073, - -2.0623898970595143 + 4.220795410120072 ], [ -0.25, -0.35, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3391,57 +3391,57 @@ [ -0.03685, -0.03685, - -2.356194490192345 + 3.9269908169872414 ], [ -0.07371, -0.07371, - -2.356194490192345 + 3.9269908169872414 ], [ -0.11039, -0.11073, - -2.337028082892005 + 3.9461572242875813 ], [ -0.14581, -0.14896, - -2.299205064504967 + 3.9839802426746194 ], [ -0.17976, -0.1885, - -2.2613820461179293 + 4.021803261061657 ], [ -0.21219, -0.22929, - -2.2235590277308916 + 4.059626279448695 ], [ -0.24305, -0.27128, - -2.1857360093438536 + 4.097449297835732 ], [ -0.27231, -0.31441, - -2.147912990956816 + 4.13527231622277 ], [ -0.29991, -0.35862, - -2.110089972569778 + 4.173095334609808 ], [ -0.32582, -0.40383, - -2.0722669541827403 + 4.2109183529968455 ], [ -0.35, -0.45, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3458,17 +3458,17 @@ [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ], [ 0.0, 0.0, - -2.1953192129940238 + 4.0878660941855625 ], [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3485,47 +3485,47 @@ [ -0.02193, -0.0427, - -2.0623898970595143 + 4.220795410120072 ], [ -0.04536, -0.0846, - -2.099115471201118 + 4.184069835978468 ], [ -0.07032, -0.12561, - -2.1358410453427217 + 4.1473442618368646 ], [ -0.09676, -0.16568, - -2.172566619484326 + 4.11061868769526 ], [ -0.12466, -0.20474, - -2.2092921936259295 + 4.073893113553657 ], [ -0.15397, -0.24276, - -2.2460177677675333 + 4.037167539412053 ], [ -0.18466, -0.27967, - -2.2827433419091374 + 4.000441965270449 ], [ -0.21669, -0.31543, - -2.319468916050741 + 3.963716391128845 ], [ -0.25, -0.35, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3542,22 +3542,22 @@ [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.0, 0.0, - -2.1416941205945834 + 4.141491186585003 ], [ 0.0, 0.0, - -2.248944305393464 + 4.034241001786122 ], [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3574,37 +3574,37 @@ [ -0.02143, -0.04286, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.04286, -0.08571, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.06429, -0.12857, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.08571, -0.17143, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.10714, -0.21429, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.12857, -0.25714, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.15, -0.3, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3621,62 +3621,62 @@ [ -0.02148, -0.04296, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.04292, -0.08595, - -2.0243778514662925 + 4.258807455713294 ], [ -0.06298, -0.12959, - -1.979019698999153 + 4.304165608180433 ], [ -0.08104, -0.1741, - -1.9336615465320135 + 4.349523760647573 ], [ -0.09707, -0.21937, - -1.888303394064874 + 4.3948819131147125 ], [ -0.11102, -0.26533, - -1.8429452415977343 + 4.440240065581852 ], [ -0.12288, -0.31188, - -1.7975870891305947 + 4.485598218048992 ], [ -0.13262, -0.35891, - -1.752228936663455 + 4.530956370516131 ], [ -0.14021, -0.40634, - -1.7068707841963153 + 4.576314522983271 ], [ -0.14565, -0.45406, - -1.6615126317291757 + 4.6216726754504105 ], [ -0.14891, -0.50198, - -1.6161544792620361 + 4.66703082791755 ], [ -0.15, -0.55, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3693,22 +3693,22 @@ [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.0, 0.0, - -1.879894732795434 + 4.403290574384152 ], [ 0.0, 0.0, - -1.7253455297951652 + 4.557839777384421 ], [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3725,62 +3725,62 @@ [ -0.00109, -0.04802, - -1.6161544792620361 + 4.66703082791755 ], [ -0.00435, -0.09594, - -1.6615126317291757 + 4.6216726754504105 ], [ -0.00979, -0.14366, - -1.7068707841963153 + 4.576314522983271 ], [ -0.01738, -0.19109, - -1.752228936663455 + 4.530956370516131 ], [ -0.02712, -0.23812, - -1.7975870891305945 + 4.485598218048992 ], [ -0.03898, -0.28467, - -1.842945241597734 + 4.440240065581852 ], [ -0.05293, -0.33063, - -1.8883033940648737 + 4.3948819131147125 ], [ -0.06896, -0.3759, - -1.9336615465320133 + 4.349523760647573 ], [ -0.08702, -0.42041, - -1.979019698999153 + 4.304165608180433 ], [ -0.10708, -0.46405, - -2.0243778514662925 + 4.258807455713294 ], [ -0.12852, -0.50704, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.15, -0.55, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3797,67 +3797,67 @@ [ -0.00049, -0.05179, - -1.596982710812605 + 4.686202596366981 ], [ -0.00279, -0.10353, - -1.6334378128945297 + 4.649747494285057 ], [ -0.00697, -0.15516, - -1.6698929149764545 + 4.613292392203132 ], [ -0.01303, -0.2066, - -1.7063480170583794 + 4.576837290121206 ], [ -0.02097, -0.25778, - -1.7428031191403042 + 4.540382188039282 ], [ -0.03076, -0.30864, - -1.779258221222229 + 4.503927085957358 ], [ -0.0424, -0.3591, - -1.8157133233041538 + 4.467471983875432 ], [ -0.05587, -0.40911, - -1.8521684253860786 + 4.431016881793507 ], [ -0.07116, -0.4586, - -1.8886235274680034 + 4.394561779711583 ], [ -0.08824, -0.5075, - -1.9250786295499283 + 4.358106677629658 ], [ -0.10709, -0.55574, - -1.961533731631853 + 4.321651575547733 ], [ -0.12769, -0.60326, - -1.997988833713778 + 4.285196473465808 ], [ -0.15, -0.65, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3874,22 +3874,22 @@ [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, 0.0, - -1.7253455297951652 + 4.557839777384421 ], [ 0.0, 0.0, - -1.879894732795434 + 4.403290574384152 ], [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -3906,32 +3906,32 @@ [ 0.0, -0.05, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.1, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.15, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.2, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.25, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.3, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -3948,62 +3948,62 @@ [ 0.00109, -0.04802, - -1.525438174327757 + 4.757747132851829 ], [ 0.00435, -0.09594, - -1.4800800218606174 + 4.803105285318969 ], [ 0.00979, -0.14366, - -1.4347218693934778 + 4.848463437786108 ], [ 0.01738, -0.19109, - -1.3893637169263382 + 4.893821590253248 ], [ 0.02712, -0.23812, - -1.3440055644591986 + 4.939179742720388 ], [ 0.03898, -0.28467, - -1.298647411992059 + 4.984537895187527 ], [ 0.05293, -0.33063, - -1.2532892595249194 + 5.029896047654667 ], [ 0.06896, -0.3759, - -1.2079311070577798 + 5.075254200121806 ], [ 0.08702, -0.42041, - -1.16257295459064 + 5.120612352588946 ], [ 0.10708, -0.46405, - -1.1172148021235004 + 5.165970505056086 ], [ 0.12852, -0.50704, - -1.1071487177940904 + 5.176036589385496 ], [ 0.15, -0.55, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4020,67 +4020,67 @@ [ 0.00049, -0.05179, - -1.5446099427771882 + 4.738575364402398 ], [ 0.00279, -0.10353, - -1.5081548406952634 + 4.775030466484322 ], [ 0.00697, -0.15516, - -1.4716997386133386 + 4.811485568566248 ], [ 0.01303, -0.2066, - -1.4352446365314138 + 4.847940670648173 ], [ 0.02097, -0.25778, - -1.398789534449489 + 4.884395772730097 ], [ 0.03076, -0.30864, - -1.3623344323675641 + 4.920850874812022 ], [ 0.0424, -0.3591, - -1.3258793302856393 + 4.957305976893947 ], [ 0.05587, -0.40911, - -1.2894242282037145 + 4.993761078975872 ], [ 0.07116, -0.4586, - -1.2529691261217897 + 5.0302161810577966 ], [ 0.08824, -0.5075, - -1.2165140240398649 + 5.066671283139721 ], [ 0.10709, -0.55574, - -1.18005892195794 + 5.103126385221646 ], [ 0.12769, -0.60326, - -1.1436038198760152 + 5.1395814873035714 ], [ 0.15, -0.65, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4097,22 +4097,22 @@ [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, 0.0, - -1.416247123794628 + 4.866938183384958 ], [ 0.0, 0.0, - -1.261697920794359 + 5.021487386385227 ], [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4129,62 +4129,62 @@ [ 0.02148, -0.04296, - -1.1071487177940904 + 5.176036589385496 ], [ 0.04292, -0.08595, - -1.1172148021235004 + 5.165970505056086 ], [ 0.06298, -0.12959, - -1.16257295459064 + 5.120612352588946 ], [ 0.08104, -0.1741, - -1.2079311070577796 + 5.075254200121806 ], [ 0.09707, -0.21937, - -1.2532892595249192 + 5.029896047654667 ], [ 0.11102, -0.26533, - -1.298647411992059 + 4.984537895187527 ], [ 0.12288, -0.31188, - -1.3440055644591986 + 4.939179742720388 ], [ 0.13262, -0.35891, - -1.3893637169263382 + 4.893821590253248 ], [ 0.14021, -0.40634, - -1.4347218693934778 + 4.848463437786108 ], [ 0.14565, -0.45406, - -1.4800800218606174 + 4.803105285318969 ], [ 0.14891, -0.50198, - -1.525438174327757 + 4.757747132851829 ], [ 0.15, -0.55, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4201,17 +4201,17 @@ [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ], [ 0.0, 0.0, - -1.3389725222944935 + 4.944212784885092 ], [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4228,37 +4228,37 @@ [ 0.02143, -0.04286, - -1.1071487177940904 + 5.176036589385496 ], [ 0.04286, -0.08571, - -1.1071487177940904 + 5.176036589385496 ], [ 0.06429, -0.12857, - -1.1071487177940904 + 5.176036589385496 ], [ 0.08571, -0.17143, - -1.1071487177940904 + 5.176036589385496 ], [ 0.10714, -0.21429, - -1.1071487177940904 + 5.176036589385496 ], [ 0.12857, -0.25714, - -1.1071487177940904 + 5.176036589385496 ], [ 0.15, -0.3, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4275,47 +4275,47 @@ [ 0.02193, -0.0427, - -1.079202756530279 + 5.203982550649307 ], [ 0.04536, -0.0846, - -1.0424771823886751 + 5.2407081247909115 ], [ 0.07032, -0.12561, - -1.0057516082470712 + 5.277433698932515 ], [ 0.09676, -0.16568, - -0.9690260341054675 + 5.314159273074119 ], [ 0.12466, -0.20474, - -0.9323004599638636 + 5.350884847215722 ], [ 0.15397, -0.24276, - -0.8955748858222597 + 5.387610421357326 ], [ 0.18466, -0.27967, - -0.8588493116806559 + 5.4243359954989305 ], [ 0.21669, -0.31543, - -0.8221237375390521 + 5.461061569640534 ], [ 0.25, -0.35, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -4332,17 +4332,17 @@ [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ], [ 0.0, 0.0, - -0.9462734405957693 + 5.336911866583817 ], [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -4359,47 +4359,47 @@ [ 0.03331, -0.03457, - -0.8221237375390521 + 5.461061569640534 ], [ 0.06534, -0.07033, - -0.8588493116806559 + 5.4243359954989305 ], [ 0.09603, -0.10724, - -0.8955748858222599 + 5.387610421357326 ], [ 0.12534, -0.14526, - -0.9323004599638637 + 5.350884847215722 ], [ 0.15324, -0.18432, - -0.9690260341054675 + 5.314159273074119 ], [ 0.17968, -0.22439, - -1.0057516082470714 + 5.277433698932515 ], [ 0.20464, -0.2654, - -1.0424771823886751 + 5.2407081247909115 ], [ 0.22807, -0.3073, - -1.079202756530279 + 5.203982550649307 ], [ 0.25, -0.35, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4416,57 +4416,57 @@ [ 0.03685, -0.03685, - -0.7853981633974483 + 5.497787143782138 ], [ 0.07371, -0.07371, - -0.7853981633974483 + 5.497787143782138 ], [ 0.11039, -0.11073, - -0.8045645706977883 + 5.478620736481798 ], [ 0.14581, -0.14896, - -0.8423875890848261 + 5.44079771809476 ], [ 0.17976, -0.1885, - -0.8802106074718639 + 5.402974699707722 ], [ 0.21219, -0.22929, - -0.9180336258589017 + 5.365151681320684 ], [ 0.24305, -0.27128, - -0.9558566442459394 + 5.327328662933647 ], [ 0.27231, -0.31441, - -0.9936796626329771 + 5.289505644546609 ], [ 0.29991, -0.35862, - -1.031502681020015 + 5.251682626159571 ], [ 0.32582, -0.40383, - -1.0693256994070528 + 5.213859607772534 ], [ 0.35, -0.45, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4483,17 +4483,17 @@ [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ], [ 0.0, 0.0, - -0.9462734405957693 + 5.336911866583817 ], [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -4510,42 +4510,42 @@ [ 0.0375, -0.0375, - -0.7853981633974483 + 5.497787143782138 ], [ 0.075, -0.075, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1125, -0.1125, - -0.7853981633974483 + 5.497787143782138 ], [ 0.15, -0.15, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1875, -0.1875, - -0.7853981633974483 + 5.497787143782138 ], [ 0.225, -0.225, - -0.7853981633974483 + 5.497787143782138 ], [ 0.2625, -0.2625, - -0.7853981633974483 + 5.497787143782138 ], [ 0.3, -0.3, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -4562,47 +4562,47 @@ [ 0.03457, -0.03331, - -0.7486725892558445 + 5.534512717923742 ], [ 0.07033, -0.06534, - -0.7119470151142406 + 5.571238292065345 ], [ 0.10724, -0.09603, - -0.6752214409726367 + 5.6079638662069495 ], [ 0.14526, -0.12534, - -0.6384958668310329 + 5.644689440348554 ], [ 0.18432, -0.15324, - -0.601770292689429 + 5.681415014490157 ], [ 0.22439, -0.17968, - -0.5650447185478252 + 5.718140588631761 ], [ 0.2654, -0.20464, - -0.5283191444062213 + 5.754866162773365 ], [ 0.3073, -0.22807, - -0.4915935702646175 + 5.7915917369149685 ], [ 0.35, -0.25, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -4619,57 +4619,57 @@ [ 0.03685, -0.03685, - -0.7853981633974483 + 5.497787143782138 ], [ 0.07371, -0.07371, - -0.7853981633974483 + 5.497787143782138 ], [ 0.11073, -0.11039, - -0.7662317560971083 + 5.516953551082478 ], [ 0.14896, -0.14581, - -0.7284087377100704 + 5.5547765694695155 ], [ 0.1885, -0.17976, - -0.6905857193230327 + 5.5925995878565535 ], [ 0.22929, -0.21219, - -0.6527627009359949 + 5.630422606243592 ], [ 0.27128, -0.24305, - -0.614939682548957 + 5.668245624630629 ], [ 0.31441, -0.27231, - -0.5771166641619194 + 5.706068643017667 ], [ 0.35862, -0.29991, - -0.5392936457748816 + 5.743891661404705 ], [ 0.40383, -0.32582, - -0.5014706273878439 + 5.781714679791742 ], [ 0.45, -0.35, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -4686,17 +4686,17 @@ [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ], [ 0.0, 0.0, - -0.6245228861991272 + 5.658662420980459 ], [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ] ] }, @@ -4713,47 +4713,47 @@ [ 0.0427, -0.02193, - -0.4915935702646175 + 5.7915917369149685 ], [ 0.0846, -0.04536, - -0.5283191444062214 + 5.754866162773364 ], [ 0.12561, -0.07032, - -0.5650447185478252 + 5.718140588631761 ], [ 0.16568, -0.09676, - -0.601770292689429 + 5.681415014490157 ], [ 0.20474, -0.12466, - -0.6384958668310329 + 5.644689440348554 ], [ 0.24276, -0.15397, - -0.6752214409726367 + 5.6079638662069495 ], [ 0.27967, -0.18466, - -0.7119470151142406 + 5.571238292065345 ], [ 0.31543, -0.21669, - -0.7486725892558445 + 5.534512717923742 ], [ 0.35, -0.25, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -4770,22 +4770,22 @@ [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ], [ 0.0, 0.0, - -0.5708977937996869 + 5.712287513379899 ], [ 0.0, 0.0, - -0.6781479785985676 + 5.605037328581019 ], [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -4802,37 +4802,37 @@ [ 0.04286, -0.02143, - -0.4636476090008061 + 5.81953769817878 ], [ 0.08571, -0.04286, - -0.4636476090008061 + 5.81953769817878 ], [ 0.12857, -0.06429, - -0.4636476090008061 + 5.81953769817878 ], [ 0.17143, -0.08571, - -0.4636476090008061 + 5.81953769817878 ], [ 0.21429, -0.10714, - -0.4636476090008061 + 5.81953769817878 ], [ 0.25714, -0.12857, - -0.4636476090008061 + 5.81953769817878 ], [ 0.3, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -4849,57 +4849,57 @@ [ 0.04296, -0.02148, - -0.4636476090008061 + 5.81953769817878 ], [ 0.08595, -0.04292, - -0.453581524671396 + 5.82960378250819 ], [ 0.12959, -0.06298, - -0.4082233722042564 + 5.87496193497533 ], [ 0.1741, -0.08104, - -0.36286521973711683 + 5.9203200874424695 ], [ 0.21937, -0.09707, - -0.31750706726997724 + 5.965678239909609 ], [ 0.26533, -0.11102, - -0.27214891480283765 + 6.011036392376749 ], [ 0.31188, -0.12288, - -0.22679076233569806 + 6.056394544843888 ], [ 0.35891, -0.13262, - -0.18143260986855847 + 6.101752697311028 ], [ 0.40634, -0.14021, - -0.13607445740141882 + 6.1471108497781675 ], [ 0.45406, -0.14565, - -0.09071630493427918 + 6.192469002245307 ], [ 0.50198, -0.14891, - -0.04535815246713959 + 6.237827154712447 ], [ 0.55, @@ -4921,17 +4921,17 @@ [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ], [ 0.0, 0.0, - -0.3090984060005373 + 5.9740869011790485 ], [ 0.0, 0.0, - -0.15454920300026886 + 6.128636104179318 ], [ 0.0, diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json index 69cd72391b7..95f8dc2d85f 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json @@ -1,6 +1,6 @@ { "version": 1.0, - "date_generated": "2022-01-19", + "date_generated": "2022-03-17", "lattice_metadata": { "motion_model": "omni", "turning_radius": 1, @@ -41,62 +41,62 @@ [ 0.04802, -0.00109, - -0.045358152467139604 + 6.237827154712447 ], [ 0.09594, -0.00435, - -0.09071630493427921 + 6.192469002245307 ], [ 0.14366, -0.00979, - -0.13607445740141882 + 6.1471108497781675 ], [ 0.19109, -0.01738, - -0.18143260986855841 + 6.101752697311028 ], [ 0.23812, -0.02712, - -0.226790762335698 + 6.056394544843888 ], [ 0.28467, -0.03898, - -0.2721489148028376 + 6.011036392376749 ], [ 0.33063, -0.05293, - -0.3175070672699772 + 5.965678239909609 ], [ 0.3759, -0.06896, - -0.36286521973711683 + 5.9203200874424695 ], [ 0.42041, -0.08702, - -0.4082233722042565 + 5.87496193497533 ], [ 0.46405, -0.10708, - -0.45358152467139606 + 5.82960378250819 ], [ 0.50704, -0.12852, - -0.4636476090008061 + 5.81953769817878 ], [ 0.55, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -113,67 +113,67 @@ [ 0.05179, -0.00049, - -0.026186384017708383 + 6.256998923161878 ], [ 0.10353, -0.00279, - -0.0626414860996332 + 6.220543821079953 ], [ 0.15516, -0.00697, - -0.09909658818155803 + 6.184088718998028 ], [ 0.2066, -0.01303, - -0.13555169026348282 + 6.147633616916103 ], [ 0.25778, -0.02097, - -0.17200679234540764 + 6.111178514834179 ], [ 0.30864, -0.03076, - -0.20846189442733246 + 6.074723412752253 ], [ 0.3591, -0.0424, - -0.24491699650925727 + 6.038268310670329 ], [ 0.40911, -0.05587, - -0.28137209859118206 + 6.001813208588404 ], [ 0.4586, -0.07116, - -0.3178272006731068 + 5.965358106506479 ], [ 0.5075, -0.08824, - -0.35428230275503164 + 5.928903004424555 ], [ 0.55574, -0.10709, - -0.3907374048369565 + 5.89244790234263 ], [ 0.60326, -0.12769, - -0.4271925069188813 + 5.855992800260705 ], [ 0.65, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -195,17 +195,17 @@ [ 0.0, 0.0, - -0.15454920300026886 + 6.128636104179318 ], [ 0.0, 0.0, - -0.3090984060005373 + 5.9740869011790485 ], [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ] ] }, @@ -502,17 +502,17 @@ [ 0.0, 0.0, - 0.15454920300026886 + 0.1545492030002687 ], [ 0.0, 0.0, - 0.3090984060005373 + 0.3090984060005374 ], [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ] ] }, @@ -601,12 +601,12 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ], [ 0.0, 0.0, - 0.23182380450040307 + 0.23182380450040305 ], [ 0.0, @@ -826,7 +826,7 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ], [ 0.0, @@ -987,7 +987,7 @@ [ 0.0, 0.0, - 0.46364760900080615 + 0.4636476090008061 ] ] }, @@ -1618,12 +1618,12 @@ [ 0.0, 0.0, - 1.2616979207943588 + 1.261697920794359 ], [ 0.0, 0.0, - 1.4162471237946281 + 1.416247123794628 ], [ 0.0, @@ -1799,12 +1799,12 @@ [ 0.0, 0.0, - 1.4162471237946281 + 1.416247123794628 ], [ 0.0, 0.0, - 1.2616979207943588 + 1.261697920794359 ], [ 0.0, @@ -2106,12 +2106,12 @@ [ 0.0, 0.0, - 1.725345529795165 + 1.7253455297951652 ], [ 0.0, 0.0, - 1.8798947327954343 + 1.879894732795434 ], [ 0.0, @@ -2210,7 +2210,7 @@ [ 0.0, 0.0, - 1.8026201312952992 + 1.8026201312952996 ], [ 0.0, @@ -3222,17 +3222,17 @@ [ 0.0, 0.0, - 2.8324942475892563 + 2.832494247589256 ], [ 0.0, 0.0, - 2.9870434505895247 + 2.9870434505895243 ], [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ] ] }, @@ -3299,12 +3299,12 @@ [ -0.50704, -0.12852, - -2.677945044588987 + 3.6052402625905993 ], [ -0.55, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3381,7 +3381,7 @@ [ -0.65, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3398,22 +3398,22 @@ [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ], [ 0.0, 0.0, - -2.9870434505895247 + 3.296141856590062 ], [ 0.0, 0.0, - -2.8324942475892563 + 3.4506910595903304 ], [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3579,17 +3579,17 @@ [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ], [ 0.0, 0.0, - 2.9870434505895247 + 2.9870434505895243 ], [ 0.0, 0.0, - 2.8324942475892563 + 2.832494247589256 ], [ 0.0, @@ -3737,37 +3737,37 @@ [ -0.04286, -0.02143, - -2.677945044588987 + 3.6052402625905993 ], [ -0.08571, -0.04286, - -2.677945044588987 + 3.6052402625905993 ], [ -0.12857, -0.06429, - -2.677945044588987 + 3.6052402625905993 ], [ -0.17143, -0.08571, - -2.677945044588987 + 3.6052402625905993 ], [ -0.21429, -0.10714, - -2.677945044588987 + 3.6052402625905993 ], [ -0.25714, -0.12857, - -2.677945044588987 + 3.6052402625905993 ], [ -0.3, -0.15, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3784,37 +3784,37 @@ [ 0.02143, -0.04286, - -2.677945044588987 + 3.6052402625905993 ], [ 0.04286, -0.08571, - -2.677945044588987 + 3.6052402625905993 ], [ 0.06429, -0.12857, - -2.677945044588987 + 3.6052402625905993 ], [ 0.08571, -0.17143, - -2.677945044588987 + 3.6052402625905993 ], [ 0.10714, -0.21429, - -2.677945044588987 + 3.6052402625905993 ], [ 0.12857, -0.25714, - -2.677945044588987 + 3.6052402625905993 ], [ 0.15, -0.3, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3831,37 +3831,37 @@ [ -0.02143, 0.04286, - -2.677945044588987 + 3.6052402625905993 ], [ -0.04286, 0.08571, - -2.677945044588987 + 3.6052402625905993 ], [ -0.06429, 0.12857, - -2.677945044588987 + 3.6052402625905993 ], [ -0.08571, 0.17143, - -2.677945044588987 + 3.6052402625905993 ], [ -0.10714, 0.21429, - -2.677945044588987 + 3.6052402625905993 ], [ -0.12857, 0.25714, - -2.677945044588987 + 3.6052402625905993 ], [ -0.15, 0.3, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -3878,47 +3878,47 @@ [ -0.0427, -0.02193, - -2.6499990833251754 + 3.633186223854411 ], [ -0.0846, -0.04536, - -2.6132735091835717 + 3.6699117979960145 ], [ -0.12561, -0.07032, - -2.576547935041968 + 3.7066373721376182 ], [ -0.16568, -0.09676, - -2.539822360900364 + 3.7433629462792224 ], [ -0.20474, -0.12466, - -2.50309678675876 + 3.780088520420826 ], [ -0.24276, -0.15397, - -2.4663712126171564 + 3.81681409456243 ], [ -0.27967, -0.18466, - -2.4296456384755523 + 3.853539668704034 ], [ -0.31543, -0.21669, - -2.3929200643339485 + 3.8902652428456377 ], [ -0.35, -0.25, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3935,17 +3935,17 @@ [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ], [ 0.0, 0.0, - -2.517069767390666 + 3.7661155397889203 ], [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -3962,57 +3962,57 @@ [ -0.04296, -0.02148, - -2.677945044588987 + 3.6052402625905993 ], [ -0.08595, -0.04292, - -2.688011128918397 + 3.595174178261189 ], [ -0.12959, -0.06298, - -2.733369281385537 + 3.5498160257940494 ], [ -0.1741, -0.08104, - -2.7787274338526764 + 3.50445787332691 ], [ -0.21937, -0.09707, - -2.824085586319816 + 3.4590997208597702 ], [ -0.26533, -0.11102, - -2.8694437387869556 + 3.4137415683926307 ], [ -0.31188, -0.12288, - -2.914801891254095 + 3.368383415925491 ], [ -0.35891, -0.13262, - -2.9601600437212348 + 3.3230252634583515 ], [ -0.40634, -0.14021, - -3.0055181961883743 + 3.277667110991212 ], [ -0.45406, -0.14565, - -3.050876348655514 + 3.2323089585240723 ], [ -0.50198, -0.14891, - -3.0962345011226535 + 3.1869508060569327 ], [ -0.55, @@ -4034,17 +4034,17 @@ [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ], [ 0.0, 0.0, - -2.90976884908939 + 3.373416458090196 ], [ 0.0, 0.0, - -3.141592653589793 + 3.141592653589793 ] ] }, @@ -4061,47 +4061,47 @@ [ -0.03457, -0.03331, - -2.3929200643339485 + 3.8902652428456377 ], [ -0.07033, -0.06534, - -2.4296456384755527 + 3.8535396687040335 ], [ -0.10724, -0.09603, - -2.4663712126171564 + 3.81681409456243 ], [ -0.14526, -0.12534, - -2.50309678675876 + 3.780088520420826 ], [ -0.18432, -0.15324, - -2.539822360900364 + 3.7433629462792224 ], [ -0.22439, -0.17968, - -2.576547935041968 + 3.7066373721376182 ], [ -0.2654, -0.20464, - -2.6132735091835717 + 3.6699117979960145 ], [ -0.3073, -0.22807, - -2.6499990833251754 + 3.633186223854411 ], [ -0.35, -0.25, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -4118,57 +4118,57 @@ [ -0.03685, -0.03685, - -2.356194490192345 + 3.9269908169872414 ], [ -0.07371, -0.07371, - -2.356194490192345 + 3.9269908169872414 ], [ -0.11073, -0.11039, - -2.3753608974926848 + 3.9078244096869015 ], [ -0.14896, -0.14581, - -2.413183915879723 + 3.8700013912998634 ], [ -0.1885, -0.17976, - -2.4510069342667604 + 3.832178372912826 ], [ -0.22929, -0.21219, - -2.488829952653798 + 3.794355354525788 ], [ -0.27128, -0.24305, - -2.526652971040836 + 3.75653233613875 ], [ -0.31441, -0.27231, - -2.5644759894278737 + 3.7187093177517125 ], [ -0.35862, -0.29991, - -2.6022990078149117 + 3.6808862993646745 ], [ -0.40383, -0.32582, - -2.6401220262019494 + 3.643063280977637 ], [ -0.45, -0.35, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -4185,17 +4185,17 @@ [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ], [ 0.0, 0.0, - -2.517069767390666 + 3.7661155397889203 ], [ 0.0, 0.0, - -2.677945044588987 + 3.6052402625905993 ] ] }, @@ -4212,42 +4212,42 @@ [ -0.0375, -0.0375, - -2.356194490192345 + 3.9269908169872414 ], [ -0.075, -0.075, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1125, -0.1125, - -2.356194490192345 + 3.9269908169872414 ], [ -0.15, -0.15, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1875, -0.1875, - -2.356194490192345 + 3.9269908169872414 ], [ -0.225, -0.225, - -2.356194490192345 + 3.9269908169872414 ], [ -0.2625, -0.2625, - -2.356194490192345 + 3.9269908169872414 ], [ -0.3, -0.3, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -4264,42 +4264,42 @@ [ 0.0375, -0.0375, - -2.356194490192345 + 3.9269908169872414 ], [ 0.075, -0.075, - -2.356194490192345 + 3.9269908169872414 ], [ 0.1125, -0.1125, - -2.356194490192345 + 3.9269908169872414 ], [ 0.15, -0.15, - -2.356194490192345 + 3.9269908169872414 ], [ 0.1875, -0.1875, - -2.356194490192345 + 3.9269908169872414 ], [ 0.225, -0.225, - -2.356194490192345 + 3.9269908169872414 ], [ 0.2625, -0.2625, - -2.356194490192345 + 3.9269908169872414 ], [ 0.3, -0.3, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -4316,42 +4316,42 @@ [ -0.0375, 0.0375, - -2.356194490192345 + 3.9269908169872414 ], [ -0.075, 0.075, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1125, 0.1125, - -2.356194490192345 + 3.9269908169872414 ], [ -0.15, 0.15, - -2.356194490192345 + 3.9269908169872414 ], [ -0.1875, 0.1875, - -2.356194490192345 + 3.9269908169872414 ], [ -0.225, 0.225, - -2.356194490192345 + 3.9269908169872414 ], [ -0.2625, 0.2625, - -2.356194490192345 + 3.9269908169872414 ], [ -0.3, 0.3, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -4368,47 +4368,47 @@ [ -0.03331, -0.03457, - -2.319468916050741 + 3.963716391128845 ], [ -0.06534, -0.07033, - -2.282743341909137 + 4.000441965270449 ], [ -0.09603, -0.10724, - -2.2460177677675333 + 4.037167539412053 ], [ -0.12534, -0.14526, - -2.2092921936259295 + 4.073893113553657 ], [ -0.15324, -0.18432, - -2.172566619484326 + 4.11061868769526 ], [ -0.17968, -0.22439, - -2.1358410453427217 + 4.1473442618368646 ], [ -0.20464, -0.2654, - -2.099115471201118 + 4.184069835978468 ], [ -0.22807, -0.3073, - -2.0623898970595143 + 4.220795410120072 ], [ -0.25, -0.35, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4425,57 +4425,57 @@ [ -0.03685, -0.03685, - -2.356194490192345 + 3.9269908169872414 ], [ -0.07371, -0.07371, - -2.356194490192345 + 3.9269908169872414 ], [ -0.11039, -0.11073, - -2.337028082892005 + 3.9461572242875813 ], [ -0.14581, -0.14896, - -2.299205064504967 + 3.9839802426746194 ], [ -0.17976, -0.1885, - -2.2613820461179293 + 4.021803261061657 ], [ -0.21219, -0.22929, - -2.2235590277308916 + 4.059626279448695 ], [ -0.24305, -0.27128, - -2.1857360093438536 + 4.097449297835732 ], [ -0.27231, -0.31441, - -2.147912990956816 + 4.13527231622277 ], [ -0.29991, -0.35862, - -2.110089972569778 + 4.173095334609808 ], [ -0.32582, -0.40383, - -2.0722669541827403 + 4.2109183529968455 ], [ -0.35, -0.45, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4492,17 +4492,17 @@ [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ], [ 0.0, 0.0, - -2.1953192129940238 + 4.0878660941855625 ], [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4519,47 +4519,47 @@ [ -0.02193, -0.0427, - -2.0623898970595143 + 4.220795410120072 ], [ -0.04536, -0.0846, - -2.099115471201118 + 4.184069835978468 ], [ -0.07032, -0.12561, - -2.1358410453427217 + 4.1473442618368646 ], [ -0.09676, -0.16568, - -2.172566619484326 + 4.11061868769526 ], [ -0.12466, -0.20474, - -2.2092921936259295 + 4.073893113553657 ], [ -0.15397, -0.24276, - -2.2460177677675333 + 4.037167539412053 ], [ -0.18466, -0.27967, - -2.2827433419091374 + 4.000441965270449 ], [ -0.21669, -0.31543, - -2.319468916050741 + 3.963716391128845 ], [ -0.25, -0.35, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -4576,22 +4576,22 @@ [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.0, 0.0, - -2.1416941205945834 + 4.141491186585003 ], [ 0.0, 0.0, - -2.248944305393464 + 4.034241001786122 ], [ 0.0, 0.0, - -2.356194490192345 + 3.9269908169872414 ] ] }, @@ -4608,37 +4608,37 @@ [ -0.02143, -0.04286, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.04286, -0.08571, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.06429, -0.12857, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.08571, -0.17143, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.10714, -0.21429, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.12857, -0.25714, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.15, -0.3, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4655,37 +4655,37 @@ [ 0.04286, -0.02143, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.08571, -0.04286, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.12857, -0.06429, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.17143, -0.08571, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.21429, -0.10714, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.25714, -0.12857, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.3, -0.15, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4702,37 +4702,37 @@ [ -0.04286, 0.02143, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.08571, 0.04286, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.12857, 0.06429, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.17143, 0.08571, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.21429, 0.10714, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.25714, 0.12857, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.3, 0.15, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4749,62 +4749,62 @@ [ -0.02148, -0.04296, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.04292, -0.08595, - -2.0243778514662925 + 4.258807455713294 ], [ -0.06298, -0.12959, - -1.979019698999153 + 4.304165608180433 ], [ -0.08104, -0.1741, - -1.9336615465320135 + 4.349523760647573 ], [ -0.09707, -0.21937, - -1.888303394064874 + 4.3948819131147125 ], [ -0.11102, -0.26533, - -1.8429452415977343 + 4.440240065581852 ], [ -0.12288, -0.31188, - -1.7975870891305947 + 4.485598218048992 ], [ -0.13262, -0.35891, - -1.752228936663455 + 4.530956370516131 ], [ -0.14021, -0.40634, - -1.7068707841963153 + 4.576314522983271 ], [ -0.14565, -0.45406, - -1.6615126317291757 + 4.6216726754504105 ], [ -0.14891, -0.50198, - -1.6161544792620361 + 4.66703082791755 ], [ -0.15, -0.55, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4821,22 +4821,22 @@ [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ], [ 0.0, 0.0, - -1.879894732795434 + 4.403290574384152 ], [ 0.0, 0.0, - -1.7253455297951652 + 4.557839777384421 ], [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -4853,62 +4853,62 @@ [ -0.00109, -0.04802, - -1.6161544792620361 + 4.66703082791755 ], [ -0.00435, -0.09594, - -1.6615126317291757 + 4.6216726754504105 ], [ -0.00979, -0.14366, - -1.7068707841963153 + 4.576314522983271 ], [ -0.01738, -0.19109, - -1.752228936663455 + 4.530956370516131 ], [ -0.02712, -0.23812, - -1.7975870891305945 + 4.485598218048992 ], [ -0.03898, -0.28467, - -1.842945241597734 + 4.440240065581852 ], [ -0.05293, -0.33063, - -1.8883033940648737 + 4.3948819131147125 ], [ -0.06896, -0.3759, - -1.9336615465320133 + 4.349523760647573 ], [ -0.08702, -0.42041, - -1.979019698999153 + 4.304165608180433 ], [ -0.10708, -0.46405, - -2.0243778514662925 + 4.258807455713294 ], [ -0.12852, -0.50704, - -2.0344439357957027 + 4.2487413713838835 ], [ -0.15, -0.55, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -4925,67 +4925,67 @@ [ -0.00049, -0.05179, - -1.596982710812605 + 4.686202596366981 ], [ -0.00279, -0.10353, - -1.6334378128945297 + 4.649747494285057 ], [ -0.00697, -0.15516, - -1.6698929149764545 + 4.613292392203132 ], [ -0.01303, -0.2066, - -1.7063480170583794 + 4.576837290121206 ], [ -0.02097, -0.25778, - -1.7428031191403042 + 4.540382188039282 ], [ -0.03076, -0.30864, - -1.779258221222229 + 4.503927085957358 ], [ -0.0424, -0.3591, - -1.8157133233041538 + 4.467471983875432 ], [ -0.05587, -0.40911, - -1.8521684253860786 + 4.431016881793507 ], [ -0.07116, -0.4586, - -1.8886235274680034 + 4.394561779711583 ], [ -0.08824, -0.5075, - -1.9250786295499283 + 4.358106677629658 ], [ -0.10709, -0.55574, - -1.961533731631853 + 4.321651575547733 ], [ -0.12769, -0.60326, - -1.997988833713778 + 4.285196473465808 ], [ -0.15, -0.65, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -5002,22 +5002,22 @@ [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, 0.0, - -1.7253455297951652 + 4.557839777384421 ], [ 0.0, 0.0, - -1.879894732795434 + 4.403290574384152 ], [ 0.0, 0.0, - -2.0344439357957027 + 4.2487413713838835 ] ] }, @@ -5034,32 +5034,32 @@ [ 0.0, -0.05, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.1, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.15, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.2, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.25, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, -0.3, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -5076,32 +5076,32 @@ [ 0.05, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.1, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.15, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.2, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.25, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.3, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -5118,32 +5118,32 @@ [ -0.05, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ -0.1, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ -0.15, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ -0.2, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ -0.25, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ -0.3, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -5160,62 +5160,62 @@ [ 0.00109, -0.04802, - -1.525438174327757 + 4.757747132851829 ], [ 0.00435, -0.09594, - -1.4800800218606174 + 4.803105285318969 ], [ 0.00979, -0.14366, - -1.4347218693934778 + 4.848463437786108 ], [ 0.01738, -0.19109, - -1.3893637169263382 + 4.893821590253248 ], [ 0.02712, -0.23812, - -1.3440055644591986 + 4.939179742720388 ], [ 0.03898, -0.28467, - -1.298647411992059 + 4.984537895187527 ], [ 0.05293, -0.33063, - -1.2532892595249194 + 5.029896047654667 ], [ 0.06896, -0.3759, - -1.2079311070577798 + 5.075254200121806 ], [ 0.08702, -0.42041, - -1.16257295459064 + 5.120612352588946 ], [ 0.10708, -0.46405, - -1.1172148021235004 + 5.165970505056086 ], [ 0.12852, -0.50704, - -1.1071487177940904 + 5.176036589385496 ], [ 0.15, -0.55, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5232,67 +5232,67 @@ [ 0.00049, -0.05179, - -1.5446099427771882 + 4.738575364402398 ], [ 0.00279, -0.10353, - -1.5081548406952634 + 4.775030466484322 ], [ 0.00697, -0.15516, - -1.4716997386133386 + 4.811485568566248 ], [ 0.01303, -0.2066, - -1.4352446365314138 + 4.847940670648173 ], [ 0.02097, -0.25778, - -1.398789534449489 + 4.884395772730097 ], [ 0.03076, -0.30864, - -1.3623344323675641 + 4.920850874812022 ], [ 0.0424, -0.3591, - -1.3258793302856393 + 4.957305976893947 ], [ 0.05587, -0.40911, - -1.2894242282037145 + 4.993761078975872 ], [ 0.07116, -0.4586, - -1.2529691261217897 + 5.0302161810577966 ], [ 0.08824, -0.5075, - -1.2165140240398649 + 5.066671283139721 ], [ 0.10709, -0.55574, - -1.18005892195794 + 5.103126385221646 ], [ 0.12769, -0.60326, - -1.1436038198760152 + 5.1395814873035714 ], [ 0.15, -0.65, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5309,22 +5309,22 @@ [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ], [ 0.0, 0.0, - -1.416247123794628 + 4.866938183384958 ], [ 0.0, 0.0, - -1.261697920794359 + 5.021487386385227 ], [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5341,62 +5341,62 @@ [ 0.02148, -0.04296, - -1.1071487177940904 + 5.176036589385496 ], [ 0.04292, -0.08595, - -1.1172148021235004 + 5.165970505056086 ], [ 0.06298, -0.12959, - -1.16257295459064 + 5.120612352588946 ], [ 0.08104, -0.1741, - -1.2079311070577796 + 5.075254200121806 ], [ 0.09707, -0.21937, - -1.2532892595249192 + 5.029896047654667 ], [ 0.11102, -0.26533, - -1.298647411992059 + 4.984537895187527 ], [ 0.12288, -0.31188, - -1.3440055644591986 + 4.939179742720388 ], [ 0.13262, -0.35891, - -1.3893637169263382 + 4.893821590253248 ], [ 0.14021, -0.40634, - -1.4347218693934778 + 4.848463437786108 ], [ 0.14565, -0.45406, - -1.4800800218606174 + 4.803105285318969 ], [ 0.14891, -0.50198, - -1.525438174327757 + 4.757747132851829 ], [ 0.15, -0.55, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -5413,17 +5413,17 @@ [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ], [ 0.0, 0.0, - -1.3389725222944935 + 4.944212784885092 ], [ 0.0, 0.0, - -1.5707963267948966 + 4.71238898038469 ] ] }, @@ -5440,37 +5440,37 @@ [ 0.02143, -0.04286, - -1.1071487177940904 + 5.176036589385496 ], [ 0.04286, -0.08571, - -1.1071487177940904 + 5.176036589385496 ], [ 0.06429, -0.12857, - -1.1071487177940904 + 5.176036589385496 ], [ 0.08571, -0.17143, - -1.1071487177940904 + 5.176036589385496 ], [ 0.10714, -0.21429, - -1.1071487177940904 + 5.176036589385496 ], [ 0.12857, -0.25714, - -1.1071487177940904 + 5.176036589385496 ], [ 0.15, -0.3, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5487,37 +5487,37 @@ [ 0.04286, 0.02143, - -1.1071487177940904 + 5.176036589385496 ], [ 0.08571, 0.04286, - -1.1071487177940904 + 5.176036589385496 ], [ 0.12857, 0.06429, - -1.1071487177940904 + 5.176036589385496 ], [ 0.17143, 0.08571, - -1.1071487177940904 + 5.176036589385496 ], [ 0.21429, 0.10714, - -1.1071487177940904 + 5.176036589385496 ], [ 0.25714, 0.12857, - -1.1071487177940904 + 5.176036589385496 ], [ 0.3, 0.15, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5534,37 +5534,37 @@ [ -0.04286, -0.02143, - -1.1071487177940904 + 5.176036589385496 ], [ -0.08571, -0.04286, - -1.1071487177940904 + 5.176036589385496 ], [ -0.12857, -0.06429, - -1.1071487177940904 + 5.176036589385496 ], [ -0.17143, -0.08571, - -1.1071487177940904 + 5.176036589385496 ], [ -0.21429, -0.10714, - -1.1071487177940904 + 5.176036589385496 ], [ -0.25714, -0.12857, - -1.1071487177940904 + 5.176036589385496 ], [ -0.3, -0.15, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5581,47 +5581,47 @@ [ 0.02193, -0.0427, - -1.079202756530279 + 5.203982550649307 ], [ 0.04536, -0.0846, - -1.0424771823886751 + 5.2407081247909115 ], [ 0.07032, -0.12561, - -1.0057516082470712 + 5.277433698932515 ], [ 0.09676, -0.16568, - -0.9690260341054675 + 5.314159273074119 ], [ 0.12466, -0.20474, - -0.9323004599638636 + 5.350884847215722 ], [ 0.15397, -0.24276, - -0.8955748858222597 + 5.387610421357326 ], [ 0.18466, -0.27967, - -0.8588493116806559 + 5.4243359954989305 ], [ 0.21669, -0.31543, - -0.8221237375390521 + 5.461061569640534 ], [ 0.25, -0.35, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5638,17 +5638,17 @@ [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ], [ 0.0, 0.0, - -0.9462734405957693 + 5.336911866583817 ], [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5665,47 +5665,47 @@ [ 0.03331, -0.03457, - -0.8221237375390521 + 5.461061569640534 ], [ 0.06534, -0.07033, - -0.8588493116806559 + 5.4243359954989305 ], [ 0.09603, -0.10724, - -0.8955748858222599 + 5.387610421357326 ], [ 0.12534, -0.14526, - -0.9323004599638637 + 5.350884847215722 ], [ 0.15324, -0.18432, - -0.9690260341054675 + 5.314159273074119 ], [ 0.17968, -0.22439, - -1.0057516082470714 + 5.277433698932515 ], [ 0.20464, -0.2654, - -1.0424771823886751 + 5.2407081247909115 ], [ 0.22807, -0.3073, - -1.079202756530279 + 5.203982550649307 ], [ 0.25, -0.35, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5722,57 +5722,57 @@ [ 0.03685, -0.03685, - -0.7853981633974483 + 5.497787143782138 ], [ 0.07371, -0.07371, - -0.7853981633974483 + 5.497787143782138 ], [ 0.11039, -0.11073, - -0.8045645706977883 + 5.478620736481798 ], [ 0.14581, -0.14896, - -0.8423875890848261 + 5.44079771809476 ], [ 0.17976, -0.1885, - -0.8802106074718639 + 5.402974699707722 ], [ 0.21219, -0.22929, - -0.9180336258589017 + 5.365151681320684 ], [ 0.24305, -0.27128, - -0.9558566442459394 + 5.327328662933647 ], [ 0.27231, -0.31441, - -0.9936796626329771 + 5.289505644546609 ], [ 0.29991, -0.35862, - -1.031502681020015 + 5.251682626159571 ], [ 0.32582, -0.40383, - -1.0693256994070528 + 5.213859607772534 ], [ 0.35, -0.45, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5789,17 +5789,17 @@ [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ], [ 0.0, 0.0, - -0.9462734405957693 + 5.336911866583817 ], [ 0.0, 0.0, - -1.1071487177940904 + 5.176036589385496 ] ] }, @@ -5816,42 +5816,42 @@ [ 0.0375, -0.0375, - -0.7853981633974483 + 5.497787143782138 ], [ 0.075, -0.075, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1125, -0.1125, - -0.7853981633974483 + 5.497787143782138 ], [ 0.15, -0.15, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1875, -0.1875, - -0.7853981633974483 + 5.497787143782138 ], [ 0.225, -0.225, - -0.7853981633974483 + 5.497787143782138 ], [ 0.2625, -0.2625, - -0.7853981633974483 + 5.497787143782138 ], [ 0.3, -0.3, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5868,42 +5868,42 @@ [ 0.0375, 0.0375, - -0.7853981633974483 + 5.497787143782138 ], [ 0.075, 0.075, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1125, 0.1125, - -0.7853981633974483 + 5.497787143782138 ], [ 0.15, 0.15, - -0.7853981633974483 + 5.497787143782138 ], [ 0.1875, 0.1875, - -0.7853981633974483 + 5.497787143782138 ], [ 0.225, 0.225, - -0.7853981633974483 + 5.497787143782138 ], [ 0.2625, 0.2625, - -0.7853981633974483 + 5.497787143782138 ], [ 0.3, 0.3, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5920,42 +5920,42 @@ [ -0.0375, -0.0375, - -0.7853981633974483 + 5.497787143782138 ], [ -0.075, -0.075, - -0.7853981633974483 + 5.497787143782138 ], [ -0.1125, -0.1125, - -0.7853981633974483 + 5.497787143782138 ], [ -0.15, -0.15, - -0.7853981633974483 + 5.497787143782138 ], [ -0.1875, -0.1875, - -0.7853981633974483 + 5.497787143782138 ], [ -0.225, -0.225, - -0.7853981633974483 + 5.497787143782138 ], [ -0.2625, -0.2625, - -0.7853981633974483 + 5.497787143782138 ], [ -0.3, -0.3, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -5972,47 +5972,47 @@ [ 0.03457, -0.03331, - -0.7486725892558445 + 5.534512717923742 ], [ 0.07033, -0.06534, - -0.7119470151142406 + 5.571238292065345 ], [ 0.10724, -0.09603, - -0.6752214409726367 + 5.6079638662069495 ], [ 0.14526, -0.12534, - -0.6384958668310329 + 5.644689440348554 ], [ 0.18432, -0.15324, - -0.601770292689429 + 5.681415014490157 ], [ 0.22439, -0.17968, - -0.5650447185478252 + 5.718140588631761 ], [ 0.2654, -0.20464, - -0.5283191444062213 + 5.754866162773365 ], [ 0.3073, -0.22807, - -0.4915935702646175 + 5.7915917369149685 ], [ 0.35, -0.25, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -6029,57 +6029,57 @@ [ 0.03685, -0.03685, - -0.7853981633974483 + 5.497787143782138 ], [ 0.07371, -0.07371, - -0.7853981633974483 + 5.497787143782138 ], [ 0.11073, -0.11039, - -0.7662317560971083 + 5.516953551082478 ], [ 0.14896, -0.14581, - -0.7284087377100704 + 5.5547765694695155 ], [ 0.1885, -0.17976, - -0.6905857193230327 + 5.5925995878565535 ], [ 0.22929, -0.21219, - -0.6527627009359949 + 5.630422606243592 ], [ 0.27128, -0.24305, - -0.614939682548957 + 5.668245624630629 ], [ 0.31441, -0.27231, - -0.5771166641619194 + 5.706068643017667 ], [ 0.35862, -0.29991, - -0.5392936457748816 + 5.743891661404705 ], [ 0.40383, -0.32582, - -0.5014706273878439 + 5.781714679791742 ], [ 0.45, -0.35, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -6096,17 +6096,17 @@ [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ], [ 0.0, 0.0, - -0.6245228861991272 + 5.658662420980459 ], [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ] ] }, @@ -6123,47 +6123,47 @@ [ 0.0427, -0.02193, - -0.4915935702646175 + 5.7915917369149685 ], [ 0.0846, -0.04536, - -0.5283191444062214 + 5.754866162773364 ], [ 0.12561, -0.07032, - -0.5650447185478252 + 5.718140588631761 ], [ 0.16568, -0.09676, - -0.601770292689429 + 5.681415014490157 ], [ 0.20474, -0.12466, - -0.6384958668310329 + 5.644689440348554 ], [ 0.24276, -0.15397, - -0.6752214409726367 + 5.6079638662069495 ], [ 0.27967, -0.18466, - -0.7119470151142406 + 5.571238292065345 ], [ 0.31543, -0.21669, - -0.7486725892558445 + 5.534512717923742 ], [ 0.35, -0.25, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -6180,22 +6180,22 @@ [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ], [ 0.0, 0.0, - -0.5708977937996869 + 5.712287513379899 ], [ 0.0, 0.0, - -0.6781479785985676 + 5.605037328581019 ], [ 0.0, 0.0, - -0.7853981633974483 + 5.497787143782138 ] ] }, @@ -6212,37 +6212,37 @@ [ 0.04286, -0.02143, - -0.4636476090008061 + 5.81953769817878 ], [ 0.08571, -0.04286, - -0.4636476090008061 + 5.81953769817878 ], [ 0.12857, -0.06429, - -0.4636476090008061 + 5.81953769817878 ], [ 0.17143, -0.08571, - -0.4636476090008061 + 5.81953769817878 ], [ 0.21429, -0.10714, - -0.4636476090008061 + 5.81953769817878 ], [ 0.25714, -0.12857, - -0.4636476090008061 + 5.81953769817878 ], [ 0.3, -0.15, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -6259,37 +6259,37 @@ [ 0.02143, 0.04286, - -0.4636476090008061 + 5.81953769817878 ], [ 0.04286, 0.08571, - -0.4636476090008061 + 5.81953769817878 ], [ 0.06429, 0.12857, - -0.4636476090008061 + 5.81953769817878 ], [ 0.08571, 0.17143, - -0.4636476090008061 + 5.81953769817878 ], [ 0.10714, 0.21429, - -0.4636476090008061 + 5.81953769817878 ], [ 0.12857, 0.25714, - -0.4636476090008061 + 5.81953769817878 ], [ 0.15, 0.3, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -6306,37 +6306,37 @@ [ -0.02143, -0.04286, - -0.4636476090008061 + 5.81953769817878 ], [ -0.04286, -0.08571, - -0.4636476090008061 + 5.81953769817878 ], [ -0.06429, -0.12857, - -0.4636476090008061 + 5.81953769817878 ], [ -0.08571, -0.17143, - -0.4636476090008061 + 5.81953769817878 ], [ -0.10714, -0.21429, - -0.4636476090008061 + 5.81953769817878 ], [ -0.12857, -0.25714, - -0.4636476090008061 + 5.81953769817878 ], [ -0.15, -0.3, - -0.4636476090008061 + 5.81953769817878 ] ] }, @@ -6353,57 +6353,57 @@ [ 0.04296, -0.02148, - -0.4636476090008061 + 5.81953769817878 ], [ 0.08595, -0.04292, - -0.453581524671396 + 5.82960378250819 ], [ 0.12959, -0.06298, - -0.4082233722042564 + 5.87496193497533 ], [ 0.1741, -0.08104, - -0.36286521973711683 + 5.9203200874424695 ], [ 0.21937, -0.09707, - -0.31750706726997724 + 5.965678239909609 ], [ 0.26533, -0.11102, - -0.27214891480283765 + 6.011036392376749 ], [ 0.31188, -0.12288, - -0.22679076233569806 + 6.056394544843888 ], [ 0.35891, -0.13262, - -0.18143260986855847 + 6.101752697311028 ], [ 0.40634, -0.14021, - -0.13607445740141882 + 6.1471108497781675 ], [ 0.45406, -0.14565, - -0.09071630493427918 + 6.192469002245307 ], [ 0.50198, -0.14891, - -0.04535815246713959 + 6.237827154712447 ], [ 0.55, @@ -6425,17 +6425,17 @@ [ 0.0, 0.0, - -0.46364760900080615 + 5.81953769817878 ], [ 0.0, 0.0, - -0.3090984060005373 + 5.9740869011790485 ], [ 0.0, 0.0, - -0.15454920300026886 + 6.128636104179318 ], [ 0.0, 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 00000000000..01476decd0e --- /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/trajectory.py b/nav2_smac_planner/lattice_primitives/trajectory.py index 4fec8c168b3..520ded328da 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory.py +++ b/nav2_smac_planner/lattice_primitives/trajectory.py @@ -14,7 +14,7 @@ from dataclasses import dataclass -from helper import angle_difference +from helper import angle_difference, normalize_angle import numpy as np @@ -127,6 +127,9 @@ def to_output_format(self): 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() diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 929e1141ab0..ad916087792 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.0.0 + 1.1.0 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index f0a3536aafc..deaa597b923 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -249,7 +249,7 @@ bool AStarAlgorithm::createPath( NodeGetter neighborGetter = [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool { - if (index < 0 || index >= max_index) { + if (index >= max_index) { return false; } diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index b267259c9cf..c0cbfb98fa7 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -86,6 +86,13 @@ bool GridCollisionChecker::inCollision( 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); @@ -164,4 +171,9 @@ float GridCollisionChecker::getCost() 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/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 0f6404183ef..cf8657118ff 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -71,7 +71,7 @@ void SmacPlanner2D::configure( node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); node->get_parameter(name + ".downsampling_factor", _downsampling_factor); nav2_util::declare_parameter_if_not_declared( - node, name + ".cost_travel_multiplier", rclcpp::ParameterValue(2.0)); + node, 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( diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 9a8d06ac1c9..279af1804c3 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -92,7 +92,7 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4)); - node->get_parameter(name + ".minimum_turning_radius", _search_info.minimum_turning_radius); + 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); @@ -150,9 +150,8 @@ void SmacPlannerHybrid::configure( if (!_downsample_costmap) { _downsampling_factor = 1; } - const double minimum_turning_radius_global_coords = _search_info.minimum_turning_radius; _search_info.minimum_turning_radius = - _search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor); + _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor); _lookup_table_dim = static_cast(_lookup_table_size) / static_cast(_costmap->getResolution() * _downsampling_factor); @@ -191,7 +190,7 @@ void SmacPlannerHybrid::configure( SmootherParams params; params.get(node, name); _smoother = std::make_unique(params); - _smoother->initialize(minimum_turning_radius_global_coords); + _smoother->initialize(_minimum_turning_radius_global_coords); } // Initialize costmap downsampler @@ -401,7 +400,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para if (_smoother) { reinit_smoother = true; } - _search_info.minimum_turning_radius = static_cast(parameter.as_double()); + _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()); @@ -481,9 +480,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para if (!_downsample_costmap) { _downsampling_factor = 1; } - const double minimum_turning_radius_global_coords = _search_info.minimum_turning_radius; _search_info.minimum_turning_radius = - _search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor); + _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor); _lookup_table_dim = static_cast(_lookup_table_size) / static_cast(_costmap->getResolution() * _downsampling_factor); @@ -538,7 +536,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para SmootherParams params; params.get(node, _name); _smoother = std::make_unique(params); - _smoother->initialize(minimum_turning_radius_global_coords); + _smoother->initialize(_minimum_turning_radius_global_coords); } } result.successful = true; diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index cf892fbfef0..09a3591928c 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -78,7 +78,8 @@ void SmacPlannerLattice::configure( // 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") + "/default_model.json")); + 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)); @@ -419,9 +420,8 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par // 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 - const double minimum_turning_radius_global_coords = _search_info.minimum_turning_radius; _search_info.minimum_turning_radius = - _search_info.minimum_turning_radius / (_costmap->getResolution()); + _metadata.min_turning_radius / (_costmap->getResolution()); float lookup_table_dim = static_cast(_lookup_table_size) / static_cast(_costmap->getResolution()); diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index d1316657a05..fd0ac34be2d 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -44,6 +44,11 @@ bool Smoother::smooth( 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; diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index d8c4440920b..76befe954cf 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -116,5 +116,3 @@ ament_add_gtest(test_lattice_node test_nodelattice.cpp) ament_target_dependencies(test_lattice_node ${dependencies}) target_link_libraries(test_lattice_node ${library_name}) - -install(FILES default_model.json DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_smac_planner/test/default_model.json b/nav2_smac_planner/test/default_model.json deleted file mode 100644 index 3633fff5d70..00000000000 --- a/nav2_smac_planner/test/default_model.json +++ /dev/null @@ -1,3932 +0,0 @@ -{ - "version": 1.0, - "date_generated": "2022-01-15", - "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, - -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": 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, - -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": 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, - -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": 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, - -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 - ] - ] - }, - { - "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, - -2.677945044588987 - ] - ] - }, - { - "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, - -2.677945044588987 - ] - ] - }, - { - "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, - -2.677945044588987 - ], - [ - -0.35, - -0.1, - -2.677945044588987 - ] - ] - }, - { - "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, - -2.0344439357957027 - ], - [ - -0.5, - -0.35, - -2.0344439357957027 - ] - ] - }, - { - "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, - -2.677945044588987 - ], - [ - -0.1, - -0.05, - -2.677945044588987 - ], - [ - -0.15, - -0.075, - -2.677945044588987 - ], - [ - -0.2, - -0.1, - -2.677945044588987 - ] - ] - }, - { - "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, - -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": 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, - -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, - 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, - -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, - 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, - -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": 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, - -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": 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, - -2.356194490192345 - ], - [ - -0.075, - -0.075, - -2.356194490192345 - ], - [ - -0.1125, - -0.1125, - -2.356194490192345 - ], - [ - -0.15, - -0.15, - -2.356194490192345 - ] - ] - }, - { - "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, - -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": 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, - -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": 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, - -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": 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, - -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": 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, - -2.0344439357957027 - ], - [ - -0.05, - -0.1, - -2.0344439357957027 - ], - [ - -0.075, - -0.15, - -2.0344439357957027 - ], - [ - -0.1, - -0.2, - -2.0344439357957027 - ] - ] - }, - { - "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, - -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": 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, - -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": 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, - -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": 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, - -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": 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, - -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": 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, - -1.5707963267948966 - ], - [ - 0.0, - -0.1, - -1.5707963267948966 - ], - [ - 0.0, - -0.15, - -1.5707963267948966 - ] - ] - }, - { - "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, - -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": 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, - -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": 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, - -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": 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, - -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": 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, - -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": 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, - -1.1071487177940904 - ], - [ - 0.05, - -0.1, - -1.1071487177940904 - ], - [ - 0.075, - -0.15, - -1.1071487177940904 - ], - [ - 0.1, - -0.2, - -1.1071487177940904 - ] - ] - }, - { - "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, - -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": 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, - -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": 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, - -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": 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, - -0.7853981633974483 - ], - [ - 0.075, - -0.075, - -0.7853981633974483 - ], - [ - 0.1125, - -0.1125, - -0.7853981633974483 - ], - [ - 0.15, - -0.15, - -0.7853981633974483 - ] - ] - }, - { - "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, - -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": 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, - -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": 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, - -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": 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, - -0.4636476090008061 - ], - [ - 0.1, - -0.05, - -0.4636476090008061 - ], - [ - 0.15, - -0.075, - -0.4636476090008061 - ], - [ - 0.2, - -0.1, - -0.4636476090008061 - ] - ] - }, - { - "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, - -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": 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, - -0.360614807000627 - ], - [ - 0.09683, - -0.03652, - -0.2575820050004478 - ], - [ - 0.14755, - -0.04712, - -0.15454920300026875 - ], - [ - 0.19909, - -0.05245, - -0.051516401000089584 - ], - [ - 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, - -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, - 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/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 6957a0c8702..68af08df501 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -185,7 +185,9 @@ TEST(AStarTest, test_a_star_lattice) info.retrospective_penalty = 0.1; info.analytic_expansion_ratio = 3.5; info.lattice_filepath = - ament_index_cpp::get_package_share_directory("nav2_smac_planner") + "/default_model.json"; + 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; @@ -223,7 +225,7 @@ TEST(AStarTest, test_a_star_lattice) // check path is the right size and collision free EXPECT_EQ(num_it, 21); - EXPECT_EQ(path.size(), 47u); + 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); } diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index 8ac66e1a4c9..a2f8a107969 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -28,7 +28,10 @@ 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 + "/default_model.json"; + 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()); @@ -73,17 +76,20 @@ TEST(NodeLatticeTest, parser_test) 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, -0.0948654249597968, 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, -0.1897308499195936, 0.015); + 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 + "/default_model.json"; + 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; @@ -111,7 +117,7 @@ TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing) 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, -1.107, 0.01); + EXPECT_NEAR(projections[0]->poses.back()._theta, 5.176, 0.01); EXPECT_NEAR( nav2_smac_planner::NodeLattice::motion_table.getLatticeMetadata( @@ -121,7 +127,10 @@ TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing) 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 + "/default_model.json"; + 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; @@ -156,7 +165,10 @@ TEST(NodeLatticeTest, test_node_lattice_conversions) 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 + "/default_model.json"; + 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; @@ -230,7 +242,10 @@ TEST(NodeLatticeTest, test_node_lattice) 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 + "/default_model.json"; + 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; diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index 50db2a03497..d7d27f1d20d 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -152,6 +152,9 @@ TEST(SmootherTest, test_full_smoother) // 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)); diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt index fe2e99c91e8..0df85f54bdb 100644 --- a/nav2_smoother/CMakeLists.txt +++ b/nav2_smoother/CMakeLists.txt @@ -8,6 +8,7 @@ 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) @@ -26,22 +27,14 @@ include_directories( ) set(executable_name smoother_server) - -add_executable(${executable_name} - src/main.cpp -) - set(library_name ${executable_name}_core) -add_library(${library_name} SHARED - src/nav2_smoother.cpp -) - set(dependencies angles rclcpp rclcpp_components rclcpp_action + rclcpp_components std_msgs nav2_msgs nav_2d_utils @@ -51,10 +44,32 @@ set(dependencies 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 @@ -64,17 +79,10 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) endif() -ament_target_dependencies(${executable_name} - ${dependencies} -) - -target_link_libraries(${executable_name} ${library_name}) - rclcpp_components_register_nodes(${library_name} "nav2_smoother::SmootherServer") install( - TARGETS - ${library_name} + TARGETS ${library_name} simple_smoother ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -96,8 +104,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries( - ${library_name}) +ament_export_libraries(${library_name} simple_smoother) ament_export_dependencies(${dependencies}) - ament_package() 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 00000000000..4b841b3c09e --- /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 index 44eb0b892a0..e0fdca6bae3 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,9 +2,10 @@ nav2_smoother - 1.0.0 + 1.1.0 Smoother action interface Matej Vargovcik + Steve Macenski Apache-2.0 ament_cmake diff --git a/nav2_smoother/plugins.xml b/nav2_smoother/plugins.xml new file mode 100644 index 00000000000..c72f7c25c88 --- /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/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index c07478d5776..ada1f664b02 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -34,10 +34,10 @@ namespace nav2_smoother { SmootherServer::SmootherServer(const rclcpp::NodeOptions & options) -: LifecycleNode("smoother_server", "", false, options), +: LifecycleNode("smoother_server", "", options), lp_loader_("nav2_core", "nav2_core::Smoother"), - default_ids_{}, default_types_{ - "nav2_smoother::CeresCostawareSmoother"} + default_ids_{"simple_smoother"}, + default_types_{"nav2_smoother::SimpleSmoother"} { RCLCPP_INFO(get_logger(), "Creating smoother server"); diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp new file mode 100644 index 00000000000..59a81fd3b11 --- /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 index f8a1818b111..67c98e3e43e 100644 --- a/nav2_smoother/test/CMakeLists.txt +++ b/nav2_smoother/test/CMakeLists.txt @@ -5,6 +5,19 @@ ament_add_gtest(test_smoother_server 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 00000000000..fccc2c2a7c5 --- /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 index a8bee7bdb48..8dbc5416dbf 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Intel Corporation +// 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. @@ -48,9 +48,9 @@ class DummySmoother : public nav2_core::Smoother virtual void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr &, - std::string, const std::shared_ptr &, - const std::shared_ptr &, - const std::shared_ptr &) {} + std::string, std::shared_ptr, + std::shared_ptr, + std::shared_ptr) {} virtual void cleanup() {} diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 96d932136b5..6cc6a32e496 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -61,9 +61,10 @@ if(BUILD_TESTING) 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}) diff --git a/nav2_system_tests/README.md b/nav2_system_tests/README.md index 04564359420..b40cba4ec4d 100644 --- a/nav2_system_tests/README.md +++ b/nav2_system_tests/README.md @@ -1,14 +1,14 @@ # System Tests -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. +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. -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 +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 - 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 recovery behaviors in a sandbox environment to ensure they trigger and complete collision checking properly +- 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 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/package.xml b/nav2_system_tests/package.xml index ec3f0d2f42a..0940173e04b 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.0.0 + 1.1.0 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_system_tests/src/behavior_tree/server_handler.cpp b/nav2_system_tests/src/behavior_tree/server_handler.cpp index 0952f607121..2108f37d58f 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.cpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.cpp @@ -40,6 +40,8 @@ ServerHandler::ServerHandler() 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"); } @@ -85,6 +87,7 @@ void ServerHandler::reset() const spin_server->reset(); wait_server->reset(); backup_server->reset(); + drive_on_heading_server->reset(); } void ServerHandler::spinThread() diff --git a/nav2_system_tests/src/behavior_tree/server_handler.hpp b/nav2_system_tests/src/behavior_tree/server_handler.hpp index c9fc50f38cc..f8869ea0374 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.hpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.hpp @@ -29,6 +29,7 @@ #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" @@ -93,6 +94,7 @@ class ServerHandler 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: 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 index 54224b4cd34..2f76823ae43 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -12,13 +12,12 @@ // 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" @@ -57,25 +56,30 @@ class BehaviorTreeHandler "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_single_trigger_bt_node", "nav2_is_battery_low_condition_bt_node", @@ -85,7 +89,12 @@ class BehaviorTreeHandler "nav2_planner_selector_bt_node", "nav2_controller_selector_bt_node", "nav2_goal_checker_selector_bt_node", - "nav2_controller_cancel_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)); diff --git a/nav2_system_tests/src/behaviors/README.md b/nav2_system_tests/src/behaviors/README.md new file mode 100644 index 00000000000..e1ac535ad5c --- /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/recoveries/backup/CMakeLists.txt b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt similarity index 54% rename from nav2_system_tests/src/recoveries/backup/CMakeLists.txt rename to nav2_system_tests/src/behaviors/backup/CMakeLists.txt index ca3f35402ae..05afe3c50da 100644 --- a/nav2_system_tests/src/recoveries/backup/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt @@ -1,22 +1,22 @@ -set(test_backup_recovery_exec test_backup_recovery_node) +set(test_backup_behavior test_backup_behavior_node) -ament_add_gtest_executable(${test_backup_recovery_exec} - test_backup_recovery_node.cpp - backup_recovery_tester.cpp +ament_add_gtest_executable(${test_backup_behavior} + test_backup_behavior_node.cpp + backup_behavior_tester.cpp ) -ament_target_dependencies(${test_backup_recovery_exec} +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_recovery_launch.py" + 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_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 7d8bb826fe0..a270c8ab082 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,19 +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"); @@ -78,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_) { @@ -97,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"); @@ -105,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_) { @@ -114,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; @@ -182,18 +178,18 @@ 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"; @@ -216,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 86% 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 43e9cb5b8d8..bf42c9881a7 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(); @@ -87,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 98% 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 97affc65168..bf7f4f04f13 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 @@ -89,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 00000000000..e629dca0bb0 --- /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 00000000000..fe17417e164 --- /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/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp new file mode 100644 index 00000000000..8db6ef6406e --- /dev/null +++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp @@ -0,0 +1,222 @@ +// 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 "drive_on_heading_behavior_tester.hpp" +#include "nav2_util/geometry_utils.hpp" + +using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT + +namespace nav2_system_tests +{ + +DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester() +: is_active_(false), + initial_pose_received_(false) +{ + 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( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), + "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(&DriveOnHeadingBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); + + stamp_ = node_->now(); +} + +DriveOnHeadingBehaviorTester::~DriveOnHeadingBehaviorTester() +{ + if (is_active_) { + deactivate(); + } +} + +void DriveOnHeadingBehaviorTester::activate() +{ + if (is_active_) { + throw std::runtime_error("Trying to activate while already active"); + return; + } + + while (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); + sendInitialPose(); + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node_); + } + + // 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(), "DriveOnHeading action server is ready"); + is_active_ = true; +} + +void DriveOnHeadingBehaviorTester::deactivate() +{ + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; +} + +bool DriveOnHeadingBehaviorTester::defaultDriveOnHeadingBehaviorTest( + const DriveOnHeading::Goal goal_msg, + 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); + + RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); + + 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"); + + 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"); + 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 dist = nav2_util::geometry_utils::euclidean_distance(initial_pose, current_pose); + + if (fabs(dist) > fabs(goal_msg.target.x) + tolerance) { + RCLCPP_ERROR( + node_->get_logger(), + "Distance from goal is %lf (tolerance %lf)", + fabs(dist - goal_msg.target.x), tolerance); + return false; + } + + return true; +} + +void DriveOnHeadingBehaviorTester::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 DriveOnHeadingBehaviorTester::amclPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) +{ + initial_pose_received_ = true; +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp new file mode 100644 index 00000000000..920164f162f --- /dev/null +++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp @@ -0,0 +1,90 @@ +// Copyright (c) 2020 Samsung Research +// 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__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_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/drive_on_heading.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/node_thread.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +namespace nav2_system_tests +{ + +class DriveOnHeadingBehaviorTester +{ +public: + using DriveOnHeading = nav2_msgs::action::DriveOnHeading; + using GoalHandleDriveOnHeading = rclcpp_action::ClientGoalHandle; + + DriveOnHeadingBehaviorTester(); + ~DriveOnHeadingBehaviorTester(); + + // Runs a single test with given target yaw + bool defaultDriveOnHeadingBehaviorTest( + const DriveOnHeading::Goal goal_msg, + double tolerance); + + void activate(); + + void deactivate(); + + bool isActive() const + { + return is_active_; + } + +private: + void sendInitialPose(); + + void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); + + bool is_active_; + bool initial_pose_received_; + rclcpp::Time stamp_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Node::SharedPtr node_; + + // Publisher to publish initial pose + rclcpp::Publisher::SharedPtr publisher_; + + // Subscriber for amcl pose + rclcpp::Subscription::SharedPtr subscription_; + + // Action client to call DriveOnHeading action + rclcpp_action::Client::SharedPtr client_ptr_; +}; + +} // namespace nav2_system_tests + +#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 00000000000..29530dd4752 --- /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 00000000000..b0813c1a254 --- /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/recoveries/spin/CMakeLists.txt b/nav2_system_tests/src/behaviors/spin/CMakeLists.txt similarity index 52% rename from nav2_system_tests/src/recoveries/spin/CMakeLists.txt rename to nav2_system_tests/src/behaviors/spin/CMakeLists.txt index 804468dda00..84ee7246e59 100644 --- a/nav2_system_tests/src/recoveries/spin/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/spin/CMakeLists.txt @@ -1,34 +1,34 @@ -set(test_spin_recovery_exec test_spin_recovery_node) +set(test_spin_behavior_exec test_spin_behavior_node) -ament_add_gtest_executable(${test_spin_recovery_exec} - test_spin_recovery_node.cpp - spin_recovery_tester.cpp +ament_add_gtest_executable(${test_spin_behavior_exec} + test_spin_behavior_node.cpp + spin_behavior_tester.cpp ) -ament_target_dependencies(${test_spin_recovery_exec} +ament_target_dependencies(${test_spin_behavior_exec} ${dependencies} ) -ament_add_test(test_spin_recovery +ament_add_test(test_spin_behavior GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_recovery_launch.py" + 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_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_recovery_fake +ament_add_test(test_spin_behavior_fake GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_recovery_fake_launch.py" + 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=$ + TEST_EXECUTABLE=$ MAKE_FAKE_COSTMAP=true ) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp similarity index 93% rename from nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp rename to nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp index 316b35b1fef..557f5b79595 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp @@ -22,7 +22,7 @@ #include #include -#include "spin_recovery_tester.hpp" +#include "spin_behavior_tester.hpp" using namespace std::chrono_literals; using namespace std::chrono; // NOLINT @@ -30,11 +30,11 @@ using namespace std::chrono; // NOLINT namespace nav2_system_tests { -SpinRecoveryTester::SpinRecoveryTester() +SpinBehaviorTester::SpinBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("spin_recovery_test"); + node_ = rclcpp::Node::make_shared("spin_behavior_test"); tf_buffer_ = std::make_shared(node_->get_clock()); @@ -65,19 +65,19 @@ SpinRecoveryTester::SpinRecoveryTester() subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&SpinRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); + std::bind(&SpinBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); stamp_ = node_->now(); } -SpinRecoveryTester::~SpinRecoveryTester() +SpinBehaviorTester::~SpinBehaviorTester() { if (is_active_) { deactivate(); } } -void SpinRecoveryTester::activate() +void SpinBehaviorTester::activate() { if (is_active_) { throw std::runtime_error("Trying to activate while already active"); @@ -94,7 +94,7 @@ void SpinRecoveryTester::activate() sendFakeOdom(0.0); } - // 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_) { @@ -113,7 +113,7 @@ void SpinRecoveryTester::activate() is_active_ = true; } -void SpinRecoveryTester::deactivate() +void SpinBehaviorTester::deactivate() { if (!is_active_) { throw std::runtime_error("Trying to deactivate while already inactive"); @@ -121,7 +121,7 @@ void SpinRecoveryTester::deactivate() is_active_ = false; } -bool SpinRecoveryTester::defaultSpinRecoveryTest( +bool SpinBehaviorTester::defaultSpinBehaviorTest( const float target_yaw, const double tolerance) { @@ -130,7 +130,7 @@ 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); if (make_fake_costmap_) { @@ -260,7 +260,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( return true; } -void SpinRecoveryTester::sendFakeCostmap(float angle) +void SpinBehaviorTester::sendFakeCostmap(float angle) { nav2_msgs::msg::Costmap fake_costmap; @@ -286,7 +286,7 @@ void SpinRecoveryTester::sendFakeCostmap(float angle) fake_costmap_publisher_->publish(fake_costmap); } -void SpinRecoveryTester::sendInitialPose() +void SpinBehaviorTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; @@ -309,7 +309,7 @@ void SpinRecoveryTester::sendInitialPose() RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); } -void SpinRecoveryTester::sendFakeOdom(float angle) +void SpinBehaviorTester::sendFakeOdom(float angle) { geometry_msgs::msg::TransformStamped transformStamped; @@ -342,7 +342,7 @@ void SpinRecoveryTester::sendFakeOdom(float angle) footprint.polygon.points[3].y = 0.22; fake_footprint_publisher_->publish(footprint); } -void SpinRecoveryTester::amclPoseCallback( +void SpinBehaviorTester::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/spin/spin_behavior_tester.hpp similarity index 91% rename from nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp rename to nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp index a00ffa21e41..745cfe42124 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp @@ -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__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ #include #include @@ -45,17 +45,17 @@ namespace nav2_system_tests { -class SpinRecoveryTester +class SpinBehaviorTester { public: using Spin = nav2_msgs::action::Spin; using GoalHandleSpin = rclcpp_action::ClientGoalHandle; - SpinRecoveryTester(); - ~SpinRecoveryTester(); + SpinBehaviorTester(); + ~SpinBehaviorTester(); // Runs a single test with given target yaw - bool defaultSpinRecoveryTest( + bool defaultSpinBehaviorTest( float target_yaw, double tolerance = 0.1); @@ -105,4 +105,4 @@ class SpinRecoveryTester } // namespace nav2_system_tests -#endif // RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ +#endif // BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_fake_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py similarity index 96% rename from nav2_system_tests/src/recoveries/spin/test_spin_recovery_fake_launch.py rename to nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py index a773d91e09b..1b0ace8f1bb 100755 --- a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_fake_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py @@ -53,7 +53,7 @@ def generate_launch_description(): param_rewrites=param_substitutions, convert_types=True) - lifecycle_nodes = ['recoveries_server'] + 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 @@ -122,9 +122,9 @@ def generate_launch_description(): arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), Node( - package='nav2_recoveries', - executable='recoveries_server', - name='recoveries_server', + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', output='screen', parameters=[configured_params], remappings=remappings), @@ -147,7 +147,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], - name='test_spin_recovery_fake_node', + name='test_spin_behavior_fake_node', output='screen') lts = LaunchTestService() diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py similarity index 98% rename from nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py rename to nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index 57a6cf327d9..d26795834b4 100755 --- a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -89,7 +89,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], - name='test_wait_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 85% 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 059331eaae8..150a6599fab 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 @@ -20,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) { @@ -34,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(); } @@ -53,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()); @@ -66,7 +66,7 @@ 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; } @@ -82,7 +82,7 @@ TEST_P(SpinRecoveryTestFixture, testSpinRecovery) 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 00000000000..d6fb45401d8 --- /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/spin/test_spin_recovery_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py similarity index 98% rename from nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py rename to nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index 9759d20c8a8..37f2ebd34cf 100755 --- a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -89,7 +89,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], - name='test_spin_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 75% 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 998808bf7ef..5aa00c56e9c 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; @@ -81,8 +81,8 @@ TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) } INSTANTIATE_TEST_SUITE_P( - WaitRecoveryTests, - WaitRecoveryTestFixture, + 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 7112289b5af..ea6e58afb7d 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 4dfb90c6626..71181e24d36 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 index e05620b366f..8a48ff08590 100644 --- a/nav2_system_tests/src/costmap_filters/CMakeLists.txt +++ b/nav2_system_tests/src/costmap_filters/CMakeLists.txt @@ -8,6 +8,7 @@ ament_add_test(test_keepout_filter 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 diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 12a4a44def7..b4f98cedea1 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -39,14 +39,6 @@ amcl: 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 @@ -54,27 +46,30 @@ bt_navigator: 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 & + # 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_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 @@ -83,13 +78,23 @@ bt_navigator: - 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_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True -bt_navigator_rclcpp_node: +bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: use_sim_time: True @@ -101,7 +106,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 @@ -159,11 +164,6 @@ controller_server: 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: @@ -210,12 +210,6 @@ local_costmap: 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: @@ -252,12 +246,6 @@ global_costmap: 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: @@ -283,26 +271,24 @@ planner_server: use_astar: False allow_unknown: True -planner_server_rclcpp_node: - ros__parameters: - use_sim_time: True - smoother_server: ros__parameters: use_sim_time: True -recoveries_server: +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_tolerance: 0.1 diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index 381e18f0d53..1e021bb4f7d 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -39,14 +39,6 @@ amcl: 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 @@ -54,27 +46,31 @@ bt_navigator: 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 & + # 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 @@ -83,13 +79,23 @@ bt_navigator: - 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_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True -bt_navigator_rclcpp_node: +bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: use_sim_time: True @@ -102,7 +108,7 @@ controller_server: 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 @@ -159,10 +165,6 @@ controller_server: 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: @@ -200,12 +202,6 @@ local_costmap: 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: @@ -243,12 +239,6 @@ global_costmap: filter_info_topic: "/costmap_filter_info" speed_limit_topic: "/speed_limit" 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: @@ -274,26 +264,24 @@ planner_server: use_astar: False allow_unknown: True -planner_server_rclcpp_node: - ros__parameters: - use_sim_time: True - smoother_server: ros__parameters: use_sim_time: True -recoveries_server: +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_tolerance: 0.1 diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index 002b3e369fa..fe03fe138d5 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -39,14 +39,6 @@ amcl: 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 @@ -54,27 +46,31 @@ bt_navigator: 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 & + # 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 @@ -83,13 +79,23 @@ bt_navigator: - 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_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True -bt_navigator_rclcpp_node: +bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: use_sim_time: True @@ -102,7 +108,7 @@ controller_server: 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 @@ -159,10 +165,6 @@ controller_server: 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: @@ -206,12 +208,6 @@ local_costmap: 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: @@ -243,12 +239,6 @@ global_costmap: 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: @@ -274,26 +264,24 @@ planner_server: use_astar: False allow_unknown: True -planner_server_rclcpp_node: - ros__parameters: - use_sim_time: True - smoother_server: ros__parameters: use_sim_time: True -recoveries_server: +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_tolerance: 0.1 diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index c2c7e093230..d8ad1123921 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -86,7 +86,12 @@ def generate_launch_description(): executable='lifecycle_manager', name='lifecycle_manager_filters', output='screen', - parameters=[{'node_names': ['filter_mask_server', 'costmap_filter_info_server']}, + parameters=[{ + 'node_names': + [ + 'filter_mask_server', 'costmap_filter_info_server', 'bt_navigator' + ] + }, {'autostart': True}]), # Nodes required for Costmap Filters configuration diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 32f6f9a4f48..2603ea0df11 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -84,7 +84,12 @@ def generate_launch_description(): executable='lifecycle_manager', name='lifecycle_manager_filters', output='screen', - parameters=[{'node_names': ['filter_mask_server', 'costmap_filter_info_server']}, + parameters=[{ + 'node_names': + [ + 'filter_mask_server', 'costmap_filter_info_server', 'bt_navigator' + ] + }, {'autostart': True}]), # Nodes required for Costmap Filters configuration diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp index 32ecf214515..092b5e9e84e 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 e50b9591844..61355a82c46 100644 --- a/nav2_system_tests/src/planning/CMakeLists.txt +++ b/nav2_system_tests/src/planning/CMakeLists.txt @@ -55,3 +55,9 @@ ament_target_dependencies(test_planner_plugins rclcpp geometry_msgs nav2_msgs ${ 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 130f420bc0d..0a2368bab6b 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); @@ -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 c667b9d7b48..b849e899087 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 00000000000..958891e02ad --- /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/recoveries/README.md b/nav2_system_tests/src/recoveries/README.md deleted file mode 100644 index e4b764aaeea..00000000000 --- 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/test_backup_recovery_node.cpp b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp deleted file mode 100644 index 8802755ec0b..00000000000 --- a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp +++ /dev/null @@ -1,109 +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. - if (target_dist < -0.1) { - 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_SUITE_P( - BackupRecoveryTests, - BackupRecoveryTestFixture, - ::testing::Values( - std::make_tuple(-0.05, 0.01), - 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/wait/CMakeLists.txt b/nav2_system_tests/src/recoveries/wait/CMakeLists.txt deleted file mode 100644 index 6f751cf6aff..00000000000 --- 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_to_pose_w_replanning_and_recovery.xml -) diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 6fb119e980e..8fd1bc60342 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -38,12 +38,13 @@ class NavTester(Node): - def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ""): - super().__init__(node_name="nav2_tester", namespace=namespace) + + 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 + PoseWithCovarianceStamped, 'initialpose', 10 ) - self.goal_pub = self.create_publisher(PoseStamped, "goal_pose", 10) + self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) pose_qos = QoSProfile( durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, @@ -53,33 +54,33 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ""): ) self.model_pose_sub = self.create_subscription( - PoseWithCovarianceStamped, "amcl_pose", self.poseCallback, pose_qos + 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") + 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") + 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") + 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") + 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.header.frame_id = 'map' msg.pose = pose return msg @@ -97,17 +98,17 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): goal_msg = NavigateToPose.Goal() goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) - self.info_msg("Sending goal request...") + 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") + self.error_msg('Goal rejected') return False - self.info_msg("Goal accepted") + self.info_msg('Goal accepted') get_result_future = goal_handle.get_result_async() future_return = True @@ -116,17 +117,17 @@ 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_SUCCEEDED: - self.info_msg(f"Goal failed with status code: {status}") + self.info_msg(f'Goal failed with status code: {status}') return False if not future_return: return False - self.info_msg("Goal succeeded!") + self.info_msg('Goal succeeded!') return True def poseCallback(self, msg): - self.info_msg("Received amcl_pose") + self.info_msg('Received amcl_pose') self.current_pose = msg.pose.pose self.initial_pose_received = True @@ -138,90 +139,90 @@ def reachesGoal(self, timeout, distance): rclpy.spin_once(self, timeout_sec=1) if self.distanceFromGoal() < distance: goalReached = True - self.info_msg("*** GOAL REACHED ***") + 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!") + 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}") + 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" + 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...") + 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...") + 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}") + self.info_msg(f'Result of get_state: {state}') else: self.error_msg( - f"Exception while calling service: {future.exception()!r}" + f'Exception while calling service: {future.exception()!r}' ) time.sleep(5) def shutdown(self): - self.info_msg("Shutting down") + self.info_msg('Shutting down') self.action_client.destroy() - transition_service = "lifecycle_manager_navigation/manage_nodes" + 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...") + 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...") + 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.") + 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" + 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...") + 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...") + 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") + self.info_msg('Shutting down localization lifecycle manager complete') except Exception as e: # noqa: B902 - self.error_msg(f"Service call failed {e!r}") + 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.info_msg('Setting initial pose') self.setInitialPose() - self.info_msg("Waiting for amcl_pose to be received") + 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.info_msg('Setting goal pose') robot_tester.publishGoalPose() - robot_tester.info_msg("Waiting 60 seconds for robot to reach goal") + robot_tester.info_msg('Waiting 60 seconds for robot to reach goal') return robot_tester.reachesGoal(timeout=60, distance=0.5) @@ -229,9 +230,9 @@ 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_node_active('amcl') robot_tester.wait_for_initial_pose() - robot_tester.wait_for_node_active("bt_navigator") + robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.runNavigateAction() if result: @@ -240,9 +241,9 @@ def run_all_tests(robot_tester): # Add more tests here if desired if result: - robot_tester.info_msg("Test PASSED") + robot_tester.info_msg('Test PASSED') else: - robot_tester.error_msg("Test FAILED") + robot_tester.error_msg('Test FAILED') return result @@ -270,15 +271,15 @@ def get_testers(args): goal_pose=fwd_pose(float(final_x), float(final_y)), ) tester.info_msg( - "Starting tester, robot going from " + 'Starting tester, robot going from ' + init_x - + ", " + + ', ' + init_y - + " to " + + ' to ' + final_x - + ", " + + ', ' + final_y - + "." + + '.' ) testers.append(tester) return testers @@ -292,15 +293,15 @@ def get_testers(args): goal_pose=fwd_pose(float(final_x), float(final_y)), ) tester.info_msg( - "Starting tester for " + 'Starting tester for ' + namespace - + " going from " + + ' going from ' + init_x - + ", " + + ', ' + init_y - + " to " + + ' to ' + final_x - + ", " + + ', ' + final_y ) testers.append(tester) @@ -309,9 +310,9 @@ def get_testers(args): def check_args(expect_failure: str): # Check if --expect_failure is True or False - if expect_failure != "True" and expect_failure != "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" + '\033[1;37;41m' + ' -e flag must be set to True or False only. ' + '\033[0m' ) exit(1) else: @@ -320,25 +321,25 @@ def check_args(expect_failure: str): 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") + 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", + '-r', + '--robot', + action='append', nargs=4, - metavar=("init_x", "init_y", "final_x", "final_y"), - help="The robot starting and final positions.", + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.', ) group.add_argument( - "-rs", - "--robots", - action="append", + '-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.", + + 'Repeating the argument for multiple robots is supported.', ) args, unknown = parser.parse_known_args() @@ -362,15 +363,15 @@ def main(argv=sys.argv[1:]): # stop and shutdown the nav stack to exit cleanly tester.shutdown() - testers[0].info_msg("Done Shutting Down.") + testers[0].info_msg('Done Shutting Down.') if passed != expect_failure: - testers[0].info_msg("Exiting failed") + testers[0].info_msg('Exiting failed') exit(1) else: - testers[0].info_msg("Exiting passed") + testers[0].info_msg('Exiting passed') exit(0) -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 67a4839abf7..c621ec96dae 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 0.4.0 + 1.1.0 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 930034f09cf..86fa5fdaee1 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -12,7 +12,6 @@ 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) @@ -29,7 +28,6 @@ set(dependencies rclcpp lifecycle_msgs rclcpp_action - test_msgs rclcpp_lifecycle bondcpp bond diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 17e654fc287..884b7b366ed 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -19,6 +19,7 @@ #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.hpp" @@ -42,48 +43,80 @@ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle) } /** - * @brief Get the L2 distance between 2 geometry_msgs::Points + * @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 - * @return double L2 distance + * @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) { - return euclidean_distance(pos1.pose, pos2.pose); + double dx = pos1.x - pos2.x; + double dy = pos1.y - pos2.y; + + return std::hypot(dx, dy); } /** @@ -107,6 +140,25 @@ inline Iter min_by(Iter begin, Iter end, Getter getCompareVal) 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 diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 5543b5fffc4..d4ba1688499 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -30,15 +30,9 @@ 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 background node threads - * and manipulating parameters + * @brief A lifecycle node wrapper to enable common Nav2 needs such as manipulating parameters */ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode { @@ -47,13 +41,11 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode * @brief A lifecycle node constructor * @param node_name Name for the node * @param namespace Namespace for the node, if any - * @param use_rclcpp_node Whether to create an internal client node * @param options Node options */ LifecycleNode( const std::string & node_name, const std::string & ns = "", - bool use_rclcpp_node = false, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); virtual ~LifecycleNode(); @@ -189,17 +181,7 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode /** * @brief Print notifications for lifecycle node */ - void print_lifecycle_node_notification(); - - // Whether or not to create a local rclcpp::Node which can be used for ROS2 classes that don't - // yet support lifecycle nodes - bool use_rclcpp_node_; - - // The local node - rclcpp::Node::SharedPtr rclcpp_node_; - - // When creating a local node, this class will launch a separate thread created to spin the node - std::unique_ptr rclcpp_thread_; + void printLifecycleNodeNotification(); // Connection to tell that server is still up std::unique_ptr bond_{nullptr}; diff --git a/nav2_util/include/nav2_util/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp index c11bbf7ce48..cceb5393684 100644 --- a/nav2_util/include/nav2_util/lifecycle_service_client.hpp +++ b/nav2_util/include/nav2_util/lifecycle_service_client.hpp @@ -40,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); @@ -51,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/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 5281ff88130..b9712044b65 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -18,10 +18,6 @@ #define NAV2_UTIL__ROBOT_UTILS_HPP_ #include -#include -#include -#include -#include #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" diff --git a/nav2_util/package.xml b/nav2_util/package.xml index fa76bfe28f4..60a2b0a093d 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.0.0 + 1.1.0 TODO Michael Jeronimo Mohammad Haghighipanah @@ -24,7 +24,6 @@ bondcpp bond rclcpp_action - test_msgs rclcpp_lifecycle launch launch_testing_ament_cmake @@ -38,6 +37,7 @@ launch launch_testing_ament_cmake std_srvs + test_msgs action_msgs launch_testing_ros ament_cmake_pytest diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index ee9f69ee55c..a639a0e59e9 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -30,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 @@ -46,6 +39,5 @@ install(TARGETS install(TARGETS lifecycle_bringup - dump_params RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_util/src/dump_params.cpp b/nav2_util/src/dump_params.cpp deleted file mode 100644 index 09c932fd15b..00000000000 --- a/nav2_util/src/dump_params.cpp +++ /dev/null @@ -1,408 +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 _WIN32 -#include -#endif - -#include -#include -#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 8948884ed18..9868a38bbbe 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -23,29 +23,11 @@ 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 & ns, bool use_rclcpp_node, + const std::string & ns, const rclcpp::NodeOptions & options) -: rclcpp_lifecycle::LifecycleNode(node_name, ns, options), - use_rclcpp_node_(use_rclcpp_node) +: rclcpp_lifecycle::LifecycleNode(node_name, ns, options) { // server side never times out from lifecycle manager this->declare_parameter(bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true); @@ -53,18 +35,7 @@ LifecycleNode::LifecycleNode( rclcpp::Parameter( bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true)); - 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( - "_", ns, rclcpp::NodeOptions(options).arguments(new_args)); - rclcpp_thread_ = std::make_unique(rclcpp_node_); - } - - print_lifecycle_node_notification(); + printLifecycleNodeNotification(); } LifecycleNode::~LifecycleNode() @@ -102,7 +73,7 @@ void LifecycleNode::destroyBond() } } -void LifecycleNode::print_lifecycle_node_notification() +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 dfad1efe387..788c44123af 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/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 4d3b5511dc8..9f1ae99bbcb 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_dump_params/dump_params_default.txt b/nav2_util/test/test_dump_params/dump_params_default.txt deleted file mode 100644 index 1d9f982e1d8..00000000000 --- 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 814e4ce6e16..00000000000 --- 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 765385262b5..00000000000 --- 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 425ad44e296..00000000000 --- a/nav2_util/test/test_dump_params/dump_params_md.txt +++ /dev/null @@ -1,14 +0,0 @@ -## test_dump_params Parameters -|Parameter|Default Value| -|---|---| -|use_sim_time|False| -|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 4f086bc4e55..00000000000 --- a/nav2_util/test/test_dump_params/dump_params_md_verbose.txt +++ /dev/null @@ -1,14 +0,0 @@ -## test_dump_params Parameters -|Parameter|Default Value|Range|Description|Additional Constraints|Read-Only| -|---|---|---|---|---|---| -|use_sim_time|False|N/A|||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 df5b6314cfa..00000000000 --- a/nav2_util/test/test_dump_params/dump_params_multiple.txt +++ /dev/null @@ -1,26 +0,0 @@ -test_dump_params: - ros__parameters: - use_sim_time: False - 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_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 ffb35a30e1c..00000000000 --- a/nav2_util/test/test_dump_params/dump_params_yaml.txt +++ /dev/null @@ -1,13 +0,0 @@ -test_dump_params: - ros__parameters: - use_sim_time: False - 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 b49e53797c0..00000000000 --- a/nav2_util/test/test_dump_params/dump_params_yaml_verbose.txt +++ /dev/null @@ -1,63 +0,0 @@ -test_dump_params: - ros__parameters: - use_sim_time: False - # Range: N/A - # Description: - # 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 e0849b398fb..00000000000 --- 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 61a85c514cd..00000000000 --- 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 4733c19f584..00000000000 --- 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 c1a0cdb814c..00000000000 --- a/nav2_util/test/test_dump_params/test_dump_params_node.py +++ /dev/null @@ -1,65 +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_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 545bf4934cf..00000000000 --- 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 52a2d4b457d..38f27ad3044 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -22,7 +22,7 @@ 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; @@ -34,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; @@ -49,7 +79,7 @@ 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) diff --git a/nav2_util/test/test_lifecycle_node.cpp b/nav2_util/test/test_lifecycle_node.cpp index 358e83ddbe5..fd204df47f2 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_velocity_smoother/CMakeLists.txt b/nav2_velocity_smoother/CMakeLists.txt new file mode 100644 index 00000000000..ae92e41d90a --- /dev/null +++ b/nav2_velocity_smoother/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_velocity_smoother) + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_util REQUIRED) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +nav2_package() + +include_directories( + include +) + +set(executable_name velocity_smoother) +set(library_name ${executable_name}_core) + +set(dependencies + rclcpp + rclcpp_components + geometry_msgs + nav2_util +) + +# Main library +add_library(${library_name} SHARED + src/velocity_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}) + +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_velocity_smoother::VelocitySmoother") + +install( + TARGETS ${library_name} + 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}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md new file mode 100644 index 00000000000..bdd55277888 --- /dev/null +++ b/nav2_velocity_smoother/README.md @@ -0,0 +1,87 @@ +# Velocity Smoother + +The ``nav2_velocity_smoother`` is a package containing a lifecycle-component node for smoothing velocities sent by Nav2 to robot controllers. +The aim of this package is to implement velocity, acceleration, and deadband smoothing from Nav2 to reduce wear-and-tear on robot motors and hardware controllers by smoothing out the accelerations/jerky movements that might be present with some local trajectory planners' control efforts. + +It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some intepretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). + +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions. + +## Features + +This package was created to do the following: + +- Limit velocity commands by kinematic constraints, including velocity and acceleration +- Limit velocities based on deadband regions +- Stop sending velocities after a given timeout duration of no new commands (due to stopped navigation) +- Send a zero-velocity command at velocity timeout to stop the robot, in case not properly handled +- Support Omni and differential drive robots (e.g. X, Y, Theta) +- Smooth velocities proportionally in the same direction as commanded, whenever possible within kinematic limits +- Provide open loop and closed loop options +- Component nodes for use in single-process systems and stand-alone node format +- Dynamically reconfigurable parameters + +## Design + +This is a lifecycle-component node, using the lifecycle manager for state management and composition for process management. +It is designed to take in a command from Nav2's controller server and smooth it for use on robot hardware controllers. +Thusly, it takes in a command via the `cmd_vel` topic and produces a smoothed output on `smoothed_cmd_vel`. + +The node is designed on a regular timer running at a configurable rate. +This is in contrast to simply computing a smoothed velocity command in the callback of each `cmd_vel` input from Nav2. +This allows us to interpolate commands at a higher frequency than Nav2's local trajectory planners can provide. +For example, if a local trajectory planner is running at 20hz, the velocity smoother can run at 100hz to provide approximately 5 messages to a robot controller which will be smoothed by kinematic limits at each timestep. +This provides a more regular stream of commands to a robot base and interpolates commands between the current velocity and the desired velocity more finely for smoother acceleration / motion profiles. +While this is not required, it is a nice design feature. +It is possible to also simply run the smoother at `cmd_vel` rate to smooth velocities alone without interpolation. + +There are two primary operation modes: open and closed loop. +In open-loop, the node assumes that the robot was able to achieve the velocity send to it in the last command which was smoothed (which should be a good assumption if acceleration limits are set properly). +This is useful when robot odometry is not particularly accurate or has significant latency relative to `smoothing_frequency` so there isn't a delay in the feedback loop. +In closed-loop, the node will read from the odometry topic and apply a smoother over it to obtain the robot's current speed. +This will be used to determine the robot's current velocity and therefore achivable velocity targets by the velocity, acceleration, and deadband constraints using live data. + +## Parameters + +See inline description of parameters in the `VelocitySmoother`. + +``` +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 # Rate to run smoother + scale_velocities: false # scale velocities proportionally if any axis is outside of acceleration range to follow same vector, if possible + feedback: "OPEN_LOOP" # Type of feedback for current speed. Open loop uses the last smoothed output. Closed loop uses robot odometry + max_velocity: [0.5, 0.0, 2.5] # Maximum velocities, ordered [Vx, Vy, Vw] + min_velocity: [-0.5, 0.0, -2.5] # Minimum velocities, ordered [Vx, Vy, Vw] + deadband_velocity: [0.0, 0.0, 0.0] # A deadband of velocities below which they should be zero-ed out for sending to the robot base controller, ordered [Vx, Vy, Vw] + velocity_timeout: 1.0 # Time (s) after which if no new velocity commands are received to zero out and stop + max_accel: [2.5, 0.0, 3.2] # Maximum acceleration, ordered [Ax, Ay, Aw] + max_decel: [-2.5, 0.0, -3.2] # Maximum deceleration, ordered [Ax, Ay, Aw] + odom_topic: "odom" # Topic of odometry to use for estimating current velocities + odom_duration: 0.1 # Period of time (s) to sample odometry information in for velocity estimation +``` + +## Topics + +| Topic | Type | Use | +|------------------|-------------------------|-------------------------------| +| smoothed_cmd_vel | geometry_msgs/Twist | Publish smoothed velocities | +| cmd_vel | geometry_msgs/Twist | Subscribe to input velocities | + + +## Install + +``` +sudo apt-get install ros--nav2-velocity-smoother +``` + +## Etc (Important Side Notes) + +Typically: if you have low rate odometry, you should use open-loop mode or set the smoothing frequency relatively similar to that of your `cmd_vel` topic. If you have high rate odometry, you can use closed-loop mode with a higher smoothing frequency since you'll have more up to date information to smooth based off of. Do not exceed the smoothing frequency to your odometry frequency in closed loop mode. Also consider the ``odom_duration`` to use relative to your odometry publication rate and noise characteristics. +If the smoothing frequency out paces odometry or poorly selected ``odom_duration``s are used, the robot can oscillate and/or accelerate slowly due to latency in closed loop mode. + +When in doubt, open-loop is a reasonable choice for most users. + +The minimum and maximum velocities for rotation (e.g. ``Vw``) represent left and right turns. While we make it possible to specify these separately, most users would be wise to set these values the same (but signed) for rotation. Additionally, the parameters are signed, so it is important to specify maximum deceleration with negative signs to represent deceleration. Minimum velocities with negatives when moving backward, so backward movement can be restricted by setting this to ``0``. + +Deadband velocities are minimum thresholds, below which we set its value to `0`. This can be useful when your robot's breaking torque from stand still is non-trivial so sending very small values will pull high amounts of current. diff --git a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp new file mode 100644 index 00000000000..c3e7835c6b8 --- /dev/null +++ b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp @@ -0,0 +1,160 @@ +// Copyright (c) 2022 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_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_ +#define NAV2_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/odometry_utils.hpp" + +namespace nav2_velocity_smoother +{ + +/** + * @class nav2_velocity_smoother::VelocitySmoother + * @brief This class that smooths cmd_vel velocities for robot bases + */ +class VelocitySmoother : public nav2_util::LifecycleNode +{ +public: + /** + * @brief A constructor for nav2_velocity_smoother::VelocitySmoother + * @param options Additional options to control creation of the node. + */ + explicit VelocitySmoother(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /** + * @brief Destructor for nav2_velocity_smoother::VelocitySmoother + */ + ~VelocitySmoother(); + + /** + * @brief Find the scale factor, eta, which scales axis into acceleration range + * @param v_curr current velocity + * @param v_cmd commanded velocity + * @param accel maximum acceleration + * @param decel maximum deceleration + * @return Scale factor, eta + */ + double findEtaConstraint( + const double v_curr, const double v_cmd, + const double accel, const double decel); + + /** + * @brief Apply acceleration and scale factor constraints + * @param v_curr current velocity + * @param v_cmd commanded velocity + * @param accel maximum acceleration + * @param decel maximum deceleration + * @param eta Scale factor + * @return Velocity command + */ + double applyConstraints( + const double v_curr, const double v_cmd, + const double accel, const double decel, const double eta); + +protected: + /** + * @brief Configures parameters and member variables + * @param state LifeCycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Activates member variables + * @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 + * @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. + * @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; + + /** + * @brief Callback for incoming velocity commands + * @param msg Twist message + */ + void inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + + /** + * @brief Main worker timer function + */ + void smootherTimer(); + + /** + * @brief Dynamic reconfigure callback + * @param parameters Parameter list to change + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + + // Network interfaces + std::unique_ptr odom_smoother_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + smoothed_cmd_pub_; + rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Clock::SharedPtr clock_; + geometry_msgs::msg::Twist last_cmd_; + geometry_msgs::msg::Twist::SharedPtr command_; + + // Parameters + double smoothing_frequency_; + double odom_duration_; + std::string odom_topic_; + bool open_loop_; + bool stopped_{true}; + bool scale_velocities_; + std::vector max_velocities_; + std::vector min_velocities_; + std::vector max_accels_; + std::vector max_decels_; + std::vector deadband_velocities_; + rclcpp::Duration velocity_timeout_{0, 0}; + rclcpp::Time last_command_time_; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; +}; + +} // namespace nav2_velocity_smoother + +#endif // NAV2_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_ diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml new file mode 100644 index 00000000000..63d6abd37bc --- /dev/null +++ b/nav2_velocity_smoother/package.xml @@ -0,0 +1,24 @@ + + + + nav2_velocity_smoother + 1.0.0 + Nav2's Output velocity smoother + Steve Macenski + Apache-2.0 + + ament_cmake + nav2_common + rclcpp + rclcpp_components + geometry_msgs + nav2_util + + ament_lint_common + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + diff --git a/nav2_velocity_smoother/src/main.cpp b/nav2_velocity_smoother/src/main.cpp new file mode 100644 index 00000000000..aa3af41c015 --- /dev/null +++ b/nav2_velocity_smoother/src/main.cpp @@ -0,0 +1,28 @@ +// Copyright (c) 2022 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 "nav2_velocity_smoother/velocity_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_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp new file mode 100644 index 00000000000..6573d74684f --- /dev/null +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -0,0 +1,366 @@ +// Copyright (c) 2022 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 + +#include "nav2_velocity_smoother/velocity_smoother.hpp" + +using namespace std::chrono_literals; +using nav2_util::declare_parameter_if_not_declared; +using std::placeholders::_1; +using rcl_interfaces::msg::ParameterType; + +namespace nav2_velocity_smoother +{ + +VelocitySmoother::VelocitySmoother(const rclcpp::NodeOptions & options) +: LifecycleNode("velocity_smoother", "", options), + last_command_time_{0, 0, get_clock()->get_clock_type()} +{ +} + +VelocitySmoother::~VelocitySmoother() +{ + if (timer_) { + timer_->cancel(); + timer_.reset(); + } +} + +nav2_util::CallbackReturn +VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Configuring velocity smoother"); + auto node = shared_from_this(); + std::string feedback_type; + double velocity_timeout_dbl; + + // Smoothing metadata + declare_parameter_if_not_declared(node, "smoothing_frequency", rclcpp::ParameterValue(20.0)); + declare_parameter_if_not_declared( + node, "feedback", rclcpp::ParameterValue(std::string("OPEN_LOOP"))); + declare_parameter_if_not_declared(node, "scale_velocities", rclcpp::ParameterValue(false)); + node->get_parameter("smoothing_frequency", smoothing_frequency_); + node->get_parameter("feedback", feedback_type); + node->get_parameter("scale_velocities", scale_velocities_); + + // Kinematics + declare_parameter_if_not_declared( + node, "max_velocity", rclcpp::ParameterValue(std::vector{0.50, 0.0, 2.5})); + declare_parameter_if_not_declared( + node, "min_velocity", rclcpp::ParameterValue(std::vector{-0.50, 0.0, -2.5})); + declare_parameter_if_not_declared( + node, "max_accel", rclcpp::ParameterValue(std::vector{2.5, 0.0, 3.2})); + declare_parameter_if_not_declared( + node, "max_decel", rclcpp::ParameterValue(std::vector{-2.5, 0.0, -3.2})); + node->get_parameter("max_velocity", max_velocities_); + node->get_parameter("min_velocity", min_velocities_); + node->get_parameter("max_accel", max_accels_); + node->get_parameter("max_decel", max_decels_); + + for (unsigned int i = 0; i != max_decels_.size(); i++) { + if (max_decels_[i] > 0.0) { + RCLCPP_WARN( + get_logger(), + "Positive values set of deceleration! These should be negative to slow down!"); + } + } + + // Get feature parameters + declare_parameter_if_not_declared(node, "odom_topic", rclcpp::ParameterValue("odom")); + declare_parameter_if_not_declared(node, "odom_duration", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, "deadband_velocity", rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + declare_parameter_if_not_declared(node, "velocity_timeout", rclcpp::ParameterValue(1.0)); + node->get_parameter("odom_topic", odom_topic_); + node->get_parameter("odom_duration", odom_duration_); + node->get_parameter("deadband_velocity", deadband_velocities_); + node->get_parameter("velocity_timeout", velocity_timeout_dbl); + velocity_timeout_ = rclcpp::Duration::from_seconds(velocity_timeout_dbl); + + if (max_velocities_.size() != 3 || min_velocities_.size() != 3 || + max_accels_.size() != 3 || max_decels_.size() != 3 || deadband_velocities_.size() != 3) + { + throw std::runtime_error( + "Invalid setting of kinematic and/or deadband limits!" + " All limits must be size of 3 representing (x, y, theta)."); + } + + // Get control type + if (feedback_type == "OPEN_LOOP") { + open_loop_ = true; + } else if (feedback_type == "CLOSED_LOOP") { + open_loop_ = false; + odom_smoother_ = std::make_unique(node, odom_duration_, odom_topic_); + } else { + throw std::runtime_error("Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + } + + // Setup inputs / outputs + smoothed_cmd_pub_ = create_publisher("cmd_vel_smoothed", 1); + cmd_sub_ = create_subscription( + "cmd_vel", rclcpp::QoS(1), + std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1)); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +VelocitySmoother::on_activate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Activating"); + smoothed_cmd_pub_->on_activate(); + double timer_duration_ms = 1000.0 / smoothing_frequency_; + timer_ = create_wall_timer( + std::chrono::milliseconds(static_cast(timer_duration_ms)), + std::bind(&VelocitySmoother::smootherTimer, this)); + + dyn_params_handler_ = this->add_on_set_parameters_callback( + std::bind(&VelocitySmoother::dynamicParametersCallback, this, _1)); + + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +VelocitySmoother::on_deactivate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + if (timer_) { + timer_->cancel(); + timer_.reset(); + } + smoothed_cmd_pub_->on_deactivate(); + dyn_params_handler_.reset(); + + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +VelocitySmoother::on_cleanup(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + smoothed_cmd_pub_.reset(); + odom_smoother_.reset(); + cmd_sub_.reset(); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +VelocitySmoother::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} + +void VelocitySmoother::inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + command_ = msg; + last_command_time_ = now(); +} + +double VelocitySmoother::findEtaConstraint( + const double v_curr, const double v_cmd, const double accel, const double decel) +{ + // Exploiting vector scaling properties + const double v_component_max = accel / smoothing_frequency_; + const double v_component_min = decel / smoothing_frequency_; + const double dv = v_cmd - v_curr; + + if (dv > v_component_max) { + return v_component_max / dv; + } + + if (dv < v_component_min) { + return v_component_min / dv; + } + + return -1.0; +} + +double VelocitySmoother::applyConstraints( + const double v_curr, const double v_cmd, + const double accel, const double decel, const double eta) +{ + double dv = v_cmd - v_curr; + const double v_component_max = accel / smoothing_frequency_; + const double v_component_min = decel / smoothing_frequency_; + return v_curr + std::clamp(eta * dv, v_component_min, v_component_max); +} + +void VelocitySmoother::smootherTimer() +{ + auto cmd_vel = std::make_unique(); + + // Check for velocity timeout. If nothing received, publish zeros to stop robot + if (now() - last_command_time_ > velocity_timeout_) { + last_cmd_ = geometry_msgs::msg::Twist(); + if (!stopped_) { + smoothed_cmd_pub_->publish(std::move(cmd_vel)); + } + stopped_ = true; + return; + } + + stopped_ = false; + + // Get current velocity based on feedback type + geometry_msgs::msg::Twist current_; + if (open_loop_) { + current_ = last_cmd_; + } else { + current_ = odom_smoother_->getTwist(); + } + + // Apply absolute velocity restrictions to the command + command_->linear.x = std::clamp(command_->linear.x, min_velocities_[0], max_velocities_[0]); + command_->linear.y = std::clamp(command_->linear.y, min_velocities_[1], max_velocities_[1]); + command_->angular.z = std::clamp(command_->angular.z, min_velocities_[2], max_velocities_[2]); + + // Find if any component is not within the acceleration constraints. If so, store the most + // significant scale factor to apply to the vector , eta, to reduce all axes + // proportionally to follow the same direction, within change of velocity bounds. + // In case eta reduces another axis out of its own limit, apply accel constraint to guarantee + // output is within limits, even if it deviates from requested command slightly. + double eta = 1.0; + if (scale_velocities_) { + double curr_eta = -1.0; + + curr_eta = findEtaConstraint( + current_.linear.x, command_->linear.x, max_accels_[0], max_decels_[0]); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + + curr_eta = findEtaConstraint( + current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1]); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + + curr_eta = findEtaConstraint( + current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2]); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + } + + cmd_vel->linear.x = applyConstraints( + current_.linear.x, command_->linear.x, max_accels_[0], max_decels_[0], eta); + cmd_vel->linear.y = applyConstraints( + current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1], eta); + cmd_vel->angular.z = applyConstraints( + current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2], eta); + + // If open loop, assume we achieved it + if (open_loop_) { + last_cmd_ = *cmd_vel; + } + + // Apply deadband restrictions & publish + cmd_vel->linear.x = fabs(cmd_vel->linear.x) < deadband_velocities_[0] ? 0.0 : cmd_vel->linear.x; + cmd_vel->linear.y = fabs(cmd_vel->linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->linear.y; + cmd_vel->angular.z = fabs(cmd_vel->angular.z) < + deadband_velocities_[2] ? 0.0 : cmd_vel->angular.z; + smoothed_cmd_pub_->publish(std::move(cmd_vel)); +} + +rcl_interfaces::msg::SetParametersResult +VelocitySmoother::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == "smoothing_frequency") { + smoothing_frequency_ = parameter.as_double(); + if (timer_) { + timer_->cancel(); + timer_.reset(); + } + + double timer_duration_ms = 1000.0 / smoothing_frequency_; + timer_ = create_wall_timer( + std::chrono::milliseconds(static_cast(timer_duration_ms)), + std::bind(&VelocitySmoother::smootherTimer, this)); + } else if (name == "velocity_timeout") { + velocity_timeout_ = rclcpp::Duration::from_seconds(parameter.as_double()); + } else if (name == "odom_duration") { + odom_duration_ = parameter.as_double(); + odom_smoother_ = + std::make_unique( + shared_from_this(), odom_duration_, odom_topic_); + } + } else if (type == ParameterType::PARAMETER_DOUBLE_ARRAY) { + if (parameter.as_double_array().size() != 3) { + RCLCPP_WARN(get_logger(), "Invalid size of parameter %s. Must be size 3", name.c_str()); + result.successful = false; + break; + } + + if (name == "max_velocity") { + max_velocities_ = parameter.as_double_array(); + } else if (name == "min_velocity") { + min_velocities_ = parameter.as_double_array(); + } else if (name == "max_accel") { + max_accels_ = parameter.as_double_array(); + } else if (name == "max_decel") { + max_decels_ = parameter.as_double_array(); + } else if (name == "deadband_velocity") { + deadband_velocities_ = parameter.as_double_array(); + } + } else if (type == ParameterType::PARAMETER_STRING) { + if (name == "feedback") { + if (parameter.as_string() == "OPEN_LOOP") { + open_loop_ = true; + odom_smoother_.reset(); + } else if (parameter.as_string() == "CLOSED_LOOP") { + open_loop_ = false; + odom_smoother_ = + std::make_unique( + shared_from_this(), odom_duration_, odom_topic_); + } else { + RCLCPP_WARN( + get_logger(), "Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + result.successful = false; + break; + } + } else if (name == "odom_topic") { + odom_topic_ = parameter.as_string(); + odom_smoother_ = + std::make_unique( + shared_from_this(), odom_duration_, odom_topic_); + } + } + } + + return result; +} + +} // namespace nav2_velocity_smoother + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_velocity_smoother::VelocitySmoother) diff --git a/nav2_velocity_smoother/test/CMakeLists.txt b/nav2_velocity_smoother/test/CMakeLists.txt new file mode 100644 index 00000000000..781049578fb --- /dev/null +++ b/nav2_velocity_smoother/test/CMakeLists.txt @@ -0,0 +1,10 @@ +# Tests +ament_add_gtest(velocity_smoother_tests + test_velocity_smoother.cpp +) +ament_target_dependencies(velocity_smoother_tests + ${dependencies} +) +target_link_libraries(velocity_smoother_tests + ${library_name} +) diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp new file mode 100644 index 00000000000..dcc82c31ac7 --- /dev/null +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -0,0 +1,337 @@ +// Copyright (c) 2022 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_velocity_smoother/velocity_smoother.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/twist.hpp" + +using namespace std::chrono_literals; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother +{ +public: + VelSmootherShim() + : VelocitySmoother() {} + void configure(const rclcpp_lifecycle::State & state) {this->on_configure(state);} + void activate(const rclcpp_lifecycle::State & state) {this->on_activate(state);} + void deactivate(const rclcpp_lifecycle::State & state) {this->on_deactivate(state);} + void cleanup(const rclcpp_lifecycle::State & state) {this->on_cleanup(state);} + void shutdown(const rclcpp_lifecycle::State & state) {this->on_shutdown(state);} + + bool isOdomSmoother() {return odom_smoother_ ? true : false;} + bool hasCommandMsg() {return last_command_time_.nanoseconds() != 0;} + geometry_msgs::msg::Twist::SharedPtr lastCommandMsg() {return command_;} + + void sendCommandMsg(geometry_msgs::msg::Twist::SharedPtr msg) {inputCommandCallback(msg);} +}; + +TEST(VelocitySmootherTest, openLoopTestTimer) +{ + auto smoother = + std::make_shared(); + std::vector deadbands{0.2, 0.0, 0.0}; + smoother->declare_parameter("scale_velocities", rclcpp::ParameterValue(true)); + smoother->set_parameter(rclcpp::Parameter("scale_velocities", true)); + smoother->declare_parameter("deadband_velocity", rclcpp::ParameterValue(deadbands)); + smoother->set_parameter(rclcpp::Parameter("deadband_velocity", deadbands)); + rclcpp_lifecycle::State state; + smoother->configure(state); + smoother->activate(state); + + std::vector linear_vels; + auto subscription = smoother->create_subscription( + "cmd_vel_smoothed", + 1, + [&](geometry_msgs::msg::Twist::SharedPtr msg) { + linear_vels.push_back(msg->linear.x); + }); + + // Send a velocity command + auto cmd = std::make_shared(); + cmd->linear.x = 1.0; // Max is 0.5, so should threshold + smoother->sendCommandMsg(cmd); + + // Process velocity smoothing and send updated odometry based on commands + auto start = smoother->now(); + while (smoother->now() - start < 1.5s) { + rclcpp::spin_some(smoother->get_node_base_interface()); + } + + // Sanity check we have the approximately right number of messages for the timespan and timeout + EXPECT_GT(linear_vels.size(), 19u); + EXPECT_LT(linear_vels.size(), 30u); + + // Should have last command be a stop since we timed out the command stream + EXPECT_EQ(linear_vels.back(), 0.0); + + // From deadband, first few should be 0 until above 0.2 + for (unsigned int i = 0; i != linear_vels.size(); i++) { + if (linear_vels[i] != 0) { + EXPECT_GT(linear_vels[i], 0.2); + break; + } + } + + // Process to make sure stops at limit in velocity, + // doesn't exceed acceleration + for (unsigned int i = 0; i != linear_vels.size(); i++) { + EXPECT_TRUE(linear_vels[i] <= 0.5); + } +} + +TEST(VelocitySmootherTest, approxClosedLoopTestTimer) +{ + auto smoother = + std::make_shared(); + smoother->declare_parameter("feedback", rclcpp::ParameterValue(std::string("CLOSED_LOOP"))); + smoother->set_parameter(rclcpp::Parameter("feedback", std::string("CLOSED_LOOP"))); + rclcpp_lifecycle::State state; + smoother->configure(state); + smoother->activate(state); + + std::vector linear_vels; + auto subscription = smoother->create_subscription( + "cmd_vel_smoothed", + 1, + [&](geometry_msgs::msg::Twist::SharedPtr msg) { + linear_vels.push_back(msg->linear.x); + }); + + auto odom_pub = smoother->create_publisher("odom", 1); + odom_pub->on_activate(); + nav_msgs::msg::Odometry odom_msg; + odom_msg.header.frame_id = "odom"; + odom_msg.child_frame_id = "base_link"; + + // Fill buffer with 0 twisted-commands + for (unsigned int i = 0; i != 30; i++) { + odom_msg.header.stamp = smoother->now() + rclcpp::Duration::from_seconds(i * 0.01); + odom_pub->publish(odom_msg); + } + + // Send a velocity command + auto cmd = std::make_shared(); + cmd->linear.x = 1.0; // Max is 0.5, so should threshold + smoother->sendCommandMsg(cmd); + + // Process velocity smoothing and send updated odometry based on commands + auto start = smoother->now(); + while (smoother->now() - start < 1.5s) { + odom_msg.header.stamp = smoother->now(); + if (linear_vels.size() > 0) { + odom_msg.twist.twist.linear.x = linear_vels.back(); + } + odom_pub->publish(odom_msg); + rclcpp::spin_some(smoother->get_node_base_interface()); + } + + // Sanity check we have the approximately right number of messages for the timespan and timeout + EXPECT_GT(linear_vels.size(), 19u); + EXPECT_LT(linear_vels.size(), 30u); + + // Should have last command be a stop since we timed out the command stream + EXPECT_EQ(linear_vels.back(), 0.0); + + // Process to make sure stops at limit in velocity, + // doesn't exceed acceleration + for (unsigned int i = 0; i != linear_vels.size(); i++) { + if (i > 0) { + double diff = linear_vels[i] - linear_vels[i - 1]; + EXPECT_LT(diff, 0.126); // default accel of 0.5 / 20 hz = 0.125 + } + + EXPECT_TRUE(linear_vels[i] <= 0.5); + } +} + +TEST(VelocitySmootherTest, testfindEtaConstraint) +{ + auto smoother = + std::make_shared(); + rclcpp_lifecycle::State state; + // default frequency is 20.0 + smoother->configure(state); + + // In range + EXPECT_EQ(smoother->findEtaConstraint(1.0, 1.0, 1.5, -2.0), -1); + EXPECT_EQ(smoother->findEtaConstraint(0.5, 0.55, 1.5, -2.0), -1); + EXPECT_EQ(smoother->findEtaConstraint(0.5, 0.45, 1.5, -2.0), -1); + // Too high + EXPECT_EQ(smoother->findEtaConstraint(1.0, 2.0, 1.5, -2.0), 0.075); + // Too low + EXPECT_EQ(smoother->findEtaConstraint(1.0, 0.0, 1.5, -2.0), 0.1); + + // In a more realistic situation accelerating linear axis + EXPECT_NEAR(smoother->findEtaConstraint(0.40, 0.50, 1.5, -2.0), 0.75, 0.001); +} + +TEST(VelocitySmootherTest, testapplyConstraints) +{ + auto smoother = + std::make_shared(); + rclcpp_lifecycle::State state; + // default frequency is 20.0 + smoother->configure(state); + double no_eta = 1.0; + + // Apply examples from testfindEtaConstraint + // In range, so no eta or acceleration limit impact + EXPECT_EQ(smoother->applyConstraints(1.0, 1.0, 1.5, -2.0, no_eta), 1.0); + EXPECT_EQ(smoother->applyConstraints(0.5, 0.55, 1.5, -2.0, no_eta), 0.55); + EXPECT_EQ(smoother->applyConstraints(0.5, 0.45, 1.5, -2.0, no_eta), 0.45); + // Too high, without eta + EXPECT_NEAR(smoother->applyConstraints(1.0, 2.0, 1.5, -2.0, no_eta), 1.075, 0.01); + // Too high, with eta applied on its own axis + EXPECT_NEAR(smoother->applyConstraints(1.0, 2.0, 1.5, -2.0, 0.075), 1.075, 0.01); + // On another virtual axis that is OK + EXPECT_NEAR(smoother->applyConstraints(0.5, 0.55, 1.5, -2.0, 0.075), 0.503, 0.01); + + // In a more realistic situation, applied to angular + EXPECT_NEAR(smoother->applyConstraints(0.8, 1.0, 3.2, -3.2, 0.75), 1.075, 0.95); +} + +TEST(VelocitySmootherTest, testCommandCallback) +{ + auto smoother = + std::make_shared(); + rclcpp_lifecycle::State state; + smoother->configure(state); + smoother->activate(state); + + auto pub = smoother->create_publisher("cmd_vel", 1); + pub->on_activate(); + auto msg = std::make_unique(); + msg->linear.x = 100.0; + pub->publish(std::move(msg)); + rclcpp::spin_some(smoother->get_node_base_interface()); + + EXPECT_TRUE(smoother->hasCommandMsg()); + EXPECT_EQ(smoother->lastCommandMsg()->linear.x, 100.0); +} + +TEST(VelocitySmootherTest, testClosedLoopSub) +{ + auto smoother = + std::make_shared(); + smoother->declare_parameter("feedback", rclcpp::ParameterValue(std::string("OPEN_LOOP"))); + smoother->set_parameter(rclcpp::Parameter("feedback", std::string("CLOSED_LOOP"))); + rclcpp_lifecycle::State state; + smoother->configure(state); + EXPECT_TRUE(smoother->isOdomSmoother()); +} + +TEST(VelocitySmootherTest, testInvalidParams) +{ + auto smoother = + std::make_shared(); + std::vector max_vels{0.0, 0.0}; // invalid size + smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(max_vels)); + rclcpp_lifecycle::State state; + EXPECT_THROW(smoother->configure(state), std::runtime_error); + + smoother->set_parameter(rclcpp::Parameter("feedback", std::string("LAWLS"))); + EXPECT_THROW(smoother->configure(state), std::runtime_error); +} + +TEST(VelocitySmootherTest, testDynamicParameter) +{ + auto smoother = + std::make_shared(); + rclcpp_lifecycle::State state; + smoother->configure(state); + smoother->activate(state); + EXPECT_FALSE(smoother->isOdomSmoother()); + + auto rec_param = std::make_shared( + smoother->get_node_base_interface(), smoother->get_node_topics_interface(), + smoother->get_node_graph_interface(), + smoother->get_node_services_interface()); + + std::vector max_vel{10.0, 10.0, 10.0}; + std::vector min_vel{0.0, 0.0, 0.0}; + std::vector max_accel{10.0, 10.0, 10.0}; + std::vector min_accel{0.0, 0.0, 0.0}; + std::vector deadband{0.0, 0.0, 0.0}; + std::vector bad_test{0.0, 0.0}; + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("smoothing_frequency", 100.0), + rclcpp::Parameter("feedback", std::string("CLOSED_LOOP")), + rclcpp::Parameter("scale_velocities", true), + rclcpp::Parameter("max_velocity", max_vel), + rclcpp::Parameter("min_velocity", min_vel), + rclcpp::Parameter("max_accel", max_accel), + rclcpp::Parameter("max_decel", min_accel), + rclcpp::Parameter("odom_topic", std::string("TEST")), + rclcpp::Parameter("odom_duration", 2.0), + rclcpp::Parameter("velocity_timeout", 4.0), + rclcpp::Parameter("deadband_velocity", deadband)}); + + rclcpp::spin_until_future_complete( + smoother->get_node_base_interface(), + results); + + EXPECT_EQ(smoother->get_parameter("smoothing_frequency").as_double(), 100.0); + EXPECT_EQ(smoother->get_parameter("feedback").as_string(), std::string("CLOSED_LOOP")); + EXPECT_EQ(smoother->get_parameter("scale_velocities").as_bool(), true); + EXPECT_EQ(smoother->get_parameter("max_velocity").as_double_array(), max_vel); + EXPECT_EQ(smoother->get_parameter("min_velocity").as_double_array(), min_vel); + EXPECT_EQ(smoother->get_parameter("max_accel").as_double_array(), max_accel); + EXPECT_EQ(smoother->get_parameter("max_decel").as_double_array(), min_accel); + EXPECT_EQ(smoother->get_parameter("odom_topic").as_string(), std::string("TEST")); + EXPECT_EQ(smoother->get_parameter("odom_duration").as_double(), 2.0); + EXPECT_EQ(smoother->get_parameter("velocity_timeout").as_double(), 4.0); + EXPECT_EQ(smoother->get_parameter("deadband_velocity").as_double_array(), deadband); + + // Test reverting + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("feedback", std::string("OPEN_LOOP"))}); + rclcpp::spin_until_future_complete( + smoother->get_node_base_interface(), results); + EXPECT_EQ(smoother->get_parameter("feedback").as_string(), std::string("OPEN_LOOP")); + + // Test invalid change + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("feedback", std::string("LAWLS"))}); + rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); + EXPECT_FALSE(results.get().successful); + + // Test invalid size + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("max_velocity", bad_test)}); + rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); + EXPECT_FALSE(results.get().successful); + + // test full state after major changes + smoother->deactivate(state); + smoother->cleanup(state); + smoother->shutdown(state); + smoother.reset(); +} 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 9e5ea941013..baff10d6840 100644 --- a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp +++ b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp @@ -232,12 +232,22 @@ class VoxelGrid if ((unsigned int)(dist) < min_length) { return; } - double scale = std::min(1.0, max_length / dist); - - // Updating starting point to the point at distance min_length from the initial point - double min_x0 = x0 + (x1 - x0) / dist * min_length; - double min_y0 = y0 + (y1 - y0) / dist * min_length; - double min_z0 = z0 + (z1 - z0) / dist * min_length; + 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 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 9390c2b8ef1..e11e73b3db5 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.0.0 + 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/test/voxel_grid_bresenham_3d.cpp b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp index 10ff23dd87a..cce36e7fc48 100644 --- a/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp +++ b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp @@ -49,6 +49,10 @@ class TestVoxel ASSERT_TRUE(off < size_); data_[off] = val; } + inline unsigned int operator()(unsigned int off) + { + return data_[off]; + } private: uint32_t * data_; @@ -122,6 +126,29 @@ TEST(voxel_grid, bresenham3DBoundariesCheck) } } +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); diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 669be9c732f..5bb9db52d00 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.0.0 + 1.1.0 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/nav2_waypoint_follower/plugins/input_at_waypoint.cpp b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp index 32c33cfc03d..5c613219ff6 100644 --- a/nav2_waypoint_follower/plugins/input_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp @@ -11,12 +11,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp" + #include #include +#include "pluginlib/class_list_macros.hpp" + #include "nav2_util/node_utils.hpp" -#include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp" namespace nav2_waypoint_follower { diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp index 7d1f4a9b1b4..8a6cb74b96a 100644 --- a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp" #include #include +#include "pluginlib/class_list_macros.hpp" + #include "nav2_util/node_utils.hpp" -#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp" namespace nav2_waypoint_follower { diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp index 3e8e6dbbcf9..917655e21a0 100644 --- a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp" + #include #include +#include "pluginlib/class_list_macros.hpp" + #include "nav2_util/node_utils.hpp" -#include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp" namespace nav2_waypoint_follower { diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index cb7ca2ebb38..804de9ad315 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -28,7 +28,7 @@ using rcl_interfaces::msg::ParameterType; using std::placeholders::_1; WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("waypoint_follower", "", false, options), +: nav2_util::LifecycleNode("waypoint_follower", "", options), waypoint_task_executor_loader_("nav2_waypoint_follower", "nav2_core::WaypointTaskExecutor") { @@ -263,7 +263,7 @@ WaypointFollower::followWaypoints() new_goal = true; if (goal_index >= goal->poses.size()) { RCLCPP_INFO( - get_logger(), "Completed all %lu waypoints requested.", + get_logger(), "Completed all %zu waypoints requested.", goal->poses.size()); result->missed_waypoints = failed_ids_; action_server_->succeeded_current(result); diff --git a/navigation2/package.xml b/navigation2/package.xml index 46b75dc5df0..35e61399a34 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.0.0 + 1.1.0 ROS2 Navigation Stack @@ -14,26 +14,31 @@ nav2_amcl + nav2_behavior_tree nav2_bt_navigator - nav2_costmap_2d + nav2_constrained_smoother + nav2_controller nav2_core + nav2_costmap_2d nav2_dwb_controller nav2_lifecycle_manager nav2_map_server - nav2_recoveries - nav2_planner - nav2_smoother 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_velocity_smoother nav2_voxel_grid - nav2_controller nav2_waypoint_follower - nav2_smac_planner - nav2_regulated_pure_pursuit_controller - nav2_rotation_shim_controller ament_cmake diff --git a/tools/planner_benchmarking/100by100_10.pgm b/tools/planner_benchmarking/100by100_10.pgm new file mode 100644 index 00000000000..c69b8a7fb0a 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 00000000000..6b8ecb1ff39 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 00000000000..0159f9763e2 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 00000000000..3865b0bbdbe --- /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 00000000000..443da00266f --- /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/run_test_suite.bash b/tools/run_test_suite.bash index 51323c566ab..3c807c5a9f1 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/underlay.repos b/tools/underlay.repos index 986e078866b..658f35d7a35 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -23,3 +23,7 @@ repositories: # 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