From 3b27ad6854153ffe734c50231b26d4d6bd87554c Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 4 Jul 2024 18:46:16 +0900 Subject: [PATCH 1/7] delete PointCloudMapCellMetaDataWithID, add cell_id to PointCloudMapMetaData Signed-off-by: Yamato Ando --- .../ndt_scan_matcher/src/map_update_module.cpp | 2 +- .../ndt_scan_matcher/test/stub_pcd_loader.hpp | 2 +- .../differential_map_loader_module.cpp | 2 +- .../partial_map_loader_module.cpp | 2 +- .../selected_map_loader_module.cpp | 16 ++++++++-------- .../test/test_differential_map_loader_module.cpp | 2 +- .../test/test_partial_map_loader_module.cpp | 2 +- ...distance_based_compare_map_filter_nodelet.hpp | 2 +- ...distance_based_compare_map_filter_nodelet.hpp | 2 +- .../voxel_grid_map_loader.hpp | 2 +- 10 files changed, 17 insertions(+), 17 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 32e5a0f2a7c3c..8505af3788b2a 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -247,7 +247,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..c746560284fbb 100644 --- a/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp +++ b/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp @@ -61,7 +61,7 @@ class StubPcdLoader : public rclcpp::Node } autoware_map_msgs::msg::PointCloudMapCellWithID pcd_map_cell_with_id; - pcd_map_cell_with_id.cell_id = "0"; + pcd_map_cell_with_id.metadata.cell_id = "0"; pcl::PointCloud cloud = make_sample_half_cubic_pcd(); for (auto & point : cloud.points) { point.x += offset_x; 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 a8d380fd81b86..6fa2be88b182c 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 @@ -84,6 +84,6 @@ DifferentialMapLoaderModule::loadPointCloudMapCellWithID( } 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; + pointcloud_map_cell_with_id.metadata.cell_id = map_id; return pointcloud_map_cell_with_id; } 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 8c9378e9dfadb..8eb8e83439ad1 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 @@ -68,6 +68,6 @@ autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPoin } 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; + pointcloud_map_cell_with_id.metadata.cell_id = map_id; return pointcloud_map_cell_with_id; } 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 3e7b046f9d178..e4aedb50edc21 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 @@ -29,14 +29,14 @@ autoware_map_msgs::msg::PointCloudMapMetaData createMetadata( // assume that the map ID = map path (for now) std::string map_id = path; - 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; @@ -102,6 +102,6 @@ SelectedMapLoaderModule::loadPointCloudMapCellWithID( } 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; + pointcloud_map_cell_with_id.metadata.cell_id = map_id; return pointcloud_map_cell_with_id; } 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..e64ddb948e88f 100644 --- a/map/map_loader/test/test_differential_map_loader_module.cpp +++ b/map/map_loader/test/test_differential_map_loader_module.cpp @@ -86,7 +86,7 @@ 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"); + EXPECT_EQ(result->new_pointcloud_with_ids[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..9310c0bf7bd69 100644 --- a/map/map_loader/test/test_partial_map_loader_module.cpp +++ b/map/map_loader/test/test_partial_map_loader_module.cpp @@ -83,7 +83,7 @@ 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"); + EXPECT_EQ(result->new_pointcloud_with_ids[0].metadata.cell_id, "/tmp/dummy.pcd"); } int main(int argc, char ** argv) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp index 301dc75839f8e..9236307324854 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp @@ -95,7 +95,7 @@ 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/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp index 0cfc8a64ab2dd..6d50350daf74d 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp @@ -118,7 +118,7 @@ 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/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp index 21cb19edcd0ec..86859ea2d4de5 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp @@ -314,7 +314,7 @@ 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(); } }; From 7cbe76ef40ac339aca4a19a6029e49b9e32dd74e Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Thu, 4 Jul 2024 19:39:42 +0900 Subject: [PATCH 2/7] rename msg Signed-off-by: Yamato Ando --- .../src/map_update_module.cpp | 2 +- .../ndt_scan_matcher/test/stub_pcd_loader.hpp | 24 ++++++++--------- .../src/map_height_fitter.cpp | 12 ++++----- .../differential_map_loader_module.cpp | 26 +++++++++---------- .../differential_map_loader_module.hpp | 2 +- .../partial_map_loader_module.cpp | 24 ++++++++--------- .../partial_map_loader_module.hpp | 2 +- .../selected_map_loader_module.cpp | 26 +++++++++---------- .../selected_map_loader_module.hpp | 2 +- .../test_differential_map_loader_module.cpp | 4 +-- .../test/test_partial_map_loader_module.cpp | 4 +-- ...tance_based_compare_map_filter_nodelet.hpp | 2 +- ...tance_based_compare_map_filter_nodelet.hpp | 2 +- .../voxel_grid_map_loader.hpp | 4 +-- .../src/voxel_grid_map_loader.cpp | 4 +-- .../elevation_map_loader_node.hpp | 2 +- .../src/elevation_map_loader_node.cpp | 16 ++++++------ 17 files changed, 79 insertions(+), 79 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 8505af3788b2a..28b96acbd0c60 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -229,7 +229,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_with_metadata; auto & map_ids_to_remove = result.get()->ids_to_remove; diagnostics_ptr->add_key_value("maps_to_add_size", maps_to_add.size()); diff --git a/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp b/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp index c746560284fbb..6074f70974910 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.metadata.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_with_metadata.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 d6a20a636733d..a15d2450af3de 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -143,17 +143,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_with_metadata.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_with_metadata) { 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 6fa2be88b182c..31f985afcc6e4 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 @@ -46,13 +46,13 @@ void DifferentialMapLoaderModule::differentialAreaLoad( int index = id_in_cached_list - cached_ids.begin(); should_remove[index] = false; } else { - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(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 = + loadPointCloudMapCellWithMetaData(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_with_metadata.push_back(pointcloud_map_cell); } } @@ -74,16 +74,16 @@ bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( return true; } -autoware_map_msgs::msg::PointCloudMapCellWithID -DifferentialMapLoaderModule::loadPointCloudMapCellWithID( +autoware_map_msgs::msg::PointCloudMapCellWithMetaData +DifferentialMapLoaderModule::loadPointCloudMapCellWithMetaData( 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.metadata.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 7069e1dbdf45c..fbd3255e28354 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,7 @@ class DifferentialMapLoaderModule void differentialAreaLoad( const autoware_map_msgs::msg::AreaInfo & area_info, const std::vector & cached_ids, GetDifferentialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + autoware_map_msgs::msg::PointCloudMapCellWithMetaData loadPointCloudMapCellWithMetaData( 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 8eb8e83439ad1..aa363e9ae5767 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 @@ -39,14 +39,14 @@ void PartialMapLoaderModule::partialAreaLoad( // skip if the pcd file is not within the queried area if (!isGridWithinQueriedArea(area, metadata)) continue; - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(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 = + loadPointCloudMapCellWithMetaData(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_with_metadata.push_back(pointcloud_map_cell); } } @@ -59,15 +59,15 @@ bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap( return true; } -autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPointCloudMapCellWithID( +autoware_map_msgs::msg::PointCloudMapCellWithMetaData PartialMapLoaderModule::loadPointCloudMapCellWithMetaData( 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.metadata.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 4d97ab90667ec..cf7cfb86c85ca 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,7 @@ class PartialMapLoaderModule void partialAreaLoad( const autoware_map_msgs::msg::AreaInfo & area, GetPartialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + autoware_map_msgs::msg::PointCloudMapCellWithMetaData loadPointCloudMapCellWithMetaData( 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 e4aedb50edc21..6b46bc28676ff 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 @@ -79,29 +79,29 @@ bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( const std::string map_id = path; PCDFileMetadata metadata = requested_selected_map_iterator->second; - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(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 = + loadPointCloudMapCellWithMetaData(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_with_metadata.push_back(pointcloud_map_cell); } res->header.frame_id = "map"; return true; } -autoware_map_msgs::msg::PointCloudMapCellWithID -SelectedMapLoaderModule::loadPointCloudMapCellWithID( +autoware_map_msgs::msg::PointCloudMapCellWithMetaData +SelectedMapLoaderModule::loadPointCloudMapCellWithMetaData( 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.metadata.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 f44d549a0f576..c9f9abcdf8d3f 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,7 @@ class SelectedMapLoaderModule bool onServiceGetSelectedPointCloudMap( GetSelectedPointCloudMap::Request::SharedPtr req, GetSelectedPointCloudMap::Response::SharedPtr res) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( + autoware_map_msgs::msg::PointCloudMapCellWithMetaData loadPointCloudMapCellWithMetaData( 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 e64ddb948e88f..b5be9269073a1 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].metadata.cell_id, "/tmp/dummy.pcd"); + ASSERT_EQ(static_cast(result->new_pointcloud_cells_with_metadata.size()), 1); + EXPECT_EQ(result->new_pointcloud_cells_with_metadata[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 9310c0bf7bd69..11a2c383bf5ea 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].metadata.cell_id, "/tmp/dummy.pcd"); + ASSERT_EQ(static_cast(result->new_pointcloud_cells_with_metadata.size()), 1); + EXPECT_EQ(result->new_pointcloud_cells_with_metadata[0].metadata.cell_id, "/tmp/dummy.pcd"); } int main(int argc, char ** argv) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp index 9236307324854..9781627631dfc 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.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; diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp index 6d50350daf74d..eeb5e38966faa 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.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; diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp index 86859ea2d4de5..fa04b86f2fb85 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp @@ -221,7 +221,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) { @@ -277,7 +277,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; diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index ef3727019c0f4..abd8149528262 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -452,11 +452,11 @@ 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()->new_pointcloud_cells_with_metadata.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_with_metadata, result.get()->ids_to_remove); if (debug_) { publish_downsampled_map(getCurrentDownsampledMapPc()); } 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..3eec374a01200 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_with_metadata) 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..b62d6143e472b 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_with_metadata); } 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_with_metadata) const { - for (const auto & new_pointcloud_with_id : new_pointcloud_with_ids) { + for (const auto & new_pointcloud_cell : new_pointcloud_cells_with_metadata) { 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()); } } } From 43657698f34c7c8f7c46830a51a9b292aa341413 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 4 Jul 2024 11:00:42 +0000 Subject: [PATCH 3/7] style(pre-commit): autofix --- .../src/pointcloud_map_loader/partial_map_loader_module.cpp | 3 ++- .../distance_based_compare_map_filter_nodelet.hpp | 3 ++- .../voxel_distance_based_compare_map_filter_nodelet.hpp | 3 ++- .../compare_map_segmentation/voxel_grid_map_loader.hpp | 3 ++- .../compare_map_segmentation/src/voxel_grid_map_loader.cpp | 3 ++- .../elevation_map_loader/elevation_map_loader_node.hpp | 4 ++-- .../elevation_map_loader/src/elevation_map_loader_node.cpp | 4 ++-- 7 files changed, 14 insertions(+), 9 deletions(-) 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 aa363e9ae5767..658df78c1b71a 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 @@ -59,7 +59,8 @@ bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap( return true; } -autoware_map_msgs::msg::PointCloudMapCellWithMetaData PartialMapLoaderModule::loadPointCloudMapCellWithMetaData( +autoware_map_msgs::msg::PointCloudMapCellWithMetaData +PartialMapLoaderModule::loadPointCloudMapCellWithMetaData( const std::string & path, const std::string & map_id) const { sensor_msgs::msg::PointCloud2 pcd; diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp index 9781627631dfc..2ab90b2a2101e 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp @@ -95,7 +95,8 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader // add (*mutex_ptr_).lock(); - current_voxel_grid_dict_.insert({map_cell_to_add.metadata.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/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp index eeb5e38966faa..712a041faa17c 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp @@ -118,7 +118,8 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader // add (*mutex_ptr_).lock(); - current_voxel_grid_dict_.insert({map_cell_to_add.metadata.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/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp index fa04b86f2fb85..421efe5d10732 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp @@ -314,7 +314,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.metadata.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/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index abd8149528262..a57dadb3076f1 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -456,7 +456,8 @@ void VoxelGridDynamicMapLoader::request_update_map(const geometry_msgs::msg::Poi result.get()->ids_to_remove.size() == 0) { return; } - updateDifferentialMapCells(result.get()->new_pointcloud_cells_with_metadata, result.get()->ids_to_remove); + updateDifferentialMapCells( + result.get()->new_pointcloud_cells_with_metadata, result.get()->ids_to_remove); if (debug_) { publish_downsampled_map(getCurrentDownsampledMapPc()); } 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 3eec374a01200..9be9c07122d9e 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,8 +82,8 @@ class ElevationMapLoaderNode : public rclcpp::Node void receiveMap(); void concatenatePointCloudMaps( sensor_msgs::msg::PointCloud2 & pointcloud_map, - const std::vector & new_pointcloud_cells_with_metadata) - const; + const std::vector & + new_pointcloud_cells_with_metadata) const; std::vector getRequestIDs(const unsigned int map_id_counter) const; void publish(); void createElevationMap(); 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 b62d6143e472b..39ae32975143c 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -293,8 +293,8 @@ void ElevationMapLoaderNode::receiveMap() void ElevationMapLoaderNode::concatenatePointCloudMaps( sensor_msgs::msg::PointCloud2 & pointcloud_map, - const std::vector & new_pointcloud_cells_with_metadata) - const + const std::vector & + new_pointcloud_cells_with_metadata) const { for (const auto & new_pointcloud_cell : new_pointcloud_cells_with_metadata) { if (pointcloud_map.width == 0) { From 9353bc6040f98e3f5258466a3696d5fd683d49f9 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 5 Jul 2024 17:49:11 +0900 Subject: [PATCH 4/7] rename Signed-off-by: Yamato Ando --- localization/ndt_scan_matcher/src/map_update_module.cpp | 2 +- localization/ndt_scan_matcher/test/stub_pcd_loader.hpp | 2 +- map/map_height_fitter/src/map_height_fitter.cpp | 4 ++-- .../differential_map_loader_module.cpp | 2 +- .../src/pointcloud_map_loader/partial_map_loader_module.cpp | 2 +- .../pointcloud_map_loader/selected_map_loader_module.cpp | 2 +- map/map_loader/test/test_differential_map_loader_module.cpp | 4 ++-- map/map_loader/test/test_partial_map_loader_module.cpp | 4 ++-- .../compare_map_segmentation/src/voxel_grid_map_loader.cpp | 4 ++-- .../elevation_map_loader/elevation_map_loader_node.hpp | 2 +- .../elevation_map_loader/src/elevation_map_loader_node.cpp | 6 +++--- 11 files changed, 17 insertions(+), 17 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 28b96acbd0c60..5b54601be98d6 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -229,7 +229,7 @@ bool MapUpdateModule::update_ndt( } diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true); - auto & maps_to_add = result.get()->new_pointcloud_cells_with_metadata; + 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()); diff --git a/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp b/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp index 6074f70974910..b11ee39662ffa 100644 --- a/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp +++ b/localization/ndt_scan_matcher/test/stub_pcd_loader.hpp @@ -79,7 +79,7 @@ class StubPcdLoader : public rclcpp::Node } RCLCPP_INFO_STREAM(get_logger(), "cloud size: " << cloud.size()); pcl::toROSMsg(cloud, pcd_map_cell.pointcloud); - res->new_pointcloud_cells_with_metadata.push_back(pcd_map_cell); + 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 a15d2450af3de..4643f5da86a59 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -143,10 +143,10 @@ 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_cells_with_metadata.size()); + res->new_pointcloud_cells.size()); sensor_msgs::msg::PointCloud2 pcd_msg; - for (const auto & pcd_cell : res->new_pointcloud_cells_with_metadata) { + for (const auto & pcd_cell : res->new_pointcloud_cells) { if (pcd_msg.width == 0) { pcd_msg = pcd_cell.pointcloud; } else { 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 31f985afcc6e4..b727f7aac1c3f 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 @@ -52,7 +52,7 @@ void DifferentialMapLoaderModule::differentialAreaLoad( 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_with_metadata.push_back(pointcloud_map_cell); + response->new_pointcloud_cells.push_back(pointcloud_map_cell); } } 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 658df78c1b71a..caabb8206f9da 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 @@ -46,7 +46,7 @@ void PartialMapLoaderModule::partialAreaLoad( pointcloud_map_cell.metadata.max_x = metadata.max.x; pointcloud_map_cell.metadata.max_y = metadata.max.y; - response->new_pointcloud_cells_with_metadata.push_back(pointcloud_map_cell); + response->new_pointcloud_cells.push_back(pointcloud_map_cell); } } 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 6b46bc28676ff..6d1511192eace 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 @@ -86,7 +86,7 @@ bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( pointcloud_map_cell.metadata.max_x = metadata.max.x; pointcloud_map_cell.metadata.max_y = metadata.max.y; - res->new_pointcloud_cells_with_metadata.push_back(pointcloud_map_cell); + res->new_pointcloud_cells.push_back(pointcloud_map_cell); } res->header.frame_id = "map"; return true; 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 b5be9269073a1..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_cells_with_metadata.size()), 1); - EXPECT_EQ(result->new_pointcloud_cells_with_metadata[0].metadata.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 11a2c383bf5ea..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_cells_with_metadata.size()), 1); - EXPECT_EQ(result->new_pointcloud_cells_with_metadata[0].metadata.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/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index a57dadb3076f1..e8ad820218253 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -452,12 +452,12 @@ void VoxelGridDynamicMapLoader::request_update_map(const geometry_msgs::msg::Poi // if (status == std::future_status::ready) { if ( - result.get()->new_pointcloud_cells_with_metadata.size() == 0 && + result.get()->new_pointcloud_cells.size() == 0 && result.get()->ids_to_remove.size() == 0) { return; } updateDifferentialMapCells( - result.get()->new_pointcloud_cells_with_metadata, result.get()->ids_to_remove); + result.get()->new_pointcloud_cells, result.get()->ids_to_remove); if (debug_) { publish_downsampled_map(getCurrentDownsampledMapPc()); } 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 9be9c07122d9e..9aa0d229ef6bb 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 @@ -83,7 +83,7 @@ class ElevationMapLoaderNode : public rclcpp::Node void concatenatePointCloudMaps( sensor_msgs::msg::PointCloud2 & pointcloud_map, const std::vector & - new_pointcloud_cells_with_metadata) const; + new_pointcloud_cells) const; std::vector getRequestIDs(const unsigned int map_id_counter) const; void publish(); void createElevationMap(); 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 39ae32975143c..9694a58634185 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_cells_with_metadata); + concatenatePointCloudMaps(pointcloud_map, result.get()->new_pointcloud_cells); } RCLCPP_DEBUG(this->get_logger(), "finish receiving"); pcl::PointCloud map_pcl; @@ -294,9 +294,9 @@ void ElevationMapLoaderNode::receiveMap() void ElevationMapLoaderNode::concatenatePointCloudMaps( sensor_msgs::msg::PointCloud2 & pointcloud_map, const std::vector & - new_pointcloud_cells_with_metadata) const + new_pointcloud_cells) const { - for (const auto & new_pointcloud_cell : new_pointcloud_cells_with_metadata) { + for (const auto & new_pointcloud_cell : new_pointcloud_cells) { if (pointcloud_map.width == 0) { pointcloud_map = new_pointcloud_cell.pointcloud; } else { From aa70ae057393fcab142226d643df03c4521150e8 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 10 Jul 2024 17:29:39 +0900 Subject: [PATCH 5/7] fix const Signed-off-by: Yamato Ando --- .../pointcloud_map_loader/differential_map_loader_module.hpp | 2 +- .../src/pointcloud_map_loader/partial_map_loader_module.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 57bd44b3c2a39..bc81679524f6d 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 @@ -51,7 +51,7 @@ class DifferentialMapLoaderModule GetDifferentialPointCloudMap::Response::SharedPtr res) const; void differential_area_load( const autoware_map_msgs::msg::AreaInfo & area_info, const std::vector & cached_ids, - GetDifferentialPointCloudMap::Response::SharedPtr & response) const; + const GetDifferentialPointCloudMap::Response::SharedPtr & response) const; 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.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index 02a810f8dceda..346ef953c16e5 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 @@ -51,7 +51,7 @@ class PartialMapLoaderModule GetPartialPointCloudMap::Response::SharedPtr res) const; void partial_area_load( const autoware_map_msgs::msg::AreaInfo & area, - GetPartialPointCloudMap::Response::SharedPtr & response) const; + const GetPartialPointCloudMap::Response::SharedPtr & response) const; autoware_map_msgs::msg::PointCloudMapCellWithMetaData load_point_cloud_map_cell_with_metadata( const std::string & path, const std::string & map_id) const; }; From 71b27d1d70b585bec2ac57d8c8361708fe8035d4 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 10 Jul 2024 17:34:16 +0900 Subject: [PATCH 6/7] fix [[nodiscard]] Signed-off-by: Yamato Ando --- .../pointcloud_map_loader/differential_map_loader_module.hpp | 2 +- .../src/pointcloud_map_loader/partial_map_loader_module.hpp | 2 +- .../src/pointcloud_map_loader/selected_map_loader_module.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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 bc81679524f6d..c3e3fdda28d0b 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,7 @@ 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; - autoware_map_msgs::msg::PointCloudMapCellWithMetaData load_point_cloud_map_cell_with_metadata( + [[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.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index 346ef953c16e5..27041837426f4 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,7 @@ class PartialMapLoaderModule void partial_area_load( const autoware_map_msgs::msg::AreaInfo & area, const GetPartialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithMetaData load_point_cloud_map_cell_with_metadata( + [[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.hpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp index 590167c428f02..55148ccf671fc 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,7 @@ class SelectedMapLoaderModule [[nodiscard]] bool on_service_get_selected_point_cloud_map( GetSelectedPointCloudMap::Request::SharedPtr req, GetSelectedPointCloudMap::Response::SharedPtr res) const; - autoware_map_msgs::msg::PointCloudMapCellWithMetaData load_point_cloud_map_cell_with_metadata( + [[nodiscard]] autoware_map_msgs::msg::PointCloudMapCellWithMetaData load_point_cloud_map_cell_with_metadata( const std::string & path, const std::string & map_id) const; }; From 63e00de3421d9d5ff62784a9c0529fa6217e6c1a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 10 Jul 2024 08:36:41 +0000 Subject: [PATCH 7/7] style(pre-commit): autofix --- .../differential_map_loader_module.hpp | 3 ++- .../pointcloud_map_loader/partial_map_loader_module.hpp | 3 ++- .../pointcloud_map_loader/selected_map_loader_module.hpp | 3 ++- .../src/voxel_grid_map_loader/voxel_grid_map_loader.cpp | 7 ++----- .../elevation_map_loader/elevation_map_loader_node.hpp | 4 ++-- .../elevation_map_loader/src/elevation_map_loader_node.cpp | 4 ++-- 6 files changed, 12 insertions(+), 12 deletions(-) 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 c3e3fdda28d0b..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::PointCloudMapCellWithMetaData load_point_cloud_map_cell_with_metadata( + [[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.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index 27041837426f4..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::PointCloudMapCellWithMetaData load_point_cloud_map_cell_with_metadata( + [[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.hpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp index 55148ccf671fc..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::PointCloudMapCellWithMetaData load_point_cloud_map_cell_with_metadata( + [[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/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp index a50e2fa86eda5..48d459937603b 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader/voxel_grid_map_loader.cpp @@ -453,13 +453,10 @@ void VoxelGridDynamicMapLoader::request_update_map(const geometry_msgs::msg::Poi } // if (status == std::future_status::ready) { - if ( - result.get()->new_pointcloud_cells.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_cells, 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/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 9aa0d229ef6bb..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,8 +82,8 @@ class ElevationMapLoaderNode : public rclcpp::Node void receiveMap(); void concatenatePointCloudMaps( sensor_msgs::msg::PointCloud2 & pointcloud_map, - const std::vector & - new_pointcloud_cells) const; + const std::vector & new_pointcloud_cells) + const; std::vector getRequestIDs(const unsigned int map_id_counter) const; void publish(); void createElevationMap(); 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 9694a58634185..abd0ebd9e9fe9 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -293,8 +293,8 @@ void ElevationMapLoaderNode::receiveMap() void ElevationMapLoaderNode::concatenatePointCloudMaps( sensor_msgs::msg::PointCloud2 & pointcloud_map, - const std::vector & - new_pointcloud_cells) const + const std::vector & new_pointcloud_cells) + const { for (const auto & new_pointcloud_cell : new_pointcloud_cells) { if (pointcloud_map.width == 0) {