From da3b53027ba356e5c206101f997c38f4ab05b769 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Fri, 23 Sep 2022 19:18:34 +0900
Subject: [PATCH] refactor(map_loader): split to member functions (#1941)

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

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../map_loader/lanelet2_map_loader_node.hpp   | 10 ++-
 .../lanelet2_map_loader_node.cpp              | 64 +++++++++++++------
 2 files changed, 54 insertions(+), 20 deletions(-)

diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp
index 33f33a05e339..72408f4af818 100644
--- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp
+++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp
@@ -22,6 +22,9 @@
 #include <lanelet2_projection/UTM.h>
 
 #include <memory>
+#include <string>
+
+using autoware_auto_mapping_msgs::msg::HADMapBin;
 
 class Lanelet2MapLoaderNode : public rclcpp::Node
 {
@@ -29,7 +32,12 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
   explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options);
 
 private:
-  rclcpp::Publisher<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr pub_map_bin_;
+  rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_;
+
+  lanelet::LaneletMapPtr load_map(
+    const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type);
+  HADMapBin create_map_bin_msg(
+    const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename);
 };
 
 #endif  // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
index 58330e44abf5..dc5d560b98e8 100644
--- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
+++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
@@ -51,47 +51,73 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
 {
   const auto lanelet2_filename = declare_parameter("lanelet2_map_path", "");
   const auto lanelet2_map_projector_type = declare_parameter("lanelet2_map_projector_type", "MGRS");
+  const auto center_line_resolution = declare_parameter("center_line_resolution", 5.0);
+
+  // load map from file
+  const auto map = load_map(lanelet2_filename, lanelet2_map_projector_type);
+  if (!map) {
+    return;
+  }
+
+  // overwrite centerline
+  lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false);
+
+  // create map bin msg
+  const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename);
+
+  // create publisher and publish
+  pub_map_bin_ =
+    create_publisher<HADMapBin>("output/lanelet2_map", rclcpp::QoS{1}.transient_local());
+  pub_map_bin_->publish(map_bin_msg);
+}
+
+lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
+  const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type)
+{
   lanelet::ErrorMessages errors{};
-  lanelet::LaneletMapPtr map;
   if (lanelet2_map_projector_type == "MGRS") {
     lanelet::projection::MGRSProjector projector{};
-    map = lanelet::load(lanelet2_filename, projector, &errors);
+    const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
+    if (errors.empty()) {
+      return map;
+    }
   } else if (lanelet2_map_projector_type == "UTM") {
-    double map_origin_lat = this->declare_parameter("latitude", 0.0);
-    double map_origin_lon = this->declare_parameter("longitude", 0.0);
+    const double map_origin_lat = declare_parameter("latitude", 0.0);
+    const double map_origin_lon = declare_parameter("longitude", 0.0);
     lanelet::GPSPoint position{map_origin_lat, map_origin_lon};
     lanelet::Origin origin{position};
     lanelet::projection::UtmProjector projector{origin};
-    map = lanelet::load(lanelet2_filename, projector, &errors);
+
+    const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
+    if (errors.empty()) {
+      return map;
+    }
   } else {
-    RCLCPP_ERROR(this->get_logger(), "lanelet2_map_projector_type is not supported");
+    RCLCPP_ERROR(get_logger(), "lanelet2_map_projector_type is not supported");
+    return nullptr;
   }
 
   for (const auto & error : errors) {
-    RCLCPP_ERROR_STREAM(this->get_logger(), error);
+    RCLCPP_ERROR_STREAM(get_logger(), error);
   }
-  if (!errors.empty()) {
-    return;
-  }
-
-  const auto center_line_resolution = this->declare_parameter("center_line_resolution", 5.0);
-  lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false);
+  return nullptr;
+}
 
+HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg(
+  const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename)
+{
   std::string format_version{}, map_version{};
   lanelet::io_handlers::AutowareOsmParser::parseVersions(
     lanelet2_filename, &format_version, &map_version);
 
-  pub_map_bin_ = this->create_publisher<autoware_auto_mapping_msgs::msg::HADMapBin>(
-    "output/lanelet2_map", rclcpp::QoS{1}.transient_local());
-
-  autoware_auto_mapping_msgs::msg::HADMapBin map_bin_msg;
-  map_bin_msg.header.stamp = this->now();
+  HADMapBin map_bin_msg;
+  map_bin_msg.header.stamp = now();
   map_bin_msg.header.frame_id = "map";
   map_bin_msg.format_version = format_version;
   map_bin_msg.map_version = map_version;
   lanelet::utils::conversion::toBinMsg(map, &map_bin_msg);
 
-  pub_map_bin_->publish(map_bin_msg);
+  return map_bin_msg;
 }
 
 #include <rclcpp_components/register_node_macro.hpp>