diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 012c72e6b3a46..55fd92afb1d80 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/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::fromROSMsg(map.pointcloud, *cloud); - ndt.addTarget(cloud, map.cell_id); + ndt.addTarget(cloud, map.metadata.cell_id); } // Remove pcd diff --git a/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp b/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp index a80c1126167a8..b11ee39662ffa 100644 --- a/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp +++ b/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 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::max(); - pcd_map_cell_with_id.metadata.min_y = std::numeric_limits::max(); - pcd_map_cell_with_id.metadata.max_x = std::numeric_limits::lowest(); - pcd_map_cell_with_id.metadata.max_y = std::numeric_limits::lowest(); + pcd_map_cell.metadata.min_x = std::numeric_limits::max(); + pcd_map_cell.metadata.min_y = std::numeric_limits::max(); + pcd_map_cell.metadata.max_x = std::numeric_limits::lowest(); + pcd_map_cell.metadata.max_y = std::numeric_limits::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; } diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index c9e199ad88422..df16f4eb67ed3 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/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; diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index da42389dcc69f..575941da2d845 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/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(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; } diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 690ffeca653c8..bd7f11231695a 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/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 & 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; }; diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 62e165dd1005b..cb53a2f972ca5 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/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; } diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index ec97661366419..3ba8a5966ed7f 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp +++ b/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; }; diff --git a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp index 76b56341b8632..cbd5a1224c130 100644 --- a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp +++ b/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; } diff --git a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp index eea8b8c1950ae..dd79c3a28e32a 100644 --- a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp +++ b/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; }; diff --git a/map/map_loader/test/test_differential_map_loader_module.cpp b/map/map_loader/test/test_differential_map_loader_module.cpp index e25a8f97a70ca..e8fe8449c0405 100644 --- a/map/map_loader/test/test_differential_map_loader_module.cpp +++ b/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(result->new_pointcloud_with_ids.size()), 1); - EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd"); + ASSERT_EQ(static_cast(result->new_pointcloud_cells.size()), 1); + EXPECT_EQ(result->new_pointcloud_cells[0].metadata.cell_id, "/tmp/dummy.pcd"); EXPECT_EQ(static_cast(result->ids_to_remove.size()), 0); } diff --git a/map/map_loader/test/test_partial_map_loader_module.cpp b/map/map_loader/test/test_partial_map_loader_module.cpp index 9ff27df29ab8b..2b39dfd2d2252 100644 --- a/map/map_loader/test/test_partial_map_loader_module.cpp +++ b/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(result->new_pointcloud_with_ids.size()), 1); - EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd"); + ASSERT_EQ(static_cast(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) diff --git a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp index d0cb5b3c62992..e3cf5af06470b 100644 --- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp +++ b/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(); } }; diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp index 15ed2a32ff213..7845eab07fe63 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp +++ b/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(); } }; diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp index b011799e5b912..48d459937603b 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp +++ b/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()); } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp b/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp index b13f2e537ee55..c92c4adc91de0 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.hpp +++ b/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 & map_cells_to_add, + const std::vector & map_cells_to_add, std::vector 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(); } }; diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index fda0fcddc1bc7..67b8ce9dc1256 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/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 & new_pointcloud_with_ids) + const std::vector & new_pointcloud_cells) const; std::vector getRequestIDs(const unsigned int map_id_counter) const; void publish(); diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 80f7d85fd53c8..abd0ebd9e9fe9 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -283,7 +283,7 @@ void ElevationMapLoaderNode::receiveMap() } // concatenate maps - concatenatePointCloudMaps(pointcloud_map, result.get()->new_pointcloud_with_ids); + concatenatePointCloudMaps(pointcloud_map, result.get()->new_pointcloud_cells); } RCLCPP_DEBUG(this->get_logger(), "finish receiving"); pcl::PointCloud map_pcl; @@ -293,18 +293,18 @@ void ElevationMapLoaderNode::receiveMap() void ElevationMapLoaderNode::concatenatePointCloudMaps( sensor_msgs::msg::PointCloud2 & pointcloud_map, - const std::vector & new_pointcloud_with_ids) + const std::vector & new_pointcloud_cells) const { - for (const auto & new_pointcloud_with_id : new_pointcloud_with_ids) { + for (const auto & new_pointcloud_cell : new_pointcloud_cells) { if (pointcloud_map.width == 0) { - pointcloud_map = new_pointcloud_with_id.pointcloud; + pointcloud_map = new_pointcloud_cell.pointcloud; } else { - pointcloud_map.width += new_pointcloud_with_id.pointcloud.width; - pointcloud_map.row_step += new_pointcloud_with_id.pointcloud.row_step; + pointcloud_map.width += new_pointcloud_cell.pointcloud.width; + pointcloud_map.row_step += new_pointcloud_cell.pointcloud.row_step; pointcloud_map.data.insert( - pointcloud_map.data.end(), new_pointcloud_with_id.pointcloud.data.begin(), - new_pointcloud_with_id.pointcloud.data.end()); + pointcloud_map.data.end(), new_pointcloud_cell.pointcloud.data.begin(), + new_pointcloud_cell.pointcloud.data.end()); } } }