diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 62734cc0f4..25d420a599 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -82,6 +82,7 @@ jobs: target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} target-files: ${{ steps.get-changed-files.outputs.changed-files }} clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci + underlay-workspace: /opt/autoware - name: Show disk space after the tasks run: df -h diff --git a/common/autoware_geography_utils/src/lanelet2_projector.cpp b/common/autoware_geography_utils/src/lanelet2_projector.cpp index 7eb14a56d0..2d565bc0ff 100644 --- a/common/autoware_geography_utils/src/lanelet2_projector.cpp +++ b/common/autoware_geography_utils/src/lanelet2_projector.cpp @@ -48,7 +48,8 @@ std::unique_ptr get_lanelet2_projector(const MapProjectorInf projector_info.map_origin.latitude, projector_info.map_origin.longitude, projector_info.map_origin.altitude}; const lanelet::Origin origin{position}; - const lanelet::projection::TransverseMercatorProjector projector{origin}; + const lanelet::projection::TransverseMercatorProjector projector{ + origin, projector_info.scale_factor}; return std::make_unique(projector); } diff --git a/common/autoware_geography_utils/test/test_lanelet2_projector.cpp b/common/autoware_geography_utils/test/test_lanelet2_projector.cpp index 470222aaaa..385654f1e7 100644 --- a/common/autoware_geography_utils/test/test_lanelet2_projector.cpp +++ b/common/autoware_geography_utils/test/test_lanelet2_projector.cpp @@ -77,6 +77,7 @@ TEST(GeographyUtilsLanelet2Projector, GetTransverseMercatorProjector) projector_info.map_origin.latitude = 35.62426; projector_info.map_origin.longitude = 139.74252; projector_info.map_origin.altitude = 0.0; + projector_info.scale_factor = 0.9996; std::unique_ptr projector = autoware::geography_utils::get_lanelet2_projector(projector_info); diff --git a/map/autoware_map_projection_loader/README.md b/map/autoware_map_projection_loader/README.md index 1f9fe656c0..3d2f33ca41 100644 --- a/map/autoware_map_projection_loader/README.md +++ b/map/autoware_map_projection_loader/README.md @@ -32,6 +32,8 @@ There are three types of transformations from latitude and longitude to XYZ coor projector_type: Local ``` +Note that even if you input scale_factor, it will be overwritten to 1.0. + #### Limitation The functionality that requires latitude and longitude will become unavailable. @@ -52,6 +54,8 @@ vertical_datum: WGS84 mgrs_grid: 54SUE ``` +Note that even if you input scale_factor, it will be overwritten to 0.9996. + #### 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. @@ -67,9 +71,10 @@ vertical_datum: WGS84 map_origin: latitude: 35.6762 # [deg] longitude: 139.6503 # [deg] - altitude: 0.0 # [m] ``` +Note that even if you input scale_factor, it will be overwritten to 0.9996. + ### Using LocalCartesian If you want to use local cartesian WGS84, please specify the map origin as well. @@ -83,12 +88,14 @@ vertical_datum: WGS84 map_origin: latitude: 35.6762 # [deg] longitude: 139.6503 # [deg] - altitude: 0.0 # [m] ``` +Note that even if you input scale_factor, it will be overwritten to 1.0. + ### Using TransverseMercator If you want to use Transverse Mercator projection, please specify the map origin as well. +And specify the scale_factor of the map. If you didn't specify the scale_factor, it will be set 0.9996 as default value. ```yaml # map_projector_info.yaml @@ -97,7 +104,7 @@ vertical_datum: WGS84 map_origin: latitude: 35.6762 # [deg] longitude: 139.6503 # [deg] - altitude: 0.0 # [m] +scale_factor: 0.9996 ``` ## Published Topics 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 c75cb4818a..fa538a368d 100644 --- a/map/autoware_map_projection_loader/src/map_projection_loader.cpp +++ b/map/autoware_map_projection_loader/src/map_projection_loader.cpp @@ -44,7 +44,7 @@ autoware_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & 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(); + msg.map_origin.altitude = 0.0; } else if (msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::LOCAL) { ; // do nothing @@ -64,6 +64,30 @@ autoware_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & "LocalCartesianUTM, " "TransverseMercator, and local"); } + + // set scale factor + static constexpr float scale_factor_for_utm = 0.9996; + static constexpr float scale_factor_for_local = 1.0; + if (msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::TRANSVERSE_MERCATOR) { + if (data["scale_factor"]) { + msg.scale_factor = data["scale_factor"].as(); + } else { + msg.scale_factor = scale_factor_for_utm; + } + } else if ( + msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::MGRS || + msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM) { + msg.scale_factor = scale_factor_for_utm; + } else if ( + msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::LOCAL || + msg.projector_type == autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN) { + msg.scale_factor = scale_factor_for_local; + } + + if (msg.scale_factor <= 0.0) { + throw std::runtime_error( + "Invalid scale factor. The scale factor must be a value greater than 0."); + } return msg; } @@ -104,10 +128,8 @@ MapProjectionLoader::MapProjectionLoader(const rclcpp::NodeOptions & options) load_map_projector_info(yaml_filename, lanelet2_map_filename); // Publish the message - MapProjectorInfo map_projector_info_specs; publisher_ = this->create_publisher( - map_projector_info_specs.name, - autoware::component_interface_specs::get_qos(map_projector_info_specs)); + MapProjectorInfo::name, autoware::component_interface_specs::get_qos(MapProjectorInfo{})); publisher_->publish(msg); } } // namespace autoware::map_projection_loader