refactor(map_loader): split to member functions (autowarefoundation#1941

) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
boyali · Oct 19, 2022 · da3b530 · da3b530
1 parent f27b651
commit da3b530
Show file tree

Hide file tree

Showing 2 changed files with 54 additions and 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
@@ -22,14 +22,22 @@
 #include <lanelet2_projection/UTM.h>
 
 #include <memory>
+#include <string>
+
+using autoware_auto_mapping_msgs::msg::HADMapBin;
 
 class Lanelet2MapLoaderNode : public rclcpp::Node
 {
 public:
   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
@@ -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>