feat: add object segmentation packages (autowarefoundation#87)

* 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

* feat: add each launch files

* fix: add other filter infomation to README

* ci(pre-commit): autofix

Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>

perception/compare_map_segmentation/CMakeLists.txt

@@ -0,0 +1,97 @@
+cmake_minimum_required(VERSION 3.5)
+project(compare_map_segmentation)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+# Ignore PCL errors in Clang
+if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wno-gnu-anonymous-struct -Wno-nested-anon-types)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED)
+find_package(PCL REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(OpenMP)
+ament_auto_find_build_dependencies()
+
+
+###########
+## Build ##
+###########
+
+include_directories(
+  include
+  ${Boost_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${GRID_MAP_INCLUDE_DIR}
+)
+
+ament_auto_add_library(compare_map_segmentation SHARED
+  src/distance_based_compare_map_filter_nodelet.cpp
+  src/voxel_based_approximate_compare_map_filter_nodelet.cpp
+  src/voxel_based_compare_map_filter_nodelet.cpp
+  src/voxel_distance_based_compare_map_filter_nodelet.cpp
+  src/compare_elevation_map_filter_node.cpp
+)
+
+target_link_libraries(compare_map_segmentation
+  ${Boost_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+  ${PCL_LIBRARIES}
+)
+
+if(OPENMP_FOUND)
+  set_target_properties(compare_map_segmentation PROPERTIES
+    COMPILE_FLAGS ${OpenMP_CXX_FLAGS}
+    LINK_FLAGS ${OpenMP_CXX_FLAGS}
+  )
+endif()
+
+# ========== Compare Map Filter ==========
+# -- Distance Based Compare Map Filter --
+rclcpp_components_register_node(compare_map_segmentation
+  PLUGIN "compare_map_segmentation::DistanceBasedCompareMapFilterComponent"
+  EXECUTABLE distance_based_compare_map_filter_node)
+
+# -- Voxel Based Approximate Compare Map Filter --
+rclcpp_components_register_node(compare_map_segmentation
+  PLUGIN "compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent"
+  EXECUTABLE voxel_based_approximate_compare_map_filter_node)
+
+# -- Voxel Based Compare Map Filter --
+rclcpp_components_register_node(compare_map_segmentation
+  PLUGIN "compare_map_segmentation::VoxelBasedCompareMapFilterComponent"
+  EXECUTABLE voxel_based_compare_map_filter_node)
+
+# -- Voxel Distance Based Compare Map Filter --
+rclcpp_components_register_node(compare_map_segmentation
+  PLUGIN "compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent"
+  EXECUTABLE voxel_distance_based_compare_map_filter_node)
+
+# -- Compare Elevation Map Filter --
+rclcpp_components_register_node(compare_map_segmentation
+  PLUGIN "compare_map_segmentation::CompareElevationMapFilterComponent"
+  EXECUTABLE compare_elevation_map_filter_node)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

perception/compare_map_segmentation/README.md

@@ -0,0 +1,83 @@
+# compare_map_segmentation
+
+## Purpose
+
+The `compare_map_segmentation` is a node that filters the ground points from the input pointcloud by using map info (e.g. pcd, elevation map).
+
+## Inner-workings / Algorithms
+
+### Compare Elevation Map Filter
+
+Compare the z of the input points with the value of elevation_map. The height difference is calculated by the binary integration of neighboring cells. Remove points whose height difference is below the `height_diff_thresh`.
+
+<p align="center">
+  <img src="./media/compare_elevation_map.png" width="1000">
+</p>
+
+### Distance Based Compare Map Filter
+
+WIP
+
+### Voxel Based Approximate Compare Map Filter
+
+WIP
+
+### Voxel Based Compare Map Filter
+
+WIP
+
+### Voxel Distance based Compare Map Filter
+
+WIP
+
+## Inputs / Outputs
+
+### Compare Elevation Map Filter
+
+#### Input
+
+| Name                    | Type                            | Description      |
+| ----------------------- | ------------------------------- | ---------------- |
+| `~/input/points`        | `sensor_msgs::msg::PointCloud2` | reference points |
+| `~/input/elevation_map` | `grid_map::msg::GridMap`        | elevation map    |
+
+#### Output
+
+| Name              | Type                            | Description     |
+| ----------------- | ------------------------------- | --------------- |
+| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points |
+
+### Other Filters
+
+#### Input
+
+| Name             | Type                            | Description      |
+| ---------------- | ------------------------------- | ---------------- |
+| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points |
+| `~/input/map`    | `grid_map::msg::GridMap`        | map              |
+
+#### Output
+
+| Name              | Type                            | Description     |
+| ----------------- | ------------------------------- | --------------- |
+| `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points |
+
+## Parameters
+
+### Core Parameters
+
+| Name                 | Type   | Description                                                                     | Default value |
+| :------------------- | :----- | :------------------------------------------------------------------------------ | :------------ |
+| `map_layer_name`     | string | elevation map layer name                                                        | elevation     |
+| `map_frame`          | float  | frame_id of the map that is temporarily used before elevation_map is subscribed | map           |
+| `height_diff_thresh` | float  | Remove points whose height difference is below this value [m]                   | 0.15          |
+
+## Assumptions / Known limits
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+## (Optional) Future extensions / Unimplemented parts

perception/compare_map_segmentation/include/compare_map_segmentation/compare_elevation_map_filter_node.hpp

@@ -0,0 +1,61 @@
+// Copyright 2021 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.
+
+#ifndef COMPARE_MAP_SEGMENTATION__COMPARE_ELEVATION_MAP_FILTER_NODE_HPP_
+#define COMPARE_MAP_SEGMENTATION__COMPARE_ELEVATION_MAP_FILTER_NODE_HPP_
+
+#include "pointcloud_preprocessor/filter.hpp"
+
+#include <grid_map_core/GridMap.hpp>
+#include <grid_map_cv/grid_map_cv.hpp>
+#include <grid_map_pcl/GridMapPclLoader.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <grid_map_msgs/msg/grid_map.hpp>
+
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+namespace compare_map_segmentation
+{
+class CompareElevationMapFilterComponent : public pointcloud_preprocessor::Filter
+{
+protected:
+  void filter(
+    const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
+
+private:
+  rclcpp::Subscription<grid_map_msgs::msg::GridMap>::SharedPtr sub_map_;
+
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_filtered_cloud_;
+  grid_map::GridMap elevation_map_;
+  cv::Mat elevation_image_;
+  grid_map::Matrix elevation_map_data_;
+  std::string layer_name_;
+  std::string map_frame_;
+  double height_diff_thresh_;
+
+  void setVerbosityLevelToDebugIfFlagSet();
+  void processPointcloud(grid_map::GridMapPclLoader * gridMapPclLoader);
+  void elevationMapCallback(const grid_map_msgs::msg::GridMap::ConstSharedPtr elevation_map);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  explicit CompareElevationMapFilterComponent(const rclcpp::NodeOptions & options);
+};
+}  // namespace compare_map_segmentation
+
+#endif  // COMPARE_MAP_SEGMENTATION__COMPARE_ELEVATION_MAP_FILTER_NODE_HPP_

perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp

@@ -0,0 +1,55 @@
+// 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.
+
+#ifndef COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_
+#define COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_
+
+#include "pointcloud_preprocessor/filter.hpp"
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/search/pcl_search.h>
+
+#include <vector>
+
+namespace compare_map_segmentation
+{
+class DistanceBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filter
+{
+protected:
+  virtual void filter(
+    const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
+
+  void input_target_callback(const PointCloud2ConstPtr map);
+
+private:
+  rclcpp::Subscription<PointCloud2>::SharedPtr sub_map_;
+  PointCloudConstPtr map_ptr_;
+  double distance_threshold_;
+  pcl::search::Search<pcl::PointXYZ>::Ptr tree_;
+
+  /** \brief Parameter service callback result : needed to be hold */
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
+  /** \brief Parameter service callback */
+  rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  explicit DistanceBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options);
+};
+}  // namespace compare_map_segmentation
+
+// clang-format off
+#endif  // COMPARE_MAP_SEGMENTATION__DISTANCE_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT
+// clang-format on

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp

@@ -0,0 +1,57 @@
+// 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.
+
+#ifndef COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_  // NOLINT
+#define COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_  // NOLINT
+
+#include "pointcloud_preprocessor/filter.hpp"
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/search/pcl_search.h>
+
+#include <vector>
+
+namespace compare_map_segmentation
+{
+class VoxelBasedApproximateCompareMapFilterComponent : public pointcloud_preprocessor::Filter
+{
+protected:
+  virtual void filter(
+    const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
+
+  void input_target_callback(const PointCloud2ConstPtr map);
+
+private:
+  // pcl::SegmentDifferences<pcl::PointXYZ> impl_;
+  rclcpp::Subscription<PointCloud2>::SharedPtr sub_map_;
+  PointCloudPtr voxel_map_ptr_;
+  double distance_threshold_;
+  pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_;
+  bool set_map_in_voxel_grid_;
+
+  /** \brief Parameter service callback result : needed to be hold */
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
+  /** \brief Parameter service callback */
+  rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  explicit VoxelBasedApproximateCompareMapFilterComponent(const rclcpp::NodeOptions & options);
+};
+}  // namespace compare_map_segmentation
+
+// clang-format off
+#endif  // COMPARE_MAP_SEGMENTATION__VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER_NODELET_HPP_ // NOLINT
+// clang-format on