add tests

Signed-off-by: Mete Fatih Cırıt <mfc@autoware.org>

Author: xmfcx

common/autoware_geography_utils/test/test_lanelet2_projector.cpp

@@ -52,6 +52,22 @@ TEST(GeographyUtilsLanelet2Projector, GetLocalCartesianUTMProjector)
   EXPECT_NE(dynamic_cast<lanelet::projection::UtmProjector *>(projector.get()), nullptr);
 }
 
+TEST(GeographyUtilsLanelet2Projector, GetLocalCartesianProjector)
+{
+  autoware_map_msgs::msg::MapProjectorInfo projector_info;
+  projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN;
+  projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
+  projector_info.map_origin.latitude = 35.62426;
+  projector_info.map_origin.longitude = 139.74252;
+  projector_info.map_origin.altitude = 0.0;
+
+  std::unique_ptr<lanelet::Projector> projector =
+    autoware::geography_utils::get_lanelet2_projector(projector_info);
+
+  // Check if the returned projector is of type UtmProjector
+  EXPECT_NE(dynamic_cast<lanelet::projection::UtmProjector *>(projector.get()), nullptr);
+}
+
 TEST(GeographyUtilsLanelet2Projector, GetTransverseMercatorProjector)
 {
   autoware_map_msgs::msg::MapProjectorInfo projector_info;

common/autoware_geography_utils/test/test_projection.cpp

@@ -159,3 +159,61 @@ TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin)
   EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001);
   EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001);
 }
+
+TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianOrigin)
+{
+  // source point
+  geographic_msgs::msg::GeoPoint geo_point;
+  geo_point.latitude = 35.62406;
+  geo_point.longitude = 139.74252;
+  geo_point.altitude = 10.0;
+
+  // target point
+  geometry_msgs::msg::Point local_point;
+  local_point.x = 0.0;
+  local_point.y = -22.18;
+  local_point.z = 20.0;
+
+  // projector info
+  autoware_map_msgs::msg::MapProjectorInfo projector_info;
+  projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN;
+  projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
+  projector_info.map_origin.latitude = 35.62426;
+  projector_info.map_origin.longitude = 139.74252;
+  projector_info.map_origin.altitude = -10.0;
+
+  // conversion
+  const geometry_msgs::msg::Point converted_point =
+    autoware::geography_utils::project_forward(geo_point, projector_info);
+
+  EXPECT_NEAR(converted_point.x, local_point.x, 1.0);
+  EXPECT_NEAR(converted_point.y, local_point.y, 1.0);
+  EXPECT_NEAR(converted_point.z, local_point.z, 1.0);
+}
+
+TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianOrigin)
+{
+  // source point
+  geographic_msgs::msg::GeoPoint geo_point;
+  geo_point.latitude = 35.62426;
+  geo_point.longitude = 139.74252;
+  geo_point.altitude = 10.0;
+
+  // projector info
+  autoware_map_msgs::msg::MapProjectorInfo projector_info;
+  projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN;
+  projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84;
+  projector_info.map_origin.latitude = 35.0;
+  projector_info.map_origin.longitude = 139.0;
+  projector_info.map_origin.altitude = 0.0;
+
+  // conversion
+  const geometry_msgs::msg::Point converted_local_point =
+    autoware::geography_utils::project_forward(geo_point, projector_info);
+  const geographic_msgs::msg::GeoPoint converted_geo_point =
+    autoware::geography_utils::project_reverse(converted_local_point, projector_info);
+
+  EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001);
+  EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001);
+  EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001);
+}