diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions new file mode 100644 index 00000000..33cf69e0 --- /dev/null +++ b/.cppcheck_suppressions @@ -0,0 +1,4 @@ +*:*/test/* + +missingIncludeSystem +unmatchedSuppression diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 30cd6ed4..dd724dae 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,3 +1,6 @@ ### Automatically generated from package.xml ### +common/autoware_geography_utils/** anh.nguyen.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners +common/autoware_node/** mfc@autoware.org @autowarefoundation/autoware-core-global-codeowners +demos/autoware_test_node/** mfc@autoware.org @autowarefoundation/autoware-core-global-codeowners ### Copied from .github/CODEOWNERS-manual ### diff --git a/.github/actions/build-and-test-differential/action.yaml b/.github/actions/build-and-test-differential/action.yaml index 89893e9f..175e71c4 100644 --- a/.github/actions/build-and-test-differential/action.yaml +++ b/.github/actions/build-and-test-differential/action.yaml @@ -5,12 +5,6 @@ inputs: rosdistro: description: "" required: true - container: - description: "" - required: true - container-suffix: - description: "" - required: true runner: description: "" required: true @@ -63,16 +57,6 @@ runs: ccache --zero-stats shell: bash - - name: Export CUDA state as a variable for adding to cache key - run: | - build_type_cuda_state=nocuda - if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then - build_type_cuda_state=cuda - fi - echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" - echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" - shell: bash - - name: Build if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-build@v1 @@ -80,7 +64,6 @@ runs: rosdistro: ${{ inputs.rosdistro }} target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} build-depends-repos: ${{ inputs.build-depends-repos }} - cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} build-pre-command: ${{ inputs.build-pre-command }} - name: Show ccache stats after build diff --git a/.github/dependabot.yaml b/.github/dependabot.yaml index 8fd9b7f4..8e2d7193 100644 --- a/.github/dependabot.yaml +++ b/.github/dependabot.yaml @@ -6,8 +6,9 @@ version: 2 updates: - package-ecosystem: github-actions directory: / + # https://docs.github.com/en/code-security/dependabot/dependabot-version-updates/configuration-options-for-the-dependabot.yml-file#scheduleinterval schedule: - interval: daily + interval: monthly open-pull-requests-limit: 1 labels: - tag:bot diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index ddc94fa9..448d57c0 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -27,6 +27,8 @@ sd " with:\n" " with:\n local-cspell-json: .cspell.json\n" {source} - source: .github/workflows/sync-files.yaml - source: .github/workflows/update-codeowners-from-packages.yaml + pre-commands: | + sd " auto-merge-method: squash\n" " auto-merge-method: squash\n global-codeowners: \"@autowarefoundation/autoware-core-global-codeowners\"\n" {source} - source: .clang-format - source: .clang-tidy - source: .markdown-link-check.json diff --git a/.github/workflows/build-and-test-daily.yaml b/.github/workflows/build-and-test-daily.yaml index 63822f8b..a79bbd62 100644 --- a/.github/workflows/build-and-test-daily.yaml +++ b/.github/workflows/build-and-test-daily.yaml @@ -7,20 +7,8 @@ on: jobs: build-and-test-daily: - runs-on: [self-hosted, linux, X64, gpu] - container: ${{ matrix.container }}${{ matrix.container-suffix }} - strategy: - fail-fast: false - matrix: - rosdistro: - - humble - container-suffix: - - "" - - -cuda - include: - - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:universe-devel - build-depends-repos: build_depends.repos + runs-on: ubuntu-24.04 + container: ghcr.io/autowarefoundation/autoware:core-devel steps: - name: Check out repository uses: actions/checkout@v4 @@ -37,33 +25,22 @@ jobs: id: get-self-packages uses: autowarefoundation/autoware-github-actions/get-self-packages@v1 - - name: Export CUDA state as a variable for adding to cache key - run: | - build_type_cuda_state=nocuda - if [[ "${{ matrix.container-suffix }}" == "-cuda" ]]; then - build_type_cuda_state=cuda - fi - echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" - echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" - shell: bash - - name: Build if: ${{ steps.get-self-packages.outputs.self-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: - rosdistro: ${{ matrix.rosdistro }} + rosdistro: humble target-packages: ${{ steps.get-self-packages.outputs.self-packages }} - build-depends-repos: ${{ matrix.build-depends-repos }} - cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} + build-depends-repos: build_depends.repos - name: Test if: ${{ steps.get-self-packages.outputs.self-packages != '' }} id: test uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: - rosdistro: ${{ matrix.rosdistro }} + rosdistro: humble target-packages: ${{ steps.get-self-packages.outputs.self-packages }} - build-depends-repos: ${{ matrix.build-depends-repos }} + build-depends-repos: build_depends.repos - name: Upload coverage to CodeCov if: ${{ steps.test.outputs.coverage-report-files != '' }} diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index e77aae16..e9375147 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -22,34 +22,11 @@ jobs: with: label: tag:run-build-and-test-differential - make-sure-require-cuda-label-is-present: - uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 - with: - label: tag:require-cuda-build-and-test - build-and-test-differential: - needs: [make-sure-label-is-present, make-sure-require-cuda-label-is-present] + needs: make-sure-label-is-present if: ${{ needs.make-sure-label-is-present.outputs.result == 'true' }} - runs-on: ${{ matrix.runner }} - container: ${{ matrix.container }}${{ matrix.container-suffix }} - strategy: - fail-fast: false - matrix: - rosdistro: - - humble - container-suffix: - - "" - - -cuda - include: - - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:universe-devel - build-depends-repos: build_depends.repos - - container-suffix: -cuda - runner: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - build-pre-command: taskset --cpu-list 0-6 - - container-suffix: "" - runner: ubuntu-latest - build-pre-command: "" + runs-on: ubuntu-24.04 + container: ghcr.io/autowarefoundation/autoware:core-devel steps: - name: Set PR fetch depth run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" @@ -62,21 +39,16 @@ jobs: fetch-depth: ${{ env.PR_FETCH_DEPTH }} - name: Run build-and-test-differential action - if: ${{ !(matrix.container-suffix == '-cuda') || needs.make-sure-require-cuda-label-is-present.outputs.result == 'true' }} uses: ./.github/actions/build-and-test-differential with: - rosdistro: ${{ matrix.rosdistro }} - container: ${{ matrix.container }} - container-suffix: ${{ matrix.container-suffix }} - runner: ${{ matrix.runner }} - build-depends-repos: ${{ matrix.build-depends-repos }} - build-pre-command: ${{ matrix.build-pre-command }} + rosdistro: humble + build-depends-repos: build_depends.repos codecov-token: ${{ secrets.CODECOV_TOKEN }} clang-tidy-differential: needs: build-and-test-differential - runs-on: ubuntu-22.04 - container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda + runs-on: ubuntu-24.04 + container: ghcr.io/autowarefoundation/autoware:core-devel steps: - name: Set PR fetch depth run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" @@ -112,7 +84,6 @@ jobs: target-files: ${{ steps.get-changed-files.outputs.changed-files }} clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci build-depends-repos: build_depends.repos - cache-key-element: cuda - name: Show disk space after the tasks run: df -h diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index a9274804..52bdc783 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -4,11 +4,11 @@ on: push: branches: - main - workflow_dispatch: concurrency: - group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.run_id }} - cancel-in-progress: true + # Ensures sequential execution of this workflow + group: ${{ github.workflow }} + cancel-in-progress: false env: CC: /usr/lib/ccache/gcc @@ -16,19 +16,8 @@ env: jobs: build-and-test: - runs-on: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - container: ${{ matrix.container }}${{ matrix.container-suffix }} - strategy: - fail-fast: false - matrix: - rosdistro: - - humble - container-suffix: - - -cuda - include: - - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:universe-devel - build-depends-repos: build_depends.repos + runs-on: ubuntu-24.04 + container: ghcr.io/autowarefoundation/autoware:core-devel steps: - name: Check out repository uses: actions/checkout@v4 @@ -59,9 +48,9 @@ jobs: with: path: | /root/.ccache - key: ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}-${{ github.sha }} + key: ccache-main-${{ runner.arch }}-humble-${{ github.sha }} restore-keys: | - ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}- + ccache-main-${{ runner.arch }}-humble- - name: Limit ccache size run: | @@ -75,47 +64,34 @@ jobs: ccache --zero-stats shell: bash - - name: Export CUDA state as a variable for adding to cache key - run: | - build_type_cuda_state=nocuda - if [[ "${{ matrix.container-suffix }}" == "-cuda" ]]; then - build_type_cuda_state=cuda - fi - echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" - echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" - shell: bash - - name: Build if: ${{ steps.get-self-packages.outputs.self-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: - rosdistro: ${{ matrix.rosdistro }} + rosdistro: humble target-packages: ${{ steps.get-self-packages.outputs.self-packages }} - build-depends-repos: ${{ matrix.build-depends-repos }} - cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} + build-depends-repos: build_depends.repos build-pre-command: taskset --cpu-list 0-6 - name: Show ccache stats after build run: du -sh ${CCACHE_DIR} && ccache -s shell: bash - # Only keep save the -cuda version because cuda packages covers non-cuda packages too - name: Push the ccache cache - if: matrix.container-suffix == '-cuda' uses: actions/cache/save@v4 with: path: | /root/.ccache - key: ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}-${{ github.sha }} + key: ccache-main-${{ runner.arch }}-humble-${{ github.sha }} - name: Test if: ${{ steps.get-self-packages.outputs.self-packages != '' }} id: test uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: - rosdistro: ${{ matrix.rosdistro }} + rosdistro: humble target-packages: ${{ steps.get-self-packages.outputs.self-packages }} - build-depends-repos: ${{ matrix.build-depends-repos }} + build-depends-repos: build_depends.repos - name: Upload coverage to CodeCov if: ${{ steps.test.outputs.coverage-report-files != '' }} diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index d39e97e5..47009a25 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -26,7 +26,7 @@ jobs: prevent-no-label-execution: uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 with: - label: tag:deploy-docs + label: run:deploy-docs deploy-docs: needs: prevent-no-label-execution diff --git a/.github/workflows/pre-commit-autoupdate.yaml b/.github/workflows/pre-commit-autoupdate.yaml index 489e32a1..60c17d9d 100644 --- a/.github/workflows/pre-commit-autoupdate.yaml +++ b/.github/workflows/pre-commit-autoupdate.yaml @@ -6,7 +6,7 @@ name: pre-commit-autoupdate on: schedule: - - cron: 0 0 * * * + - cron: 0 0 1 1,4,7,10 * # quarterly workflow_dispatch: jobs: diff --git a/.github/workflows/update-codeowners-from-packages.yaml b/.github/workflows/update-codeowners-from-packages.yaml index c9ecdb10..e71e6f62 100644 --- a/.github/workflows/update-codeowners-from-packages.yaml +++ b/.github/workflows/update-codeowners-from-packages.yaml @@ -35,3 +35,4 @@ jobs: tag:bot tag:update-codeowners-from-packages auto-merge-method: squash + global-codeowners: "@autowarefoundation/autoware-core-global-codeowners" diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e6dcc3f4..48a97c13 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,12 +2,16 @@ # https://github.com/autowarefoundation/sync-file-templates # To make changes, update the source repository and follow the guidelines in its README. +# https://pre-commit.ci/#configuration ci: autofix_commit_msg: "style(pre-commit): autofix" + # we already have our own daily update mechanism, we set this to quarterly + autoupdate_schedule: quarterly + autoupdate_commit_msg: "ci(pre-commit): quarterly autoupdate" repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - id: check-json - id: check-merge-conflict @@ -22,7 +26,7 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.41.0 + rev: v0.43.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] @@ -53,7 +57,7 @@ repos: - id: shellcheck - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.9.0-1 + rev: v3.10.0-2 hooks: - id: shfmt args: [-w, -s, -i=4] @@ -64,26 +68,26 @@ repos: - id: isort - repo: https://github.com/psf/black - rev: 24.8.0 + rev: 24.10.0 hooks: - id: black args: [--line-length=100] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.8 + rev: v19.1.5 hooks: - id: clang-format types_or: [c++, c, cuda] - repo: https://github.com/cpplint/cpplint - rev: 1.6.1 + rev: 2.0.0 hooks: - id: cpplint args: [--quiet] exclude: .cu - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.2 + rev: 0.30.0 hooks: - id: check-metaschema files: ^.+/schema/.*schema\.json$ diff --git a/CPPLINT.cfg b/CPPLINT.cfg index 81869c92..159042db 100644 --- a/CPPLINT.cfg +++ b/CPPLINT.cfg @@ -7,10 +7,12 @@ set noparent linelength=100 includeorder=standardcfirst filter=-build/c++11 # we do allow C++11 +filter=-build/c++17 # we allow filter=-build/namespaces_literals # we allow using namespace for literals filter=-runtime/references # we consider passing non-const references to be ok filter=-whitespace/braces # we wrap open curly braces for namespaces, classes and functions filter=-whitespace/indent # we don't indent keywords like public, protected and private with one space +filter=-whitespace/newline # we allow the developer to decide about newline at the end of file (it's clashing with clang-format) filter=-whitespace/parens # we allow closing parenthesis to be on the next line filter=-whitespace/semicolon # we allow the developer to decide about whitespace after a semicolon filter=-build/header_guard # we automatically fix the names of header guards using pre-commit diff --git a/build_depends.repos b/build_depends.repos index b9ee75e1..0b81f204 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -3,7 +3,3 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_msgs.git version: main - autoware_common: - type: git - url: https://github.com/autowarefoundation/autoware_common.git - version: main diff --git a/common/autoware_geography_utils/CHANGELOG.rst b/common/autoware_geography_utils/CHANGELOG.rst new file mode 100644 index 00000000..e3b57382 --- /dev/null +++ b/common/autoware_geography_utils/CHANGELOG.rst @@ -0,0 +1,35 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_geography_utils +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2025-01-09) +------------------ +* refactor(autoware_geography_utils): apply modern C++17 style (`#109 `_) + * use using + * refactor height + * refactor projection + * refactor lanelet2_projector + * set class name + * revert string + --------- +* test(autoware_geography_utils): add `lanelet2_projector` test (`#128 `_) + * add test + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* chore: sync files (`#115 `_) + * chore: sync files + * style(pre-commit): autofix + * include what you use + --------- + Co-authored-by: github-actions + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: M. Fatih Cırıt +* docs(autoware_geography_utils): update `README.md` (`#111 `_) + update readme +* feat: port autoware_geography_utils from autoware.universe (`#100 `_) + Co-authored-by: Yutaka Kondo +* Contributors: Ryohsuke Mitsudome, Yutaka Kondo, awf-autoware-bot[bot] + +0.0.0 (2024-12-02) +------------------ diff --git a/common/autoware_geography_utils/CMakeLists.txt b/common/autoware_geography_utils/CMakeLists.txt new file mode 100644 index 00000000..b4ab5c2f --- /dev/null +++ b/common/autoware_geography_utils/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_geography_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# GeographicLib +find_package(PkgConfig) +find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h + PATH_SUFFIXES GeographicLib +) +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) +find_library(GeographicLib_LIBRARIES NAMES Geographic) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/height.cpp + src/projection.cpp + src/lanelet2_projector.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${GeographicLib_LIBRARIES} +) + +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + + file(GLOB_RECURSE test_files test/*.cpp) + + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + +ament_auto_package() diff --git a/common/autoware_geography_utils/README.md b/common/autoware_geography_utils/README.md new file mode 100644 index 00000000..f560d8fb --- /dev/null +++ b/common/autoware_geography_utils/README.md @@ -0,0 +1,5 @@ +# autoware_geography_utils + +## Purpose + +This package contains geography-related utility functions used by other Autoware packages. It provides functionality for geographic coordinate transformations, height calculations, and Lanelet2 map projections. diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp new file mode 100644 index 00000000..0b0dbec0 --- /dev/null +++ b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp @@ -0,0 +1,34 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ +#define AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ + +#include + +namespace autoware::geography_utils +{ + +using HeightConversionFunction = + double (*)(const double height, const double latitude, const double longitude); + +double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude); +double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude); +double convert_height( + const double height, const double latitude, const double longitude, + const std::string & source_vertical_datum, const std::string & target_vertical_datum); + +} // namespace autoware::geography_utils + +#endif // AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp new file mode 100644 index 00000000..0eea2a9f --- /dev/null +++ b/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp @@ -0,0 +1,32 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ +#define AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ + +#include + +#include + +#include + +namespace autoware::geography_utils +{ +using MapProjectorInfo = autoware_map_msgs::msg::MapProjectorInfo; + +std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info); + +} // namespace autoware::geography_utils + +#endif // AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp new file mode 100644 index 00000000..86869dde --- /dev/null +++ b/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ +#define AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ + +#include +#include +#include + +namespace autoware::geography_utils +{ +using MapProjectorInfo = autoware_map_msgs::msg::MapProjectorInfo; +using GeoPoint = geographic_msgs::msg::GeoPoint; +using LocalPoint = geometry_msgs::msg::Point; + +[[nodiscard]] LocalPoint project_forward( + const GeoPoint & geo_point, const MapProjectorInfo & projector_info); +[[nodiscard]] GeoPoint project_reverse( + const LocalPoint & local_point, const MapProjectorInfo & projector_info); + +} // namespace autoware::geography_utils + +#endif // AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ diff --git a/common/autoware_geography_utils/package.xml b/common/autoware_geography_utils/package.xml new file mode 100644 index 00000000..7a18b1f0 --- /dev/null +++ b/common/autoware_geography_utils/package.xml @@ -0,0 +1,33 @@ + + + + autoware_geography_utils + 0.1.0 + The autoware_geography_utils package + Yamato Ando + Masahiro Sakamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto + Apache License 2.0 + Koji Minoda + + ament_cmake_auto + autoware_cmake + + autoware_lanelet2_extension + autoware_map_msgs + geographic_msgs + geographiclib + geometry_msgs + lanelet2_io + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_geography_utils/src/height.cpp b/common/autoware_geography_utils/src/height.cpp new file mode 100644 index 00000000..3c8b8d62 --- /dev/null +++ b/common/autoware_geography_utils/src/height.cpp @@ -0,0 +1,62 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include + +namespace autoware::geography_utils +{ + +double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude) +{ + GeographicLib::Geoid egm2008("egm2008-1"); + // cSpell: ignore ELLIPSOIDTOGEOID + return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); +} + +double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude) +{ + GeographicLib::Geoid egm2008("egm2008-1"); + // cSpell: ignore GEOIDTOELLIPSOID + return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); +} + +double convert_height( + const double height, const double latitude, const double longitude, + const std::string & source_vertical_datum, const std::string & target_vertical_datum) +{ + if (source_vertical_datum == target_vertical_datum) { + return height; + } + static const std::map, HeightConversionFunction> + conversion_map{ + {{"WGS84", "EGM2008"}, convert_wgs84_to_egm2008}, + {{"EGM2008", "WGS84"}, convert_egm2008_to_wgs84}, + }; + + const auto key = std::make_pair(source_vertical_datum, target_vertical_datum); + if (const auto it = conversion_map.find(key); it != conversion_map.end()) { + return it->second(height, latitude, longitude); + } + + throw std::invalid_argument( + "Invalid conversion types: " + source_vertical_datum + " to " + target_vertical_datum); +} + +} // namespace autoware::geography_utils diff --git a/common/autoware_geography_utils/src/lanelet2_projector.cpp b/common/autoware_geography_utils/src/lanelet2_projector.cpp new file mode 100644 index 00000000..9eee54b5 --- /dev/null +++ b/common/autoware_geography_utils/src/lanelet2_projector.cpp @@ -0,0 +1,59 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +#include +#include + +namespace autoware::geography_utils +{ + +std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info) +{ + if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { + const lanelet::GPSPoint position{ + projector_info.map_origin.latitude, projector_info.map_origin.longitude, + projector_info.map_origin.altitude}; + const lanelet::Origin origin{position}; + const lanelet::projection::UtmProjector projector{origin}; + return std::make_unique(projector); + } + + if (projector_info.projector_type == MapProjectorInfo::MGRS) { + lanelet::projection::MGRSProjector projector{}; + projector.setMGRSCode(projector_info.mgrs_grid); + return std::make_unique(projector); + } + + if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) { + const lanelet::GPSPoint position{ + projector_info.map_origin.latitude, projector_info.map_origin.longitude, + projector_info.map_origin.altitude}; + const lanelet::Origin origin{position}; + const lanelet::projection::TransverseMercatorProjector projector{origin}; + return std::make_unique(projector); + } + + throw std::invalid_argument( + "Invalid map projector type: " + projector_info.projector_type + + ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator"); +} + +} // namespace autoware::geography_utils diff --git a/common/autoware_geography_utils/src/projection.cpp b/common/autoware_geography_utils/src/projection.cpp new file mode 100644 index 00000000..bf3e50ea --- /dev/null +++ b/common/autoware_geography_utils/src/projection.cpp @@ -0,0 +1,94 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +namespace autoware::geography_utils +{ + +[[nodiscard]] Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) +{ + return Eigen::Vector3d{src.x, src.y, src.z}; +} + +LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info) +{ + std::unique_ptr projector = get_lanelet2_projector(projector_info); + const lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; + + lanelet::BasicPoint3d projected_local_point; + if (projector_info.projector_type == MapProjectorInfo::MGRS) { + constexpr int mgrs_precision = 9; // set precision as 100 micro meter + const auto mgrs_projector = dynamic_cast(projector.get()); + + // project x and y using projector + // note that the altitude is ignored in MGRS projection conventionally + projected_local_point = mgrs_projector->forward(position, mgrs_precision); + } else { + // project x and y using projector + // note that the original projector such as UTM projector does not compensate for the altitude + // offset + projected_local_point = projector->forward(position); + + // correct z based on the map origin + // note that the converted altitude in local point is in the same vertical datum as the geo + // point + projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude; + } + + LocalPoint local_point; + local_point.x = projected_local_point.x(); + local_point.y = projected_local_point.y(); + local_point.z = projected_local_point.z(); + + return local_point; +} + +GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info) +{ + std::unique_ptr projector = get_lanelet2_projector(projector_info); + + lanelet::GPSPoint projected_gps_point; + if (projector_info.projector_type == MapProjectorInfo::MGRS) { + const auto * mgrs_projector = + dynamic_cast(projector.get()); + // project latitude and longitude using projector + // note that the z is ignored in MGRS projection conventionally + projected_gps_point = + mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); + } else { + // project latitude and longitude using projector + // note that the original projector such as UTM projector does not compensate for the altitude + // offset + projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); + + // correct altitude based on the map origin + // note that the converted altitude in local point is in the same vertical datum as the geo + // point + projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude; + } + + GeoPoint geo_point; + geo_point.latitude = projected_gps_point.lat; + geo_point.longitude = projected_gps_point.lon; + geo_point.altitude = projected_gps_point.ele; + return geo_point; +} + +} // namespace autoware::geography_utils diff --git a/common/autoware_geography_utils/test/test_geography_utils.cpp b/common/autoware_geography_utils/test/test_geography_utils.cpp new file mode 100644 index 00000000..ee0e7428 --- /dev/null +++ b/common/autoware_geography_utils/test/test_geography_utils.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/geography_utils/height.hpp" +#include "autoware/geography_utils/lanelet2_projector.hpp" +#include "autoware/geography_utils/projection.hpp" + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + bool result = RUN_ALL_TESTS(); + return result; +} diff --git a/common/autoware_geography_utils/test/test_height.cpp b/common/autoware_geography_utils/test/test_height.cpp new file mode 100644 index 00000000..f624f6c3 --- /dev/null +++ b/common/autoware_geography_utils/test/test_height.cpp @@ -0,0 +1,86 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +// Test case to verify if same source and target datums return original height +TEST(GeographyUtils, SameSourceTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + const std::string datum = "WGS84"; + + double converted_height = + autoware::geography_utils::convert_height(height, latitude, longitude, datum, datum); + + EXPECT_DOUBLE_EQ(height, converted_height); +} + +// Test case to verify valid source and target datums +TEST(GeographyUtils, ValidSourceTargetDatum) +{ + // Calculated with + // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + const double target_height = -30.18; + + double converted_height = + autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); + + EXPECT_NEAR(target_height, converted_height, 0.1); +} + +// Test case to verify invalid source and target datums +TEST(GeographyUtils, InvalidSourceTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), + std::invalid_argument); +} + +// Test case to verify invalid source datums +TEST(GeographyUtils, InvalidSourceDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), + std::invalid_argument); +} + +// Test case to verify invalid target datums +TEST(GeographyUtils, InvalidTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), + std::invalid_argument); +} diff --git a/common/autoware_geography_utils/test/test_lanelet2_projector.cpp b/common/autoware_geography_utils/test/test_lanelet2_projector.cpp new file mode 100644 index 00000000..cb646890 --- /dev/null +++ b/common/autoware_geography_utils/test/test_lanelet2_projector.cpp @@ -0,0 +1,81 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include + +#include +#include + +TEST(GeographyUtilsLanelet2Projector, GetMGRSProjector) +{ + autoware_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; + + std::unique_ptr projector = + autoware::geography_utils::get_lanelet2_projector(projector_info); + + // Check if the returned projector is of type MGRSProjector + EXPECT_NE(dynamic_cast(projector.get()), nullptr); +} + +TEST(GeographyUtilsLanelet2Projector, GetLocalCartesianUTMProjector) +{ + autoware_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; + projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.62426; + projector_info.map_origin.longitude = 139.74252; + projector_info.map_origin.altitude = 0.0; + + std::unique_ptr projector = + autoware::geography_utils::get_lanelet2_projector(projector_info); + + // Check if the returned projector is of type UtmProjector + EXPECT_NE(dynamic_cast(projector.get()), nullptr); +} + +TEST(GeographyUtilsLanelet2Projector, GetTransverseMercatorProjector) +{ + autoware_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::TRANSVERSE_MERCATOR; + projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.62426; + projector_info.map_origin.longitude = 139.74252; + projector_info.map_origin.altitude = 0.0; + + std::unique_ptr projector = + autoware::geography_utils::get_lanelet2_projector(projector_info); + + // Check if the returned projector is of type TransverseMercatorProjector + EXPECT_NE( + dynamic_cast(projector.get()), nullptr); +} + +TEST(GeographyUtilsLanelet2Projector, InvalidProjectorType) +{ + autoware_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = "INVALID_TYPE"; + projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; + + // Check if the function throws an invalid_argument exception for invalid projector type + EXPECT_THROW( + autoware::geography_utils::get_lanelet2_projector(projector_info), std::invalid_argument); +} diff --git a/common/autoware_geography_utils/test/test_projection.cpp b/common/autoware_geography_utils/test/test_projection.cpp new file mode 100644 index 00000000..b8d036c1 --- /dev/null +++ b/common/autoware_geography_utils/test/test_projection.cpp @@ -0,0 +1,161 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +TEST(GeographyUtilsProjection, ProjectForwardToMGRS) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // target point + geometry_msgs::msg::Point local_point; + local_point.x = 86128.0; + local_point.y = 43002.0; + local_point.z = 10.0; + + // projector info + autoware_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geometry_msgs::msg::Point converted_point = + autoware::geography_utils::project_forward(geo_point, projector_info); + + EXPECT_NEAR(converted_point.x, local_point.x, 1.0); + EXPECT_NEAR(converted_point.y, local_point.y, 1.0); + EXPECT_NEAR(converted_point.z, local_point.z, 1.0); +} + +TEST(GeographyUtilsProjection, ProjectReverseFromMGRS) +{ + // source point + geometry_msgs::msg::Point local_point; + local_point.x = 86128.0; + local_point.y = 43002.0; + local_point.z = 10.0; + + // target point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + autoware_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geographic_msgs::msg::GeoPoint converted_point = + autoware::geography_utils::project_reverse(local_point, projector_info); + + EXPECT_NEAR(converted_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_point.altitude, geo_point.altitude, 0.0001); +} + +TEST(GeographyUtilsProjection, ProjectForwardAndReverseMGRS) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + autoware_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::MGRS; + projector_info.mgrs_grid = "54SUE"; + projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; + + // conversion + const geometry_msgs::msg::Point converted_local_point = + autoware::geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = + autoware::geography_utils::project_reverse(converted_local_point, projector_info); + + EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); +} + +TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62406; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // target point + geometry_msgs::msg::Point local_point; + local_point.x = 0.0; + local_point.y = -22.18; + local_point.z = 20.0; + + // projector info + autoware_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; + projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.62426; + projector_info.map_origin.longitude = 139.74252; + projector_info.map_origin.altitude = -10.0; + + // conversion + const geometry_msgs::msg::Point converted_point = + autoware::geography_utils::project_forward(geo_point, projector_info); + + EXPECT_NEAR(converted_point.x, local_point.x, 1.0); + EXPECT_NEAR(converted_point.y, local_point.y, 1.0); + EXPECT_NEAR(converted_point.z, local_point.z, 1.0); +} + +TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin) +{ + // source point + geographic_msgs::msg::GeoPoint geo_point; + geo_point.latitude = 35.62426; + geo_point.longitude = 139.74252; + geo_point.altitude = 10.0; + + // projector info + autoware_map_msgs::msg::MapProjectorInfo projector_info; + projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; + projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; + projector_info.map_origin.latitude = 35.0; + projector_info.map_origin.longitude = 139.0; + projector_info.map_origin.altitude = 0.0; + + // conversion + const geometry_msgs::msg::Point converted_local_point = + autoware::geography_utils::project_forward(geo_point, projector_info); + const geographic_msgs::msg::GeoPoint converted_geo_point = + autoware::geography_utils::project_reverse(converted_local_point, projector_info); + + EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); + EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); + EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); +} diff --git a/common/autoware_node/CHANGELOG.rst b/common/autoware_node/CHANGELOG.rst new file mode 100644 index 00000000..9fc5ead2 --- /dev/null +++ b/common/autoware_node/CHANGELOG.rst @@ -0,0 +1,12 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2025-01-09) +------------------ +* fix: change autoware_node from a static library to a shared library (`#121 `_) +* feat: add autoware_node and autoware_test node (`#113 `_) +* Contributors: M. Fatih Cırıt, SakodaShintaro + +0.0.0 (2024-12-02) +------------------ diff --git a/common/autoware_node/CMakeLists.txt b/common/autoware_node/CMakeLists.txt new file mode 100644 index 00000000..007167fe --- /dev/null +++ b/common/autoware_node/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_node) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED src/node.cpp) + +if(BUILD_TESTING) + file(GLOB_RECURSE TEST_FILES test/*.cpp) + + foreach(TEST_FILE ${TEST_FILES}) + # Get the test name without directory and extension + get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE) + + # Add each test separately + ament_add_ros_isolated_gtest(${TEST_NAME} ${TEST_FILE} TIMEOUT 10) + target_include_directories(${TEST_NAME} PRIVATE src/include) + target_link_libraries(${TEST_NAME} ${PROJECT_NAME}) + ament_target_dependencies(${TEST_NAME} + rclcpp + rclcpp_lifecycle) + endforeach() +endif() + +ament_auto_package(INSTALL_TO_SHARE) diff --git a/common/autoware_node/README.md b/common/autoware_node/README.md new file mode 100644 index 00000000..28e39e54 --- /dev/null +++ b/common/autoware_node/README.md @@ -0,0 +1,23 @@ +# Autoware Node + +## Abbreviations + +- **AN:** Autoware Node + +## Overview + +AN is an `autoware.core` package designed to provide a base class for all future nodes in the +system. +It also inherits all lifecycle control capabilities of the base +class [LifecycleNode](https://docs.ros2.org/latest/api/rclcpp_lifecycle/classrclcpp__lifecycle_1_1LifecycleNode.html) + +## Usage + +Check the [autoware_test_node](../../demos/autoware_test_node/README.md) package for an example of how to use `autoware::Node`. + +## Design + +### Lifecycle + +AN inherits from ROS 2 [rclcpp_lifecycle::LifecycleNode](https://design.ros2.org/articles/node_lifecycle.html) and has +all the basic functions of it. diff --git a/common/autoware_node/include/autoware/node/node.hpp b/common/autoware_node/include/autoware/node/node.hpp new file mode 100644 index 00000000..4531bf48 --- /dev/null +++ b/common/autoware_node/include/autoware/node/node.hpp @@ -0,0 +1,41 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 AUTOWARE__NODE__NODE_HPP_ +#define AUTOWARE__NODE__NODE_HPP_ + +#include "autoware/node/visibility_control.hpp" + +#include + +#include + +namespace autoware::node +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class Node : public rclcpp_lifecycle::LifecycleNode +{ +public: + AUTOWARE_NODE_PUBLIC + explicit Node( + const std::string & node_name, const std::string & ns = "", + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +protected: + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; +}; +} // namespace autoware::node + +#endif // AUTOWARE__NODE__NODE_HPP_ diff --git a/common/autoware_node/include/autoware/node/visibility_control.hpp b/common/autoware_node/include/autoware/node/visibility_control.hpp new file mode 100644 index 00000000..b7a6bbd0 --- /dev/null +++ b/common/autoware_node/include/autoware/node/visibility_control.hpp @@ -0,0 +1,26 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 AUTOWARE__NODE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__NODE__VISIBILITY_CONTROL_HPP_ + +#include "rcutils/visibility_control_macros.h" +#ifdef AUTOWARE_NODE_BUILDING_DLL +#define AUTOWARE_NODE_PUBLIC RCUTILS_EXPORT +#else +#define AUTOWARE_NODE_PUBLIC RCUTILS_IMPORT +#endif // !AUTOWARE_NODE_BUILDING_DLL +#define AUTOWARE_NODE_LOCAL RCUTILS_LOCAL + +#endif // AUTOWARE__NODE__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_node/package.xml b/common/autoware_node/package.xml new file mode 100644 index 00000000..4a036614 --- /dev/null +++ b/common/autoware_node/package.xml @@ -0,0 +1,21 @@ + + + + autoware_node + 0.1.0 + Autoware Node is an Autoware.Core package designed to provide a base class for all nodes in the system. + M. Fatih Cırıt + Apache-2.0 + + ament_cmake_auto + autoware_cmake + + rclcpp_lifecycle + + ament_cmake_ros + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_node/src/node.cpp b/common/autoware_node/src/node.cpp new file mode 100644 index 00000000..385d3731 --- /dev/null +++ b/common/autoware_node/src/node.cpp @@ -0,0 +1,38 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 + +namespace autoware::node +{ +Node::Node( + const std::string & node_name, const std::string & ns, const rclcpp::NodeOptions & options) +: LifecycleNode(node_name, ns, options) +{ + RCLCPP_DEBUG( + get_logger(), "Node %s constructor was called.", + get_node_base_interface()->get_fully_qualified_name()); +} + +CallbackReturn Node::on_shutdown(const rclcpp_lifecycle::State & state) +{ + RCLCPP_DEBUG( + get_logger(), "Node %s shutdown was called with state %s.", + get_node_base_interface()->get_fully_qualified_name(), state.label().c_str()); + return CallbackReturn::SUCCESS; +} +} // namespace autoware::node diff --git a/common/autoware_node/test/test_an_init_shutdown.cpp b/common/autoware_node/test/test_an_init_shutdown.cpp new file mode 100644 index 00000000..1224c17c --- /dev/null +++ b/common/autoware_node/test/test_an_init_shutdown.cpp @@ -0,0 +1,60 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 + +class AutowareNodeInitShutdown : public ::testing::Test +{ +public: + void SetUp() override { rclcpp::init(0, nullptr); } + + void TearDown() override { rclcpp::shutdown(); } + + rclcpp::NodeOptions node_options_an_; +}; + +TEST_F(AutowareNodeInitShutdown, NodeInitShutdown) +{ + autoware::node::Node::SharedPtr autoware_node = + std::make_shared("test_node", "test_ns", node_options_an_); + + auto executor = std::make_shared(); + executor->add_node(autoware_node->get_node_base_interface()); + + std::thread thread_spin = std::thread([&executor]() { executor->spin(); }); + + ASSERT_EQ( + autoware_node->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto state = autoware_node->shutdown(); + + ASSERT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + // wait until executor is spinning + while (!executor->is_spinning()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + executor->cancel(); // make sure cancel is called after spin + if (thread_spin.joinable()) { + thread_spin.join(); + } +} diff --git a/demos/autoware_test_node/CHANGELOG.rst b/demos/autoware_test_node/CHANGELOG.rst new file mode 100644 index 00000000..09b8d43f --- /dev/null +++ b/demos/autoware_test_node/CHANGELOG.rst @@ -0,0 +1,11 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_test_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2025-01-09) +------------------ +* feat: add autoware_node and autoware_test node (`#113 `_) +* Contributors: M. Fatih Cırıt + +0.0.0 (2024-12-02) +------------------ diff --git a/demos/autoware_test_node/CMakeLists.txt b/demos/autoware_test_node/CMakeLists.txt new file mode 100644 index 00000000..64db7b3b --- /dev/null +++ b/demos/autoware_test_node/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_test_node) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/test_node.cpp) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::test_node::TestNode" + EXECUTABLE ${PROJECT_NAME}_node) + +ament_auto_package(INSTALL_TO_SHARE + launch) diff --git a/demos/autoware_test_node/README.md b/demos/autoware_test_node/README.md new file mode 100644 index 00000000..91a07cd7 --- /dev/null +++ b/demos/autoware_test_node/README.md @@ -0,0 +1,53 @@ +# autoware_test_node + +This package contains a simple example of how to use `autoware::Node`. + +## Usage + +```bash +ros2 launch autoware_test_node autoware_test_node.launch.xml +``` + +### Lifecycle control + +Information on Lifecycle nodes can be found [here](https://design.ros2.org/articles/node_lifecycle.html). + +Output a list of nodes with lifecycle: + +```console +$ ros2 lifecycle nodes +/test_ns1/test_node1 +``` + +Get the current state of a node: + +```console +$ ros2 lifecycle get /test_ns1/test_node1 +unconfigured [1] +``` + +List the available transitions for the node: + +```console +$ ros2 lifecycle list /test_ns1/test_node1 +- configure [1] + Start: unconfigured + Goal: configuring +- shutdown [5] + Start: unconfigured + Goal: shuttingdown +``` + +Shutdown the node: + +```console +$ ros2 lifecycle set /test_ns1/test_node1 shutdown +Transitioning successful +``` + +```console +$ ros2 lifecycle get /test_ns1/test_node1 +finalized [4] +``` + +The node will remain alive in the `finalized` state until it is killed by the user. diff --git a/demos/autoware_test_node/launch/autoware_test_node.launch.xml b/demos/autoware_test_node/launch/autoware_test_node.launch.xml new file mode 100644 index 00000000..c034fa08 --- /dev/null +++ b/demos/autoware_test_node/launch/autoware_test_node.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/demos/autoware_test_node/package.xml b/demos/autoware_test_node/package.xml new file mode 100644 index 00000000..8d4be81e --- /dev/null +++ b/demos/autoware_test_node/package.xml @@ -0,0 +1,21 @@ + + + + autoware_test_node + 0.1.0 + Test package for Autoware Node. + M. Fatih Cırıt + Apache-2.0 + + ament_cmake_auto + autoware_cmake + + autoware_node + rclcpp + rclcpp_components + rclcpp_lifecycle + + + ament_cmake + + diff --git a/demos/autoware_test_node/src/include/test_node.hpp b/demos/autoware_test_node/src/include/test_node.hpp new file mode 100644 index 00000000..027171d2 --- /dev/null +++ b/demos/autoware_test_node/src/include/test_node.hpp @@ -0,0 +1,32 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 TEST_NODE_HPP_ +#define TEST_NODE_HPP_ + +#include +#include + +namespace autoware::test_node +{ + +class TestNode : public autoware::node::Node +{ +public: + explicit TestNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); +}; + +} // namespace autoware::test_node + +#endif // TEST_NODE_HPP_ diff --git a/demos/autoware_test_node/src/test_node.cpp b/demos/autoware_test_node/src/test_node.cpp new file mode 100644 index 00000000..2e2b60b8 --- /dev/null +++ b/demos/autoware_test_node/src/test_node.cpp @@ -0,0 +1,31 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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/test_node.hpp" + +#include + +namespace autoware::test_node +{ +TestNode::TestNode(const rclcpp::NodeOptions & options) +: autoware::node::Node("test_node", "", options) +{ + RCLCPP_DEBUG( + get_logger(), "TestNode %s constructor was called.", + get_node_base_interface()->get_fully_qualified_name()); +} +} // namespace autoware::test_node + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::test_node::TestNode)