feat: add euclidean cluster package (autowarefoundation#122)

* release v0.4.0 * remove ROS1 packages temporarily Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Revert "remove ROS1 packages temporarily" This reverts commit d6a59ac4c3762cb58ce6ca3e2cb31b3b8fc810ea. Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Rename launch files to launch.xml (autowarefoundation#28) * Rename h files to hpp (autowarefoundation#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143) * Use quotes for includes where appropriate (autowarefoundation#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Port euclidean cluster (autowarefoundation#120) * porting CmakeLists.txt and package.xml in progress * ported CMakeLists.txt and package.xml to ROS2 * Ported euclidean_cluster from ROS1 to ROS2 * deleted unnecesary files * fixed transient_local Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Fix perception launches (autowarefoundation#240) * fix roi_cluster_fusion launch Signed-off-by: Takamasa Horibe <takamasa.horib@gmail.com> * add comment on launch Signed-off-by: Takamasa Horibe <takamasa.horib@gmail.com> Co-authored-by: Takamasa Horibe <takamasa.horib@gmail.com> * Ros2 v0.8.0 euclidean cluster (autowarefoundation#310) * restore euclidean cluster files for v0.8.0 update * fix typos in perception (autowarefoundation#862) * Feature/camera lidar perception (autowarefoundation#937) * add object splitter Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * add object merger Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * change pkg name Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * cosmetic change Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * add comment Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * remove litter Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * bug fix : debug code Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * enable vehicle to unknown track Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * bug fix * add object position in clustering * 🚮 * change param * fix name * bug fix * add install * Feature/fusion debug (autowarefoundation#1051) * add debuger * change param * add publisher * Revert "restore euclidean cluster files for v0.8.0 update" This reverts commit 894cb8746622b4eb88f2cf1b036cc8c94ab1ac96. * fix bug * use containter to launch nodelet * add line * [euclidean_cluster]: Fix launch python and CMakeLists.txt Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> * add use_sim-time option (autowarefoundation#454) * Format launch files (autowarefoundation#1219) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Unify Apache-2.0 license name (autowarefoundation#1242) * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix lint errors (autowarefoundation#1378) * Fix lint errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix variable names Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Porting euclidean cluster (autowarefoundation#1291) * create config for euclidean cluster (autowarefoundation#1206) * create config * fix EOF * fix cmake list (autowarefoundation#1208) * Add load_composable_node_param Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Use tier4 voxel grid filter Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Refactor voxel grid based euclidean cluster launch Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix launch xml tag Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Add missing arguments Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix pointcloud subscriber qos Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix dependency in CMakeLists.txt Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * Invoke code formatter at pre-commit (autowarefoundation#1935) * Run ament_uncrustify at pre-commit * Reformat existing files * Fix copyright and cpplint errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Feature/clustering lib (autowarefoundation#1914) * change to lib for detection by tracking * refactor * apply format * Detection by tracker (autowarefoundation#1910) * initial commit * backup * apply format * cosmetic change * implement divided under segmenterd clusters * cosmetic change * bug fix * bug fix * bug fix * modify launch * add debug and bug fix * bug fix * bug fix * add no found tracked object * modify parameters and cmake * bug fix * remove debug info * add readme * modify clustering launch * run pre-commit * cosmetic change * cosmetic change * cosmetic change * apply markdownlint * modify launch * modify for cpplint * modify qos * change int to size_T * bug fix * change perception qos * Update perception/object_recognition/detection/detection_by_tracker/package.xml Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * cosmetic change * cosmetic change * fix launch * Update perception/object_recognition/detection/detection_by_tracker/src/utils.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * modify header include order * change include order * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * change to std::optional * cosmetic change * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * bug fix * modify readme Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * port euclidean_cluster (autowarefoundation#533) * delete COLCON_IGNORE * change dynamic_object_with_feature_array to detected_objects_with_feature * add classification * add classification.probability * add README of euclidean_cluster (autowarefoundation#614) * add README of euclidean_cluster * add period Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * update README Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * auto fix no ground pointcloud topic name (autowarefoundation#704) * fix/rename segmentation namespace (autowarefoundation#742) * rename segmentation directory * fix namespace: system stack * fix namespace: planning * fix namespace: control stack * fix namespace: perception stack * fix readme Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> Co-authored-by: Nikolai Morin <nnmmgit@gmail.com> Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Takamasa Horibe <takamasa.horib@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Takeshi Ishita <ishitah.takeshi@gmail.com> Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
badai-nguyen · Dec 6, 2021 · f5225e8 · f5225e8
1 parent 1d510e8
commit f5225e8
Show file tree

Hide file tree

Showing 23 changed files with 1,293 additions and 0 deletions.
diff --git a/perception/euclidean_cluster/CMakeLists.txt 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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+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
@@ -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
+
+<!-- Write assumptions and limitations of your implementation.
+
+Example:
+  This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
+  Also, this algorithm doesn't care about blind spots. In general, since too close obstacles aren't visible due to the sensing performance limit, please take enough margin to obstacles.
+-->
+
+## (Optional) Error detection and handling
+
+<!-- Write how to detect errors and how to recover from them.
+
+Example:
+  This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
+-->
+
+## (Optional) Performance characterization
+
+<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary.
+
+Example:
+  ### Complexity
+
+  This algorithm is O(N).
+
+  ### Processing time
+
+  ...
+-->
+
+## (Optional) References/External links
+
+<!-- Write links you referred to when you implemented.
+
+Example:
+  [1] {link_to_a_thesis}
+  [2] {link_to_an_issue}
+-->
+
+## (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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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 <pcl/point_types.h>
+
+#include <vector>
+
+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<pcl::PointXYZ>::ConstPtr & pointcloud,
+    std::vector<pcl::PointCloud<pcl::PointXYZ>> & 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
@@ -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 <rclcpp/rclcpp.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <vector>
+
+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<pcl::PointXYZ>::ConstPtr & pointcloud,
+    std::vector<pcl::PointCloud<pcl::PointXYZ>> & 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
@@ -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 <autoware_perception_msgs/msg/detected_objects_with_feature.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <vector>
+
+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<pcl::PointCloud<pcl::PointXYZ>> & 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/...eption/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/...eption/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 <pcl/filters/voxel_grid.h>
+#include <pcl/point_types.h>
+
+#include <vector>
+
+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<pcl::PointXYZ>::ConstPtr & pointcloud,
+    std::vector<pcl::PointCloud<pcl::PointXYZ>> & 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<pcl::PointXYZ> voxel_grid_;
+  float tolerance_;
+  float voxel_leaf_size_;
+  int min_points_number_per_voxel_;
+};
+
+}  // namespace euclidean_cluster