fix(static_centerline_optimizer): fix latlon values of generated LL2 map (autowarefoundation#6727)

* fix(static_centerline_optimizer): fix lat/lon value of generated LL2 map

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp

@@ -22,6 +22,8 @@
 #include "static_centerline_optimizer/type_alias.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
 
+#include <geography_utils/lanelet2_projector.hpp>
+
 #include "std_msgs/msg/bool.hpp"
 #include "std_msgs/msg/int32.hpp"
 
@@ -69,6 +71,7 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
   lanelet::LaneletMapPtr original_map_ptr_{nullptr};
   HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr};
   std::shared_ptr<RouteHandler> route_handler_ptr_{nullptr};
+  std::unique_ptr<lanelet::Projector> map_projector_{nullptr};
 
   int traj_start_index_{0};
   int traj_end_index_{0};

planning/static_centerline_optimizer/package.xml

@@ -19,11 +19,13 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>behavior_path_planner_common</depend>
+  <depend>geography_utils</depend>
   <depend>geometry_msgs</depend>
   <depend>global_parameter_loader</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
   <depend>map_loader</depend>
+  <depend>map_projection_loader</depend>
   <depend>mission_planner</depend>
   <depend>motion_utils</depend>
   <depend>obstacle_avoidance_planner</depend>

planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp

@@ -18,6 +18,7 @@
 #include "lanelet2_extension/utility/query.hpp"
 #include "lanelet2_extension/utility/utilities.hpp"
 #include "map_loader/lanelet2_map_loader_node.hpp"
+#include "map_projection_loader/load_info_from_lanelet2_map.hpp"
 #include "motion_utils/resample/resample.hpp"
 #include "motion_utils/trajectory/conversion.hpp"
 #include "obstacle_avoidance_planner/node.hpp"
@@ -28,6 +29,7 @@
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 #include "tier4_autoware_utils/ros/parameter.hpp"
 
+#include <geography_utils/lanelet2_projector.hpp>
 #include <mission_planner/mission_planner_plugin.hpp>
 #include <pluginlib/class_loader.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
@@ -331,14 +333,17 @@ void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_
   // load map by the map_loader package
   map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr {
     // load map
-    tier4_map_msgs::msg::MapProjectorInfo map_projector_info;
-    map_projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS;
+    const auto map_projector_info = load_info_from_lanelet2_map(lanelet2_input_file_path);
     const auto map_ptr =
       Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info);
     if (!map_ptr) {
       return nullptr;
     }
 
+    // NOTE: generate map projector for lanelet::write().
+    //       Without this, lat/lon of the generated LL2 map will be wrong.
+    map_projector_ = geography_utils::get_lanelet2_projector(map_projector_info);
+
     // NOTE: The original map is stored here since the various ids in the lanelet map will change
     //       after lanelet::utils::overwriteLaneletCenterline, and saving map will fail.
     original_map_ptr_ =
@@ -750,7 +755,7 @@ void StaticCenterlineOptimizerNode::save_map(
   RCLCPP_INFO(get_logger(), "Updated centerline in map.");
 
   // save map with modified center line
-  lanelet::write(lanelet2_output_file_path, *original_map_ptr_);
+  lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector_);
   RCLCPP_INFO(get_logger(), "Saved map.");
 }
 }  // namespace static_centerline_optimizer