From 7b4bb28830bc78b8bc7f86778c6fc1f084bbf1ea Mon Sep 17 00:00:00 2001 From: yoshiri Date: Mon, 8 Apr 2024 20:22:25 +0900 Subject: [PATCH 1/7] feat: add downsampler for occuapancy grid map creation Signed-off-by: yoshiri --- ...tcloud_based_occupancy_grid_map.param.yaml | 4 + ...tcloud_based_occupancy_grid_map.param.yaml | 4 + ...ntcloud_based_occupancy_grid_map.launch.py | 77 ++++++++++++++++++- ...ntcloud_based_occupancy_grid_map.launch.py | 69 +++++++++++++++-- 4 files changed, 146 insertions(+), 8 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml index f9a1c4f930001..833c85a2d812e 100644 --- a/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml +++ b/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml @@ -12,6 +12,10 @@ map_length_x: 150.0 # [m] map_length_y: 150.0 # [m] + # pointcloud donwsampler + downsample_input_pointcloud: true + downsample_voxel_size: 0.125 # [m] + # each grid map parameters ogm_creation_config: height_filter: diff --git a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml index 8077bdec5008e..cbf2e6c69946d 100644 --- a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml +++ b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml @@ -8,6 +8,10 @@ min_height: -1.0 max_height: 2.0 + # pointcloud donwsampler + downsample_input_pointcloud: true + downsample_voxel_size: 0.125 # [m] + enable_single_frame_mode: false # use sensor pointcloud to filter obstacle pointcloud filter_obstacle_pointcloud_by_raw_pointcloud: false diff --git a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py index dccddfbe54196..8f61ae836fb4c 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py @@ -92,6 +92,52 @@ def get_ogm_creation_config(total_config: dict, list_iter: int) -> dict: return ogm_creation_config +# generate downsampler +def get_downsample_filter_node(setting: dict) -> ComposableNode: + plugin_str = setting["plugin"] + voxel_size = setting["voxel_size"] + node_name = setting["node_name"] + return ComposableNode( + package="pointcloud_preprocessor", + plugin=plugin_str, + name=node_name, + remappings=[ + ("input", setting["input_topic"]), + ("output", setting["output_topic"]), + ], + parameters=[ + { + "voxel_size_x": voxel_size, + "voxel_size_y": voxel_size, + "voxel_size_z": voxel_size, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + +def get_downsample_preprocess_nodes(voxel_size: float, raw_pointcloud_topics: list) -> list: + nodes = [] + for i in range(len(raw_pointcloud_topics)): + raw_settings = { + "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + "node_name": "raw_pc_downsample_filter_" + str(i), + "input_topic": raw_pointcloud_topics[i], + "output_topic": raw_pointcloud_topics[i] + "_downsampled", + "voxel_size": voxel_size, + } + nodes.append(get_downsample_filter_node(raw_settings)) + obstacle_settings = { + "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + "node_name": "obstacle_pc_downsample_filter", + "input_topic": LaunchConfiguration("input/obstacle_pointcloud"), + "output_topic": "/perception/obstacle_segmentation/downsample/pointcloud", + "voxel_size": voxel_size, + } + nodes.append(get_downsample_filter_node(obstacle_settings)) + return nodes + + def launch_setup(context, *args, **kwargs): """Launch fusion based occupancy grid map creation nodes. @@ -118,20 +164,39 @@ def launch_setup(context, *args, **kwargs): LaunchConfiguration("pointcloud_container_name").perform(context), ) + # Down sample settings + downsample_input_pointcloud: bool = total_config["downsample_input_pointcloud"] + downsample_voxel_size: float = total_config["downsample_voxel_size"] + + # get obstacle pointcloud + obstacle_pointcloud_topic: str = ( + "/perception/obstacle_segmentation/downsample/pointcloud" + if downsample_input_pointcloud + else LaunchConfiguration("input/obstacle_pointcloud").perform(context) + ) + for i in range(number_of_nodes): # load parameter file ogm_creation_config = get_ogm_creation_config(total_config, i) ogm_creation_config["updater_type"] = LaunchConfiguration("updater_type").perform(context) ogm_creation_config.update(updater_config) + raw_pointcloud_topic: str = fusion_config["raw_pointcloud_topics"][i] + frame_name: str = raw_pointcloud_topic.split("/")[ + -2 + ] # assume `/sensing/lidar/top/pointcloud` -> `top + if downsample_input_pointcloud: + raw_pointcloud_topic += "_downsampled" + # generate composable node node = ComposableNode( package="probabilistic_occupancy_grid_map", plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", - name="occupancy_grid_map_node_in_" + str(i), + name="occupancy_grid_map_node" + str(i), + namespace=frame_name, remappings=[ - ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), - ("~/input/raw_pointcloud", fusion_config["raw_pointcloud_topics"][i]), + ("~/input/obstacle_pointcloud", obstacle_pointcloud_topic), + ("~/input/raw_pointcloud", raw_pointcloud_topic), ("~/output/occupancy_grid_map", fusion_config["fusion_input_ogm_topics"][i]), ], parameters=[ogm_creation_config], @@ -139,6 +204,12 @@ def launch_setup(context, *args, **kwargs): ) gridmap_generation_composable_nodes.append(node) + if downsample_input_pointcloud: + downsapmle_nodes = get_downsample_preprocess_nodes( + downsample_voxel_size, fusion_config["raw_pointcloud_topics"] + ) + gridmap_generation_composable_nodes.extend(downsapmle_nodes) + # 2. launch occupancy grid map fusion node gridmap_fusion_node = [ ComposableNode( diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index 29435c4ea8e24..faa0486bd52ee 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -26,6 +26,47 @@ import yaml +def get_downsample_filter_node(setting: dict) -> ComposableNode: + plugin_str = setting["plugin"] + voxel_size = setting["voxel_size"] + node_name = setting["node_name"] + return ComposableNode( + package="pointcloud_preprocessor", + plugin=plugin_str, + name=node_name, + remappings=[ + ("input", setting["input_topic"]), + ("output", setting["output_topic"]), + ], + parameters=[ + { + "voxel_size_x": voxel_size, + "voxel_size_y": voxel_size, + "voxel_size_z": voxel_size, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + +def get_downsample_preprocess_nodes(voxel_size: float) -> list: + raw_settings = { + "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + "node_name": "raw_pc_downsample_filter", + "input_topic": LaunchConfiguration("input/raw_pointcloud"), + "output_topic": "/sensing/lidar/concatenated/downsample/pointcloud", + "voxel_size": voxel_size, + } + obstacle_settings = { + "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + "node_name": "obstacle_pc_downsample_filter", + "input_topic": LaunchConfiguration("input/obstacle_pointcloud"), + "output_topic": "/perception/obstacle_segmentation/downsample/pointcloud", + "voxel_size": voxel_size, + } + return [get_downsample_filter_node(raw_settings), get_downsample_filter_node(obstacle_settings)] + + def launch_setup(context, *args, **kwargs): # load parameter files param_file = LaunchConfiguration("param_file").perform(context) @@ -38,7 +79,21 @@ def launch_setup(context, *args, **kwargs): with open(updater_param_file, "r") as f: occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"] - composable_nodes = [ + # composable nodes + composable_nodes = [] + + # add downsample process + downsample_input_pointcloud: bool = pointcloud_based_occupancy_grid_map_node_params[ + "downsample_input_pointcloud" + ] + if downsample_input_pointcloud: + voxel_grid_size: float = pointcloud_based_occupancy_grid_map_node_params[ + "downsample_voxel_size" + ] + downsample_preprocess_nodes = get_downsample_preprocess_nodes(voxel_grid_size) + composable_nodes.extend(downsample_preprocess_nodes) + + composable_nodes.append( ComposableNode( package="probabilistic_occupancy_grid_map", plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", @@ -46,11 +101,15 @@ def launch_setup(context, *args, **kwargs): remappings=[ ( "~/input/obstacle_pointcloud", - LaunchConfiguration("input/obstacle_pointcloud"), + LaunchConfiguration("input/obstacle_pointcloud") + if downsample_input_pointcloud + else "/perception/obstacle_segmentation/downsample/pointcloud", ), ( "~/input/raw_pointcloud", - LaunchConfiguration("input/raw_pointcloud"), + LaunchConfiguration("input/raw_pointcloud") + if downsample_input_pointcloud + else "/sensing/lidar/concatenated/downsample/pointcloud", ), ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], @@ -60,8 +119,8 @@ def launch_setup(context, *args, **kwargs): {"updater_type": LaunchConfiguration("updater_type")}, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ), - ] + ) + ) occupancy_grid_map_container = ComposableNodeContainer( name=LaunchConfiguration("individual_container_name"), From a68f7dc2877d0a036d2e74941aece0e2a9eae812 Mon Sep 17 00:00:00 2001 From: Yukihito Saito Date: Tue, 12 Mar 2024 01:15:46 +0900 Subject: [PATCH 2/7] add approximate extract common pointcloud function Signed-off-by: Yukihito Saito --- .../include/utils/utils.hpp | 4 + ...intcloud_based_occupancy_grid_map_node.cpp | 4 +- .../src/utils/utils.cpp | 139 ++++++++++++++++++ 3 files changed, 145 insertions(+), 2 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp index 46bd14b843dae..44cb3576768ef 100644 --- a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp @@ -116,6 +116,10 @@ bool extractCommonPointCloud( const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, sensor_msgs::msg::PointCloud2 & output_obstacle_pc); +bool extractApproximateCommonPointCloud( + const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, + const float voxel_size, sensor_msgs::msg::PointCloud2 & output_obstacle_pc); + unsigned char getApproximateOccupancyState(const unsigned char & value); } // namespace utils diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index e7a51028a1159..54cc181a43ea2 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -162,8 +162,8 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( // Filter obstacle pointcloud by raw pointcloud PointCloud2 filtered_obstacle_pc_common{}; if (filter_obstacle_pointcloud_by_raw_pointcloud_) { - if (!utils::extractCommonPointCloud( - filtered_obstacle_pc, filtered_raw_pc, filtered_obstacle_pc_common)) { + if (!utils::extractApproximateCommonPointCloud( + filtered_obstacle_pc, filtered_raw_pc, 0.1f, filtered_obstacle_pc_common)) { filtered_obstacle_pc_common = filtered_obstacle_pc; } } else { diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index b876004d89b0a..412a8887eaef8 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -18,6 +18,35 @@ #include +namespace +{ +/** + * @brief Hash function for voxel keys. + * Utilizes prime numbers to calculate a unique hash for each voxel key. + */ +struct VoxelKeyHash +{ + std::size_t operator()(const std::array & k) const + { + // Primes based on the following paper: 'Investigating the Use of Primes in Hashing for + // Volumetric Data'. + return (k[0] * 73856093 ^ k[1] * 19349663 ^ k[2] * 83492791); + } +}; + +/** + * @brief Equality function for voxel keys. + * Checks if two voxel keys are equal. + */ +struct VoxelKeyEqual +{ + bool operator()(const std::array & a, const std::array & b) const + { + return a == b; + } +}; +} // namespace + namespace utils { @@ -182,6 +211,116 @@ bool extractCommonPointCloud( return true; } +/** + * @brief extract Common Pointcloud between obstacle pc and raw pc + * @param obstacle_pc + * @param raw_pc + * @param voxel_size + * @param output_obstacle_pc + */ +bool extractApproximateCommonPointCloud( + const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, + const float voxel_size, sensor_msgs::msg::PointCloud2 & output_obstacle_pc) +{ + using VoxelKey = std::array; + std::unordered_map obstacle_voxel_map; + std::unordered_map raw_voxel_map; + + constexpr float large_num_offset = 100000.0; + const float & voxel_size_x = voxel_size; + const float & voxel_size_y = voxel_size; + const float & voxel_size_z = voxel_size; + const float inverse_voxel_size_x = 1.0 / voxel_size_x; + const float inverse_voxel_size_y = 1.0 / voxel_size_y; + const float inverse_voxel_size_z = 1.0 / voxel_size_z; + + { + const int x_offset = raw_pc.fields[pcl::getFieldIndex(raw_pc, "x")].offset; + const int y_offset = raw_pc.fields[pcl::getFieldIndex(raw_pc, "y")].offset; + const int z_offset = raw_pc.fields[pcl::getFieldIndex(raw_pc, "z")].offset; + + // Process each point in the point cloud + for (size_t global_offset = 0; global_offset + raw_pc.point_step <= raw_pc.data.size(); + global_offset += raw_pc.point_step) { + const float & x = *reinterpret_cast(&raw_pc.data[global_offset + x_offset]); + const float & y = *reinterpret_cast(&raw_pc.data[global_offset + y_offset]); + const float & z = *reinterpret_cast(&raw_pc.data[global_offset + z_offset]); + + // The reason for adding a large value is that when converting from float to int, values + // around -1 to 1 are all rounded down to 0. Therefore, to prevent the numbers from becoming + // negative, a large value is added. It has been tuned to reduce computational costs, and + // deliberately avoids using round or floor functions. + VoxelKey key = { + static_cast((x + large_num_offset) * inverse_voxel_size_x), + static_cast((y + large_num_offset) * inverse_voxel_size_y), + static_cast((z + large_num_offset) * inverse_voxel_size_z)}; + + if (raw_voxel_map.find(key) == raw_voxel_map.end()) { + raw_voxel_map[key] = global_offset; + } + } + } + { + const int x_offset = obstacle_pc.fields[pcl::getFieldIndex(obstacle_pc, "x")].offset; + const int y_offset = obstacle_pc.fields[pcl::getFieldIndex(obstacle_pc, "y")].offset; + const int z_offset = obstacle_pc.fields[pcl::getFieldIndex(obstacle_pc, "z")].offset; + + for (size_t global_offset = 0; + global_offset + obstacle_pc.point_step <= obstacle_pc.data.size(); + global_offset += obstacle_pc.point_step) { + const float & x = + *reinterpret_cast(&obstacle_pc.data[global_offset + x_offset]); + const float & y = + *reinterpret_cast(&obstacle_pc.data[global_offset + y_offset]); + const float & z = + *reinterpret_cast(&obstacle_pc.data[global_offset + z_offset]); + + // The reason for adding a large value is that when converting from float to int, values + // around -1 to 1 are all rounded down to 0. Therefore, to prevent the numbers from becoming + // negative, a large value is added. It has been tuned to reduce computational costs, and + // deliberately avoids using round or floor functions. + VoxelKey key = { + static_cast((x + large_num_offset) * inverse_voxel_size_x), + static_cast((y + large_num_offset) * inverse_voxel_size_y), + static_cast((z + large_num_offset) * inverse_voxel_size_z)}; + + if (raw_voxel_map.find(key) == raw_voxel_map.end()) { + obstacle_voxel_map[key] = global_offset; + } + } + + // Populate the output point cloud + size_t output_global_offset = 0; + output_obstacle_pc.data.resize(obstacle_voxel_map.size() * obstacle_pc.point_step); + for (const auto & kv : obstacle_voxel_map) { + std::memcpy( + &output_obstacle_pc.data[output_global_offset + x_offset], + &obstacle_pc.data[kv.second + x_offset], sizeof(float)); + std::memcpy( + &output_obstacle_pc.data[output_global_offset + y_offset], + &obstacle_pc.data[kv.second + y_offset], sizeof(float)); + std::memcpy( + &output_obstacle_pc.data[output_global_offset + z_offset], + &obstacle_pc.data[kv.second + z_offset], sizeof(float)); + output_global_offset += obstacle_pc.point_step; + } + + // Set the output point cloud metadata + output_obstacle_pc.header.frame_id = obstacle_pc.header.frame_id; + output_obstacle_pc.height = 1; + output_obstacle_pc.fields = obstacle_pc.fields; + output_obstacle_pc.is_bigendian = obstacle_pc.is_bigendian; + output_obstacle_pc.point_step = obstacle_pc.point_step; + output_obstacle_pc.is_dense = obstacle_pc.is_dense; + output_obstacle_pc.width = static_cast( + output_obstacle_pc.data.size() / output_obstacle_pc.height / output_obstacle_pc.point_step); + output_obstacle_pc.row_step = + static_cast(output_obstacle_pc.data.size() / output_obstacle_pc.height); + } + + return true; +} + /** * @brief Convert unsigned char value to closest cost value * @param cost Cost value From 4d4bd9e030bfe233fe3e4a0df1d5be02f9ad957e Mon Sep 17 00:00:00 2001 From: yoshiri Date: Tue, 9 Apr 2024 15:15:39 +0900 Subject: [PATCH 3/7] feat: enable to choose pointcloud filtering method in ogm creation Signed-off-by: yoshiri --- ...intcloud_based_occupancy_grid_map_node.cpp | 22 ++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 54cc181a43ea2..24bc0f2f059fc 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -162,9 +162,16 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( // Filter obstacle pointcloud by raw pointcloud PointCloud2 filtered_obstacle_pc_common{}; if (filter_obstacle_pointcloud_by_raw_pointcloud_) { - if (!utils::extractApproximateCommonPointCloud( - filtered_obstacle_pc, filtered_raw_pc, 0.1f, filtered_obstacle_pc_common)) { - filtered_obstacle_pc_common = filtered_obstacle_pc; + const bool use_exact_common_pointcloud = true; + const bool success_to_filter = + use_exact_common_pointcloud + ? utils::extractCommonPointCloud( + filtered_obstacle_pc, filtered_raw_pc, filtered_obstacle_pc_common) + : utils::extractApproximateCommonPointCloud( + filtered_obstacle_pc, filtered_raw_pc, 0.15f, filtered_obstacle_pc_common); + if (!success_to_filter) { + // RCLCPP_WARN_STREAM(get_logger(), "pointcloud filtering is failed in + // filter_obstacle_pointcloud_by_raw_pointcloud."); } } else { filtered_obstacle_pc_common = filtered_obstacle_pc; @@ -190,8 +197,13 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( occupancy_grid_map_ptr_->updateOrigin( gridmap_origin.position.x - occupancy_grid_map_ptr_->getSizeInMetersX() / 2, gridmap_origin.position.y - occupancy_grid_map_ptr_->getSizeInMetersY() / 2); - occupancy_grid_map_ptr_->updateWithPointCloud( - filtered_raw_pc, filtered_obstacle_pc_common, robot_pose, scan_origin); + // check the raw/obstacle pointcloud size + if (filtered_raw_pc.data.size() == 0 || filtered_obstacle_pc_common.data.size() == 0) { + RCLCPP_WARN_STREAM(get_logger(), "pointcloud size is 0"); + } else { + occupancy_grid_map_ptr_->updateWithPointCloud( + filtered_raw_pc, filtered_obstacle_pc_common, robot_pose, scan_origin); + } if (enable_single_frame_mode_) { // publish From e7b452b56ea7e77c4ed9c0bd3e4f9890cdcdbe3d Mon Sep 17 00:00:00 2001 From: Taiga <53041471+TakanoTaiga@users.noreply.github.com> Date: Thu, 14 Mar 2024 11:31:18 +0900 Subject: [PATCH 4/7] perf(occupancy_grid_map_outlier_filter): improve performance (#5980) * improve perfomance Signed-off-by: Taiga Takano * style(pre-commit): autofix Signed-off-by: Taiga Takano * fixed the bug and corrected the spelling mistake. Signed-off-by: Taiga Takano * style(pre-commit): autofix Signed-off-by: Taiga Takano * fix bug Signed-off-by: Taiga Takano * style(pre-commit): autofix Signed-off-by: Taiga Takano * change filter Signed-off-by: Taiga Takano --------- Signed-off-by: Taiga Takano Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yoshi Ri --- ...upancy_grid_map_outlier_filter_nodelet.cpp | 41 +++++++++++++++---- 1 file changed, 32 insertions(+), 9 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 691c13a6d4701..78f76d4f980c3 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -239,19 +239,42 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc) { - PclPointCloud tmp_behind_pc; - PclPointCloud tmp_front_pc; - for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"), y(*input_pc, "y"), - z(*input_pc, "z"); - x != x.end(); ++x, ++y, ++z) { + size_t front_count = 0; + size_t behind_count = 0; + + for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"); x != x.end(); ++x) { if (*x < 0.0) { - tmp_behind_pc.push_back(pcl::PointXYZ(*x, *y, *z)); + behind_count++; } else { - tmp_front_pc.push_back(pcl::PointXYZ(*x, *y, *z)); + front_count++; } } - pcl::toROSMsg(tmp_front_pc, front_pc); - pcl::toROSMsg(tmp_behind_pc, behind_pc); + + sensor_msgs::PointCloud2Modifier front_pc_modifier(front_pc); + sensor_msgs::PointCloud2Modifier behind_pc_modifier(behind_pc); + front_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); + behind_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); + front_pc_modifier.resize(front_count); + behind_pc_modifier.resize(behind_count); + + sensor_msgs::PointCloud2Iterator fr_iter(front_pc, "x"); + sensor_msgs::PointCloud2Iterator be_iter(behind_pc, "x"); + + for (sensor_msgs::PointCloud2ConstIterator in_iter(*input_pc, "x"); + in_iter != in_iter.end(); ++in_iter) { + if (*in_iter < 0.0) { + *be_iter = in_iter[0]; + be_iter[1] = in_iter[1]; + be_iter[2] = in_iter[2]; + ++be_iter; + } else { + *fr_iter = in_iter[0]; + fr_iter[1] = in_iter[1]; + fr_iter[2] = in_iter[2]; + ++fr_iter; + } + } + front_pc.header = input_pc->header; behind_pc.header = input_pc->header; } From 33747c9f865b77194a69fac768df40cf4782d5cf Mon Sep 17 00:00:00 2001 From: j4tfwm6z Date: Tue, 9 Apr 2024 19:25:12 +0900 Subject: [PATCH 5/7] fix: reversed topic condition Signed-off-by: j4tfwm6z --- .../launch/pointcloud_based_occupancy_grid_map.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index faa0486bd52ee..cf1ca758f3945 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -102,13 +102,13 @@ def launch_setup(context, *args, **kwargs): ( "~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud") - if downsample_input_pointcloud + if not downsample_input_pointcloud else "/perception/obstacle_segmentation/downsample/pointcloud", ), ( "~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud") - if downsample_input_pointcloud + if not downsample_input_pointcloud else "/sensing/lidar/concatenated/downsample/pointcloud", ), ("~/output/occupancy_grid_map", LaunchConfiguration("output")), From b812b3cfaf0f8a0813dacb6f679f333324020345 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Tue, 23 Apr 2024 00:36:14 +0900 Subject: [PATCH 6/7] feat: add raw pointcloud downsampler Signed-off-by: yoshiri --- ...ti_lidar_pointcloud_based_occupancy_grid_map.launch.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py index 8f61ae836fb4c..efe53b487ea6a 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py @@ -135,6 +135,14 @@ def get_downsample_preprocess_nodes(voxel_size: float, raw_pointcloud_topics: li "voxel_size": voxel_size, } nodes.append(get_downsample_filter_node(obstacle_settings)) + raw_settings = { + "plugin": "pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + "node_name": "obstacle_pc_downsample_filter", + "input_topic": LaunchConfiguration("input/obstacle_pointcloud"), + "output_topic": "/perception/obstacle_segmentation/downsample/pointcloud", + "voxel_size": voxel_size, + } + nodes.append(get_downsample_filter_node(raw_settings)) return nodes From c098795f860bdda44928dde93aedf43e973d62f6 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Sat, 27 Apr 2024 19:06:18 +0900 Subject: [PATCH 7/7] fix: add dependency to pointcloud preprocessor Signed-off-by: yoshiri --- perception/probabilistic_occupancy_grid_map/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index a90ae78b03597..3efed07535504 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -34,6 +34,7 @@ tier4_autoware_utils visualization_msgs + pointcloud_preprocessor pointcloud_to_laserscan ament_cmake_gtest @@ -44,6 +45,7 @@ launch_testing launch_testing_ament_cmake launch_testing_ros + pointcloud_preprocessor ament_cmake