Skip to content

Commit 08c74b8

Browse files
sebekxSebastian Zęderowskipre-commit-ci[bot]xmfcxyoutalk
authored
feat(autoware_geography_utils): add support for local cartesian projection (#181)
* feat(autoware_geography_utils): add support for local cartesian projection Signed-off-by: Sebastian Zęderowski <szederowski@autonomous-systems.pl> * feat(autoware_geography_utils): add support for local cartesian projection Signed-off-by: Sebastian Zęderowski <szederowski@autonomous-systems.pl> * style(pre-commit): autofix * feat(autoware_geography_utils): add support for local cartesian projection Signed-off-by: Sebastian Zęderowski <szederowski@autonomous-systems.pl> * add tests Signed-off-by: Mete Fatih Cırıt <mfc@autoware.org> * fix the test Signed-off-by: Mete Fatih Cırıt <mfc@autoware.org> * Update common/autoware_geography_utils/test/test_lanelet2_projector.cpp --------- Signed-off-by: Sebastian Zęderowski <szederowski@autonomous-systems.pl> Signed-off-by: Mete Fatih Cırıt <mfc@autoware.org> Co-authored-by: Sebastian Zęderowski <szederowski@autonomous-systems.pl> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Mete Fatih Cırıt <mfc@autoware.org> Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
1 parent 5ca07fa commit 08c74b8

File tree

3 files changed

+86
-1
lines changed

3 files changed

+86
-1
lines changed

‎common/autoware_geography_utils/src/lanelet2_projector.cpp

+11-1
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
#include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
1818
#include <autoware_lanelet2_extension/projection/transverse_mercator_projector.hpp>
1919

20+
#include <lanelet2_projection/LocalCartesian.h>
2021
#include <lanelet2_projection/UTM.h>
2122

2223
#include <memory>
@@ -51,9 +52,18 @@ std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInf
5152
return std::make_unique<lanelet::projection::TransverseMercatorProjector>(projector);
5253
}
5354

55+
if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN) {
56+
const lanelet::GPSPoint position{
57+
projector_info.map_origin.latitude, projector_info.map_origin.longitude,
58+
projector_info.map_origin.altitude};
59+
const lanelet::Origin origin{position};
60+
const lanelet::projection::LocalCartesianProjector projector{origin};
61+
return std::make_unique<lanelet::projection::LocalCartesianProjector>(projector);
62+
}
63+
5464
throw std::invalid_argument(
5565
"Invalid map projector type: " + projector_info.projector_type +
56-
". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator");
66+
". Currently supported types: MGRS, LocalCartesianUTM, LocalCartesian and TransverseMercator");
5767
}
5868

5969
} // namespace autoware::geography_utils

‎common/autoware_geography_utils/test/test_lanelet2_projector.cpp

+17
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
#include <autoware_lanelet2_extension/projection/transverse_mercator_projector.hpp>
1818

1919
#include <gtest/gtest.h>
20+
#include <lanelet2_projection/LocalCartesian.h>
2021
#include <lanelet2_projection/UTM.h>
2122

2223
#include <memory>
@@ -52,6 +53,22 @@ TEST(GeographyUtilsLanelet2Projector, GetLocalCartesianUTMProjector)
5253
EXPECT_NE(dynamic_cast<lanelet::projection::UtmProjector *>(projector.get()), nullptr);
5354
}
5455

56+
TEST(GeographyUtilsLanelet2Projector, GetLocalCartesianProjector)
57+
{
58+
autoware_map_msgs::msg::MapProjectorInfo projector_info;
59+
projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN;
60+
projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
61+
projector_info.map_origin.latitude = 35.62426;
62+
projector_info.map_origin.longitude = 139.74252;
63+
projector_info.map_origin.altitude = 0.0;
64+
65+
std::unique_ptr<lanelet::Projector> projector =
66+
autoware::geography_utils::get_lanelet2_projector(projector_info);
67+
68+
// Check if the returned projector is of type LocalCartesianProjector
69+
EXPECT_NE(dynamic_cast<lanelet::projection::LocalCartesianProjector *>(projector.get()), nullptr);
70+
}
71+
5572
TEST(GeographyUtilsLanelet2Projector, GetTransverseMercatorProjector)
5673
{
5774
autoware_map_msgs::msg::MapProjectorInfo projector_info;

‎common/autoware_geography_utils/test/test_projection.cpp

+58
Original file line numberDiff line numberDiff line change
@@ -159,3 +159,61 @@ TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin)
159159
EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001);
160160
EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001);
161161
}
162+
163+
TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianOrigin)
164+
{
165+
// source point
166+
geographic_msgs::msg::GeoPoint geo_point;
167+
geo_point.latitude = 35.62406;
168+
geo_point.longitude = 139.74252;
169+
geo_point.altitude = 10.0;
170+
171+
// target point
172+
geometry_msgs::msg::Point local_point;
173+
local_point.x = 0.0;
174+
local_point.y = -22.18;
175+
local_point.z = 20.0;
176+
177+
// projector info
178+
autoware_map_msgs::msg::MapProjectorInfo projector_info;
179+
projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN;
180+
projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
181+
projector_info.map_origin.latitude = 35.62426;
182+
projector_info.map_origin.longitude = 139.74252;
183+
projector_info.map_origin.altitude = -10.0;
184+
185+
// conversion
186+
const geometry_msgs::msg::Point converted_point =
187+
autoware::geography_utils::project_forward(geo_point, projector_info);
188+
189+
EXPECT_NEAR(converted_point.x, local_point.x, 1.0);
190+
EXPECT_NEAR(converted_point.y, local_point.y, 1.0);
191+
EXPECT_NEAR(converted_point.z, local_point.z, 1.0);
192+
}
193+
194+
TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianOrigin)
195+
{
196+
// source point
197+
geographic_msgs::msg::GeoPoint geo_point;
198+
geo_point.latitude = 35.62426;
199+
geo_point.longitude = 139.74252;
200+
geo_point.altitude = 10.0;
201+
202+
// projector info
203+
autoware_map_msgs::msg::MapProjectorInfo projector_info;
204+
projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN;
205+
projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
206+
projector_info.map_origin.latitude = 35.0;
207+
projector_info.map_origin.longitude = 139.0;
208+
projector_info.map_origin.altitude = 0.0;
209+
210+
// conversion
211+
const geometry_msgs::msg::Point converted_local_point =
212+
autoware::geography_utils::project_forward(geo_point, projector_info);
213+
const geographic_msgs::msg::GeoPoint converted_geo_point =
214+
autoware::geography_utils::project_reverse(converted_local_point, projector_info);
215+
216+
EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001);
217+
EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001);
218+
EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001);
219+
}

0 commit comments

Comments
 (0)