From 52492a0086e6025e732d2f58050c363fad4bee49 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Fri, 13 Dec 2024 01:47:10 +0900 Subject: [PATCH 1/6] feat: move autoware_map_projection_loader package from Autoware Universe Signed-off-by: Ryohsuke Mitsudome --- .../CHANGELOG.rst | 80 + .../CMakeLists.txt | 63 + map/autoware_map_projection_loader/README.md | 95 + .../config/map_projection_loader.param.yaml | 4 + .../docs/map_projector_type.svg | 3010 +++++++++++++++++ .../load_info_from_lanelet2_map.hpp | 32 + .../map_projection_loader.hpp | 42 + .../launch/map_projection_loader.launch.xml | 10 + .../package.xml | 36 + .../schema/map_projection_loader.schema.json | 38 + .../src/load_info_from_lanelet2_map.cpp | 63 + .../src/map_projection_loader.cpp | 112 + .../test/data/map_projector_info_local.yaml | 1 + ...ap_projector_info_local_cartesian_utm.yaml | 6 + .../test/data/map_projector_info_mgrs.yaml | 3 + ...ap_projector_info_transverse_mercator.yaml | 6 + .../test/test_load_info_from_lanelet2_map.cpp | 137 + ...load_local_cartesian_utm_from_yaml.test.py | 150 + .../test_node_load_local_from_yaml.test.py | 143 + .../test_node_load_mgrs_from_yaml.test.py | 145 + ...load_transverse_mercator_from_yaml.test.py | 150 + 21 files changed, 4326 insertions(+) create mode 100644 map/autoware_map_projection_loader/CHANGELOG.rst create mode 100644 map/autoware_map_projection_loader/CMakeLists.txt create mode 100644 map/autoware_map_projection_loader/README.md create mode 100644 map/autoware_map_projection_loader/config/map_projection_loader.param.yaml create mode 100644 map/autoware_map_projection_loader/docs/map_projector_type.svg create mode 100644 map/autoware_map_projection_loader/include/autoware/map_projection_loader/load_info_from_lanelet2_map.hpp create mode 100644 map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp create mode 100644 map/autoware_map_projection_loader/launch/map_projection_loader.launch.xml create mode 100644 map/autoware_map_projection_loader/package.xml create mode 100644 map/autoware_map_projection_loader/schema/map_projection_loader.schema.json create mode 100644 map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp create mode 100644 map/autoware_map_projection_loader/src/map_projection_loader.cpp create mode 100644 map/autoware_map_projection_loader/test/data/map_projector_info_local.yaml create mode 100644 map/autoware_map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml create mode 100644 map/autoware_map_projection_loader/test/data/map_projector_info_mgrs.yaml create mode 100644 map/autoware_map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml create mode 100644 map/autoware_map_projection_loader/test/test_load_info_from_lanelet2_map.cpp create mode 100644 map/autoware_map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py create mode 100644 map/autoware_map_projection_loader/test/test_node_load_local_from_yaml.test.py create mode 100644 map/autoware_map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py create mode 100644 map/autoware_map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py diff --git a/map/autoware_map_projection_loader/CHANGELOG.rst b/map/autoware_map_projection_loader/CHANGELOG.rst new file mode 100644 index 00000000..eef30e5c --- /dev/null +++ b/map/autoware_map_projection_loader/CHANGELOG.rst @@ -0,0 +1,80 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_map_projection_loader +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.40.0 (2024-12-12) +------------------- +* Merge branch 'main' into release-0.40.0 +* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" + This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. +* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) +* chore(package.xml): bump version to 0.39.0 (`#9587 `_) + * chore(package.xml): bump version to 0.39.0 + * fix: fix ticket links in CHANGELOG.rst + * fix: remove unnecessary diff + --------- + Co-authored-by: Yutaka Kondo +* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) +* fix(cpplint): include what you use - map (`#9568 `_) +* feat!: replace tier4_map_msgs with autoware_map_msgs for MapProjectorInfo (`#9392 `_) +* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) + * make lanelet2_map_visualization independent + * remove unused files + * remove unused package + * fix package name + * add autoware\_ prefix + * add autoware to exec name + * add autoware prefix + * removed unnecessary dependency + --------- +* 0.39.0 +* update changelog +* Merge commit '6a1ddbd08bd' into release-0.39.0 +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) + * make lanelet2_map_visualization independent + * remove unused files + * remove unused package + * fix package name + * add autoware\_ prefix + * add autoware to exec name + * add autoware prefix + * removed unnecessary dependency + --------- +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) + * unify package.xml version to 0.37.0 + * remove system_monitor/CHANGELOG.rst + * add changelog + * 0.38.0 + --------- +* refactor(component_interface_utils): prefix package and namespace with autoware (`#9092 `_) +* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Masaki Baba, Ryohsuke Mitsudome, Yutaka Kondo + +0.39.0 (2024-11-25) +------------------- +* Merge commit '6a1ddbd08bd' into release-0.39.0 +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) + * unify package.xml version to 0.37.0 + * remove system_monitor/CHANGELOG.rst + * add changelog + * 0.38.0 + --------- +* refactor(component_interface_utils): prefix package and namespace with autoware (`#9092 `_) +* Contributors: Esteve Fernandez, Yutaka Kondo + +0.38.0 (2024-11-08) +------------------- +* unify package.xml version to 0.37.0 +* refactor(component_interface_specs): prefix package and namespace with autoware (`#9094 `_) +* refactor(map_projection_loader)!: prefix package and namespace with autoware (`#8420 `_) + * add autoware\_ prefix + * add autoware\_ prefix + --------- + Co-authored-by: SakodaShintaro +* Contributors: Esteve Fernandez, Masaki Baba, Yutaka Kondo + +0.26.0 (2024-04-03) +------------------- diff --git a/map/autoware_map_projection_loader/CMakeLists.txt b/map/autoware_map_projection_loader/CMakeLists.txt new file mode 100644 index 00000000..19c5a98e --- /dev/null +++ b/map/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/map/autoware_map_projection_loader/README.md b/map/autoware_map_projection_loader/README.md new file mode 100644 index 00000000..ad35a666 --- /dev/null +++ b/map/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 `autoware_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` (autoware_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/map/autoware_map_projection_loader/config/map_projection_loader.param.yaml b/map/autoware_map_projection_loader/config/map_projection_loader.param.yaml new file mode 100644 index 00000000..6ec30030 --- /dev/null +++ b/map/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/map/autoware_map_projection_loader/docs/map_projector_type.svg b/map/autoware_map_projection_loader/docs/map_projector_type.svg new file mode 100644 index 00000000..e1c8c2ac --- /dev/null +++ b/map/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/map/autoware_map_projection_loader/include/autoware/map_projection_loader/load_info_from_lanelet2_map.hpp b/map/autoware_map_projection_loader/include/autoware/map_projection_loader/load_info_from_lanelet2_map.hpp new file mode 100644 index 00000000..da0ebb83 --- /dev/null +++ b/map/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 "autoware_map_msgs/msg/map_projector_info.hpp" + +#include + +namespace autoware::map_projection_loader +{ +autoware_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/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp b/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp new file mode 100644 index 00000000..1ca876dc --- /dev/null +++ b/map/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 +{ +autoware_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename); +autoware_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 = autoware::component_interface_specs::map::MapProjectorInfo; + autoware::component_interface_utils::Publisher::SharedPtr publisher_; +}; +} // namespace autoware::map_projection_loader + +#endif // AUTOWARE__MAP_PROJECTION_LOADER__MAP_PROJECTION_LOADER_HPP_ diff --git a/map/autoware_map_projection_loader/launch/map_projection_loader.launch.xml b/map/autoware_map_projection_loader/launch/map_projection_loader.launch.xml new file mode 100644 index 00000000..224598e2 --- /dev/null +++ b/map/autoware_map_projection_loader/launch/map_projection_loader.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/map/autoware_map_projection_loader/package.xml b/map/autoware_map_projection_loader/package.xml new file mode 100644 index 00000000..ad31dfdc --- /dev/null +++ b/map/autoware_map_projection_loader/package.xml @@ -0,0 +1,36 @@ + + + + autoware_map_projection_loader + 0.40.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_component_interface_specs + autoware_component_interface_utils + autoware_lanelet2_extension + autoware_map_msgs + rclcpp + rclcpp_components + yaml-cpp + + ament_cmake_gmock + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + ros_testing + + + ament_cmake + + diff --git a/map/autoware_map_projection_loader/schema/map_projection_loader.schema.json b/map/autoware_map_projection_loader/schema/map_projection_loader.schema.json new file mode 100644 index 00000000..bb7fe5d2 --- /dev/null +++ b/map/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/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp new file mode 100644 index 00000000..750a73c8 --- /dev/null +++ b/map/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 "autoware_map_msgs/msg/map_projector_info.hpp" + +#include +#include +#include +#include + +#include + +namespace autoware::map_projection_loader +{ +autoware_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; + } + } + + autoware_map_msgs::msg::MapProjectorInfo msg; + if (is_local) { + msg.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL; + } else { + msg.projector_type = autoware_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 = autoware_map_msgs::msg::MapProjectorInfo::WGS84; + return msg; +} +} // namespace autoware::map_projection_loader diff --git a/map/autoware_map_projection_loader/src/map_projection_loader.cpp b/map/autoware_map_projection_loader/src/map_projection_loader.cpp new file mode 100644 index 00000000..981380d1 --- /dev/null +++ b/map/autoware_map_projection_loader/src/map_projection_loader.cpp @@ -0,0 +1,112 @@ +// 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 +#include +#include + +namespace autoware::map_projection_loader +{ +autoware_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & filename) +{ + YAML::Node data = YAML::LoadFile(filename); + + autoware_map_msgs::msg::MapProjectorInfo msg; + msg.projector_type = data["projector_type"].as(); + if (msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::MGRS) { + msg.vertical_datum = data["vertical_datum"].as(); + msg.mgrs_grid = data["mgrs_grid"].as(); + + } else if ( + msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM || + msg.projector_type == autoware_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 == autoware_map_msgs::msg::MapProjectorInfo::LOCAL) { + ; // do nothing + + } else if (msg.projector_type == "local") { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("MapProjectionLoader"), + "Load " << filename << "\n" + << "DEPRECATED WARNING: projector type \"local\" is deprecated." + "Please use \"Local\" instead. For more info, visit " + "https://github.com/autowarefoundation/autoware.universe/blob/main/map/" + "map_projection_loader README.md"); + msg.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL; + } else { + throw std::runtime_error( + "Invalid map projector type. Currently supported types: MGRS, LocalCartesianUTM, " + "TransverseMercator, and Local"); + } + return msg; +} + +autoware_map_msgs::msg::MapProjectorInfo load_map_projector_info( + const std::string & yaml_filename, const std::string & lanelet2_map_filename) +{ + autoware_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 autoware_map_msgs::msg::MapProjectorInfo msg = + load_map_projector_info(yaml_filename, lanelet2_map_filename); + + // Publish the message + const auto adaptor = autoware::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/map/autoware_map_projection_loader/test/data/map_projector_info_local.yaml b/map/autoware_map_projection_loader/test/data/map_projector_info_local.yaml new file mode 100644 index 00000000..b1f30b12 --- /dev/null +++ b/map/autoware_map_projection_loader/test/data/map_projector_info_local.yaml @@ -0,0 +1 @@ +projector_type: Local diff --git a/map/autoware_map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml b/map/autoware_map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml new file mode 100644 index 00000000..854a6ad7 --- /dev/null +++ b/map/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/map/autoware_map_projection_loader/test/data/map_projector_info_mgrs.yaml b/map/autoware_map_projection_loader/test/data/map_projector_info_mgrs.yaml new file mode 100644 index 00000000..5446de48 --- /dev/null +++ b/map/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/map/autoware_map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml b/map/autoware_map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml new file mode 100644 index 00000000..de1febeb --- /dev/null +++ b/map/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/map/autoware_map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/autoware_map_projection_loader/test/test_load_info_from_lanelet2_map.cpp new file mode 100644 index 00000000..bf2207a7 --- /dev/null +++ b/map/autoware_map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -0,0 +1,137 @@ +// 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 +#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/map/autoware_map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/map/autoware_map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py new file mode 100644 index 00000000..30cede4b --- /dev/null +++ b/map/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 +from autoware_map_msgs.msg import MapProjectorInfo +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 +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/map/autoware_map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/autoware_map_projection_loader/test/test_node_load_local_from_yaml.test.py new file mode 100644 index 00000000..9c364f09 --- /dev/null +++ b/map/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 +from autoware_map_msgs.msg import MapProjectorInfo +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 +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/map/autoware_map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/autoware_map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py new file mode 100644 index 00000000..6586a9ae --- /dev/null +++ b/map/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 +from autoware_map_msgs.msg import MapProjectorInfo +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 +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/map/autoware_map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/map/autoware_map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py new file mode 100644 index 00000000..3035ce75 --- /dev/null +++ b/map/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 +from autoware_map_msgs.msg import MapProjectorInfo +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 +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) From a5402da7d85f6d8f7da176cb171eccc841b9045b Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Fri, 13 Dec 2024 01:48:37 +0900 Subject: [PATCH 2/6] chore: reset version in package.xml and remove CHANGELOG.rst Signed-off-by: Ryohsuke Mitsudome --- .../CHANGELOG.rst | 80 ------------------- .../package.xml | 2 +- 2 files changed, 1 insertion(+), 81 deletions(-) delete mode 100644 map/autoware_map_projection_loader/CHANGELOG.rst diff --git a/map/autoware_map_projection_loader/CHANGELOG.rst b/map/autoware_map_projection_loader/CHANGELOG.rst deleted file mode 100644 index eef30e5c..00000000 --- a/map/autoware_map_projection_loader/CHANGELOG.rst +++ /dev/null @@ -1,80 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_map_projection_loader -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - map (`#9568 `_) -* feat!: replace tier4_map_msgs with autoware_map_msgs for MapProjectorInfo (`#9392 `_) -* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) - * make lanelet2_map_visualization independent - * remove unused files - * remove unused package - * fix package name - * add autoware\_ prefix - * add autoware to exec name - * add autoware prefix - * removed unnecessary dependency - --------- -* 0.39.0 -* update changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) - * make lanelet2_map_visualization independent - * remove unused files - * remove unused package - * fix package name - * add autoware\_ prefix - * add autoware to exec name - * add autoware prefix - * removed unnecessary dependency - --------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(component_interface_utils): prefix package and namespace with autoware (`#9092 `_) -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Masaki Baba, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(component_interface_utils): prefix package and namespace with autoware (`#9092 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(component_interface_specs): prefix package and namespace with autoware (`#9094 `_) -* refactor(map_projection_loader)!: prefix package and namespace with autoware (`#8420 `_) - * add autoware\_ prefix - * add autoware\_ prefix - --------- - Co-authored-by: SakodaShintaro -* Contributors: Esteve Fernandez, Masaki Baba, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/map/autoware_map_projection_loader/package.xml b/map/autoware_map_projection_loader/package.xml index ad31dfdc..e2e71a28 100644 --- a/map/autoware_map_projection_loader/package.xml +++ b/map/autoware_map_projection_loader/package.xml @@ -2,7 +2,7 @@ autoware_map_projection_loader - 0.40.0 + 0.0.0 autoware_map_projection_loader package as a ROS 2 node Yamato Ando Masahiro Sakamoto From 226274d0d817edff4e4a0b0aee132c056a0c0c75 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Fri, 13 Dec 2024 03:23:54 +0900 Subject: [PATCH 3/6] feat: remove dependency to Autoware Universe package Signed-off-by: Ryohsuke Mitsudome --- .../map_projection_loader/map_projection_loader.hpp | 7 +++---- map/autoware_map_projection_loader/package.xml | 3 +-- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp b/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp index 1ca876dc..0b6857da 100644 --- a/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp +++ b/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp @@ -17,8 +17,7 @@ #include "rclcpp/rclcpp.hpp" -#include -#include +#include #include @@ -34,8 +33,8 @@ class MapProjectionLoader : public rclcpp::Node explicit MapProjectionLoader(const rclcpp::NodeOptions & options); private: - using MapProjectorInfo = autoware::component_interface_specs::map::MapProjectorInfo; - autoware::component_interface_utils::Publisher::SharedPtr publisher_; + using MapProjectorInfo = autoware::core_component_interface_specs::map::MapProjectorInfo; + rclcpp::Publisher::SharedPtr publisher_; }; } // namespace autoware::map_projection_loader diff --git a/map/autoware_map_projection_loader/package.xml b/map/autoware_map_projection_loader/package.xml index e2e71a28..ff5337fa 100644 --- a/map/autoware_map_projection_loader/package.xml +++ b/map/autoware_map_projection_loader/package.xml @@ -16,8 +16,7 @@ ament_cmake_auto autoware_cmake - autoware_component_interface_specs - autoware_component_interface_utils + autoware_core_component_interface_specs autoware_lanelet2_extension autoware_map_msgs rclcpp From f5ad1b0742be7e5332209a25094ad1921d0ce571 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Mon, 16 Dec 2024 16:00:31 +0900 Subject: [PATCH 4/6] feat: use autoware_core_component_interface_specs Signed-off-by: Ryohsuke Mitsudome --- .../src/map_projection_loader.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/map/autoware_map_projection_loader/src/map_projection_loader.cpp b/map/autoware_map_projection_loader/src/map_projection_loader.cpp index 981380d1..f29018ba 100644 --- a/map/autoware_map_projection_loader/src/map_projection_loader.cpp +++ b/map/autoware_map_projection_loader/src/map_projection_loader.cpp @@ -102,8 +102,7 @@ MapProjectionLoader::MapProjectionLoader(const rclcpp::NodeOptions & options) load_map_projector_info(yaml_filename, lanelet2_map_filename); // Publish the message - const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this); - adaptor.init_pub(publisher_); + publisher_ = this->create_publisher("map_projector_info", MapProjectionInfo::get_qos()); publisher_->publish(msg); } } // namespace autoware::map_projection_loader From a05a08d6d3e2e513b6710e5d5d32654cc74f9669 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Mon, 16 Dec 2024 16:34:55 +0900 Subject: [PATCH 5/6] fix: fix namespace Signed-off-by: Ryohsuke Mitsudome --- .../src/map_projection_loader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/autoware_map_projection_loader/src/map_projection_loader.cpp b/map/autoware_map_projection_loader/src/map_projection_loader.cpp index f29018ba..82370654 100644 --- a/map/autoware_map_projection_loader/src/map_projection_loader.cpp +++ b/map/autoware_map_projection_loader/src/map_projection_loader.cpp @@ -102,7 +102,7 @@ MapProjectionLoader::MapProjectionLoader(const rclcpp::NodeOptions & options) load_map_projector_info(yaml_filename, lanelet2_map_filename); // Publish the message - publisher_ = this->create_publisher("map_projector_info", MapProjectionInfo::get_qos()); + publisher_ = this->create_publisher("map_projector_info", MapProjectorInfo::get_qos()); publisher_->publish(msg); } } // namespace autoware::map_projection_loader From b50f6ade1b3da75b84692713d50fb8caf1f8026c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 16 Dec 2024 08:15:28 +0000 Subject: [PATCH 6/6] style(pre-commit): autofix --- .../src/map_projection_loader.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/map/autoware_map_projection_loader/src/map_projection_loader.cpp b/map/autoware_map_projection_loader/src/map_projection_loader.cpp index 82370654..a3201cbe 100644 --- a/map/autoware_map_projection_loader/src/map_projection_loader.cpp +++ b/map/autoware_map_projection_loader/src/map_projection_loader.cpp @@ -102,7 +102,8 @@ MapProjectionLoader::MapProjectionLoader(const rclcpp::NodeOptions & options) load_map_projector_info(yaml_filename, lanelet2_map_filename); // Publish the message - publisher_ = this->create_publisher("map_projector_info", MapProjectorInfo::get_qos()); + publisher_ = this->create_publisher( + "map_projector_info", MapProjectorInfo::get_qos()); publisher_->publish(msg); } } // namespace autoware::map_projection_loader