Merge branch 'main' into feat/sensing/pointcloud_preprocessor

src/perception/ground_segmentation/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.5)
+project(ground_segmentation)
+
+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)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+include_directories(include)
+
+ament_auto_add_library(ground_segmentation SHARED
+  src/ground_filter.cpp
+  src/ransac_ground_filter/ransac_ground_filter.cpp
+)
+
+rclcpp_components_register_node(ground_segmentation
+  PLUGIN "ground_segmentation::ransac_ground_filter::RANSACGroundFilter"
+  EXECUTABLE ransac_ground_filter_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  launch
+)

src/perception/ground_segmentation/config/ransac_ground_filter.param.yaml

@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    footprint_frame: base_link
+    leaf_size: 0.1
+    distance_threshold: 0.3
+    max_iterations: 1000
+    slope_threshold: 10.0

src/perception/ground_segmentation/include/ground_segmentation/ground_filter.hpp

@@ -0,0 +1,56 @@
+// Copyright 2024 Fool Stuck Engineers
+//
+// 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 GROUND_SEGMENTATION__GROUND_FILTER_HPP_
+#define GROUND_SEGMENTATION__GROUND_FILTER_HPP_
+
+#include <pcl_ros/transforms.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <string>
+
+namespace ground_segmentation
+{
+
+using PointCloud2 = sensor_msgs::msg::PointCloud2;
+
+class GroundFilter : public rclcpp::Node
+{
+public:
+  explicit GroundFilter(const std::string & node_name, const rclcpp::NodeOptions & options);
+
+protected:
+  virtual bool filter(const PointCloud2 & input, PointCloud2 & output) = 0;
+  [[nodiscard]] bool try_transform_pointcloud(
+    const std::string & target_frame, const PointCloud2 & input, PointCloud2 & output,
+    const double timeout = 1.0);
+
+private:
+  void input_callback(const PointCloud2::SharedPtr msg);
+
+  rclcpp::Subscription<PointCloud2>::SharedPtr input_sub_;
+  rclcpp::Publisher<PointCloud2>::SharedPtr output_pub_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+};
+}  // namespace ground_segmentation
+
+#endif  // GROUND_SEGMENTATION__GROUND_FILTER_HPP_

src/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter/ransac_ground_filter.hpp

@@ -0,0 +1,52 @@
+// Copyright 2024 Fool Stuck Engineers
+//
+// 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 GROUND_SEGMENTATION__RANSAC_GROUND_FILTER__RANSAC_GROUND_FILTER_HPP_
+#define GROUND_SEGMENTATION__RANSAC_GROUND_FILTER__RANSAC_GROUND_FILTER_HPP_
+
+#include "ground_segmentation/ground_filter.hpp"
+
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <string>
+
+namespace ground_segmentation::ransac_ground_filter
+{
+
+using PointType = pcl::PointXYZ;
+
+class RANSACGroundFilter : public GroundFilter
+{
+public:
+  explicit RANSACGroundFilter(const rclcpp::NodeOptions & options);
+
+protected:
+  bool filter(const PointCloud2 & input, PointCloud2 & output) override;
+
+private:
+  Eigen::Vector3d unit_z_vec_ = Eigen::Vector3d::UnitZ();
+
+  std::string footprint_frame_;
+  double leaf_size_;
+  double distance_threshold_;
+  int max_iterations_;
+  double slope_threshold_rad_;
+};
+
+}  // namespace ground_segmentation::ransac_ground_filter
+
+#endif  // GROUND_SEGMENTATION__RANSAC_GROUND_FILTER__RANSAC_GROUND_FILTER_HPP_

src/perception/ground_segmentation/launch/ground_segmentation.launch.xml

@@ -0,0 +1,14 @@
+<launch>
+  <arg name="method" default="ransac"/>
+
+  <arg name="input_points_topic" default="/velodyne_points"/>
+  <arg name="output_points_topic" default="/velodyne_points/obstacle"/>
+
+  <arg name="config_file" default="$(find-pkg-share ground_segmentation)/config/$(var method)_ground_filter.param.yaml"/>
+
+  <node pkg="ground_segmentation" name="$(var method)_ground_filter" exec="$(var method)_ground_filter_node" output="screen">
+    <remap from="~/input/points" to="$(var input_points_topic)"/>
+    <remap from="~/output/points" to="$(var output_points_topic)"/>
+    <param from="$(var config_file)"/>
+  </node>
+</launch>

src/perception/ground_segmentation/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ground_segmentation</name>
+  <version>0.0.0</version>
+  <description>The ground_segmentation package</description>
+  <maintainer email="[email protected]">Akiro Harada</maintainer>
+
+  <license>Apache 2</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>pcl_ros</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_ros</depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/perception/ground_segmentation/src/ground_filter.cpp

@@ -0,0 +1,57 @@
+// Copyright 2024 Fool Stuck Engineers
+//
+// 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 "ground_segmentation/ground_filter.hpp"
+
+namespace ground_segmentation
+{
+GroundFilter::GroundFilter(const std::string & node_name, const rclcpp::NodeOptions & options)
+: Node(node_name, options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_)
+{
+  input_sub_ = create_subscription<PointCloud2>(
+    "~/input/points", 1, std::bind(&GroundFilter::input_callback, this, std::placeholders::_1));
+  output_pub_ = create_publisher<PointCloud2>("~/output/points", 1);
+}
+
+bool GroundFilter::try_transform_pointcloud(
+  const std::string & target_frame, const PointCloud2 & input, PointCloud2 & output,
+  const double timeout)
+{
+  if (input.header.frame_id == target_frame) {
+    output = input;
+    return true;
+  }
+
+  geometry_msgs::msg::TransformStamped transform;
+  try {
+    transform = tf_buffer_.lookupTransform(
+      target_frame, input.header.frame_id, input.header.stamp, tf2::durationFromSec(timeout));
+  } catch (const tf2::TransformException & e) {
+    return false;
+  }
+
+  Eigen::Matrix4f transform_matrix = tf2::transformToEigen(transform).matrix().cast<float>();
+  pcl_ros::transformPointCloud(transform_matrix, input, output);
+  output.header.frame_id = target_frame;
+
+  return true;
+}
+
+void GroundFilter::input_callback(const PointCloud2::SharedPtr msg)
+{
+  PointCloud2 output;
+  if (!filter(*msg, output)) return;
+  output_pub_->publish(output);
+}
+}  // namespace ground_segmentation

src/perception/ground_segmentation/src/ransac_ground_filter/ransac_ground_filter.cpp

@@ -0,0 +1,114 @@
+// Copyright 2024 Fool Stuck Engineers
+//
+// 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 "ground_segmentation/ransac_ground_filter/ransac_ground_filter.hpp"
+
+namespace ground_segmentation::ransac_ground_filter
+{
+RANSACGroundFilter::RANSACGroundFilter(const rclcpp::NodeOptions & options)
+: GroundFilter("ransac_ground_filter", options)
+{
+  footprint_frame_ = declare_parameter<std::string>("footprint_frame", "base_link");
+  leaf_size_ = declare_parameter<double>("leaf_size", 0.1);
+  distance_threshold_ = declare_parameter<double>("distance_threshold", 0.1);
+  max_iterations_ = declare_parameter<int>("max_iterations", 1000);
+  slope_threshold_rad_ = declare_parameter<double>("slope_threshold", 10.0) * M_PI / 180.0;
+}
+
+bool RANSACGroundFilter::filter(const PointCloud2 & input, PointCloud2 & output)
+{
+  // Transform input to base_link
+  PointCloud2::SharedPtr input_transformed(new PointCloud2);
+  {
+    if (!try_transform_pointcloud(footprint_frame_, input, *input_transformed)) return false;
+  }
+
+  // Convert to PCL
+  pcl::PointCloud<PointType>::Ptr input_cloud(new pcl::PointCloud<PointType>);
+  {
+    pcl::fromROSMsg(*input_transformed, *input_cloud);
+  }
+
+  // Downsample the input cloud
+  pcl::PointCloud<PointType>::Ptr downsampled_cloud(new pcl::PointCloud<PointType>);
+  {
+    pcl::VoxelGrid<PointType> voxel_grid;
+    voxel_grid.setInputCloud(input_cloud);
+    voxel_grid.setLeafSize(leaf_size_, leaf_size_, leaf_size_);
+    voxel_grid.filter(*downsampled_cloud);
+  }
+
+  // Apply RANSAC
+  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
+  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
+  {
+    pcl::SACSegmentation<PointType> seg;
+    seg.setOptimizeCoefficients(true);
+    seg.setModelType(pcl::SACMODEL_PLANE);
+    seg.setMethodType(pcl::SAC_RANSAC);
+    seg.setMaxIterations(max_iterations_);
+    seg.setDistanceThreshold(distance_threshold_);
+
+    seg.setInputCloud(downsampled_cloud);
+    seg.segment(*inliers, *coefficients);
+  }
+
+  // Return input as output if no ground plane is found
+  if (coefficients->values.empty()) {
+    RCLCPP_WARN(get_logger(), "Failed to find a ground plane");
+    output = input;
+    return true;
+  }
+
+  // Filter tilted plane
+  {
+    Eigen::Vector3d normal(
+      coefficients->values[0], coefficients->values[1], coefficients->values[2]);
+    const auto slope_rad =
+      std::abs(std::acos(normal.dot(unit_z_vec_) / (normal.norm() * unit_z_vec_.norm())));
+    if (slope_rad > slope_threshold_rad_) {
+      output = input;
+      return true;
+    }
+  }
+
+  // Extract ground points and obstacle points
+  pcl::PointCloud<PointType>::Ptr ground_cloud(new pcl::PointCloud<PointType>);
+  pcl::PointCloud<PointType>::Ptr obstacle_cloud(new pcl::PointCloud<PointType>);
+  {
+    pcl::ExtractIndices<PointType> extract;
+    extract.setInputCloud(downsampled_cloud);
+    extract.setIndices(inliers);
+
+    extract.setNegative(false);
+    extract.filter(*ground_cloud);
+
+    extract.setNegative(true);
+    extract.filter(*obstacle_cloud);
+  }
+
+  // Convert obstacle cloud to ROS message
+  {
+    PointCloud2 obstacle_cloud_msg;
+    pcl::toROSMsg(*obstacle_cloud, obstacle_cloud_msg);
+    obstacle_cloud_msg.header = input_transformed->header;
+    output = obstacle_cloud_msg;
+  }
+
+  return true;
+}
+}  // namespace ground_segmentation::ransac_ground_filter
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(ground_segmentation::ransac_ground_filter::RANSACGroundFilter)