diff --git a/.github/cppcheck-problem-matcher.json b/.github/cppcheck-problem-matcher.json new file mode 100644 index 00000000..3b18fecc --- /dev/null +++ b/.github/cppcheck-problem-matcher.json @@ -0,0 +1,16 @@ +{ + "problemMatcher": [ + { + "owner": "cppcheck", + "pattern": [ + { + "regexp": "^([^:]+):(\\d+):(\\d*):\\s(style|portability|performance|warning|error):\\s(.*)$", + "file": 1, + "line": 2, + "column": 3, + "message": 5 + } + ] + } + ] +} diff --git a/.github/workflows/build-and-test-daily.yaml b/.github/workflows/build-and-test-daily.yaml new file mode 100644 index 00000000..66275444 --- /dev/null +++ b/.github/workflows/build-and-test-daily.yaml @@ -0,0 +1,79 @@ +name: build-and-test-daily + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +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:latest-autoware-universe + build-depends-repos: build_depends.repos + steps: + - name: Check out repository + uses: actions/checkout@v4 + with: + fetch-depth: 1 + + - name: Show disk space before the tasks + run: df -h + + - name: Remove exec_depend + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 + + - name: Get self packages + 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 }} + target-packages: ${{ steps.get-self-packages.outputs.self-packages }} + build-depends-repos: ${{ matrix.build-depends-repos }} + cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} + + - name: Test + if: ${{ steps.get-self-packages.outputs.self-packages != '' }} + id: test + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 + with: + rosdistro: ${{ matrix.rosdistro }} + target-packages: ${{ steps.get-self-packages.outputs.self-packages }} + build-depends-repos: ${{ matrix.build-depends-repos }} + + - name: Upload coverage to CodeCov + if: ${{ steps.test.outputs.coverage-report-files != '' }} + uses: codecov/codecov-action@v4 + with: + files: ${{ steps.test.outputs.coverage-report-files }} + fail_ci_if_error: false + verbose: true + flags: total + token: ${{ secrets.CODECOV_TOKEN }} + + - 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 228b547e..0f239cd3 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -2,27 +2,44 @@ name: build-and-test on: push: - schedule: - - cron: 0 0 * * * + branches: + - main workflow_dispatch: +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.run_id }} + cancel-in-progress: true + +env: + CC: /usr/lib/ccache/gcc + CXX: /usr/lib/ccache/g++ + jobs: build-and-test: - if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }} - runs-on: ubuntu-latest - container: ${{ matrix.container }} + 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: ros:humble + container: ghcr.io/autowarefoundation/autoware:latest-autoware-universe build-depends-repos: build_depends.repos steps: - name: Check out repository uses: actions/checkout@v4 + with: + fetch-depth: 1 + + - name: Show disk space before the tasks + run: df -h + + - name: Show machine specs + run: lscpu && free -h - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 @@ -31,6 +48,43 @@ jobs: id: get-self-packages uses: autowarefoundation/autoware-github-actions/get-self-packages@v1 + - name: Create ccache directory + run: | + mkdir -p ${CCACHE_DIR} + du -sh ${CCACHE_DIR} && ccache -s + shell: bash + + - name: Attempt to restore ccache + uses: actions/cache/restore@v4 + with: + path: | + /root/.ccache + key: ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}-${{ github.sha }} + restore-keys: | + ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}- + + - name: Limit ccache size + run: | + rm -f "${CCACHE_DIR}/ccache.conf" + echo -e "# Set maximum cache size\nmax_size = 600MB" >> "${CCACHE_DIR}/ccache.conf" + shell: bash + + - name: Show ccache stats before build and reset stats + run: | + du -sh ${CCACHE_DIR} && ccache -s + 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 @@ -38,6 +92,21 @@ jobs: rosdistro: ${{ matrix.rosdistro }} 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-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 }} - name: Test if: ${{ steps.get-self-packages.outputs.self-packages != '' }} @@ -56,3 +125,7 @@ jobs: fail_ci_if_error: false verbose: true flags: total + token: ${{ secrets.CODECOV_TOKEN }} + + - name: Show disk space after the tasks + run: df -h diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml new file mode 100644 index 00000000..36239577 --- /dev/null +++ b/.github/workflows/cppcheck-differential.yaml @@ -0,0 +1,95 @@ +name: cppcheck-differential + +on: + pull_request: + +jobs: + cppcheck-differential: + runs-on: ubuntu-22.04 + + steps: + - name: Set PR fetch depth + run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + + - name: Checkout PR branch and all PR commits + uses: actions/checkout@v4 + with: + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y git + + - name: Install colcon + run: | + sudo sh -c 'echo "deb [arch=amd64,arm64] http://repo.ros2.org/ubuntu/main `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' + curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - + sudo apt update + sudo apt install python3-colcon-common-extensions + + # cppcheck from apt does not yet support --check-level args, and thus install from snap + - name: Install Cppcheck from snap + run: | + sudo snap install cppcheck + + - name: Fetch the base branch with enough history for a common merge-base commit + run: git fetch origin ${{ github.base_ref }} + shell: bash + + - name: Get modified packages + id: get-modified-packages + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + + - name: Get full paths of modified packages + id: get-full-paths + run: | + modified_packages="${{ steps.get-modified-packages.outputs.modified-packages }}" + paths="" + for pkg in $modified_packages; do + path=$(colcon list --packages-select $pkg | awk '{print $2}' | xargs realpath) + paths="$paths $path" + done + echo "full-paths=$paths" >> $GITHUB_OUTPUT + + - name: Filter packages with no cpp/hpp files + id: filter-paths-no-cpp-files + run: | + filtered_paths="" + for dir in ${{ steps.get-full-paths.outputs.full-paths }}; do + if [ -d "$dir" ] && find "$dir" -name "*.cpp" | grep -q .; then + filtered_paths="$filtered_paths $dir" + fi + done + echo "filtered-full-paths=$filtered_paths" >> $GITHUB_OUTPUT + + # cspell: ignore suppr, Dslots + - name: Run Cppcheck on modified packages + if: ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths != '' }} + continue-on-error: true + id: cppcheck + run: | + echo "Running Cppcheck on modified packages: ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths }}" + cppcheck --enable=all --inconclusive --check-level=exhaustive -D'PLUGINLIB_EXPORT_CLASS(class_type, base_class)=' -Dslots= -DQ_SLOTS= --error-exitcode=1 --suppressions-list=.cppcheck_suppressions --inline-suppr ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths }} 2> cppcheck-report.txt + shell: bash + + - name: Setup Problem Matchers for cppcheck + if: ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths != '' }} + run: echo "::add-matcher::.github/cppcheck-problem-matcher.json" + + - name: Show cppcheck-report result + if: ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths != '' }} + run: | + cat cppcheck-report.txt + + - name: Upload Cppcheck report + if: ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths != '' }} + uses: actions/upload-artifact@v4 + with: + name: cppcheck-report + path: cppcheck-report.txt + + - name: Fail the job if Cppcheck failed + if: ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths != '' && steps.cppcheck.outcome == 'failure' }} + run: exit 1 diff --git a/.github/workflows/cppcheck-weekly.yaml b/.github/workflows/cppcheck-weekly.yaml new file mode 100644 index 00000000..c75a72e1 --- /dev/null +++ b/.github/workflows/cppcheck-weekly.yaml @@ -0,0 +1,47 @@ +name: cppcheck-weekly + +on: + schedule: + - cron: 0 0 * * 1 + workflow_dispatch: + +jobs: + cppcheck-weekly: + runs-on: ubuntu-22.04 + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + # cppcheck from apt does not yet support --check-level args, and thus install from snap + - name: Install Cppcheck from snap + run: | + sudo snap install cppcheck + + # cspell: ignore suppr, Dslots + - name: Run Cppcheck on all files + continue-on-error: true + id: cppcheck + run: | + cppcheck --enable=all --inconclusive --check-level=exhaustive -D'PLUGINLIB_EXPORT_CLASS(class_type, base_class)=' -Dslots= -DQ_SLOTS= --suppress=*:*/test/* --error-exitcode=1 --xml --inline-suppr . 2> cppcheck-report.xml + shell: bash + + - name: Count errors by error ID and severity + run: | + #!/bin/bash + temp_file=$(mktemp) + grep -oP '(?<=id=")[^"]+" severity="[^"]+' cppcheck-report.xml | sed 's/" severity="/,/g' > "$temp_file" + echo "Error counts by error ID and severity:" + sort "$temp_file" | uniq -c + rm "$temp_file" + shell: bash + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v4 + with: + name: cppcheck-report + path: cppcheck-report.xml + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/.github/workflows/json-schema-check.yaml b/.github/workflows/json-schema-check.yaml new file mode 100644 index 00000000..be9b8e3c --- /dev/null +++ b/.github/workflows/json-schema-check.yaml @@ -0,0 +1,39 @@ +name: json-schema-check + +on: + pull_request: + workflow_dispatch: + +jobs: + check-if-relevant-files-changed: + runs-on: ubuntu-22.04 + outputs: + run-check: ${{ steps.paths_filter.outputs.json_or_yaml }} + steps: + - uses: actions/checkout@v4 + - uses: dorny/paths-filter@v3 + id: paths_filter + with: + filters: | + json_or_yaml: + - '**/schema/*.schema.json' + - '**/config/*.param.yaml' + + json-schema-check: + needs: check-if-relevant-files-changed + if: needs.check-if-relevant-files-changed.outputs.run-check == 'true' + runs-on: ubuntu-22.04 + steps: + - name: Check out repository + uses: actions/checkout@v4 + + - name: Run json-schema-check + uses: autowarefoundation/autoware-github-actions/json-schema-check@v1 + + no-relevant-changes: + needs: check-if-relevant-files-changed + if: needs.check-if-relevant-files-changed.outputs.run-check == 'false' + runs-on: ubuntu-22.04 + steps: + - name: Dummy step + run: echo "No relevant changes, passing check" diff --git a/autoware_geography_utils/CMakeLists.txt b/autoware_geography_utils/CMakeLists.txt new file mode 100644 index 00000000..b4ab5c2f --- /dev/null +++ b/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/autoware_geography_utils/README.md b/autoware_geography_utils/README.md new file mode 100644 index 00000000..fb4c2dc3 --- /dev/null +++ b/autoware_geography_utils/README.md @@ -0,0 +1,5 @@ +# geography_utils + +## Purpose + +This package contains geography-related functions used by other packages, so please refer to them as needed. diff --git a/autoware_geography_utils/include/autoware/geography_utils/height.hpp b/autoware_geography_utils/include/autoware/geography_utils/height.hpp new file mode 100644 index 00000000..1f205eb8 --- /dev/null +++ b/autoware_geography_utils/include/autoware/geography_utils/height.hpp @@ -0,0 +1,33 @@ +// 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 +{ + +typedef double (*HeightConversionFunction)( + 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/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp b/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp new file mode 100644 index 00000000..0eea2a9f --- /dev/null +++ b/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/autoware_geography_utils/include/autoware/geography_utils/projection.hpp b/autoware_geography_utils/include/autoware/geography_utils/projection.hpp new file mode 100644 index 00000000..5c4a69b1 --- /dev/null +++ b/autoware_geography_utils/include/autoware/geography_utils/projection.hpp @@ -0,0 +1,33 @@ +// 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; + +LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info); +GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info); + +} // namespace autoware::geography_utils + +#endif // AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ diff --git a/autoware_geography_utils/package.xml b/autoware_geography_utils/package.xml new file mode 100644 index 00000000..4f07802e --- /dev/null +++ b/autoware_geography_utils/package.xml @@ -0,0 +1,27 @@ + + + + autoware_geography_utils + 0.1.0 + The autoware_geography_utils package + Koji Minoda + Apache License 2.0 + + 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/autoware_geography_utils/src/height.cpp b/autoware_geography_utils/src/height.cpp new file mode 100644 index 00000000..745dbf5b --- /dev/null +++ b/autoware_geography_utils/src/height.cpp @@ -0,0 +1,63 @@ +// 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; + } + std::map, HeightConversionFunction> conversion_map; + conversion_map[{"WGS84", "EGM2008"}] = convert_wgs84_to_egm2008; + conversion_map[{"EGM2008", "WGS84"}] = convert_egm2008_to_wgs84; + + auto key = std::make_pair(source_vertical_datum, target_vertical_datum); + if (conversion_map.find(key) != conversion_map.end()) { + return conversion_map[key](height, latitude, longitude); + } else { + std::string error_message = + "Invalid conversion types: " + std::string(source_vertical_datum.c_str()) + " to " + + std::string(target_vertical_datum.c_str()); + + throw std::invalid_argument(error_message); + } +} + +} // namespace autoware::geography_utils diff --git a/autoware_geography_utils/src/lanelet2_projector.cpp b/autoware_geography_utils/src/lanelet2_projector.cpp new file mode 100644 index 00000000..7de0935a --- /dev/null +++ b/autoware_geography_utils/src/lanelet2_projector.cpp @@ -0,0 +1,54 @@ +// 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 +{ + +std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info) +{ + if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { + lanelet::GPSPoint position{ + projector_info.map_origin.latitude, projector_info.map_origin.longitude, + projector_info.map_origin.altitude}; + lanelet::Origin origin{position}; + lanelet::projection::UtmProjector projector{origin}; + return std::make_unique(projector); + + } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { + lanelet::projection::MGRSProjector projector{}; + projector.setMGRSCode(projector_info.mgrs_grid); + return std::make_unique(projector); + + } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) { + lanelet::GPSPoint position{ + projector_info.map_origin.latitude, projector_info.map_origin.longitude, + projector_info.map_origin.altitude}; + lanelet::Origin origin{position}; + lanelet::projection::TransverseMercatorProjector projector{origin}; + return std::make_unique(projector); + } + const std::string error_msg = + "Invalid map projector type: " + projector_info.projector_type + + ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator"; + throw std::invalid_argument(error_msg); +} + +} // namespace autoware::geography_utils diff --git a/autoware_geography_utils/src/projection.cpp b/autoware_geography_utils/src/projection.cpp new file mode 100644 index 00000000..3ab18b1d --- /dev/null +++ b/autoware_geography_utils/src/projection.cpp @@ -0,0 +1,95 @@ +// 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 + +namespace autoware::geography_utils +{ + +Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) +{ + Eigen::Vector3d dst; + dst.x() = src.x; + dst.y() = src.y; + dst.z() = src.z; + return dst; +} + +LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info) +{ + std::unique_ptr projector = get_lanelet2_projector(projector_info); + lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; + + lanelet::BasicPoint3d projected_local_point; + if (projector_info.projector_type == MapProjectorInfo::MGRS) { + const 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/autoware_geography_utils/test/test_geography_utils.cpp b/autoware_geography_utils/test/test_geography_utils.cpp new file mode 100644 index 00000000..ee0e7428 --- /dev/null +++ b/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/autoware_geography_utils/test/test_height.cpp b/autoware_geography_utils/test/test_height.cpp new file mode 100644 index 00000000..f624f6c3 --- /dev/null +++ b/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/autoware_geography_utils/test/test_projection.cpp b/autoware_geography_utils/test/test_projection.cpp new file mode 100644 index 00000000..b8d036c1 --- /dev/null +++ b/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/autoware_map_projection_loader/CMakeLists.txt b/autoware_map_projection_loader/CMakeLists.txt new file mode 100644 index 00000000..19c5a98e --- /dev/null +++ b/autoware_map_projection_loader/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_map_projection_loader) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_find_build_dependencies() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/map_projection_loader.cpp + src/load_info_from_lanelet2_map.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::map_projection_loader::MapProjectionLoader" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + +target_link_libraries(${PROJECT_NAME} yaml-cpp) + +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_link_libraries("${test_name}" ${PROJECT_NAME}) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + +if(BUILD_TESTING) + # Test python scripts + add_launch_test( + test/test_node_load_mgrs_from_yaml.test.py + TIMEOUT "30" + ) + add_launch_test( + test/test_node_load_local_from_yaml.test.py + TIMEOUT "30" + ) + add_launch_test( + test/test_node_load_local_cartesian_utm_from_yaml.test.py + TIMEOUT "30" + ) + add_launch_test( + test/test_node_load_transverse_mercator_from_yaml.test.py + TIMEOUT "30" + ) + install(DIRECTORY + test/data/ + DESTINATION share/${PROJECT_NAME}/test/data/ + ) + + # Test c++ scripts + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_testcase(test/test_load_info_from_lanelet2_map.cpp) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/autoware_map_projection_loader/README.md b/autoware_map_projection_loader/README.md new file mode 100644 index 00000000..11246c09 --- /dev/null +++ b/autoware_map_projection_loader/README.md @@ -0,0 +1,95 @@ +# autoware_map_projection_loader + +## Feature + +`autoware_map_projection_loader` is responsible for publishing `map_projector_info` that defines in which kind of coordinate Autoware is operating. +This is necessary information especially when you want to convert from global (geoid) to local coordinate or the other way around. + +- If `map_projector_info_path` DOES exist, this node loads it and publishes the map projection information accordingly. +- If `map_projector_info_path` does NOT exist, the node assumes that you are using the `MGRS` projection type, and loads the lanelet2 map instead to extract the MGRS grid. + - **DEPRECATED WARNING: This interface that uses the lanelet2 map is not recommended. Please prepare the YAML file instead.** + +## Map projector info file specification + +You need to provide a YAML file, namely `map_projector_info.yaml` under the `map_path` directory. For `pointcloud_map_metadata.yaml`, please refer to the Readme of `map_loader`. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map.pcd +├── map_projector_info.yaml +└── pointcloud_map_metadata.yaml +``` + +There are three types of transformations from latitude and longitude to XYZ coordinate system as shown in the figure below. Please refer to the following details for the necessary parameters for each projector type. + +![node_diagram](docs/map_projector_type.svg) + +### Using local coordinate + +```yaml +# map_projector_info.yaml +projector_type: local +``` + +#### Limitation + +The functionality that requires latitude and longitude will become unavailable. + +The currently identified unavailable functionalities are: + +- GNSS localization +- Sending the self-position in latitude and longitude using ADAPI + +### Using MGRS + +If you want to use MGRS, please specify the MGRS grid as well. + +```yaml +# map_projector_info.yaml +projector_type: MGRS +vertical_datum: WGS84 +mgrs_grid: 54SUE +``` + +#### Limitation + +It cannot be used with maps that span across two or more MGRS grids. Please use it only when it falls within the scope of a single MGRS grid. + +### Using LocalCartesianUTM + +If you want to use local cartesian UTM, please specify the map origin as well. + +```yaml +# map_projector_info.yaml +projector_type: LocalCartesianUTM +vertical_datum: WGS84 +map_origin: + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] +``` + +### Using TransverseMercator + +If you want to use Transverse Mercator projection, please specify the map origin as well. + +```yaml +# map_projector_info.yaml +projector_type: TransverseMercator +vertical_datum: WGS84 +map_origin: + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] +``` + +## Published Topics + +- `~/map_projector_info` (tier4_map_msgs/MapProjectorInfo) : This topic shows the definition of map projector information + +## Parameters + +Note that these parameters are assumed to be passed from launch arguments, and it is not recommended to directly write them in `map_projection_loader.param.yaml`. + +{{ json_to_markdown("map/autoware_map_projection_loader/schema/map_projection_loader.schema.json") }} diff --git a/autoware_map_projection_loader/config/map_projection_loader.param.yaml b/autoware_map_projection_loader/config/map_projection_loader.param.yaml new file mode 100644 index 00000000..6ec30030 --- /dev/null +++ b/autoware_map_projection_loader/config/map_projection_loader.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + map_projector_info_path: $(var map_projector_info_path) + lanelet2_map_path: $(var lanelet2_map_path) diff --git a/autoware_map_projection_loader/docs/map_projector_type.svg b/autoware_map_projection_loader/docs/map_projector_type.svg new file mode 100644 index 00000000..e1c8c2ac --- /dev/null +++ b/autoware_map_projection_loader/docs/map_projector_type.svg @@ -0,0 +1,3010 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + MGRS + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + MGRS + + + X(east) + + + O + + + Y(north) + + + + Strict Boundary. + + Outside of the mgrs grid is + projected to undefined xy + + + mgrs_grid=54SUE + + + 54SUE + + + 54S(UTM grid) + + + meridian + + + LocalCartesianUTM + + + TransverseMercator + + + + + map_origin.latitude=35.6 + map_origin.longitude=139.5 + + + 3+6*floor(map_origin.longitude/6)[deg] + + + 3+6n[deg] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + X(east) + + + O + + + Y(north) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + map_origin.latitude=35.6 + map_origin.longitude=139.5 + + + map_origin.longitude[deg] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + X(east) + + + O + + + Y(north) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + No boundary, + but farther away from the meridian, + the greater the projection error becomes + + + + No boundary, + but farther away from the meridian, + the greater the projection error becomes + + + diff --git a/autoware_map_projection_loader/include/autoware/map_projection_loader/load_info_from_lanelet2_map.hpp b/autoware_map_projection_loader/include/autoware/map_projection_loader/load_info_from_lanelet2_map.hpp new file mode 100644 index 00000000..2f1db145 --- /dev/null +++ b/autoware_map_projection_loader/include/autoware/map_projection_loader/load_info_from_lanelet2_map.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__MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ +#define AUTOWARE__MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ + +#include +#include +#include +#include + +#include "tier4_map_msgs/msg/map_projector_info.hpp" + +#include + +namespace autoware::map_projection_loader +{ +tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename); +} // namespace autoware::map_projection_loader + +#endif // AUTOWARE__MAP_PROJECTION_LOADER__LOAD_INFO_FROM_LANELET2_MAP_HPP_ diff --git a/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp b/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp new file mode 100644 index 00000000..feb7483f --- /dev/null +++ b/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp @@ -0,0 +1,42 @@ +// 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__MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ +#define AUTOWARE__MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +#include + +namespace autoware::map_projection_loader +{ +tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename); + +class MapProjectionLoader : public rclcpp::Node +{ +public: + explicit MapProjectionLoader(const rclcpp::NodeOptions & options); + +private: + using MapProjectorInfo = map_interface::MapProjectorInfo; + component_interface_utils::Publisher::SharedPtr publisher_; +}; +} // namespace autoware::map_projection_loader + +#endif // AUTOWARE__MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ diff --git a/autoware_map_projection_loader/launch/map_projection_loader.launch.xml b/autoware_map_projection_loader/launch/map_projection_loader.launch.xml new file mode 100644 index 00000000..224598e2 --- /dev/null +++ b/autoware_map_projection_loader/launch/map_projection_loader.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/autoware_map_projection_loader/package.xml b/autoware_map_projection_loader/package.xml new file mode 100644 index 00000000..ca25cbf7 --- /dev/null +++ b/autoware_map_projection_loader/package.xml @@ -0,0 +1,36 @@ + + + + autoware_map_projection_loader + 0.1.0 + autoware_map_projection_loader package as a ROS 2 node + Yamato Ando + Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_lanelet2_extension + component_interface_specs + component_interface_utils + rclcpp + rclcpp_components + tier4_map_msgs + yaml-cpp + + ament_cmake_gmock + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + ros_testing + + + ament_cmake + + diff --git a/autoware_map_projection_loader/schema/map_projection_loader.schema.json b/autoware_map_projection_loader/schema/map_projection_loader.schema.json new file mode 100644 index 00000000..bb7fe5d2 --- /dev/null +++ b/autoware_map_projection_loader/schema/map_projection_loader.schema.json @@ -0,0 +1,38 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for map_projection_loader", + "type": "object", + "definitions": { + "map_projection_loader": { + "type": "object", + "properties": { + "map_projector_info_path": { + "type": "string", + "description": "The path where map_projector_info.yaml is located", + "default": "$(var map_projector_info_path)" + }, + "lanelet2_map_path": { + "type": "string", + "description": "The path where the lanelet2 map file (.osm) is located", + "default": "$(var lanelet2_map_path)" + } + }, + "required": ["map_projector_info_path", "lanelet2_map_path"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/map_projection_loader" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp b/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp new file mode 100644 index 00000000..ce4cda5c --- /dev/null +++ b/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp @@ -0,0 +1,63 @@ +// 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/map_projection_loader/load_info_from_lanelet2_map.hpp" + +#include "tier4_map_msgs/msg/map_projector_info.hpp" + +#include +#include +#include +#include + +#include + +namespace autoware::map_projection_loader +{ +tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::string & filename) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + const lanelet::LaneletMapPtr map = lanelet::load(filename, projector, &errors); + if (!errors.empty()) { + throw std::runtime_error("Error occurred while loading lanelet2 map"); + } + + // If the lat & lon values in all the points of lanelet2 map are all zeros, + // it will be interpreted as a local map. + // If any single point exists with non-zero lat or lon values, it will be interpreted as MGRS. + bool is_local = true; + for (const auto & point : map->pointLayer) { + const auto gps_point = projector.reverse(point); + if (gps_point.lat != 0.0 || gps_point.lon != 0.0) { + is_local = false; + break; + } + } + + tier4_map_msgs::msg::MapProjectorInfo msg; + if (is_local) { + msg.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL; + } else { + msg.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS; + msg.mgrs_grid = projector.getProjectedMGRSGrid(); + } + + // We assume that the vertical datum of the map is WGS84 when using lanelet2 map. + // However, do note that this is not always true, and may cause problems in the future. + // Thus, please consider using the map_projector_info.yaml instead of this deprecated function. + msg.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84; + return msg; +} +} // namespace autoware::map_projection_loader diff --git a/autoware_map_projection_loader/src/map_projection_loader.cpp b/autoware_map_projection_loader/src/map_projection_loader.cpp new file mode 100644 index 00000000..18ad425c --- /dev/null +++ b/autoware_map_projection_loader/src/map_projection_loader.cpp @@ -0,0 +1,101 @@ +// 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/map_projection_loader/map_projection_loader.hpp" + +#include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp" + +#include + +#include + +#include +#include + +namespace autoware::map_projection_loader +{ +tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) +{ + YAML::Node data = YAML::LoadFile(filename); + + tier4_map_msgs::msg::MapProjectorInfo msg; + msg.projector_type = data["projector_type"].as(); + if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::MGRS) { + msg.vertical_datum = data["vertical_datum"].as(); + msg.mgrs_grid = data["mgrs_grid"].as(); + + } else if ( + msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM || + msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::TRANSVERSE_MERCATOR) { + msg.vertical_datum = data["vertical_datum"].as(); + msg.map_origin.latitude = data["map_origin"]["latitude"].as(); + msg.map_origin.longitude = data["map_origin"]["longitude"].as(); + msg.map_origin.altitude = data["map_origin"]["altitude"].as(); + + } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { + ; // do nothing + + } else { + throw std::runtime_error( + "Invalid map projector type. Currently supported types: MGRS, LocalCartesianUTM, " + "TransverseMercator, and local"); + } + return msg; +} + +tier4_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename) +{ + tier4_map_msgs::msg::MapProjectorInfo msg; + + if (std::filesystem::exists(yaml_filename)) { + std::cout << "Load " << yaml_filename << std::endl; + msg = load_info_from_yaml(yaml_filename); + } else if (std::filesystem::exists(lanelet2_map_filename)) { + std::cout << "Load " << lanelet2_map_filename << std::endl; + std::cout + << "DEPRECATED WARNING: Loading map projection info from lanelet2 map may soon be deleted. " + "Please use map_projector_info.yaml instead. For more info, visit " + "https://github.com/autowarefoundation/autoware.universe/blob/main/map/" + "map_projection_loader/" + "README.md" + << std::endl; + msg = load_info_from_lanelet2_map(lanelet2_map_filename); + } else { + throw std::runtime_error( + "No map projector info files found. Please provide either " + "map_projector_info.yaml or lanelet2_map.osm"); + } + return msg; +} + +MapProjectionLoader::MapProjectionLoader(const rclcpp::NodeOptions & options) +: rclcpp::Node("map_projection_loader", options) +{ + const std::string yaml_filename = this->declare_parameter("map_projector_info_path"); + const std::string lanelet2_map_filename = + this->declare_parameter("lanelet2_map_path"); + + const tier4_map_msgs::msg::MapProjectorInfo msg = + load_map_projector_info(yaml_filename, lanelet2_map_filename); + + // Publish the message + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(publisher_); + publisher_->publish(msg); +} +} // namespace autoware::map_projection_loader + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::map_projection_loader::MapProjectionLoader) diff --git a/autoware_map_projection_loader/test/data/map_projector_info_local.yaml b/autoware_map_projection_loader/test/data/map_projector_info_local.yaml new file mode 100644 index 00000000..2e5d27c3 --- /dev/null +++ b/autoware_map_projection_loader/test/data/map_projector_info_local.yaml @@ -0,0 +1 @@ +projector_type: local diff --git a/autoware_map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml b/autoware_map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml new file mode 100644 index 00000000..854a6ad7 --- /dev/null +++ b/autoware_map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml @@ -0,0 +1,6 @@ +projector_type: LocalCartesianUTM +vertical_datum: WGS84 +map_origin: + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] diff --git a/autoware_map_projection_loader/test/data/map_projector_info_mgrs.yaml b/autoware_map_projection_loader/test/data/map_projector_info_mgrs.yaml new file mode 100644 index 00000000..5446de48 --- /dev/null +++ b/autoware_map_projection_loader/test/data/map_projector_info_mgrs.yaml @@ -0,0 +1,3 @@ +projector_type: MGRS +vertical_datum: WGS84 +mgrs_grid: 54SUE diff --git a/autoware_map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml b/autoware_map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml new file mode 100644 index 00000000..de1febeb --- /dev/null +++ b/autoware_map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml @@ -0,0 +1,6 @@ +projector_type: TransverseMercator +vertical_datum: WGS84 +map_origin: + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] diff --git a/autoware_map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/autoware_map_projection_loader/test/test_load_info_from_lanelet2_map.cpp new file mode 100644 index 00000000..26658e88 --- /dev/null +++ b/autoware_map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -0,0 +1,135 @@ +// 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/map_projection_loader/load_info_from_lanelet2_map.hpp" + +#include +#include + +#include + +#include + +void save_dummy_mgrs_lanelet2_map(const std::string & mgrs_coord, const std::string & output_path) +{ + int zone = std::stoi(mgrs_coord.substr(0, 2)); + bool is_north = false; + int precision = 0; + double utm_x{}; + double utm_y{}; + double lat{}; + double lon{}; + GeographicLib::MGRS::Reverse(mgrs_coord, zone, is_north, utm_x, utm_y, precision, false); + GeographicLib::UTMUPS::Reverse(zone, is_north, utm_x, utm_y, lat, lon); + + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << R"( \n"; + file << ""; + + file.close(); +} + +void save_dummy_local_lanelet2_map(const std::string & output_path) +{ + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << " \n"; + file << " \n"; + file << " \n"; + file << ""; + + file.close(); +} + +void save_dummy_mgrs_lanelet2_map_with_one_zero_point(const std::string & output_path) +{ + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << " \n"; + file << " \n"; + file << " \n"; + file << ""; + + file.close(); +} + +TEST(TestLoadFromLanelet2Map, LoadMGRSGrid) +{ + // Save dummy lanelet2 map + const std::string mgrs_grid = "54SUE"; + const std::string mgrs_coord = mgrs_grid + "1000010000"; + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_mgrs_lanelet2_map(mgrs_coord, output_path); + + // Test the function + const auto projector_info = + autoware::map_projection_loader::load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.projector_type, "MGRS"); + EXPECT_EQ(projector_info.mgrs_grid, mgrs_grid); +} + +TEST(TestLoadFromLanelet2Map, LoadLocalGrid) +{ + // Save dummy lanelet2 map + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_local_lanelet2_map(output_path); + + // Test the function + const auto projector_info = + autoware::map_projection_loader::load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.projector_type, "local"); +} + +TEST(TestLoadFromLanelet2Map, LoadNoLocalGrid) +{ + // Save dummy lanelet2 map + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_mgrs_lanelet2_map_with_one_zero_point(output_path); + + // Test the function + const auto projector_info = + autoware::map_projection_loader::load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.projector_type, "MGRS"); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/autoware_map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/autoware_map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py new file mode 100644 index 00000000..495f0092 --- /dev/null +++ b/autoware_map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +YAML_FILE_PATH = "test/data/map_projector_info_local_cartesian_utm.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projector_info_path = os.path.join( + get_package_share_directory("autoware_map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="autoware_map_projection_loader", + executable="autoware_map_projection_loader_node", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projector_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_generator to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadUTMFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projector_info_path = os.path.join( + get_package_share_directory("autoware_map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projector_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) + self.assertEqual(self.received_message.vertical_datum, yaml_data["vertical_datum"]) + self.assertEqual( + self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] + ) + self.assertEqual( + self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] + ) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/autoware_map_projection_loader/test/test_node_load_local_from_yaml.test.py b/autoware_map_projection_loader/test/test_node_load_local_from_yaml.test.py new file mode 100644 index 00000000..8517f09e --- /dev/null +++ b/autoware_map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +YAML_FILE_PATH = "test/data/map_projector_info_local.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projector_info_path = os.path.join( + get_package_share_directory("autoware_map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="autoware_map_projection_loader", + executable="autoware_map_projection_loader_node", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projector_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_generator to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadLocalFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projector_info_path = os.path.join( + get_package_share_directory("autoware_map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projector_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/autoware_map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/autoware_map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py new file mode 100644 index 00000000..ad19e61f --- /dev/null +++ b/autoware_map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +YAML_FILE_PATH = "test/data/map_projector_info_mgrs.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projector_info_path = os.path.join( + get_package_share_directory("autoware_map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="autoware_map_projection_loader", + executable="autoware_map_projection_loader_node", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projector_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_generator to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadMGRSFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projector_info_path = os.path.join( + get_package_share_directory("autoware_map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projector_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) + self.assertEqual(self.received_message.vertical_datum, yaml_data["vertical_datum"]) + self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"]) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/autoware_map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/autoware_map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py new file mode 100644 index 00000000..ed2eb455 --- /dev/null +++ b/autoware_map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +YAML_FILE_PATH = "test/data/map_projector_info_transverse_mercator.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projector_info_path = os.path.join( + get_package_share_directory("autoware_map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="autoware_map_projection_loader", + executable="autoware_map_projection_loader_node", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projector_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_generator to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadUTMFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projector_info_path = os.path.join( + get_package_share_directory("autoware_map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projector_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) + self.assertEqual(self.received_message.vertical_datum, yaml_data["vertical_datum"]) + self.assertEqual( + self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] + ) + self.assertEqual( + self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] + ) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/build_depends.repos b/build_depends.repos index b9ee75e1..72d9118d 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -3,6 +3,10 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_msgs.git version: main + tier4_autoware_msgs: + type: git + url: https://github.com/tier4/tier4_autoware_msgs.git + version: tier4/universe autoware_common: type: git url: https://github.com/autowarefoundation/autoware_common.git diff --git a/component_interface_specs/CMakeLists.txt b/component_interface_specs/CMakeLists.txt new file mode 100644 index 00000000..146f3688 --- /dev/null +++ b/component_interface_specs/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(component_interface_specs) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +if(BUILD_TESTING) + ament_auto_add_gtest(gtest_${PROJECT_NAME} + test/gtest_main.cpp + test/test_planning.cpp + test/test_control.cpp + test/test_localization.cpp + test/test_system.cpp + test/test_map.cpp + test/test_perception.cpp + test/test_vehicle.cpp + ) +endif() + +ament_auto_package() diff --git a/component_interface_specs/README.md b/component_interface_specs/README.md new file mode 100644 index 00000000..b9fcd83a --- /dev/null +++ b/component_interface_specs/README.md @@ -0,0 +1,3 @@ +# component_interface_specs + +This package is a specification of component interfaces. diff --git a/component_interface_specs/include/component_interface_specs/control.hpp b/component_interface_specs/include/component_interface_specs/control.hpp new file mode 100644 index 00000000..dfa708dd --- /dev/null +++ b/component_interface_specs/include/component_interface_specs/control.hpp @@ -0,0 +1,70 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_SPECS__CONTROL_HPP_ +#define COMPONENT_INTERFACE_SPECS__CONTROL_HPP_ + +#include + +#include +#include +#include +#include +#include + +namespace control_interface +{ + +struct SetPause +{ + using Service = tier4_control_msgs::srv::SetPause; + static constexpr char name[] = "/control/vehicle_cmd_gate/set_pause"; +}; + +struct IsPaused +{ + using Message = tier4_control_msgs::msg::IsPaused; + static constexpr char name[] = "/control/vehicle_cmd_gate/is_paused"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +struct IsStartRequested +{ + using Message = tier4_control_msgs::msg::IsStartRequested; + static constexpr char name[] = "/control/vehicle_cmd_gate/is_start_requested"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +struct SetStop +{ + using Service = tier4_control_msgs::srv::SetStop; + static constexpr char name[] = "/control/vehicle_cmd_gate/set_stop"; +}; + +struct IsStopped +{ + using Message = tier4_control_msgs::msg::IsStopped; + static constexpr char name[] = "/control/vehicle_cmd_gate/is_stopped"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +} // namespace control_interface + +#endif // COMPONENT_INTERFACE_SPECS__CONTROL_HPP_ diff --git a/component_interface_specs/include/component_interface_specs/localization.hpp b/component_interface_specs/include/component_interface_specs/localization.hpp new file mode 100644 index 00000000..e00d8cef --- /dev/null +++ b/component_interface_specs/include/component_interface_specs/localization.hpp @@ -0,0 +1,63 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_ +#define COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_ + +#include + +#include +#include +#include +#include + +namespace localization_interface +{ + +struct Initialize +{ + using Service = tier4_localization_msgs::srv::InitializeLocalization; + static constexpr char name[] = "/localization/initialize"; +}; + +struct InitializationState +{ + using Message = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + static constexpr char name[] = "/localization/initialization_state"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +struct KinematicState +{ + using Message = nav_msgs::msg::Odometry; + static constexpr char name[] = "/localization/kinematic_state"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +struct Acceleration +{ + using Message = geometry_msgs::msg::AccelWithCovarianceStamped; + static constexpr char name[] = "/localization/acceleration"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace localization_interface + +#endif // COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_ diff --git a/component_interface_specs/include/component_interface_specs/map.hpp b/component_interface_specs/include/component_interface_specs/map.hpp new file mode 100644 index 00000000..dc162d4a --- /dev/null +++ b/component_interface_specs/include/component_interface_specs/map.hpp @@ -0,0 +1,36 @@ +// 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 COMPONENT_INTERFACE_SPECS__MAP_HPP_ +#define COMPONENT_INTERFACE_SPECS__MAP_HPP_ + +#include + +#include + +namespace map_interface +{ + +struct MapProjectorInfo +{ + using Message = tier4_map_msgs::msg::MapProjectorInfo; + static constexpr char name[] = "/map/map_projector_info"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +} // namespace map_interface + +#endif // COMPONENT_INTERFACE_SPECS__MAP_HPP_ diff --git a/component_interface_specs/include/component_interface_specs/perception.hpp b/component_interface_specs/include/component_interface_specs/perception.hpp new file mode 100644 index 00000000..0c72dbdb --- /dev/null +++ b/component_interface_specs/include/component_interface_specs/perception.hpp @@ -0,0 +1,36 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ +#define COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ + +#include + +#include + +namespace perception_interface +{ + +struct ObjectRecognition +{ + using Message = autoware_perception_msgs::msg::PredictedObjects; + static constexpr char name[] = "/perception/object_recognition/objects"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace perception_interface + +#endif // COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ diff --git a/component_interface_specs/include/component_interface_specs/planning.hpp b/component_interface_specs/include/component_interface_specs/planning.hpp new file mode 100644 index 00000000..58aba53d --- /dev/null +++ b/component_interface_specs/include/component_interface_specs/planning.hpp @@ -0,0 +1,78 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_SPECS__PLANNING_HPP_ +#define COMPONENT_INTERFACE_SPECS__PLANNING_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +namespace planning_interface +{ + +struct SetLaneletRoute +{ + using Service = tier4_planning_msgs::srv::SetLaneletRoute; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/set_lanelet_route"; +}; + +struct SetWaypointRoute +{ + using Service = tier4_planning_msgs::srv::SetWaypointRoute; + static constexpr char name[] = + "/planning/mission_planning/route_selector/main/set_waypoint_route"; +}; + +struct ClearRoute +{ + using Service = tier4_planning_msgs::srv::ClearRoute; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/clear_route"; +}; + +struct RouteState +{ + using Message = tier4_planning_msgs::msg::RouteState; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/state"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +struct LaneletRoute +{ + using Message = autoware_planning_msgs::msg::LaneletRoute; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/route"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +struct Trajectory +{ + using Message = autoware_planning_msgs::msg::Trajectory; + static constexpr char name[] = "/planning/scenario_planning/trajectory"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace planning_interface + +#endif // COMPONENT_INTERFACE_SPECS__PLANNING_HPP_ diff --git a/component_interface_specs/include/component_interface_specs/system.hpp b/component_interface_specs/include/component_interface_specs/system.hpp new file mode 100644 index 00000000..2132d16d --- /dev/null +++ b/component_interface_specs/include/component_interface_specs/system.hpp @@ -0,0 +1,60 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_ +#define COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_ + +#include + +#include +#include +#include +#include + +namespace system_interface +{ + +struct MrmState +{ + using Message = autoware_adapi_v1_msgs::msg::MrmState; + static constexpr char name[] = "/system/fail_safe/mrm_state"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +struct ChangeAutowareControl +{ + using Service = tier4_system_msgs::srv::ChangeAutowareControl; + static constexpr char name[] = "/system/operation_mode/change_autoware_control"; +}; + +struct ChangeOperationMode +{ + using Service = tier4_system_msgs::srv::ChangeOperationMode; + static constexpr char name[] = "/system/operation_mode/change_operation_mode"; +}; + +struct OperationModeState +{ + using Message = autoware_adapi_v1_msgs::msg::OperationModeState; + static constexpr char name[] = "/system/operation_mode/state"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + +} // namespace system_interface + +#endif // COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_ diff --git a/component_interface_specs/include/component_interface_specs/vehicle.hpp b/component_interface_specs/include/component_interface_specs/vehicle.hpp new file mode 100644 index 00000000..e7ce2c52 --- /dev/null +++ b/component_interface_specs/include/component_interface_specs/vehicle.hpp @@ -0,0 +1,100 @@ +// 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 COMPONENT_INTERFACE_SPECS__VEHICLE_HPP_ +#define COMPONENT_INTERFACE_SPECS__VEHICLE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vehicle_interface +{ + +struct SteeringStatus +{ + using Message = autoware_vehicle_msgs::msg::SteeringReport; + static constexpr char name[] = "/vehicle/status/steering_status"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +struct GearStatus +{ + using Message = autoware_vehicle_msgs::msg::GearReport; + static constexpr char name[] = "/vehicle/status/gear_status"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +struct TurnIndicatorStatus +{ + using Message = autoware_vehicle_msgs::msg::TurnIndicatorsReport; + static constexpr char name[] = "/vehicle/status/turn_indicators_status"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +struct HazardLightStatus +{ + using Message = autoware_vehicle_msgs::msg::HazardLightsReport; + static constexpr char name[] = "/vehicle/status/hazard_lights_status"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +struct EnergyStatus +{ + using Message = tier4_vehicle_msgs::msg::BatteryStatus; + static constexpr char name[] = "/vehicle/status/battery_charge"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +struct DoorCommand +{ + using Service = autoware_adapi_v1_msgs::srv::SetDoorCommand; + static constexpr char name[] = "/vehicle/doors/command"; +}; + +struct DoorLayout +{ + using Service = autoware_adapi_v1_msgs::srv::GetDoorLayout; + static constexpr char name[] = "/vehicle/doors/layout"; +}; + +struct DoorStatus +{ + using Message = autoware_adapi_v1_msgs::msg::DoorStatusArray; + static constexpr char name[] = "/vehicle/doors/status"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace vehicle_interface + +#endif // COMPONENT_INTERFACE_SPECS__VEHICLE_HPP_ diff --git a/component_interface_specs/package.xml b/component_interface_specs/package.xml new file mode 100644 index 00000000..fa57bb16 --- /dev/null +++ b/component_interface_specs/package.xml @@ -0,0 +1,36 @@ + + + + component_interface_specs + 0.0.0 + The component_interface_specs package + Takagi, Isamu + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_vehicle_msgs + nav_msgs + rcl + rclcpp + rosidl_runtime_cpp + tier4_control_msgs + tier4_localization_msgs + tier4_map_msgs + tier4_planning_msgs + tier4_system_msgs + tier4_vehicle_msgs + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/component_interface_specs/test/gtest_main.cpp b/component_interface_specs/test/gtest_main.cpp new file mode 100644 index 00000000..81d9d534 --- /dev/null +++ b/component_interface_specs/test/gtest_main.cpp @@ -0,0 +1,21 @@ +// Copyright 2023 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 "gtest/gtest.h" + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/component_interface_specs/test/test_control.cpp b/component_interface_specs/test/test_control.cpp new file mode 100644 index 00000000..366641ea --- /dev/null +++ b/component_interface_specs/test/test_control.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 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 "component_interface_specs/control.hpp" +#include "gtest/gtest.h" + +TEST(control, interface) +{ + { + using control_interface::IsPaused; + IsPaused is_paused; + size_t depth = 1; + EXPECT_EQ(is_paused.depth, depth); + EXPECT_EQ(is_paused.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_paused.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using control_interface::IsStartRequested; + IsStartRequested is_start_requested; + size_t depth = 1; + EXPECT_EQ(is_start_requested.depth, depth); + EXPECT_EQ(is_start_requested.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_start_requested.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using control_interface::IsStopped; + IsStopped is_stopped; + size_t depth = 1; + EXPECT_EQ(is_stopped.depth, depth); + EXPECT_EQ(is_stopped.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_stopped.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/component_interface_specs/test/test_localization.cpp b/component_interface_specs/test/test_localization.cpp new file mode 100644 index 00000000..31d8e139 --- /dev/null +++ b/component_interface_specs/test/test_localization.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 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 "component_interface_specs/localization.hpp" +#include "gtest/gtest.h" + +TEST(localization, interface) +{ + { + using localization_interface::InitializationState; + InitializationState initialization_state; + size_t depth = 1; + EXPECT_EQ(initialization_state.depth, depth); + EXPECT_EQ(initialization_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(initialization_state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using localization_interface::KinematicState; + KinematicState kinematic_state; + size_t depth = 1; + EXPECT_EQ(kinematic_state.depth, depth); + EXPECT_EQ(kinematic_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(kinematic_state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using localization_interface::Acceleration; + Acceleration acceleration; + size_t depth = 1; + EXPECT_EQ(acceleration.depth, depth); + EXPECT_EQ(acceleration.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(acceleration.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/component_interface_specs/test/test_map.cpp b/component_interface_specs/test/test_map.cpp new file mode 100644 index 00000000..a65f2cb7 --- /dev/null +++ b/component_interface_specs/test/test_map.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 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 "component_interface_specs/map.hpp" +#include "gtest/gtest.h" + +TEST(map, interface) +{ + { + using map_interface::MapProjectorInfo; + MapProjectorInfo map_projector; + size_t depth = 1; + EXPECT_EQ(map_projector.depth, depth); + EXPECT_EQ(map_projector.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(map_projector.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/component_interface_specs/test/test_perception.cpp b/component_interface_specs/test/test_perception.cpp new file mode 100644 index 00000000..ec80c06b --- /dev/null +++ b/component_interface_specs/test/test_perception.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 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 "component_interface_specs/perception.hpp" +#include "gtest/gtest.h" + +TEST(perception, interface) +{ + { + using perception_interface::ObjectRecognition; + ObjectRecognition object_recognition; + size_t depth = 1; + EXPECT_EQ(object_recognition.depth, depth); + EXPECT_EQ(object_recognition.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(object_recognition.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/component_interface_specs/test/test_planning.cpp b/component_interface_specs/test/test_planning.cpp new file mode 100644 index 00000000..c9cf353d --- /dev/null +++ b/component_interface_specs/test/test_planning.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 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 "component_interface_specs/planning.hpp" +#include "gtest/gtest.h" + +TEST(planning, interface) +{ + { + using planning_interface::RouteState; + RouteState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::LaneletRoute; + LaneletRoute route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::Trajectory; + Trajectory trajectory; + size_t depth = 1; + EXPECT_EQ(trajectory.depth, depth); + EXPECT_EQ(trajectory.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(trajectory.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/component_interface_specs/test/test_system.cpp b/component_interface_specs/test/test_system.cpp new file mode 100644 index 00000000..416d2eff --- /dev/null +++ b/component_interface_specs/test/test_system.cpp @@ -0,0 +1,37 @@ +// Copyright 2023 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 "component_interface_specs/system.hpp" +#include "gtest/gtest.h" + +TEST(system, interface) +{ + { + using system_interface::MrmState; + MrmState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using system_interface::OperationModeState; + OperationModeState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/component_interface_specs/test/test_vehicle.cpp b/component_interface_specs/test/test_vehicle.cpp new file mode 100644 index 00000000..1f2041b6 --- /dev/null +++ b/component_interface_specs/test/test_vehicle.cpp @@ -0,0 +1,64 @@ +// Copyright 2023 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 "component_interface_specs/vehicle.hpp" +#include "gtest/gtest.h" + +TEST(vehicle, interface) +{ + { + using vehicle_interface::SteeringStatus; + SteeringStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::GearStatus; + GearStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::TurnIndicatorStatus; + TurnIndicatorStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::HazardLightStatus; + HazardLightStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::EnergyStatus; + EnergyStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/component_interface_utils/CMakeLists.txt b/component_interface_utils/CMakeLists.txt new file mode 100644 index 00000000..435d5535 --- /dev/null +++ b/component_interface_utils/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(component_interface_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +include_directories( + include + SYSTEM + ${rclcpp_INCLUDE_DIRS} + ${rosidl_runtime_cpp_INCLUDE_DIRS} + ${rcl_INCLUDE_DIRS} + ${autoware_adapi_v1_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_component_interface_utils + test/test_component_interface_utils.cpp + ) + + target_include_directories(test_component_interface_utils + PRIVATE include + ) +endif() + +ament_auto_package() diff --git a/component_interface_utils/README.md b/component_interface_utils/README.md new file mode 100644 index 00000000..2a2b191c --- /dev/null +++ b/component_interface_utils/README.md @@ -0,0 +1,101 @@ +# component_interface_utils + +## Features + +This is a utility package that provides the following features: + +- Instantiation of the wrapper class +- Logging for service and client +- Service exception for response +- Relays for topic and service + +## Design + +This package provides the wrappers for the interface classes of rclcpp. +The wrappers limit the usage of the original class to enforce the processing recommended by the component interface. +Do not inherit the class of rclcpp, and forward or wrap the member function that is allowed to be used. + +## Instantiation of the wrapper class + +The wrapper class requires interface information in this format. + +```cpp +struct SampleService +{ + using Service = sample_msgs::srv::ServiceType; + static constexpr char name[] = "/sample/service"; +}; + +struct SampleMessage +{ + using Message = sample_msgs::msg::MessageType; + static constexpr char name[] = "/sample/message"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; +``` + +Create the wrapper using the above definition as follows. + +```cpp +// header file +component_interface_utils::Service::SharedPtr srv_; +component_interface_utils::Client::SharedPtr cli_; +component_interface_utils::Publisher::SharedPtr pub_; +component_interface_utils::Subscription::SharedPtr sub_; + +// source file +const auto node = component_interface_utils::NodeAdaptor(this); +node.init_srv(srv_, callback); +node.init_cli(cli_); +node.init_pub(pub_); +node.init_sub(sub_, callback); +``` + +## Logging for service and client + +If the wrapper class is used, logging is automatically enabled. The log level is `RCLCPP_INFO`. + +## Service exception for response + +If the wrapper class is used and the service response has status, throwing `ServiceException` will automatically catch and set it to status. +This is useful when returning an error from a function called from the service callback. + +```cpp +void service_callback(Request req, Response res) +{ + function(); + res->status.success = true; +} + +void function() +{ + throw ServiceException(ERROR_CODE, "message"); +} +``` + +If the wrapper class is not used or the service response has no status, manually catch the `ServiceException` as follows. + +```cpp +void service_callback(Request req, Response res) +{ + try { + function(); + res->status.success = true; + } catch (const ServiceException & error) { + res->status = error.status(); + } +} +``` + +## Relays for topic and service + +There are utilities for relaying services and messages of the same type. + +```cpp +const auto node = component_interface_utils::NodeAdaptor(this); +service_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); +node.relay_message(pub_, sub_); +node.relay_service(cli_, srv_, service_callback_group_); // group is for avoiding deadlocks +``` diff --git a/component_interface_utils/include/component_interface_utils/rclcpp.hpp b/component_interface_utils/include/component_interface_utils/rclcpp.hpp new file mode 100644 index 00000000..8099bea3 --- /dev/null +++ b/component_interface_utils/include/component_interface_utils/rclcpp.hpp @@ -0,0 +1,128 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_UTILS__RCLCPP_HPP_ +#define COMPONENT_INTERFACE_UTILS__RCLCPP_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace component_interface_utils +{ + +class NodeAdaptor +{ +private: + using CallbackGroup = rclcpp::CallbackGroup::SharedPtr; + + template + using MessageCallback = + void (InstanceT::*)(const typename SharedPtrT::element_type::SpecType::Message::ConstSharedPtr); + + template + using ServiceCallback = void (InstanceT::*)( + const typename SharedPtrT::element_type::SpecType::Service::Request::SharedPtr, + const typename SharedPtrT::element_type::SpecType::Service::Response::SharedPtr); + +public: + /// Constructor. + explicit NodeAdaptor(rclcpp::Node * node) { interface_ = std::make_shared(node); } + + /// Create a client wrapper for logging. + template + void init_cli(SharedPtrT & cli, CallbackGroup group = nullptr) const + { + using SpecT = typename SharedPtrT::element_type::SpecType; + cli = create_client_impl(interface_, group); + } + + /// Create a service wrapper for logging. + template + void init_srv(SharedPtrT & srv, CallbackT && callback, CallbackGroup group = nullptr) const + { + using SpecT = typename SharedPtrT::element_type::SpecType; + srv = create_service_impl(interface_, std::forward(callback), group); + } + + /// Create a publisher using traits like services. + template + void init_pub(SharedPtrT & pub) const + { + using SpecT = typename SharedPtrT::element_type::SpecType; + pub = create_publisher_impl(interface_->node); + } + + /// Create a subscription using traits like services. + template + void init_sub(SharedPtrT & sub, CallbackT && callback) const + { + using SpecT = typename SharedPtrT::element_type::SpecType; + sub = create_subscription_impl(interface_->node, std::forward(callback)); + } + + /// Relay message. + template + void relay_message(P & pub, S & sub) const + { + using MsgT = typename P::element_type::SpecType::Message::ConstSharedPtr; + init_pub(pub); + init_sub(sub, [pub](MsgT msg) { pub->publish(*msg); }); + } + + /// Relay service. + template + void relay_service( + C & cli, S & srv, CallbackGroup group, std::optional timeout = std::nullopt) const + { + init_cli(cli); + init_srv(srv, [cli, timeout](auto req, auto res) { *res = *cli->call(req, timeout); }, group); + } + + /// Create a subscription wrapper. + template + void init_sub( + SharedPtrT & sub, InstanceT * instance, + MessageCallback && callback) const + { + using std::placeholders::_1; + init_sub(sub, std::bind(callback, instance, _1)); + } + + /// Create a service wrapper for logging. + template + void init_srv( + SharedPtrT & srv, InstanceT * instance, ServiceCallback && callback, + CallbackGroup group = nullptr) const + { + using std::placeholders::_1; + using std::placeholders::_2; + init_srv(srv, std::bind(callback, instance, _1, _2), group); + } + +private: + // Use a node pointer because shared_from_this cannot be used in constructor. + NodeInterface::SharedPtr interface_; +}; + +} // namespace component_interface_utils + +#endif // COMPONENT_INTERFACE_UTILS__RCLCPP_HPP_ diff --git a/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp b/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp new file mode 100644 index 00000000..d5af211b --- /dev/null +++ b/component_interface_utils/include/component_interface_utils/rclcpp/create_interface.hpp @@ -0,0 +1,77 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_UTILS__RCLCPP__CREATE_INTERFACE_HPP_ +#define COMPONENT_INTERFACE_UTILS__RCLCPP__CREATE_INTERFACE_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace component_interface_utils +{ + +/// Create a client wrapper for logging. This is a private implementation. +template +typename Client::SharedPtr create_client_impl( + NodeInterface::SharedPtr interface, rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + // This function is a wrapper for the following. + // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L253-L265 + return Client::make_shared(interface, group); +} + +/// Create a service wrapper for logging. This is a private implementation. +template +typename Service::SharedPtr create_service_impl( + NodeInterface::SharedPtr interface, CallbackT && callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + // This function is a wrapper for the following. + // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L267-L281 + return Service::make_shared(interface, std::forward(callback), group); +} + +/// Create a publisher using traits like services. This is a private implementation. +template +typename Publisher::SharedPtr create_publisher_impl(NodeT * node) +{ + // This function is a wrapper for the following. + // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L167-L205 + auto publisher = + node->template create_publisher(SpecT::name, get_qos()); + return Publisher::make_shared(publisher); +} + +/// Create a subscription using traits like services. This is a private implementation. +template +typename Subscription::SharedPtr create_subscription_impl( + NodeT * node, CallbackT && callback) +{ + // This function is a wrapper for the following. + // https://github.com/ros2/rclcpp/blob/48068130edbb43cdd61076dc1851672ff1a80408/rclcpp/include/rclcpp/node.hpp#L207-L238 + auto subscription = node->template create_subscription( + SpecT::name, get_qos(), std::forward(callback)); + return Subscription::make_shared(subscription); +} + +} // namespace component_interface_utils + +#endif // COMPONENT_INTERFACE_UTILS__RCLCPP__CREATE_INTERFACE_HPP_ diff --git a/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp b/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp new file mode 100644 index 00000000..e0a60648 --- /dev/null +++ b/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp @@ -0,0 +1,109 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_UTILS__RCLCPP__EXCEPTIONS_HPP_ +#define COMPONENT_INTERFACE_UTILS__RCLCPP__EXCEPTIONS_HPP_ + +#include + +#include +#include + +namespace component_interface_utils +{ + +class ServiceException : public std::exception +{ +public: + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + + ServiceException(ResponseStatusCode code, const std::string & message, bool success = false) + { + success_ = success; + code_ = code; + message_ = message; + } + + template + void set(T & status) const + { + status.success = success_; + status.code = code_; + status.message = message_; + } + + ResponseStatus status() const + { + ResponseStatus status; + status.success = success_; + status.code = code_; + status.message = message_; + return status; + } + +private: + bool success_; + ResponseStatusCode code_; + std::string message_; +}; + +class ServiceUnready : public ServiceException +{ +public: + explicit ServiceUnready(const std::string & message) + : ServiceException(ResponseStatus::SERVICE_UNREADY, message, false) + { + } +}; + +class ServiceTimeout : public ServiceException +{ +public: + explicit ServiceTimeout(const std::string & message) + : ServiceException(ResponseStatus::SERVICE_TIMEOUT, message, false) + { + } +}; + +class TransformError : public ServiceException +{ +public: + explicit TransformError(const std::string & message) + : ServiceException(ResponseStatus::TRANSFORM_ERROR, message, false) + { + } +}; + +class ParameterError : public ServiceException +{ +public: + explicit ParameterError(const std::string & message) + : ServiceException(ResponseStatus::PARAMETER_ERROR, message, false) + { + } +}; + +class NoEffectWarning : public ServiceException +{ +public: + explicit NoEffectWarning(const std::string & message) + : ServiceException(ResponseStatus::NO_EFFECT, message, true) + { + } +}; + +} // namespace component_interface_utils + +#endif // COMPONENT_INTERFACE_UTILS__RCLCPP__EXCEPTIONS_HPP_ diff --git a/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp b/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp new file mode 100644 index 00000000..97f46933 --- /dev/null +++ b/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp @@ -0,0 +1,73 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_UTILS__RCLCPP__INTERFACE_HPP_ +#define COMPONENT_INTERFACE_UTILS__RCLCPP__INTERFACE_HPP_ + +#include + +#include + +#include +#include +#include + +namespace component_interface_utils +{ + +struct NodeInterface +{ + using SharedPtr = std::shared_ptr; + using ServiceLog = tier4_system_msgs::msg::ServiceLog; + + explicit NodeInterface(rclcpp::Node * node) + { + this->node = node; + this->logger = node->create_publisher("/service_log", 10); + + node_name = node->get_namespace(); + if (node_name.empty() || node_name.back() != '/') { + node_name += "/"; + } + node_name += node->get_name(); + } + + void log(ServiceLog::_type_type type, const std::string & name, const std::string & yaml = "") + { + static const auto type_text = std::unordered_map( + {{ServiceLog::CLIENT_REQUEST, "client call"}, + {ServiceLog::SERVER_REQUEST, "server call"}, + {ServiceLog::SERVER_RESPONSE, "server exit"}, + {ServiceLog::CLIENT_RESPONSE, "client exit"}, + {ServiceLog::ERROR_UNREADY, "client unready"}, + {ServiceLog::ERROR_TIMEOUT, "client timeout"}}); + RCLCPP_DEBUG_STREAM(node->get_logger(), type_text.at(type) << ": " << name); + + ServiceLog msg; + msg.stamp = node->now(); + msg.type = type; + msg.name = name; + msg.node = node_name; + msg.yaml = yaml; + logger->publish(msg); + } + + rclcpp::Node * node; + rclcpp::Publisher::SharedPtr logger; + std::string node_name; +}; + +} // namespace component_interface_utils + +#endif // COMPONENT_INTERFACE_UTILS__RCLCPP__INTERFACE_HPP_ diff --git a/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp b/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp new file mode 100644 index 00000000..e9cf68fa --- /dev/null +++ b/component_interface_utils/include/component_interface_utils/rclcpp/service_client.hpp @@ -0,0 +1,109 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_CLIENT_HPP_ +#define COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_CLIENT_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include + +namespace component_interface_utils +{ + +/// The wrapper class of rclcpp::Client for logging. +template +class Client +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Client) + using SpecType = SpecT; + using WrapType = rclcpp::Client; + using ServiceLog = tier4_system_msgs::msg::ServiceLog; + + /// Constructor. + Client(NodeInterface::SharedPtr interface, rclcpp::CallbackGroup::SharedPtr group) + : interface_(interface) + { + client_ = interface->node->create_client( + SpecT::name, rmw_qos_profile_services_default, group); + } + + /// Send request. + typename WrapType::SharedResponse call( + const typename WrapType::SharedRequest request, std::optional timeout = std::nullopt) + { + if (!client_->service_is_ready()) { + interface_->log(ServiceLog::ERROR_UNREADY, SpecType::name); + throw ServiceUnready(SpecT::name); + } + + const auto future = this->async_send_request(request); + if (timeout) { + const auto duration = std::chrono::duration>(timeout.value()); + if (future.wait_for(duration) != std::future_status::ready) { + interface_->log(ServiceLog::ERROR_TIMEOUT, SpecType::name); + throw ServiceTimeout(SpecT::name); + } + } + return future.get(); + } + + /// Send request. + typename WrapType::SharedFuture async_send_request(typename WrapType::SharedRequest request) + { + return this->async_send_request(request, [](typename WrapType::SharedFuture) {}); + } + + /// Send request. + template + typename WrapType::SharedFuture async_send_request( + typename WrapType::SharedRequest request, CallbackT && callback) + { +#ifdef ROS_DISTRO_GALACTIC + using rosidl_generator_traits::to_yaml; +#endif + + const auto wrapped = [this, callback](typename WrapType::SharedFuture future) { + interface_->log(ServiceLog::CLIENT_RESPONSE, SpecType::name, to_yaml(*future.get())); + callback(future); + }; + + interface_->log(ServiceLog::CLIENT_REQUEST, SpecType::name, to_yaml(*request)); + +#ifdef ROS_DISTRO_GALACTIC + return client_->async_send_request(request, wrapped); +#else + return client_->async_send_request(request, wrapped).future; +#endif + } + + /// Check if the service is ready. + bool service_is_ready() const { return client_->service_is_ready(); } + +private: + RCLCPP_DISABLE_COPY(Client) + typename WrapType::SharedPtr client_; + NodeInterface::SharedPtr interface_; +}; + +} // namespace component_interface_utils + +#endif // COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_CLIENT_HPP_ diff --git a/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp b/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp new file mode 100644 index 00000000..762f95fe --- /dev/null +++ b/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp @@ -0,0 +1,99 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_SERVER_HPP_ +#define COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_SERVER_HPP_ + +#include +#include +#include + +#include + +#include + +namespace component_interface_utils +{ + +/// The wrapper class of rclcpp::Service for logging. +template +class Service +{ +private: + // Detect if the service response has status. + template class, class = std::void_t<>> + struct detect : std::false_type + { + }; + template class Check> + struct detect>> : std::true_type + { + }; + template + using has_status_impl = decltype(std::declval().status); + template + using has_status_type = detect; + +public: + RCLCPP_SMART_PTR_DEFINITIONS(Service) + using SpecType = SpecT; + using WrapType = rclcpp::Service; + using ServiceLog = tier4_system_msgs::msg::ServiceLog; + + /// Constructor. + template + Service( + NodeInterface::SharedPtr interface, CallbackT && callback, + rclcpp::CallbackGroup::SharedPtr group) + : interface_(interface) + { + service_ = interface_->node->create_service( + SpecT::name, wrap(callback), rmw_qos_profile_services_default, group); + } + + /// Create a service callback with logging added. + template + typename WrapType::CallbackType wrap(CallbackT && callback) + { + auto wrapped = [this, callback]( + typename SpecT::Service::Request::SharedPtr request, + typename SpecT::Service::Response::SharedPtr response) { +#ifdef ROS_DISTRO_GALACTIC + using rosidl_generator_traits::to_yaml; +#endif + // If the response has status, convert it from the exception. + interface_->log(ServiceLog::SERVER_REQUEST, SpecType::name, to_yaml(*request)); + if constexpr (!has_status_type::value) { + callback(request, response); + } else { + try { + callback(request, response); + } catch (const ServiceException & error) { + error.set(response->status); + } + } + interface_->log(ServiceLog::SERVER_RESPONSE, SpecType::name, to_yaml(*response)); + }; + return wrapped; + } + +private: + RCLCPP_DISABLE_COPY(Service) + typename WrapType::SharedPtr service_; + NodeInterface::SharedPtr interface_; +}; + +} // namespace component_interface_utils + +#endif // COMPONENT_INTERFACE_UTILS__RCLCPP__SERVICE_SERVER_HPP_ diff --git a/component_interface_utils/include/component_interface_utils/rclcpp/topic_publisher.hpp b/component_interface_utils/include/component_interface_utils/rclcpp/topic_publisher.hpp new file mode 100644 index 00000000..b2c00261 --- /dev/null +++ b/component_interface_utils/include/component_interface_utils/rclcpp/topic_publisher.hpp @@ -0,0 +1,48 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_UTILS__RCLCPP__TOPIC_PUBLISHER_HPP_ +#define COMPONENT_INTERFACE_UTILS__RCLCPP__TOPIC_PUBLISHER_HPP_ + +#include + +namespace component_interface_utils +{ + +/// The wrapper class of rclcpp::Publisher. This is for future use and no functionality now. +template +class Publisher +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Publisher) + using SpecType = SpecT; + using WrapType = rclcpp::Publisher; + + /// Constructor. + explicit Publisher(typename WrapType::SharedPtr publisher) + { + publisher_ = publisher; // to keep the reference count + } + + /// Publish a message. + void publish(const typename SpecT::Message & msg) { publisher_->publish(msg); } + +private: + RCLCPP_DISABLE_COPY(Publisher) + typename WrapType::SharedPtr publisher_; +}; + +} // namespace component_interface_utils + +#endif // COMPONENT_INTERFACE_UTILS__RCLCPP__TOPIC_PUBLISHER_HPP_ diff --git a/component_interface_utils/include/component_interface_utils/rclcpp/topic_subscription.hpp b/component_interface_utils/include/component_interface_utils/rclcpp/topic_subscription.hpp new file mode 100644 index 00000000..195a04c3 --- /dev/null +++ b/component_interface_utils/include/component_interface_utils/rclcpp/topic_subscription.hpp @@ -0,0 +1,45 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_UTILS__RCLCPP__TOPIC_SUBSCRIPTION_HPP_ +#define COMPONENT_INTERFACE_UTILS__RCLCPP__TOPIC_SUBSCRIPTION_HPP_ + +#include + +namespace component_interface_utils +{ + +/// The wrapper class of rclcpp::Subscription. This is for future use and no functionality now. +template +class Subscription +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Subscription) + using SpecType = SpecT; + using WrapType = rclcpp::Subscription; + + /// Constructor. + explicit Subscription(typename WrapType::SharedPtr subscription) + { + subscription_ = subscription; // to keep the reference count + } + +private: + RCLCPP_DISABLE_COPY(Subscription) + typename WrapType::SharedPtr subscription_; +}; + +} // namespace component_interface_utils + +#endif // COMPONENT_INTERFACE_UTILS__RCLCPP__TOPIC_SUBSCRIPTION_HPP_ diff --git a/component_interface_utils/include/component_interface_utils/specs.hpp b/component_interface_utils/include/component_interface_utils/specs.hpp new file mode 100644 index 00000000..db3bb951 --- /dev/null +++ b/component_interface_utils/include/component_interface_utils/specs.hpp @@ -0,0 +1,34 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_UTILS__SPECS_HPP_ +#define COMPONENT_INTERFACE_UTILS__SPECS_HPP_ + +#include + +namespace component_interface_utils +{ + +template +rclcpp::QoS get_qos() +{ + rclcpp::QoS qos(SpecT::depth); + qos.reliability(SpecT::reliability); + qos.durability(SpecT::durability); + return qos; +} + +} // namespace component_interface_utils + +#endif // COMPONENT_INTERFACE_UTILS__SPECS_HPP_ diff --git a/component_interface_utils/include/component_interface_utils/status.hpp b/component_interface_utils/include/component_interface_utils/status.hpp new file mode 100644 index 00000000..bf4d67ae --- /dev/null +++ b/component_interface_utils/include/component_interface_utils/status.hpp @@ -0,0 +1,31 @@ +// Copyright 2022 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 COMPONENT_INTERFACE_UTILS__STATUS_HPP_ +#define COMPONENT_INTERFACE_UTILS__STATUS_HPP_ + +namespace component_interface_utils::status +{ + +template +void copy(const T1 & src, T2 & dst) // NOLINT(build/include_what_you_use): cpplint false positive +{ + dst->status.success = src->status.success; + dst->status.code = src->status.code; + dst->status.message = src->status.message; +} + +} // namespace component_interface_utils::status + +#endif // COMPONENT_INTERFACE_UTILS__STATUS_HPP_ diff --git a/component_interface_utils/package.xml b/component_interface_utils/package.xml new file mode 100755 index 00000000..7a6a6d12 --- /dev/null +++ b/component_interface_utils/package.xml @@ -0,0 +1,29 @@ + + + + component_interface_utils + 0.0.0 + The component_interface_utils package + Takagi, Isamu + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + tier4_system_msgs + + autoware_adapi_v1_msgs + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + tier4_system_msgs + + + ament_cmake + + diff --git a/component_interface_utils/test/test_component_interface_utils.cpp b/component_interface_utils/test/test_component_interface_utils.cpp new file mode 100644 index 00000000..3c41f4a8 --- /dev/null +++ b/component_interface_utils/test/test_component_interface_utils.cpp @@ -0,0 +1,76 @@ +// Copyright 2023 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 "component_interface_utils/rclcpp/exceptions.hpp" +#include "component_interface_utils/specs.hpp" +#include "component_interface_utils/status.hpp" +#include "gtest/gtest.h" + +TEST(interface, utils) +{ + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + + ResponseStatusCode code = 10; + const std::string message = "test_exception"; + ServiceException service(code, message); + ResponseStatus code_back; + code_back = service.status(); + EXPECT_EQ(code_back.code, code); + EXPECT_EQ(code_back.message, message); + } + + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + + ResponseStatusCode code = 10; + const std::string message = "test_exception"; + ServiceException service(code, message); + ResponseStatus code_set; + service.set(code_set); + EXPECT_EQ(code_set.code, code); + EXPECT_EQ(code_set.message, message); + } + + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + using component_interface_utils::status::copy; + + class status_test + { + public: + status_test(ResponseStatusCode code, const std::string & message, bool success = false) + { + status.code = code; + status.message = message; + status.success = success; + } + ResponseStatus status; + }; + + const status_test status_in(10, "test_exception", true); + auto status_copy = std::make_shared(100, "test_exception_copy", false); + copy(&status_in, status_copy); + + EXPECT_EQ(status_in.status.code, status_copy->status.code); + EXPECT_EQ(status_in.status.message, status_copy->status.message); + EXPECT_EQ(status_in.status.success, status_copy->status.success); + } +}