diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/euclidean_cluster/CMakeLists.txt new file mode 100644 index 0000000000000..d3e2f3b07d7dd --- /dev/null +++ b/perception/euclidean_cluster/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.5) +project(euclidean_cluster) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +find_package(PCL REQUIRED QUIET COMPONENTS common search filters segmentation) +ament_auto_find_build_dependencies() + + +include_directories( + include + ${PCL_COMMON_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +ament_auto_add_library(cluster_lib SHARED + lib/utils.cpp + lib/euclidean_cluster.cpp + lib/voxel_grid_based_euclidean_cluster.cpp +) + +target_link_libraries(cluster_lib + ${PCL_LIBRARIES} +) + +target_include_directories(cluster_lib + PUBLIC + $ + $ +) + +ament_auto_add_library(euclidean_cluster_node_core SHARED + src/euclidean_cluster_node.cpp +) +target_link_libraries(euclidean_cluster_node_core + ${PCL_LIBRARIES} + cluster_lib +) + +rclcpp_components_register_node(euclidean_cluster_node_core + PLUGIN "euclidean_cluster::EuclideanClusterNode" + EXECUTABLE euclidean_cluster_node +) + +ament_auto_add_library(voxel_grid_based_euclidean_cluster_node_core SHARED + src/voxel_grid_based_euclidean_cluster_node.cpp +) +target_link_libraries(voxel_grid_based_euclidean_cluster_node_core + ${PCL_LIBRARIES} + cluster_lib +) + +rclcpp_components_register_node(voxel_grid_based_euclidean_cluster_node_core + PLUGIN "euclidean_cluster::VoxelGridBasedEuclideanClusterNode" + EXECUTABLE voxel_grid_based_euclidean_cluster_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/perception/euclidean_cluster/README.md b/perception/euclidean_cluster/README.md new file mode 100644 index 0000000000000..1d3401ecdaeb9 --- /dev/null +++ b/perception/euclidean_cluster/README.md @@ -0,0 +1,102 @@ +# euclidean_cluster + +## Purpose + +euclidean_cluster is a package for clustering points into smaller parts to classify objects. + +This package has two clustering methods: `euclidean_cluster` and `voxel_grid_based_euclidean_cluster`. + +## Inner-workings / Algorithms + +### euclidean_cluster + +`pcl::EuclideanClusterExtraction` is applied to points. See [official document](https://pcl.readthedocs.io/en/latest/cluster_extraction.html) for details. + +### voxel_grid_based_euclidean_cluster + +1. A centroid in each voxel is calculated by `pcl::VoxelGrid`. +2. The centroids are clustered by `pcl::EuclideanClusterExtraction`. +3. The input points are clustered based on the clustered centroids. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------- | ------------------------------- | ---------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | + +### Output + +| Name | Type | Description | +| ---------------- | ----------------------------------------------------------- | -------------------------------------------- | +| `output` | `autoware_perception_msgs::msg::DetectedObjectsWithFeature` | cluster pointcloud | +| `debug/clusters` | `sensor_msgs::msg::PointCloud2` | colored cluster pointcloud for visualization | + +## Parameters + +### Core Parameters + +#### euclidean_cluster + +| Name | Type | Description | +| ------------------ | ----- | -------------------------------------------------------------------------------------------- | +| `use_height` | bool | use point.z for clustering | +| `min_cluster_size` | int | the minimum number of points that a cluster needs to contain in order to be considered valid | +| `max_cluster_size` | int | the maximum number of points that a cluster needs to contain in order to be considered valid | +| `tolerance` | float | the spatial cluster tolerance as a measure in the L2 Euclidean space | + +#### voxel_grid_based_euclidean_cluster + +| Name | Type | Description | +| ----------------------------- | ----- | -------------------------------------------------------------------------------------------- | +| `use_height` | bool | use point.z for clustering | +| `min_cluster_size` | int | the minimum number of points that a cluster needs to contain in order to be considered valid | +| `max_cluster_size` | int | the maximum number of points that a cluster needs to contain in order to be considered valid | +| `tolerance` | float | the spatial cluster tolerance as a measure in the L2 Euclidean space | +| `voxel_leaf_size` | float | the voxel leaf size of x and y | +| `min_points_number_per_voxel` | int | the minimum number of points for a voxel | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + +The `use_height` option of `voxel_grid_based_euclidean_cluster` isn't implemented yet. diff --git a/perception/euclidean_cluster/config/compare_map.param.yaml b/perception/euclidean_cluster/config/compare_map.param.yaml new file mode 100644 index 0000000000000..7d000c527b29f --- /dev/null +++ b/perception/euclidean_cluster/config/compare_map.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + distance_threshold: 0.5 diff --git a/perception/euclidean_cluster/config/euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/euclidean_cluster.param.yaml new file mode 100644 index 0000000000000..1411f766b42d4 --- /dev/null +++ b/perception/euclidean_cluster/config/euclidean_cluster.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + max_cluster_size: 1000 + min_cluster_size: 10 + tolerance: 0.7 + use_height: false diff --git a/perception/euclidean_cluster/config/outlier.param.yaml b/perception/euclidean_cluster/config/outlier.param.yaml new file mode 100644 index 0000000000000..1962fba1f332a --- /dev/null +++ b/perception/euclidean_cluster/config/outlier.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + input_frame: base_link + output_frame: base_link + voxel_size_x: 0.3 + voxel_size_y: 0.3 + voxel_size_z: 100.0 + voxel_points_threshold: 3 diff --git a/perception/euclidean_cluster/config/voxel_grid.param.yaml b/perception/euclidean_cluster/config/voxel_grid.param.yaml new file mode 100644 index 0000000000000..3ff32bfbb7146 --- /dev/null +++ b/perception/euclidean_cluster/config/voxel_grid.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + input_frame: base_link + output_frame: base_link + voxel_size_x: 0.15 + voxel_size_y: 0.15 + voxel_size_z: 0.15 diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml new file mode 100644 index 0000000000000..27ba5a8178fcf --- /dev/null +++ b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + tolerance: 0.7 + voxel_leaf_size: 0.3 + min_points_number_per_voxel: 1 + min_cluster_size: 10 + max_cluster_size: 3000 + use_height: false diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp new file mode 100644 index 0000000000000..f1e275a919a8a --- /dev/null +++ b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp @@ -0,0 +1,41 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "euclidean_cluster/euclidean_cluster_interface.hpp" +#include "euclidean_cluster/utils.hpp" + +#include + +#include + +namespace euclidean_cluster +{ +class EuclideanCluster : public EuclideanClusterInterface +{ +public: + EuclideanCluster(); + EuclideanCluster(bool use_height, int min_cluster_size, int max_cluster_size); + EuclideanCluster(bool use_height, int min_cluster_size, int max_cluster_size, float tolerance); + bool cluster( + const pcl::PointCloud::ConstPtr & pointcloud, + std::vector> & clusters) override; + void setTolerance(float tolerance) { tolerance_ = tolerance; } + +private: + float tolerance_; +}; + +} // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp new file mode 100644 index 0000000000000..70c21daabcd66 --- /dev/null +++ b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp @@ -0,0 +1,50 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include + +#include + +namespace euclidean_cluster +{ +class EuclideanClusterInterface +{ +public: + EuclideanClusterInterface() = default; + EuclideanClusterInterface(bool use_height, int min_cluster_size, int max_cluster_size) + : use_height_(use_height), + min_cluster_size_(min_cluster_size), + max_cluster_size_(max_cluster_size) + { + } + virtual ~EuclideanClusterInterface() = default; + void setUseHeight(bool use_height) { use_height_ = use_height; } + void setMinClusterSize(int size) { min_cluster_size_ = size; } + void setMaxClusterSize(int size) { max_cluster_size_ = size; } + virtual bool cluster( + const pcl::PointCloud::ConstPtr & pointcloud, + std::vector> & clusters) = 0; + +protected: + bool use_height_ = true; + int min_cluster_size_; + int max_cluster_size_; +}; + +} // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp b/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp new file mode 100644 index 0000000000000..a1a15013f7603 --- /dev/null +++ b/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include + +namespace euclidean_cluster +{ +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); +void convertPointCloudClusters2Msg( + const std_msgs::msg::Header & header, + const std::vector> & clusters, + autoware_perception_msgs::msg::DetectedObjectsWithFeature & msg); +void convertObjectMsg2SensorMsg( + const autoware_perception_msgs::msg::DetectedObjectsWithFeature & input, + sensor_msgs::msg::PointCloud2 & output); +} // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp new file mode 100644 index 0000000000000..d6a07503c5ca5 --- /dev/null +++ b/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "euclidean_cluster/euclidean_cluster_interface.hpp" +#include "euclidean_cluster/utils.hpp" + +#include +#include + +#include + +namespace euclidean_cluster +{ +class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface +{ +public: + VoxelGridBasedEuclideanCluster(); + VoxelGridBasedEuclideanCluster(bool use_height, int min_cluster_size, int max_cluster_size); + VoxelGridBasedEuclideanCluster( + bool use_height, int min_cluster_size, int max_cluster_size, float tolerance, + float voxel_leaf_size, int min_points_number_per_voxel); + bool cluster( + const pcl::PointCloud::ConstPtr & pointcloud, + std::vector> & clusters) override; + void setVoxelLeafSize(float voxel_leaf_size) { voxel_leaf_size_ = voxel_leaf_size; } + void setTolerance(float tolerance) { tolerance_ = tolerance; } + void setMinPointsNumberPerVoxel(int min_points_number_per_voxel) + { + min_points_number_per_voxel_ = min_points_number_per_voxel; + } + +private: + pcl::VoxelGrid voxel_grid_; + float tolerance_; + float voxel_leaf_size_; + int min_points_number_per_voxel_; +}; + +} // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py new file mode 100644 index 0000000000000..4e34538127f13 --- /dev/null +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -0,0 +1,131 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import AnonName +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + # https://github.com/ros2/launch_ros/issues/156 + def load_composable_node_param(param_path): + with open(LaunchConfiguration(param_path).perform(context), "r") as f: + return yaml.safe_load(f)["/**"]["ros__parameters"] + + ns = "euclidean_cluster" + pkg = "euclidean_cluster" + + # set voxel grid filter as a component + voxel_grid_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + name=AnonName("voxel_grid_filter"), + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("output", "voxel_grid_filtered/pointcloud"), + ], + parameters=[load_composable_node_param("voxel_grid_param_path")], + ) + + # set compare map filter as a component + compare_map_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::VoxelBasedCompareMapFilterComponent", + name=AnonName("compare_map_filter"), + remappings=[ + ("input", "voxel_grid_filtered/pointcloud"), + ("map", LaunchConfiguration("input_map")), + ("output", "compare_map_filtered/pointcloud"), + ], + ) + + use_map_euclidean_cluster_component = ComposableNode( + package=pkg, + plugin="euclidean_cluster::EuclideanClusterNode", + name=AnonName("euclidean_cluster"), + remappings=[ + ("input", "compare_map_filtered/pointcloud"), + ("output", LaunchConfiguration("output_clusters")), + ], + parameters=[load_composable_node_param("euclidean_param_path")], + ) + + disuse_map_euclidean_cluster_component = ComposableNode( + package=pkg, + plugin="euclidean_cluster::EuclideanClusterNode", + name=AnonName("euclidean_cluster"), + remappings=[ + ("input", "voxel_grid_filtered/pointcloud"), + ("output", LaunchConfiguration("output_clusters")), + ], + parameters=[load_composable_node_param("euclidean_param_path")], + ) + + container = ComposableNodeContainer( + name="euclidean_cluster_container", + namespace=ns, + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[voxel_grid_filter_component], + output="screen", + ) + + use_map_loader = LoadComposableNodes( + composable_node_descriptions=[ + compare_map_filter_component, + use_map_euclidean_cluster_component, + ], + target_container=container, + condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), + ) + + disuse_map_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_map_euclidean_cluster_component], + target_container=container, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), + ) + + return [container, use_map_loader, disuse_map_loader] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + return launch.LaunchDescription( + [ + add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), + add_launch_arg("input_map", "/map/pointcloud_map"), + add_launch_arg("output_clusters", "clusters"), + add_launch_arg("use_pointcloud_map", "false"), + add_launch_arg( + "voxel_grid_param_path", + [FindPackageShare("euclidean_cluster"), "/config/voxel_grid.param.yaml"], + ), + add_launch_arg( + "euclidean_param_path", + [FindPackageShare("euclidean_cluster"), "/config/euclidean_cluster.param.yaml"], + ), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml new file mode 100644 index 0000000000000..0b34f181d023b --- /dev/null +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py new file mode 100644 index 0000000000000..8d36f818cf133 --- /dev/null +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -0,0 +1,154 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import AnonName +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + # https://github.com/ros2/launch_ros/issues/156 + def load_composable_node_param(param_path): + with open(LaunchConfiguration(param_path).perform(context), "r") as f: + return yaml.safe_load(f)["/**"]["ros__parameters"] + + ns = "clustering" + pkg = "euclidean_cluster" + + # set compare map filter as a component + compare_map_filter_component = ComposableNode( + package="compare_map_segmentation", + namespace=ns, + plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", + name=AnonName("compare_map_filter"), + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("map", LaunchConfiguration("input_map")), + ("output", "map_filter/pointcloud"), + ], + parameters=[load_composable_node_param("compare_map_param_path")], + ) + + # set voxel grid filter as a component + use_map_voxel_grid_filter_component = ComposableNode( + package="pointcloud_preprocessor", + namespace=ns, + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + name=AnonName("voxel_grid_filter"), + remappings=[("input", "map_filter/pointcloud"), ("output", "downsampled/pointcloud")], + parameters=[load_composable_node_param("voxel_grid_param_path")], + ) + disuse_map_voxel_grid_filter_component = ComposableNode( + package="pointcloud_preprocessor", + namespace=ns, + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + name=AnonName("voxel_grid_filter"), + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("output", "downsampled/pointcloud"), + ], + parameters=[load_composable_node_param("voxel_grid_param_path")], + ) + + # set outlier filter as a component + outlier_filter_component = ComposableNode( + package="pointcloud_preprocessor", + namespace=ns, + plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", + name="outlier_filter", + remappings=[("input", "downsampled/pointcloud"), ("output", "outlier_filter/pointcloud")], + parameters=[load_composable_node_param("outlier_param_path")], + ) + + # set euclidean cluster as a component + euclidean_cluster_component = ComposableNode( + package=pkg, + namespace=ns, + plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + name="euclidean_cluster", + remappings=[ + ("input", "outlier_filter/pointcloud"), + ("output", LaunchConfiguration("output_clusters")), + ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], + ) + + container = ComposableNodeContainer( + name="euclidean_cluster_container", + package="rclcpp_components", + namespace=ns, + executable="component_container", + composable_node_descriptions=[outlier_filter_component, euclidean_cluster_component], + output="screen", + ) + + use_map_loader = LoadComposableNodes( + composable_node_descriptions=[ + compare_map_filter_component, + use_map_voxel_grid_filter_component, + ], + target_container=container, + condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), + ) + + disuse_map_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_map_voxel_grid_filter_component], + target_container=container, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), + ) + + return [container, use_map_loader, disuse_map_loader] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + return launch.LaunchDescription( + [ + add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), + add_launch_arg("input_map", "/map/pointcloud_map"), + add_launch_arg("output_clusters", "clusters"), + add_launch_arg("use_pointcloud_map", "false"), + add_launch_arg( + "voxel_grid_param_path", + [FindPackageShare("euclidean_cluster"), "/config/voxel_grid.param.yaml"], + ), + add_launch_arg( + "outlier_param_path", + [FindPackageShare("euclidean_cluster"), "/config/outlier.param.yaml"], + ), + add_launch_arg( + "compare_map_param_path", + [FindPackageShare("euclidean_cluster"), "/config/compare_map.param.yaml"], + ), + add_launch_arg( + "voxel_grid_based_euclidean_param_path", + [ + FindPackageShare("euclidean_cluster"), + "/config/voxel_grid_based_euclidean_cluster.param.yaml", + ], + ), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml new file mode 100644 index 0000000000000..0deeb08127d02 --- /dev/null +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/euclidean_cluster/lib/euclidean_cluster.cpp new file mode 100644 index 0000000000000..425cac67aa0e2 --- /dev/null +++ b/perception/euclidean_cluster/lib/euclidean_cluster.cpp @@ -0,0 +1,85 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "euclidean_cluster/euclidean_cluster.hpp" + +#include +#include + +namespace euclidean_cluster +{ +EuclideanCluster::EuclideanCluster() {} + +EuclideanCluster::EuclideanCluster(bool use_height, int min_cluster_size, int max_cluster_size) +: EuclideanClusterInterface(use_height, min_cluster_size, max_cluster_size) +{ +} + +EuclideanCluster::EuclideanCluster( + bool use_height, int min_cluster_size, int max_cluster_size, float tolerance) +: EuclideanClusterInterface(use_height, min_cluster_size, max_cluster_size), tolerance_(tolerance) +{ +} + +bool EuclideanCluster::cluster( + const pcl::PointCloud::ConstPtr & pointcloud, + std::vector> & clusters) +{ + // convert 2d pointcloud + pcl::PointCloud::ConstPtr pointcloud_ptr(new pcl::PointCloud); + if (!use_height_) { + pcl::PointCloud::Ptr pointcloud_2d_ptr(new pcl::PointCloud); + for (const auto & point : pointcloud->points) { + pcl::PointXYZ point2d; + point2d.x = point.x; + point2d.y = point.y; + point2d.z = 0.0; + pointcloud_2d_ptr->push_back(point2d); + } + pointcloud_ptr = pointcloud_2d_ptr; + } else { + pointcloud_ptr = pointcloud; + } + + // create tree + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(pointcloud_ptr); + + // clustering + std::vector cluster_indices; + pcl::EuclideanClusterExtraction pcl_euclidean_cluster; + pcl_euclidean_cluster.setClusterTolerance(tolerance_); + pcl_euclidean_cluster.setMinClusterSize(min_cluster_size_); + pcl_euclidean_cluster.setMaxClusterSize(max_cluster_size_); + pcl_euclidean_cluster.setSearchMethod(tree); + pcl_euclidean_cluster.setInputCloud(pointcloud_ptr); + pcl_euclidean_cluster.extract(cluster_indices); + + // build output + { + for (const auto & cluster : cluster_indices) { + pcl::PointCloud::Ptr cloud_cluster(new pcl::PointCloud); + for (const auto & point_idx : cluster.indices) { + cloud_cluster->points.push_back(pointcloud->points[point_idx]); + } + clusters.push_back(*cloud_cluster); + clusters.back().width = cloud_cluster->points.size(); + clusters.back().height = 1; + clusters.back().is_dense = false; + } + } + return true; +} + +} // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/euclidean_cluster/lib/utils.cpp new file mode 100644 index 0000000000000..f0f205853b848 --- /dev/null +++ b/perception/euclidean_cluster/lib/utils.cpp @@ -0,0 +1,112 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "euclidean_cluster/utils.hpp" + +#include +#include +#include +#include +#include + +namespace euclidean_cluster +{ +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +{ + geometry_msgs::msg::Point centroid; + centroid.x = 0.0f; + centroid.y = 0.0f; + centroid.z = 0.0f; + for (sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"), + iter_y(pointcloud, "y"), iter_z(pointcloud, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + centroid.x += *iter_x; + centroid.y += *iter_y; + centroid.z += *iter_z; + } + const size_t size = pointcloud.width * pointcloud.height; + centroid.x = centroid.x / static_cast(size); + centroid.y = centroid.y / static_cast(size); + centroid.z = centroid.z / static_cast(size); + return centroid; +} + +void convertPointCloudClusters2Msg( + const std_msgs::msg::Header & header, + const std::vector> & clusters, + autoware_perception_msgs::msg::DetectedObjectsWithFeature & msg) +{ + msg.header = header; + for (const auto & cluster : clusters) { + sensor_msgs::msg::PointCloud2 ros_pointcloud; + autoware_perception_msgs::msg::DetectedObjectWithFeature feature_object; + pcl::toROSMsg(cluster, ros_pointcloud); + ros_pointcloud.header = header; + feature_object.feature.cluster = ros_pointcloud; + feature_object.object.kinematics.pose_with_covariance.pose.position = + getCentroid(ros_pointcloud); + autoware_auto_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + classification.probability = 1.0f; + feature_object.object.classification.emplace_back(classification); + msg.feature_objects.push_back(feature_object); + } +} +void convertObjectMsg2SensorMsg( + const autoware_perception_msgs::msg::DetectedObjectsWithFeature & input, + sensor_msgs::msg::PointCloud2 & output) +{ + output.header = input.header; + + size_t pointcloud_size = 0; + for (const auto & feature_object : input.feature_objects) { + pointcloud_size += feature_object.feature.cluster.width * feature_object.feature.cluster.height; + } + + sensor_msgs::PointCloud2Modifier modifier(output); + modifier.setPointCloud2Fields( + 4, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, "rgb", 1, sensor_msgs::msg::PointField::FLOAT32); + modifier.resize(pointcloud_size); + + sensor_msgs::PointCloud2Iterator iter_out_x(output, "x"); + sensor_msgs::PointCloud2Iterator iter_out_y(output, "y"); + sensor_msgs::PointCloud2Iterator iter_out_z(output, "z"); + sensor_msgs::PointCloud2Iterator iter_out_r(output, "r"); + sensor_msgs::PointCloud2Iterator iter_out_g(output, "g"); + sensor_msgs::PointCloud2Iterator iter_out_b(output, "b"); + + constexpr uint8_t color_data[] = {200, 0, 0, 0, 200, 0, 0, 0, 200, + 200, 200, 0, 200, 0, 200, 0, 200, 200}; // 6 pattern + for (size_t i = 0; i < input.feature_objects.size(); ++i) { + const auto & feature_object = input.feature_objects.at(i); + sensor_msgs::PointCloud2ConstIterator iter_in_x(feature_object.feature.cluster, "x"); + sensor_msgs::PointCloud2ConstIterator iter_in_y(feature_object.feature.cluster, "y"); + sensor_msgs::PointCloud2ConstIterator iter_in_z(feature_object.feature.cluster, "z"); + for (; iter_in_x != iter_in_x.end(); ++iter_in_x, ++iter_in_y, ++iter_in_z, ++iter_out_x, + ++iter_out_y, ++iter_out_z, ++iter_out_r, ++iter_out_g, + ++iter_out_b) { + *iter_out_x = *iter_in_x; + *iter_out_y = *iter_in_y; + *iter_out_z = *iter_in_z; + *iter_out_r = color_data[3 * (i % 6) + 0]; + *iter_out_g = color_data[3 * (i % 6) + 1]; + *iter_out_b = color_data[3 * (i % 6) + 2]; + } + } + + output.width = pointcloud_size; + output.height = 1; + output.is_dense = false; +} +} // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp new file mode 100644 index 0000000000000..5f747b332936e --- /dev/null +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -0,0 +1,117 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" + +#include +#include + +#include + +namespace euclidean_cluster +{ +VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster() {} + +VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster( + bool use_height, int min_cluster_size, int max_cluster_size) +: EuclideanClusterInterface(use_height, min_cluster_size, max_cluster_size) +{ +} + +VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster( + bool use_height, int min_cluster_size, int max_cluster_size, float tolerance, + float voxel_leaf_size, int min_points_number_per_voxel) +: EuclideanClusterInterface(use_height, min_cluster_size, max_cluster_size), + tolerance_(tolerance), + voxel_leaf_size_(voxel_leaf_size), + min_points_number_per_voxel_(min_points_number_per_voxel) +{ +} + +bool VoxelGridBasedEuclideanCluster::cluster( + const pcl::PointCloud::ConstPtr & pointcloud, + std::vector> & clusters) +{ + // TODO(Saito) implement use_height is false version + + // create voxel + pcl::PointCloud::Ptr voxel_map_ptr(new pcl::PointCloud); + voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, 100000.0); + voxel_grid_.setMinimumPointsNumberPerVoxel(min_points_number_per_voxel_); + voxel_grid_.setInputCloud(pointcloud); + voxel_grid_.setSaveLeafLayout(true); + voxel_grid_.filter(*voxel_map_ptr); + + // voxel is pressed 2d + pcl::PointCloud::Ptr pointcloud_2d_ptr(new pcl::PointCloud); + for (const auto & point : voxel_map_ptr->points) { + pcl::PointXYZ point2d; + point2d.x = point.x; + point2d.y = point.y; + point2d.z = 0.0; + pointcloud_2d_ptr->push_back(point2d); + } + + // create tree + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(pointcloud_2d_ptr); + + // clustering + std::vector cluster_indices; + pcl::EuclideanClusterExtraction pcl_euclidean_cluster; + pcl_euclidean_cluster.setClusterTolerance(tolerance_); + pcl_euclidean_cluster.setMinClusterSize(1); + pcl_euclidean_cluster.setMaxClusterSize(max_cluster_size_); + pcl_euclidean_cluster.setSearchMethod(tree); + pcl_euclidean_cluster.setInputCloud(pointcloud_2d_ptr); + pcl_euclidean_cluster.extract(cluster_indices); + + // create map to search cluster index from voxel grid index + std::unordered_map map; + for (size_t cluster_idx = 0; cluster_idx < cluster_indices.size(); ++cluster_idx) { + const auto & cluster = cluster_indices.at(cluster_idx); + for (const auto & point_idx : cluster.indices) { + map[point_idx] = cluster_idx; + } + } + + // create vector of point cloud cluster. vector index is voxel grid index. + std::vector> temporary_clusters; // no check about cluster size + temporary_clusters.resize(cluster_indices.size()); + for (const auto & point : pointcloud->points) { + const int index = + voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z)); + if (map.find(index) != map.end()) { + temporary_clusters.at(map[index]).points.push_back(point); + } + } + + // build output and check cluster size + { + for (const auto & cluster : temporary_clusters) { + if (!(min_cluster_size_ <= static_cast(cluster.points.size()) && + static_cast(cluster.points.size()) <= max_cluster_size_)) { + continue; + } + clusters.push_back(cluster); + clusters.back().width = cluster.points.size(); + clusters.back().height = 1; + clusters.back().is_dense = false; + } + } + + return true; +} + +} // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml new file mode 100644 index 0000000000000..e04bce6e83221 --- /dev/null +++ b/perception/euclidean_cluster/package.xml @@ -0,0 +1,27 @@ + + + euclidean_cluster + 0.1.0 + The euclidean_cluster package + + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_perception_msgs + autoware_perception_msgs + compare_map_segmentation + geometry_msgs + libpcl-all-dev + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + + autoware_lint_common + + + ament_cmake + + diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp new file mode 100644 index 0000000000000..075f545dc3f90 --- /dev/null +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.cpp @@ -0,0 +1,74 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "euclidean_cluster_node.hpp" + +#include "euclidean_cluster/utils.hpp" + +#include + +namespace euclidean_cluster +{ +EuclideanClusterNode::EuclideanClusterNode(const rclcpp::NodeOptions & options) +: Node("euclidean_cluster_node", options) +{ + const bool use_height = this->declare_parameter("use_height", false); + const int min_cluster_size = this->declare_parameter("min_cluster_size", 3); + const int max_cluster_size = this->declare_parameter("max_cluster_size", 200); + const float tolerance = this->declare_parameter("tolerance", 1.0); + cluster_ = + std::make_shared(use_height, min_cluster_size, max_cluster_size, tolerance); + + using std::placeholders::_1; + pointcloud_sub_ = this->create_subscription( + "input", rclcpp::SensorDataQoS().keep_last(1), + std::bind(&EuclideanClusterNode::onPointCloud, this, _1)); + + cluster_pub_ = this->create_publisher( + "output", rclcpp::QoS{1}); + debug_pub_ = this->create_publisher("debug/clusters", 1); +} + +void EuclideanClusterNode::onPointCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) +{ + // convert ros to pcl + pcl::PointCloud::Ptr raw_pointcloud_ptr(new pcl::PointCloud); + pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); + + // clustering + std::vector> clusters; + cluster_->cluster(raw_pointcloud_ptr, clusters); + + // build output msg + autoware_perception_msgs::msg::DetectedObjectsWithFeature output; + convertPointCloudClusters2Msg(input_msg->header, clusters, output); + cluster_pub_->publish(output); + + // build debug msg + if (debug_pub_->get_subscription_count() < 1) { + return; + } + { + sensor_msgs::msg::PointCloud2 debug; + convertObjectMsg2SensorMsg(output, debug); + debug_pub_->publish(debug); + } +} + +} // namespace euclidean_cluster + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::EuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp new file mode 100644 index 0000000000000..c45c0022981ed --- /dev/null +++ b/perception/euclidean_cluster/src/euclidean_cluster_node.hpp @@ -0,0 +1,45 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "euclidean_cluster/euclidean_cluster.hpp" + +#include + +#include +#include +#include + +#include + +namespace euclidean_cluster +{ +class EuclideanClusterNode : public rclcpp::Node +{ +public: + explicit EuclideanClusterNode(const rclcpp::NodeOptions & options); + +private: + void onPointCloud(sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); + + rclcpp::Subscription::SharedPtr pointcloud_sub_; + rclcpp::Publisher::SharedPtr + cluster_pub_; + rclcpp::Publisher::SharedPtr debug_pub_; + + std::shared_ptr cluster_; +}; + +} // namespace euclidean_cluster diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp new file mode 100644 index 0000000000000..5c8f45fb79786 --- /dev/null +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -0,0 +1,78 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "voxel_grid_based_euclidean_cluster_node.hpp" + +#include "euclidean_cluster/utils.hpp" + +#include + +namespace euclidean_cluster +{ +VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode( + const rclcpp::NodeOptions & options) +: Node("voxel_grid_based_euclidean_cluster_node", options) +{ + const bool use_height = this->declare_parameter("use_height", false); + const int min_cluster_size = this->declare_parameter("min_cluster_size", 1); + const int max_cluster_size = this->declare_parameter("max_cluster_size", 500); + const float tolerance = this->declare_parameter("tolerance", 1.0); + const float voxel_leaf_size = this->declare_parameter("voxel_leaf_size", 0.5); + const int min_points_number_per_voxel = this->declare_parameter("min_points_number_per_voxel", 3); + cluster_ = std::make_shared( + use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size, + min_points_number_per_voxel); + + using std::placeholders::_1; + pointcloud_sub_ = this->create_subscription( + "input", rclcpp::SensorDataQoS().keep_last(1), + std::bind(&VoxelGridBasedEuclideanClusterNode::onPointCloud, this, _1)); + + cluster_pub_ = this->create_publisher( + "output", rclcpp::QoS{1}); + debug_pub_ = this->create_publisher("debug/clusters", 1); +} + +void VoxelGridBasedEuclideanClusterNode::onPointCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) +{ + // convert ros to pcl + pcl::PointCloud::Ptr raw_pointcloud_ptr(new pcl::PointCloud); + pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); + + // clustering + std::vector> clusters; + cluster_->cluster(raw_pointcloud_ptr, clusters); + + // build output msg + autoware_perception_msgs::msg::DetectedObjectsWithFeature output; + convertPointCloudClusters2Msg(input_msg->header, clusters, output); + cluster_pub_->publish(output); + + // build debug msg + if (debug_pub_->get_subscription_count() < 1) { + return; + } + { + sensor_msgs::msg::PointCloud2 debug; + convertObjectMsg2SensorMsg(output, debug); + debug_pub_->publish(debug); + } +} + +} // namespace euclidean_cluster + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(euclidean_cluster::VoxelGridBasedEuclideanClusterNode) diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp new file mode 100644 index 0000000000000..19043d9c188d3 --- /dev/null +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp @@ -0,0 +1,45 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" + +#include + +#include +#include +#include + +#include + +namespace euclidean_cluster +{ +class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node +{ +public: + explicit VoxelGridBasedEuclideanClusterNode(const rclcpp::NodeOptions & options); + +private: + void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); + + rclcpp::Subscription::SharedPtr pointcloud_sub_; + rclcpp::Publisher::SharedPtr + cluster_pub_; + rclcpp::Publisher::SharedPtr debug_pub_; + + std::shared_ptr cluster_; +}; + +} // namespace euclidean_cluster