Add autoware_map_msgs/LaneletMapBin

Signed-off-by: Barış Zeren <bzeren1819@gmail.com>

Author: StepTurtle

map/autoware_dynamic_lanelet_provider/include/autoware/dynamic_lanelet_provider/dynamic_lanelet_provider.hpp

@@ -42,6 +42,10 @@ class DynamicLaneletProviderNode : public rclcpp::Node
   void updateMap(const geometry_msgs::msg::Point & pose);
   bool should_update_map() const;
 
+  static void convertLaneletMapBinToHADMapBin(
+    const autoware_map_msgs::msg::LaneletMapBin & lanelet_map_bin,
+    autoware_auto_mapping_msgs::msg::HADMapBin & had_map_bin);
+
   rclcpp::Publisher<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr dynamic_map_pub_;
 
   rclcpp::Client<autoware_map_msgs::srv::GetDifferentialLanelet2Map>::SharedPtr map_loader_client_;

map/autoware_dynamic_lanelet_provider/src/dynamic_lanelet_provider.cpp

@@ -160,7 +160,9 @@ void DynamicLaneletProviderNode::updateMap(const geometry_msgs::msg::Point & pos
     status = result.wait_for(std::chrono::seconds(1));
   }
 
-  dynamic_map_pub_->publish(result.get()->differential_map);
+  autoware_auto_mapping_msgs::msg::HADMapBin had_map_bin;
+  convertLaneletMapBinToHADMapBin(result.get()->differential_map, had_map_bin);
+  dynamic_map_pub_->publish(had_map_bin);
 }
 
 void DynamicLaneletProviderNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr & msg)
@@ -178,6 +180,17 @@ bool DynamicLaneletProviderNode::should_update_map() const
   return distance > dynamic_map_loading_update_distance_;
 }
 
+void DynamicLaneletProviderNode::convertLaneletMapBinToHADMapBin(
+  const autoware_map_msgs::msg::LaneletMapBin & lanelet_map_bin,
+  autoware_auto_mapping_msgs::msg::HADMapBin & had_map_bin)
+{
+  had_map_bin.header = lanelet_map_bin.header;
+  had_map_bin.data = lanelet_map_bin.data;
+  had_map_bin.map_version = lanelet_map_bin.version_map;
+//  had_map_bin.map_format =
+//    static_cast<unsigned char>(std::stoi(lanelet_map_bin.version_map_format));
+}
+
 }  // namespace dynamic_lanelet_provider
 }  // namespace autoware
 

map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp

@@ -23,6 +23,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
 #include <tier4_map_msgs/msg/map_projector_info.hpp>
 
 #include <lanelet2_projection/UTM.h>
@@ -31,6 +32,7 @@
 #include <string>
 
 using autoware_auto_mapping_msgs::msg::HADMapBin;
+using autoware_map_msgs::msg::LaneletMapBin;
 using tier4_map_msgs::msg::MapProjectorInfo;
 
 class Lanelet2MapLoaderNode : public rclcpp::Node
@@ -44,6 +46,9 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
   static HADMapBin create_map_bin_msg(
     const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename,
     const rclcpp::Time & now);
+  static LaneletMapBin create_lanelet_map_bin_msg(
+    const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename,
+    const rclcpp::Time & now);
 
 private:
   using MapProjectorInfo = map_interface::MapProjectorInfo;

map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp

@@ -160,7 +160,7 @@ bool Lanelet2DifferentialLoaderModule::differentialLanelet2Load(
     lanelet::utils::overwriteLaneletsCenterline(map, 5.0, false);
 
     const auto map_bin_msg =
-      Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
+      Lanelet2MapLoaderNode::create_lanelet_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
 
     response->differential_map = map_bin_msg;
   } else {
@@ -178,7 +178,7 @@ bool Lanelet2DifferentialLoaderModule::differentialLanelet2Load(
     lanelet::utils::overwriteLaneletsCenterline(map, 5.0, false);
 
     const auto map_bin_msg =
-      Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
+      Lanelet2MapLoaderNode::create_lanelet_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
 
     response->differential_map = map_bin_msg;
   }

map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

@@ -198,6 +198,23 @@ HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg(
   return map_bin_msg;
 }
 
+LaneletMapBin Lanelet2MapLoaderNode::create_lanelet_map_bin_msg(
+  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);
+
+  LaneletMapBin map_bin_msg;
+  map_bin_msg.header.stamp = now;
+  map_bin_msg.header.frame_id = "map";
+  map_bin_msg.version_map_format = format_version;
+  map_bin_msg.version_map = map_version;
+  lanelet::utils::conversion::toBinMsg(map, &map_bin_msg);
+
+  return map_bin_msg;
+}
+
 std::vector<std::string> Lanelet2MapLoaderNode::getLanelet2Paths(
   const std::vector<std::string> & lanelet2_paths_or_directory) const
 {