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..0b6857da --- /dev/null +++ b/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp @@ -0,0 +1,41 @@ +// 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 + +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::core_component_interface_specs::map::MapProjectorInfo; + rclcpp::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..ff5337fa --- /dev/null +++ b/map/autoware_map_projection_loader/package.xml @@ -0,0 +1,35 @@ + + + + autoware_map_projection_loader + 0.0.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_core_component_interface_specs + 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..a3201cbe --- /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 + publisher_ = this->create_publisher( + "map_projector_info", MapProjectorInfo::get_qos()); + 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)