diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp index f25a6771aa0cb..3e6aafd3a9954 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp +++ b/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 + #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 route_handler_ptr_{nullptr}; + std::unique_ptr map_projector_{nullptr}; int traj_start_index_{0}; int traj_end_index_{0}; diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index 363c88bebf0ea..eef133581622f 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -19,11 +19,13 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs behavior_path_planner_common + geography_utils geometry_msgs global_parameter_loader interpolation lanelet2_extension map_loader + map_projection_loader mission_planner motion_utils obstacle_avoidance_planner diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index 844352f577432..ea7e1c302fd68 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/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 #include #include #include @@ -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