feat(geography_utils): add projection in geography_utils (autowarefou…

…ndation#4833) * feat(gnss_poser): Subscribe map_projector_info Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * update readme Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * small fix Signed-off-by: kminoda <[email protected]> * update commetn Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * add local cartesian Signed-off-by: kminoda <[email protected]> * update launch file Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * fix Signed-off-by: kminoda <[email protected]> * create new function for conversion of height Signed-off-by: kminoda <[email protected]> * minor update Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * update Signed-off-by: kminoda <[email protected]> * rename Signed-off-by: kminoda <[email protected]> * remove unnecessary include Signed-off-by: kminoda <[email protected]> * add projection in geography_utils Signed-off-by: kminoda <[email protected]> * update projection.cpp Signed-off-by: kminoda <[email protected]> * use projection in default_ad_api Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * Feat/gnss poser/subscribe map projector info (#24) * fix map origin Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * remove config file Signed-off-by: kminoda <[email protected]> * rfix readme Signed-off-by: kminoda <[email protected]> --------- Signed-off-by: kminoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * use projection.cpp in gnss_poser Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * remove unnecessary files Signed-off-by: kminoda <[email protected]> * remove unnecessary parts Signed-off-by: kminoda <[email protected]> * remove lanelet2 dependency from default_ad_api Signed-off-by: kminoda <[email protected]> * remove geography_utils.hpp Signed-off-by: kminoda <[email protected]> * add get_lanelet2_projector and use that in whole system Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * remove unnecessary parts Signed-off-by: kminoda <[email protected]> * revert using Point Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * remove unnecessary files Signed-off-by: kminoda <[email protected]> * update test Signed-off-by: kminoda <[email protected]> * update comment Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * style(pre-commit): autofix * use constant string instead Signed-off-by: kminoda <[email protected]> * use reference Signed-off-by: kminoda <[email protected]> * fix test name Signed-off-by: kminoda <[email protected]> * fix bug Signed-off-by: kminoda <[email protected]> --------- Signed-off-by: kminoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yamato Ando <[email protected]>
h-ohta · Sep 4, 2023 · d339c09 · d339c09
1 parent fa8da4c
commit d339c09
Show file tree

Hide file tree

Showing 15 changed files with 330 additions and 270 deletions.
diff --git a/common/geography_utils/CMakeLists.txt b/common/geography_utils/CMakeLists.txt
@@ -14,6 +14,7 @@ find_library(GeographicLib_LIBRARIES NAMES Geographic)
 
 ament_auto_add_library(geography_utils SHARED
   src/height.cpp
+  src/projection.cpp
   src/lanelet2_projector.cpp
 )
 

diff --git a/common/geography_utils/include/geography_utils/height.hpp b/common/geography_utils/include/geography_utils/height.hpp
@@ -15,10 +15,7 @@
 #ifndef GEOGRAPHY_UTILS__HEIGHT_HPP_
 #define GEOGRAPHY_UTILS__HEIGHT_HPP_
 
-#include <map>
-#include <stdexcept>
 #include <string>
-#include <utility>
 
 namespace geography_utils
 {

diff --git a/common/geography_utils/include/geography_utils/projection.hpp b/common/geography_utils/include/geography_utils/projection.hpp
@@ -0,0 +1,33 @@
+// 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 GEOGRAPHY_UTILS__PROJECTION_HPP_
+#define GEOGRAPHY_UTILS__PROJECTION_HPP_
+
+#include <geographic_msgs/msg/geo_point.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <tier4_map_msgs/msg/map_projector_info.hpp>
+
+namespace geography_utils
+{
+using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo;
+using GeoPoint = geographic_msgs::msg::GeoPoint;
+using LocalPoint = geometry_msgs::msg::Point;
+
+LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info);
+GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info);
+
+}  // namespace geography_utils
+
+#endif  // GEOGRAPHY_UTILS__PROJECTION_HPP_
diff --git a/common/geography_utils/package.xml b/common/geography_utils/package.xml
@@ -10,7 +10,9 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>geographic_msgs</depend>
   <depend>geographiclib</depend>
+  <depend>geometry_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>lanelet2_io</depend>
   <depend>tier4_map_msgs</depend>

diff --git a/common/geography_utils/src/projection.cpp b/common/geography_utils/src/projection.cpp
@@ -0,0 +1,95 @@
+// 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 <GeographicLib/Geoid.hpp>
+#include <geography_utils/lanelet2_projector.hpp>
+#include <geography_utils/projection.hpp>
+#include <lanelet2_extension/projection/mgrs_projector.hpp>
+
+namespace geography_utils
+{
+
+Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src)
+{
+  Eigen::Vector3d dst;
+  dst.x() = src.x;
+  dst.y() = src.y;
+  dst.z() = src.z;
+  return dst;
+}
+
+LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info)
+{
+  std::unique_ptr<lanelet::Projector> projector = get_lanelet2_projector(projector_info);
+  lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude};
+
+  lanelet::BasicPoint3d projected_local_point;
+  if (projector_info.projector_type == MapProjectorInfo::MGRS) {
+    const int mgrs_precision = 9;  // set precision as 100 micro meter
+    const auto mgrs_projector = dynamic_cast<lanelet::projection::MGRSProjector *>(projector.get());
+
+    // project x and y using projector
+    // note that the altitude is ignored in MGRS projection conventionally
+    projected_local_point = mgrs_projector->forward(position, mgrs_precision);
+  } else {
+    // project x and y using projector
+    // note that the original projector such as UTM projector does not compensate for the altitude
+    // offset
+    projected_local_point = projector->forward(position);
+
+    // correct z based on the map origin
+    // note that the converted altitude in local point is in the same vertical datum as the geo
+    // point
+    projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude;
+  }
+
+  LocalPoint local_point;
+  local_point.x = projected_local_point.x();
+  local_point.y = projected_local_point.y();
+  local_point.z = projected_local_point.z();
+
+  return local_point;
+}
+
+GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info)
+{
+  std::unique_ptr<lanelet::Projector> projector = get_lanelet2_projector(projector_info);
+
+  lanelet::GPSPoint projected_gps_point;
+  if (projector_info.projector_type == MapProjectorInfo::MGRS) {
+    const auto mgrs_projector = dynamic_cast<lanelet::projection::MGRSProjector *>(projector.get());
+    // project latitude and longitude using projector
+    // note that the z is ignored in MGRS projection conventionally
+    projected_gps_point =
+      mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid);
+  } else {
+    // project latitude and longitude using projector
+    // note that the original projector such as UTM projector does not compensate for the altitude
+    // offset
+    projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point));
+
+    // correct altitude based on the map origin
+    // note that the converted altitude in local point is in the same vertical datum as the geo
+    // point
+    projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude;
+  }
+
+  GeoPoint geo_point;
+  geo_point.latitude = projected_gps_point.lat;
+  geo_point.longitude = projected_gps_point.lon;
+  geo_point.altitude = projected_gps_point.ele;
+  return geo_point;
+}
+
+}  // namespace geography_utils
diff --git a/...ss_poser/include/gnss_poser/gnss_stat.hpp → ...raphy_utils/test/test_geography_utils.cpp b/...ss_poser/include/gnss_poser/gnss_stat.hpp → ...raphy_utils/test/test_geography_utils.cpp
@@ -1,4 +1,4 @@
-// Copyright 2020 Tier IV, Inc.
+// 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.
@@ -11,38 +11,16 @@
 // 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 GNSS_POSER__GNSS_STAT_HPP_
-#define GNSS_POSER__GNSS_STAT_HPP_
 
-#include <string>
+#include "geography_utils/height.hpp"
+#include "geography_utils/lanelet2_projector.hpp"
+#include "geography_utils/projection.hpp"
 
-namespace gnss_poser
-{
-struct GNSSStat
-{
-  GNSSStat()
-  : east_north_up(true),
-    zone(0),
-    mgrs_zone(""),
-    x(0),
-    y(0),
-    z(0),
-    latitude(0),
-    longitude(0),
-    altitude(0)
-  {
-  }
+#include <gtest/gtest.h>
 
-  bool east_north_up;
-  int zone;
-  std::string mgrs_zone;
-  double x;
-  double y;
-  double z;
-  double latitude;
-  double longitude;
-  double altitude;
-};
-}  // namespace gnss_poser
-
-#endif  // GNSS_POSER__GNSS_STAT_HPP_
+int main(int argc, char * argv[])
+{
+  testing::InitGoogleTest(&argc, argv);
+  bool result = RUN_ALL_TESTS();
+  return result;
+}
diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp
@@ -20,7 +20,7 @@
 #include <string>
 
 // Test case to verify if same source and target datums return original height
-TEST(Tier4GeographyUtils, SameSourceTargetDatum)
+TEST(GeographyUtils, SameSourceTargetDatum)
 {
   const double height = 10.0;
   const double latitude = 35.0;
@@ -34,7 +34,7 @@ TEST(Tier4GeographyUtils, SameSourceTargetDatum)
 }
 
 // Test case to verify valid source and target datums
-TEST(Tier4GeographyUtils, ValidSourceTargetDatum)
+TEST(GeographyUtils, ValidSourceTargetDatum)
 {
   // Calculated with
   // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html
@@ -50,7 +50,7 @@ TEST(Tier4GeographyUtils, ValidSourceTargetDatum)
 }
 
 // Test case to verify invalid source and target datums
-TEST(Tier4GeographyUtils, InvalidSourceTargetDatum)
+TEST(GeographyUtils, InvalidSourceTargetDatum)
 {
   const double height = 10.0;
   const double latitude = 35.0;
@@ -62,7 +62,7 @@ TEST(Tier4GeographyUtils, InvalidSourceTargetDatum)
 }
 
 // Test case to verify invalid source datums
-TEST(Tier4GeographyUtils, InvalidSourceDatum)
+TEST(GeographyUtils, InvalidSourceDatum)
 {
   const double height = 10.0;
   const double latitude = 35.0;
@@ -74,7 +74,7 @@ TEST(Tier4GeographyUtils, InvalidSourceDatum)
 }
 
 // Test case to verify invalid target datums
-TEST(Tier4GeographyUtils, InvalidTargetDatum)
+TEST(GeographyUtils, InvalidTargetDatum)
 {
   const double height = 10.0;
   const double latitude = 35.0;
@@ -84,9 +84,3 @@ TEST(Tier4GeographyUtils, InvalidTargetDatum)
     geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"),
     std::invalid_argument);
 }
-
-int main(int argc, char ** argv)
-{
-  ::testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}