Skip to content
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.

Commit 54ea3cc

Browse files
mitsudome-rmitsudome-r
committedDec 3, 2024·
feat: copy autoware_geography_utils from autoware.universe
Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>
1 parent df9cd36 commit 54ea3cc

13 files changed

+673
-0
lines changed
 

‎common/autoware_geography_utils/CHANGELOG.rst

+15
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2+
Changelog for package autoware_geography_utils
3+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4+
5+
0.38.0 (2024-11-08)
6+
-------------------
7+
* unify package.xml version to 0.37.0
8+
* refactor(geography_utils): prefix package and namespace with autoware (`#7790 <https://github.com/autowarefoundation/autoware.universe/issues/7790>`_)
9+
* refactor(geography_utils): prefix package and namespace with autoware
10+
* move headers to include/autoware/
11+
---------
12+
* Contributors: Esteve Fernandez, Yutaka Kondo
13+
14+
0.26.0 (2024-04-03)
15+
-------------------

‎common/autoware_geography_utils/CMakeLists.txt

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(autoware_geography_utils)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
# GeographicLib
8+
find_package(PkgConfig)
9+
find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h
10+
PATH_SUFFIXES GeographicLib
11+
)
12+
set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR})
13+
find_library(GeographicLib_LIBRARIES NAMES Geographic)
14+
15+
ament_auto_add_library(${PROJECT_NAME} SHARED
16+
src/height.cpp
17+
src/projection.cpp
18+
src/lanelet2_projector.cpp
19+
)
20+
21+
target_link_libraries(${PROJECT_NAME}
22+
${GeographicLib_LIBRARIES}
23+
)
24+
25+
if(BUILD_TESTING)
26+
find_package(ament_cmake_ros REQUIRED)
27+
28+
file(GLOB_RECURSE test_files test/*.cpp)
29+
30+
ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files})
31+
32+
target_link_libraries(test_${PROJECT_NAME}
33+
${PROJECT_NAME}
34+
)
35+
endif()
36+
37+
ament_auto_package()

‎common/autoware_geography_utils/README.md

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
# geography_utils
2+
3+
## Purpose
4+
5+
This package contains geography-related functions used by other packages, so please refer to them as needed.

‎common/autoware_geography_utils/include/autoware/geography_utils/height.hpp

+33
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_
16+
#define AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_
17+
18+
#include <string>
19+
20+
namespace autoware::geography_utils
21+
{
22+
23+
typedef double (*HeightConversionFunction)(
24+
const double height, const double latitude, const double longitude);
25+
double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude);
26+
double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude);
27+
double convert_height(
28+
const double height, const double latitude, const double longitude,
29+
const std::string & source_vertical_datum, const std::string & target_vertical_datum);
30+
31+
} // namespace autoware::geography_utils
32+
33+
#endif // AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_

‎common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp

+32
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_
16+
#define AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_
17+
18+
#include <autoware_map_msgs/msg/map_projector_info.hpp>
19+
20+
#include <lanelet2_io/Projection.h>
21+
22+
#include <memory>
23+
24+
namespace autoware::geography_utils
25+
{
26+
using MapProjectorInfo = autoware_map_msgs::msg::MapProjectorInfo;
27+
28+
std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInfo & projector_info);
29+
30+
} // namespace autoware::geography_utils
31+
32+
#endif // AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_

‎common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp

+33
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_
16+
#define AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_
17+
18+
#include <autoware_map_msgs/msg/map_projector_info.hpp>
19+
#include <geographic_msgs/msg/geo_point.hpp>
20+
#include <geometry_msgs/msg/point.hpp>
21+
22+
namespace autoware::geography_utils
23+
{
24+
using MapProjectorInfo = autoware_map_msgs::msg::MapProjectorInfo;
25+
using GeoPoint = geographic_msgs::msg::GeoPoint;
26+
using LocalPoint = geometry_msgs::msg::Point;
27+
28+
LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info);
29+
GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info);
30+
31+
} // namespace autoware::geography_utils
32+
33+
#endif // AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_

‎common/autoware_geography_utils/package.xml

+33
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>autoware_geography_utils</name>
5+
<version>0.38.0</version>
6+
<description>The autoware_geography_utils package</description>
7+
<maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
8+
<maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
9+
<maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
10+
<maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
11+
<maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
12+
<maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
13+
<license>Apache License 2.0</license>
14+
<author email="koji.minoda@tier4.jp">Koji Minoda</author>
15+
16+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
17+
<buildtool_depend>autoware_cmake</buildtool_depend>
18+
19+
<depend>autoware_lanelet2_extension</depend>
20+
<depend>autoware_map_msgs</depend>
21+
<depend>geographic_msgs</depend>
22+
<depend>geographiclib</depend>
23+
<depend>geometry_msgs</depend>
24+
<depend>lanelet2_io</depend>
25+
26+
<test_depend>ament_cmake_ros</test_depend>
27+
<test_depend>ament_lint_auto</test_depend>
28+
<test_depend>autoware_lint_common</test_depend>
29+
30+
<export>
31+
<build_type>ament_cmake</build_type>
32+
</export>
33+
</package>

‎common/autoware_geography_utils/src/height.cpp

+63
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <GeographicLib/Geoid.hpp>
16+
#include <autoware/geography_utils/height.hpp>
17+
18+
#include <map>
19+
#include <stdexcept>
20+
#include <string>
21+
#include <utility>
22+
23+
namespace autoware::geography_utils
24+
{
25+
26+
double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude)
27+
{
28+
GeographicLib::Geoid egm2008("egm2008-1");
29+
// cSpell: ignore ELLIPSOIDTOGEOID
30+
return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID);
31+
}
32+
33+
double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude)
34+
{
35+
GeographicLib::Geoid egm2008("egm2008-1");
36+
// cSpell: ignore GEOIDTOELLIPSOID
37+
return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID);
38+
}
39+
40+
double convert_height(
41+
const double height, const double latitude, const double longitude,
42+
const std::string & source_vertical_datum, const std::string & target_vertical_datum)
43+
{
44+
if (source_vertical_datum == target_vertical_datum) {
45+
return height;
46+
}
47+
std::map<std::pair<std::string, std::string>, HeightConversionFunction> conversion_map;
48+
conversion_map[{"WGS84", "EGM2008"}] = convert_wgs84_to_egm2008;
49+
conversion_map[{"EGM2008", "WGS84"}] = convert_egm2008_to_wgs84;
50+
51+
auto key = std::make_pair(source_vertical_datum, target_vertical_datum);
52+
if (conversion_map.find(key) != conversion_map.end()) {
53+
return conversion_map[key](height, latitude, longitude);
54+
} else {
55+
std::string error_message =
56+
"Invalid conversion types: " + std::string(source_vertical_datum.c_str()) + " to " +
57+
std::string(target_vertical_datum.c_str());
58+
59+
throw std::invalid_argument(error_message);
60+
}
61+
}
62+
63+
} // namespace autoware::geography_utils

‎common/autoware_geography_utils/src/lanelet2_projector.cpp

+54
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <GeographicLib/Geoid.hpp>
16+
#include <autoware/geography_utils/lanelet2_projector.hpp>
17+
#include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
18+
#include <autoware_lanelet2_extension/projection/transverse_mercator_projector.hpp>
19+
20+
#include <lanelet2_projection/UTM.h>
21+
22+
namespace autoware::geography_utils
23+
{
24+
25+
std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInfo & projector_info)
26+
{
27+
if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) {
28+
lanelet::GPSPoint position{
29+
projector_info.map_origin.latitude, projector_info.map_origin.longitude,
30+
projector_info.map_origin.altitude};
31+
lanelet::Origin origin{position};
32+
lanelet::projection::UtmProjector projector{origin};
33+
return std::make_unique<lanelet::projection::UtmProjector>(projector);
34+
35+
} else if (projector_info.projector_type == MapProjectorInfo::MGRS) {
36+
lanelet::projection::MGRSProjector projector{};
37+
projector.setMGRSCode(projector_info.mgrs_grid);
38+
return std::make_unique<lanelet::projection::MGRSProjector>(projector);
39+
40+
} else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) {
41+
lanelet::GPSPoint position{
42+
projector_info.map_origin.latitude, projector_info.map_origin.longitude,
43+
projector_info.map_origin.altitude};
44+
lanelet::Origin origin{position};
45+
lanelet::projection::TransverseMercatorProjector projector{origin};
46+
return std::make_unique<lanelet::projection::TransverseMercatorProjector>(projector);
47+
}
48+
const std::string error_msg =
49+
"Invalid map projector type: " + projector_info.projector_type +
50+
". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator";
51+
throw std::invalid_argument(error_msg);
52+
}
53+
54+
} // namespace autoware::geography_utils

‎common/autoware_geography_utils/src/projection.cpp

+95
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <GeographicLib/Geoid.hpp>
16+
#include <autoware/geography_utils/lanelet2_projector.hpp>
17+
#include <autoware/geography_utils/projection.hpp>
18+
#include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
19+
20+
namespace autoware::geography_utils
21+
{
22+
23+
Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src)
24+
{
25+
Eigen::Vector3d dst;
26+
dst.x() = src.x;
27+
dst.y() = src.y;
28+
dst.z() = src.z;
29+
return dst;
30+
}
31+
32+
LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info)
33+
{
34+
std::unique_ptr<lanelet::Projector> projector = get_lanelet2_projector(projector_info);
35+
lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude};
36+
37+
lanelet::BasicPoint3d projected_local_point;
38+
if (projector_info.projector_type == MapProjectorInfo::MGRS) {
39+
const int mgrs_precision = 9; // set precision as 100 micro meter
40+
const auto mgrs_projector = dynamic_cast<lanelet::projection::MGRSProjector *>(projector.get());
41+
42+
// project x and y using projector
43+
// note that the altitude is ignored in MGRS projection conventionally
44+
projected_local_point = mgrs_projector->forward(position, mgrs_precision);
45+
} else {
46+
// project x and y using projector
47+
// note that the original projector such as UTM projector does not compensate for the altitude
48+
// offset
49+
projected_local_point = projector->forward(position);
50+
51+
// correct z based on the map origin
52+
// note that the converted altitude in local point is in the same vertical datum as the geo
53+
// point
54+
projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude;
55+
}
56+
57+
LocalPoint local_point;
58+
local_point.x = projected_local_point.x();
59+
local_point.y = projected_local_point.y();
60+
local_point.z = projected_local_point.z();
61+
62+
return local_point;
63+
}
64+
65+
GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info)
66+
{
67+
std::unique_ptr<lanelet::Projector> projector = get_lanelet2_projector(projector_info);
68+
69+
lanelet::GPSPoint projected_gps_point;
70+
if (projector_info.projector_type == MapProjectorInfo::MGRS) {
71+
const auto mgrs_projector = dynamic_cast<lanelet::projection::MGRSProjector *>(projector.get());
72+
// project latitude and longitude using projector
73+
// note that the z is ignored in MGRS projection conventionally
74+
projected_gps_point =
75+
mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid);
76+
} else {
77+
// project latitude and longitude using projector
78+
// note that the original projector such as UTM projector does not compensate for the altitude
79+
// offset
80+
projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point));
81+
82+
// correct altitude based on the map origin
83+
// note that the converted altitude in local point is in the same vertical datum as the geo
84+
// point
85+
projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude;
86+
}
87+
88+
GeoPoint geo_point;
89+
geo_point.latitude = projected_gps_point.lat;
90+
geo_point.longitude = projected_gps_point.lon;
91+
geo_point.altitude = projected_gps_point.ele;
92+
return geo_point;
93+
}
94+
95+
} // namespace autoware::geography_utils

‎common/autoware_geography_utils/test/test_geography_utils.cpp

+26
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "autoware/geography_utils/height.hpp"
16+
#include "autoware/geography_utils/lanelet2_projector.hpp"
17+
#include "autoware/geography_utils/projection.hpp"
18+
19+
#include <gtest/gtest.h>
20+
21+
int main(int argc, char * argv[])
22+
{
23+
testing::InitGoogleTest(&argc, argv);
24+
bool result = RUN_ALL_TESTS();
25+
return result;
26+
}

‎common/autoware_geography_utils/test/test_height.cpp

+86
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <autoware/geography_utils/height.hpp>
16+
17+
#include <gtest/gtest.h>
18+
19+
#include <stdexcept>
20+
#include <string>
21+
22+
// Test case to verify if same source and target datums return original height
23+
TEST(GeographyUtils, SameSourceTargetDatum)
24+
{
25+
const double height = 10.0;
26+
const double latitude = 35.0;
27+
const double longitude = 139.0;
28+
const std::string datum = "WGS84";
29+
30+
double converted_height =
31+
autoware::geography_utils::convert_height(height, latitude, longitude, datum, datum);
32+
33+
EXPECT_DOUBLE_EQ(height, converted_height);
34+
}
35+
36+
// Test case to verify valid source and target datums
37+
TEST(GeographyUtils, ValidSourceTargetDatum)
38+
{
39+
// Calculated with
40+
// https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html
41+
const double height = 10.0;
42+
const double latitude = 35.0;
43+
const double longitude = 139.0;
44+
const double target_height = -30.18;
45+
46+
double converted_height =
47+
autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008");
48+
49+
EXPECT_NEAR(target_height, converted_height, 0.1);
50+
}
51+
52+
// Test case to verify invalid source and target datums
53+
TEST(GeographyUtils, InvalidSourceTargetDatum)
54+
{
55+
const double height = 10.0;
56+
const double latitude = 35.0;
57+
const double longitude = 139.0;
58+
59+
EXPECT_THROW(
60+
autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"),
61+
std::invalid_argument);
62+
}
63+
64+
// Test case to verify invalid source datums
65+
TEST(GeographyUtils, InvalidSourceDatum)
66+
{
67+
const double height = 10.0;
68+
const double latitude = 35.0;
69+
const double longitude = 139.0;
70+
71+
EXPECT_THROW(
72+
autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"),
73+
std::invalid_argument);
74+
}
75+
76+
// Test case to verify invalid target datums
77+
TEST(GeographyUtils, InvalidTargetDatum)
78+
{
79+
const double height = 10.0;
80+
const double latitude = 35.0;
81+
const double longitude = 139.0;
82+
83+
EXPECT_THROW(
84+
autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"),
85+
std::invalid_argument);
86+
}

‎common/autoware_geography_utils/test/test_projection.cpp

+161
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,161 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <autoware/geography_utils/projection.hpp>
16+
17+
#include <gtest/gtest.h>
18+
19+
#include <stdexcept>
20+
#include <string>
21+
22+
TEST(GeographyUtilsProjection, ProjectForwardToMGRS)
23+
{
24+
// source point
25+
geographic_msgs::msg::GeoPoint geo_point;
26+
geo_point.latitude = 35.62426;
27+
geo_point.longitude = 139.74252;
28+
geo_point.altitude = 10.0;
29+
30+
// target point
31+
geometry_msgs::msg::Point local_point;
32+
local_point.x = 86128.0;
33+
local_point.y = 43002.0;
34+
local_point.z = 10.0;
35+
36+
// projector info
37+
autoware_map_msgs::msg::MapProjectorInfo projector_info;
38+
projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::MGRS;
39+
projector_info.mgrs_grid = "54SUE";
40+
projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
41+
42+
// conversion
43+
const geometry_msgs::msg::Point converted_point =
44+
autoware::geography_utils::project_forward(geo_point, projector_info);
45+
46+
EXPECT_NEAR(converted_point.x, local_point.x, 1.0);
47+
EXPECT_NEAR(converted_point.y, local_point.y, 1.0);
48+
EXPECT_NEAR(converted_point.z, local_point.z, 1.0);
49+
}
50+
51+
TEST(GeographyUtilsProjection, ProjectReverseFromMGRS)
52+
{
53+
// source point
54+
geometry_msgs::msg::Point local_point;
55+
local_point.x = 86128.0;
56+
local_point.y = 43002.0;
57+
local_point.z = 10.0;
58+
59+
// target point
60+
geographic_msgs::msg::GeoPoint geo_point;
61+
geo_point.latitude = 35.62426;
62+
geo_point.longitude = 139.74252;
63+
geo_point.altitude = 10.0;
64+
65+
// projector info
66+
autoware_map_msgs::msg::MapProjectorInfo projector_info;
67+
projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::MGRS;
68+
projector_info.mgrs_grid = "54SUE";
69+
projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
70+
71+
// conversion
72+
const geographic_msgs::msg::GeoPoint converted_point =
73+
autoware::geography_utils::project_reverse(local_point, projector_info);
74+
75+
EXPECT_NEAR(converted_point.latitude, geo_point.latitude, 0.0001);
76+
EXPECT_NEAR(converted_point.longitude, geo_point.longitude, 0.0001);
77+
EXPECT_NEAR(converted_point.altitude, geo_point.altitude, 0.0001);
78+
}
79+
80+
TEST(GeographyUtilsProjection, ProjectForwardAndReverseMGRS)
81+
{
82+
// source point
83+
geographic_msgs::msg::GeoPoint geo_point;
84+
geo_point.latitude = 35.62426;
85+
geo_point.longitude = 139.74252;
86+
geo_point.altitude = 10.0;
87+
88+
// projector info
89+
autoware_map_msgs::msg::MapProjectorInfo projector_info;
90+
projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::MGRS;
91+
projector_info.mgrs_grid = "54SUE";
92+
projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
93+
94+
// conversion
95+
const geometry_msgs::msg::Point converted_local_point =
96+
autoware::geography_utils::project_forward(geo_point, projector_info);
97+
const geographic_msgs::msg::GeoPoint converted_geo_point =
98+
autoware::geography_utils::project_reverse(converted_local_point, projector_info);
99+
100+
EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001);
101+
EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001);
102+
EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001);
103+
}
104+
105+
TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin)
106+
{
107+
// source point
108+
geographic_msgs::msg::GeoPoint geo_point;
109+
geo_point.latitude = 35.62406;
110+
geo_point.longitude = 139.74252;
111+
geo_point.altitude = 10.0;
112+
113+
// target point
114+
geometry_msgs::msg::Point local_point;
115+
local_point.x = 0.0;
116+
local_point.y = -22.18;
117+
local_point.z = 20.0;
118+
119+
// projector info
120+
autoware_map_msgs::msg::MapProjectorInfo projector_info;
121+
projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM;
122+
projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
123+
projector_info.map_origin.latitude = 35.62426;
124+
projector_info.map_origin.longitude = 139.74252;
125+
projector_info.map_origin.altitude = -10.0;
126+
127+
// conversion
128+
const geometry_msgs::msg::Point converted_point =
129+
autoware::geography_utils::project_forward(geo_point, projector_info);
130+
131+
EXPECT_NEAR(converted_point.x, local_point.x, 1.0);
132+
EXPECT_NEAR(converted_point.y, local_point.y, 1.0);
133+
EXPECT_NEAR(converted_point.z, local_point.z, 1.0);
134+
}
135+
136+
TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin)
137+
{
138+
// source point
139+
geographic_msgs::msg::GeoPoint geo_point;
140+
geo_point.latitude = 35.62426;
141+
geo_point.longitude = 139.74252;
142+
geo_point.altitude = 10.0;
143+
144+
// projector info
145+
autoware_map_msgs::msg::MapProjectorInfo projector_info;
146+
projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM;
147+
projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
148+
projector_info.map_origin.latitude = 35.0;
149+
projector_info.map_origin.longitude = 139.0;
150+
projector_info.map_origin.altitude = 0.0;
151+
152+
// conversion
153+
const geometry_msgs::msg::Point converted_local_point =
154+
autoware::geography_utils::project_forward(geo_point, projector_info);
155+
const geographic_msgs::msg::GeoPoint converted_geo_point =
156+
autoware::geography_utils::project_reverse(converted_local_point, projector_info);
157+
158+
EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001);
159+
EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001);
160+
EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001);
161+
}

0 commit comments

Comments
 (0)
Please sign in to comment.