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 72408f4af8185..82a90dd5c4ab4 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 @@ -31,13 +31,15 @@ class Lanelet2MapLoaderNode : public rclcpp::Node public: explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); + static lanelet::LaneletMapPtr load_map( + rclcpp::Node & node, const std::string & lanelet2_filename, + const std::string & lanelet2_map_projector_type); + static HADMapBin create_map_bin_msg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, + const rclcpp::Time & now); + private: rclcpp::Publisher::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 dc5d560b98e8e..da35f74eaa59c 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 @@ -54,7 +54,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options 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); + const auto map = load_map(*this, lanelet2_filename, lanelet2_map_projector_type); if (!map) { return; } @@ -63,7 +63,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); // create map bin msg - const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename); + const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename, now()); // create publisher and publish pub_map_bin_ = @@ -72,7 +72,8 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options } lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( - const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type) + rclcpp::Node & node, const std::string & lanelet2_filename, + const std::string & lanelet2_map_projector_type) { lanelet::ErrorMessages errors{}; if (lanelet2_map_projector_type == "MGRS") { @@ -82,8 +83,8 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return map; } } else if (lanelet2_map_projector_type == "UTM") { - const double map_origin_lat = declare_parameter("latitude", 0.0); - const double map_origin_lon = declare_parameter("longitude", 0.0); + const double map_origin_lat = node.declare_parameter("latitude", 0.0); + const double map_origin_lon = node.declare_parameter("longitude", 0.0); lanelet::GPSPoint position{map_origin_lat, map_origin_lon}; lanelet::Origin origin{position}; lanelet::projection::UtmProjector projector{origin}; @@ -93,25 +94,25 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return map; } } else { - RCLCPP_ERROR(get_logger(), "lanelet2_map_projector_type is not supported"); + RCLCPP_ERROR(rclcpp::get_logger("map_loader"), "lanelet2_map_projector_type is not supported"); return nullptr; } for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(get_logger(), error); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); } return nullptr; } HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg( - const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename) + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { std::string format_version{}, map_version{}; lanelet::io_handlers::AutowareOsmParser::parseVersions( lanelet2_filename, &format_version, &map_version); HADMapBin map_bin_msg; - map_bin_msg.header.stamp = now(); + 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;