refactor(autoware_map_msgs): modify pcd metadata msg (#7852)

Signed-off-by: Yamato Ando <yamato.ando@tier4.jp>

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -265,7 +265,7 @@ bool MapUpdateModule::update_ndt(
   }
   diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true);
 
-  auto & maps_to_add = result.get()->new_pointcloud_with_ids;
+  auto & maps_to_add = result.get()->new_pointcloud_cells;
   auto & map_ids_to_remove = result.get()->ids_to_remove;
 
   diagnostics_ptr->add_key_value("maps_to_add_size", maps_to_add.size());
@@ -283,7 +283,7 @@ bool MapUpdateModule::update_ndt(
     auto cloud = pcl::make_shared<pcl::PointCloud<PointTarget>>();
 
     pcl::fromROSMsg(map.pointcloud, *cloud);
-    ndt.addTarget(cloud, map.cell_id);
+    ndt.addTarget(cloud, map.metadata.cell_id);
   }
 
   // Remove pcd

localization/ndt_scan_matcher/test/stub_pcd_loader.hpp

@@ -60,26 +60,26 @@ class StubPcdLoader : public rclcpp::Node
       return true;
     }
 
-    autoware_map_msgs::msg::PointCloudMapCellWithID pcd_map_cell_with_id;
-    pcd_map_cell_with_id.cell_id = "0";
+    autoware_map_msgs::msg::PointCloudMapCellWithMetaData pcd_map_cell;
+    pcd_map_cell.metadata.cell_id = "0";
     pcl::PointCloud<pcl::PointXYZ> cloud = make_sample_half_cubic_pcd();
     for (auto & point : cloud.points) {
       point.x += offset_x;
       point.y += offset_y;
     }
-    pcd_map_cell_with_id.metadata.min_x = std::numeric_limits<float>::max();
-    pcd_map_cell_with_id.metadata.min_y = std::numeric_limits<float>::max();
-    pcd_map_cell_with_id.metadata.max_x = std::numeric_limits<float>::lowest();
-    pcd_map_cell_with_id.metadata.max_y = std::numeric_limits<float>::lowest();
+    pcd_map_cell.metadata.min_x = std::numeric_limits<float>::max();
+    pcd_map_cell.metadata.min_y = std::numeric_limits<float>::max();
+    pcd_map_cell.metadata.max_x = std::numeric_limits<float>::lowest();
+    pcd_map_cell.metadata.max_y = std::numeric_limits<float>::lowest();
     for (const auto & point : cloud.points) {
-      pcd_map_cell_with_id.metadata.min_x = std::min(pcd_map_cell_with_id.metadata.min_x, point.x);
-      pcd_map_cell_with_id.metadata.min_y = std::min(pcd_map_cell_with_id.metadata.min_y, point.y);
-      pcd_map_cell_with_id.metadata.max_x = std::max(pcd_map_cell_with_id.metadata.max_x, point.x);
-      pcd_map_cell_with_id.metadata.max_y = std::max(pcd_map_cell_with_id.metadata.max_y, point.y);
+      pcd_map_cell.metadata.min_x = std::min(pcd_map_cell.metadata.min_x, point.x);
+      pcd_map_cell.metadata.min_y = std::min(pcd_map_cell.metadata.min_y, point.y);
+      pcd_map_cell.metadata.max_x = std::max(pcd_map_cell.metadata.max_x, point.x);
+      pcd_map_cell.metadata.max_y = std::max(pcd_map_cell.metadata.max_y, point.y);
     }
     RCLCPP_INFO_STREAM(get_logger(), "cloud size: " << cloud.size());
-    pcl::toROSMsg(cloud, pcd_map_cell_with_id.pointcloud);
-    res->new_pointcloud_with_ids.push_back(pcd_map_cell_with_id);
+    pcl::toROSMsg(cloud, pcd_map_cell.pointcloud);
+    res->new_pointcloud_cells.push_back(pcd_map_cell);
     res->header.frame_id = "map";
     return true;
   }

map/map_height_fitter/src/map_height_fitter.cpp

@@ -145,17 +145,17 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
   const auto res = future.get();
   RCLCPP_DEBUG(
     logger, "Loaded partial pcd map from map_loader (grid size: %lu)",
-    res->new_pointcloud_with_ids.size());
+    res->new_pointcloud_cells.size());
 
   sensor_msgs::msg::PointCloud2 pcd_msg;
-  for (const auto & pcd_with_id : res->new_pointcloud_with_ids) {
+  for (const auto & pcd_cell : res->new_pointcloud_cells) {
     if (pcd_msg.width == 0) {
-      pcd_msg = pcd_with_id.pointcloud;
+      pcd_msg = pcd_cell.pointcloud;
     } else {
-      pcd_msg.width += pcd_with_id.pointcloud.width;
-      pcd_msg.row_step += pcd_with_id.pointcloud.row_step;
+      pcd_msg.width += pcd_cell.pointcloud.width;
+      pcd_msg.row_step += pcd_cell.pointcloud.row_step;
       pcd_msg.data.insert(
-        pcd_msg.data.end(), pcd_with_id.pointcloud.data.begin(), pcd_with_id.pointcloud.data.end());
+        pcd_msg.data.end(), pcd_cell.pointcloud.data.begin(), pcd_cell.pointcloud.data.end());
     }
   }
   map_frame_ = res->header.frame_id;

map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp

@@ -48,13 +48,13 @@ void DifferentialMapLoaderModule::differential_area_load(
       int index = static_cast<int>(id_in_cached_list - cached_ids.begin());
       should_remove[index] = false;
     } else {
-      autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id =
-        load_point_cloud_map_cell_with_id(path, map_id);
-      pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x;
-      pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y;
-      pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x;
-      pointcloud_map_cell_with_id.metadata.max_y = metadata.max.y;
-      response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id);
+      autoware_map_msgs::msg::PointCloudMapCellWithMetaData pointcloud_map_cell =
+        load_point_cloud_map_cell_with_metadata(path, map_id);
+      pointcloud_map_cell.metadata.min_x = metadata.min.x;
+      pointcloud_map_cell.metadata.min_y = metadata.min.y;
+      pointcloud_map_cell.metadata.max_x = metadata.max.x;
+      pointcloud_map_cell.metadata.max_y = metadata.max.y;
+      response->new_pointcloud_cells.push_back(pointcloud_map_cell);
     }
   }
 
@@ -76,16 +76,16 @@ bool DifferentialMapLoaderModule::on_service_get_differential_point_cloud_map(
   return true;
 }
 
-autoware_map_msgs::msg::PointCloudMapCellWithID
-DifferentialMapLoaderModule::load_point_cloud_map_cell_with_id(
+autoware_map_msgs::msg::PointCloudMapCellWithMetaData
+DifferentialMapLoaderModule::load_point_cloud_map_cell_with_metadata(
   const std::string & path, const std::string & map_id) const
 {
   sensor_msgs::msg::PointCloud2 pcd;
   if (pcl::io::loadPCDFile(path, pcd) == -1) {
     RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path);
   }
-  autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id;
-  pointcloud_map_cell_with_id.pointcloud = pcd;
-  pointcloud_map_cell_with_id.cell_id = map_id;
-  return pointcloud_map_cell_with_id;
+  autoware_map_msgs::msg::PointCloudMapCellWithMetaData pointcloud_map_cell;
+  pointcloud_map_cell.pointcloud = pcd;
+  pointcloud_map_cell.metadata.cell_id = map_id;
+  return pointcloud_map_cell;
 }

map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp

@@ -52,7 +52,8 @@ class DifferentialMapLoaderModule
   void differential_area_load(
     const autoware_map_msgs::msg::AreaInfo & area_info, const std::vector<std::string> & cached_ids,
     const GetDifferentialPointCloudMap::Response::SharedPtr & response) const;
-  [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id(
+  [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithMetaData
+  load_point_cloud_map_cell_with_metadata(
     const std::string & path, const std::string & map_id) const;
 };
 

map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp

@@ -42,14 +42,14 @@ void PartialMapLoaderModule::partial_area_load(
     // skip if the pcd file is not within the queried area
     if (!is_grid_within_queried_area(area, metadata)) continue;
 
-    autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id =
-      load_point_cloud_map_cell_with_id(path, map_id);
-    pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x;
-    pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y;
-    pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x;
-    pointcloud_map_cell_with_id.metadata.max_y = metadata.max.y;
+    autoware_map_msgs::msg::PointCloudMapCellWithMetaData pointcloud_map_cell =
+      load_point_cloud_map_cell_with_metadata(path, map_id);
+    pointcloud_map_cell.metadata.min_x = metadata.min.x;
+    pointcloud_map_cell.metadata.min_y = metadata.min.y;
+    pointcloud_map_cell.metadata.max_x = metadata.max.x;
+    pointcloud_map_cell.metadata.max_y = metadata.max.y;
 
-    response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id);
+    response->new_pointcloud_cells.push_back(pointcloud_map_cell);
   }
 }
 
@@ -63,16 +63,16 @@ bool PartialMapLoaderModule::on_service_get_partial_point_cloud_map(
   return true;
 }
 
-autoware_map_msgs::msg::PointCloudMapCellWithID
-PartialMapLoaderModule::load_point_cloud_map_cell_with_id(
+autoware_map_msgs::msg::PointCloudMapCellWithMetaData
+PartialMapLoaderModule::load_point_cloud_map_cell_with_metadata(
   const std::string & path, const std::string & map_id) const
 {
   sensor_msgs::msg::PointCloud2 pcd;
   if (pcl::io::loadPCDFile(path, pcd) == -1) {
     RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path);
   }
-  autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id;
-  pointcloud_map_cell_with_id.pointcloud = pcd;
-  pointcloud_map_cell_with_id.cell_id = map_id;
-  return pointcloud_map_cell_with_id;
+  autoware_map_msgs::msg::PointCloudMapCellWithMetaData pointcloud_map_cell;
+  pointcloud_map_cell.pointcloud = pcd;
+  pointcloud_map_cell.metadata.cell_id = map_id;
+  return pointcloud_map_cell;
 }

map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp

@@ -52,7 +52,8 @@ class PartialMapLoaderModule
   void partial_area_load(
     const autoware_map_msgs::msg::AreaInfo & area,
     const GetPartialPointCloudMap::Response::SharedPtr & response) const;
-  [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id(
+  [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithMetaData
+  load_point_cloud_map_cell_with_metadata(
     const std::string & path, const std::string & map_id) const;
 };
 

map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp

@@ -30,14 +30,14 @@ autoware_map_msgs::msg::PointCloudMapMetaData create_metadata(
     // assume that the map ID = map path (for now)
     const std::string & map_id = ele.first;
 
-    autoware_map_msgs::msg::PointCloudMapCellMetaDataWithID cell_metadata_with_id;
-    cell_metadata_with_id.cell_id = map_id;
-    cell_metadata_with_id.metadata.min_x = metadata.min.x;
-    cell_metadata_with_id.metadata.min_y = metadata.min.y;
-    cell_metadata_with_id.metadata.max_x = metadata.max.x;
-    cell_metadata_with_id.metadata.max_y = metadata.max.y;
+    autoware_map_msgs::msg::PointCloudMapCellMetaData cell_metadata;
+    cell_metadata.cell_id = map_id;
+    cell_metadata.min_x = metadata.min.x;
+    cell_metadata.min_y = metadata.min.y;
+    cell_metadata.max_x = metadata.max.x;
+    cell_metadata.max_y = metadata.max.y;
 
-    metadata_msg.metadata_list.push_back(cell_metadata_with_id);
+    metadata_msg.metadata_list.push_back(cell_metadata);
   }
 
   return metadata_msg;
@@ -81,29 +81,29 @@ bool SelectedMapLoaderModule::on_service_get_selected_point_cloud_map(
     const std::string & map_id = path;
     PCDFileMetadata metadata = requested_selected_map_iterator->second;
 
-    autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id =
-      load_point_cloud_map_cell_with_id(path, map_id);
-    pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x;
-    pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y;
-    pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x;
-    pointcloud_map_cell_with_id.metadata.max_y = metadata.max.y;
+    autoware_map_msgs::msg::PointCloudMapCellWithMetaData pointcloud_map_cell =
+      load_point_cloud_map_cell_with_metadata(path, map_id);
+    pointcloud_map_cell.metadata.min_x = metadata.min.x;
+    pointcloud_map_cell.metadata.min_y = metadata.min.y;
+    pointcloud_map_cell.metadata.max_x = metadata.max.x;
+    pointcloud_map_cell.metadata.max_y = metadata.max.y;
 
-    res->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id);
+    res->new_pointcloud_cells.push_back(pointcloud_map_cell);
   }
   res->header.frame_id = "map";
   return true;
 }
 
-autoware_map_msgs::msg::PointCloudMapCellWithID
-SelectedMapLoaderModule::load_point_cloud_map_cell_with_id(
+autoware_map_msgs::msg::PointCloudMapCellWithMetaData
+SelectedMapLoaderModule::load_point_cloud_map_cell_with_metadata(
   const std::string & path, const std::string & map_id) const
 {
   sensor_msgs::msg::PointCloud2 pcd;
   if (pcl::io::loadPCDFile(path, pcd) == -1) {
     RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path);
   }
-  autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id;
-  pointcloud_map_cell_with_id.pointcloud = pcd;
-  pointcloud_map_cell_with_id.cell_id = map_id;
-  return pointcloud_map_cell_with_id;
+  autoware_map_msgs::msg::PointCloudMapCellWithMetaData pointcloud_map_cell;
+  pointcloud_map_cell.pointcloud = pcd;
+  pointcloud_map_cell.metadata.cell_id = map_id;
+  return pointcloud_map_cell;
 }

map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp

@@ -52,7 +52,8 @@ class SelectedMapLoaderModule
   [[nodiscard]] bool on_service_get_selected_point_cloud_map(
     GetSelectedPointCloudMap::Request::SharedPtr req,
     GetSelectedPointCloudMap::Response::SharedPtr res) const;
-  [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithID load_point_cloud_map_cell_with_id(
+  [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithMetaData
+  load_point_cloud_map_cell_with_metadata(
     const std::string & path, const std::string & map_id) const;
 };
 

map/map_loader/test/test_differential_map_loader_module.cpp

@@ -85,8 +85,8 @@ TEST_F(TestDifferentialMapLoaderModule, LoadDifferentialPCDFiles)
 
   // Check the result
   auto result = result_future.get();
-  ASSERT_EQ(static_cast<int>(result->new_pointcloud_with_ids.size()), 1);
-  EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd");
+  ASSERT_EQ(static_cast<int>(result->new_pointcloud_cells.size()), 1);
+  EXPECT_EQ(result->new_pointcloud_cells[0].metadata.cell_id, "/tmp/dummy.pcd");
   EXPECT_EQ(static_cast<int>(result->ids_to_remove.size()), 0);
 }
 

map/map_loader/test/test_partial_map_loader_module.cpp

@@ -82,8 +82,8 @@ TEST_F(TestPartialMapLoaderModule, LoadPartialPCDFiles)
 
   // Check the result
   auto result = result_future.get();
-  ASSERT_EQ(static_cast<int>(result->new_pointcloud_with_ids.size()), 1);
-  EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd");
+  ASSERT_EQ(static_cast<int>(result->new_pointcloud_cells.size()), 1);
+  EXPECT_EQ(result->new_pointcloud_cells[0].metadata.cell_id, "/tmp/dummy.pcd");
 }
 
 int main(int argc, char ** argv)

perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp

@@ -64,7 +64,7 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
   bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
 
   inline void addMapCellAndFilter(
-    const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override
+    const autoware_map_msgs::msg::PointCloudMapCellWithMetaData & map_cell_to_add) override
   {
     map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
     map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
@@ -95,7 +95,8 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
 
     // add
     (*mutex_ptr_).lock();
-    current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});
+    current_voxel_grid_dict_.insert(
+      {map_cell_to_add.metadata.cell_id, current_voxel_grid_list_item});
     (*mutex_ptr_).unlock();
   }
 };

perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp

@@ -71,7 +71,7 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
   bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
 
   inline void addMapCellAndFilter(
-    const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override
+    const autoware_map_msgs::msg::PointCloudMapCellWithMetaData & map_cell_to_add) override
   {
     map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
     map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
@@ -118,7 +118,8 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
 
     // add
     (*mutex_ptr_).lock();
-    current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});
+    current_voxel_grid_dict_.insert(
+      {map_cell_to_add.metadata.cell_id, current_voxel_grid_list_item});
     (*mutex_ptr_).unlock();
   }
 };

perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp

@@ -453,12 +453,10 @@ void VoxelGridDynamicMapLoader::request_update_map(const geometry_msgs::msg::Poi
   }
   //
   if (status == std::future_status::ready) {
-    if (
-      result.get()->new_pointcloud_with_ids.size() == 0 &&
-      result.get()->ids_to_remove.size() == 0) {
+    if (result.get()->new_pointcloud_cells.size() == 0 && result.get()->ids_to_remove.size() == 0) {
       return;
     }
-    updateDifferentialMapCells(result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove);
+    updateDifferentialMapCells(result.get()->new_pointcloud_cells, result.get()->ids_to_remove);
     if (debug_) {
       publish_downsampled_map(getCurrentDownsampledMapPc());
     }

perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp

@@ -223,7 +223,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
     return current_map_ids;
   }
   inline void updateDifferentialMapCells(
-    const std::vector<autoware_map_msgs::msg::PointCloudMapCellWithID> & map_cells_to_add,
+    const std::vector<autoware_map_msgs::msg::PointCloudMapCellWithMetaData> & map_cells_to_add,
     std::vector<std::string> map_cell_ids_to_remove)
   {
     for (const auto & map_cell_to_add : map_cells_to_add) {
@@ -279,7 +279,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
   }
 
   virtual inline void addMapCellAndFilter(
-    const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add)
+    const autoware_map_msgs::msg::PointCloudMapCellWithMetaData & map_cell_to_add)
   {
     map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
     map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
@@ -316,7 +316,8 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
     current_voxel_grid_list_item.map_cell_pc_ptr = std::move(map_cell_downsampled_pc_ptr_tmp);
     // add
     (*mutex_ptr_).lock();
-    current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});
+    current_voxel_grid_dict_.insert(
+      {map_cell_to_add.metadata.cell_id, current_voxel_grid_list_item});
     (*mutex_ptr_).unlock();
   }
 };

perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp

@@ -82,7 +82,7 @@ class ElevationMapLoaderNode : public rclcpp::Node
   void receiveMap();
   void concatenatePointCloudMaps(
     sensor_msgs::msg::PointCloud2 & pointcloud_map,
-    const std::vector<autoware_map_msgs::msg::PointCloudMapCellWithID> & new_pointcloud_with_ids)
+    const std::vector<autoware_map_msgs::msg::PointCloudMapCellWithMetaData> & new_pointcloud_cells)
     const;
   std::vector<std::string> getRequestIDs(const unsigned int map_id_counter) const;
   void publish();