feat: create velodyne cloud separator (#47)

* create files and define PointXYZIR for velodyne_points Signed-off-by: Autumn60 <[email protected]> * fix member scope Signed-off-by: Autumn60 <[email protected]> * implement separate function Signed-off-by: Autumn60 <[email protected]> * chore: Initialize parameters and perform initialization in VelodyneCloudSeparator constructor Signed-off-by: Autumn60 <[email protected]> * create launch and config Signed-off-by: Autumn60 <[email protected]> --------- Signed-off-by: Autumn60 <[email protected]>
Fool-Stuck · Jul 14, 2024 · f31404d · f31404d
1 parent 96b6ccb
commit f31404d
Show file tree

Hide file tree

Showing 7 changed files with 381 additions and 0 deletions.
diff --git a/src/perception/velodyne_cloud_separator/CMakeLists.txt b/src/perception/velodyne_cloud_separator/CMakeLists.txt
@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(velodyne_cloud_separator)
+
+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()
+
+ament_auto_add_library(velodyne_cloud_separator SHARED
+  src/velodyne_cloud_separator.cpp
+)
+
+rclcpp_components_register_node(velodyne_cloud_separator
+  PLUGIN "velodyne_cloud_separator::VelodyneCloudSeparator"
+  EXECUTABLE velodyne_cloud_separator_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
+)
diff --git a/src/perception/velodyne_cloud_separator/config/velodyne_cloud_separator.param.yaml b/src/perception/velodyne_cloud_separator/config/velodyne_cloud_separator.param.yaml
@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    update_rate_hz: 20.0
+    sensor_height_m: 0.68
+    radius_coef_close: 0.2
+    radius_coef_far: 0.7
+    max_slope_deg: 10.0
+    gap_threshold_m: 0.2
+    min_points_per_cluster: 2
diff --git a/src/perception/velodyne_cloud_separator/include/point_types.hpp b/src/perception/velodyne_cloud_separator/include/point_types.hpp
@@ -0,0 +1,18 @@
+#ifndef POINT_TYPES_HPP_
+#define POINT_TYPES_HPP_
+
+#include <pcl/point_types.h>
+
+struct PointXYZIR
+{
+  PCL_ADD_POINT4D;
+  float intensity;
+  uint16_t ring;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+  PointXYZIR,
+  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint16_t, ring, ring))
+
+#endif  // POINT_TYPES_HPP_
diff --git a/...on/velodyne_cloud_separator/include/velodyne_cloud_separator/velodyne_cloud_separator.hpp b/...on/velodyne_cloud_separator/include/velodyne_cloud_separator/velodyne_cloud_separator.hpp
@@ -0,0 +1,57 @@
+#ifndef VELODYNE_CLOUD_SEPARATOR__VELODYNE_CLOUD_SEPARATOR_HPP_
+#define VELODYNE_CLOUD_SEPARATOR__VELODYNE_CLOUD_SEPARATOR_HPP_
+
+#include "point_types.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <wheel_stuck_utils/ros/no_callback_subscription.hpp>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/point_cloud.h>
+
+#ifndef LASERS_NUM
+#define LASERS_NUM 32
+#endif
+
+namespace velodyne_cloud_separator
+{
+
+using PointCloud2 = sensor_msgs::msg::PointCloud2;
+
+using PointCloud2Subscription = wheel_stuck_utils::ros::NoCallbackSubscription<PointCloud2>;
+using PointCloud2Publisher = rclcpp::Publisher<PointCloud2>;
+
+class VelodyneCloudSeparator : public rclcpp::Node
+{
+private:
+  enum class PointType { UNKNOWN = 0, GROUND = 1, OBSTACLE = 2 };
+
+public:
+  explicit VelodyneCloudSeparator(const rclcpp::NodeOptions & options);
+
+private:
+  void init(
+    const double sensor_height, const double radius_coef_close, const double radius_coef_far);
+  void update();
+  bool try_subscribe_pc();
+
+  rclcpp::TimerBase::SharedPtr update_timer_;
+
+  PointCloud2Subscription::SharedPtr pc_sub_;
+  PointCloud2Publisher::SharedPtr pc_ground_pub_;
+  PointCloud2Publisher::SharedPtr pc_obstacle_pub_;
+
+  pcl::PointCloud<PointXYZIR>::Ptr pc_;
+
+  static const int height_ = LASERS_NUM;
+
+  double radius_array_[LASERS_NUM];
+
+  double limiting_ratio_;
+  double gap_threshold_;
+  int min_points_num_;
+};
+}  // namespace velodyne_cloud_separator
+
+#endif  // VELODYNE_CLOUD_SEPARATOR__VELODYNE_CLOUD_SEPARATOR_HPP_
diff --git a/src/perception/velodyne_cloud_separator/launch/velodyne_cloud_separator.launch.xml b/src/perception/velodyne_cloud_separator/launch/velodyne_cloud_separator.launch.xml
@@ -0,0 +1,14 @@
+<launch>
+  <arg name="points_topic" default="/velodyne_points"/>
+  <arg name="ground_points_topic" default="/velodyne_points/ground"/>
+  <arg name="obstacle_points_topic" default="/velodyne_points/obstacle"/>
+
+  <arg name="config_file" default="$(find-pkg-share velodyne_cloud_separator)/config/velodyne_cloud_separator.param.yaml"/>
+
+  <node pkg="velodyne_cloud_separator" name="velodyne_cloud_separator" exec="velodyne_cloud_separator_node" output="screen">
+    <remap from="~/input/points" to="$(var points_topic)"/>
+    <remap from="~/output/ground_points" to="$(var ground_points_topic)"/>
+    <remap from="~/output/obstacle_points" to="$(var obstacle_points_topic)"/>
+    <param from="$(var config_file)"/>
+  </node>
+</launch>
diff --git a/src/perception/velodyne_cloud_separator/package.xml b/src/perception/velodyne_cloud_separator/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>velodyne_cloud_separator</name>
+  <version>0.0.0</version>
+  <description>The velodyne_cloud_separator package</description>
+  <maintainer email="[email protected]">Akiro Harada</maintainer>
+
+  <license>Apache 2</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>wheel_stuck_utils</depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/perception/velodyne_cloud_separator/src/velodyne_cloud_separator.cpp b/src/perception/velodyne_cloud_separator/src/velodyne_cloud_separator.cpp
@@ -0,0 +1,225 @@
+#include "velodyne_cloud_separator/velodyne_cloud_separator.hpp"
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <limits>
+#include <vector>
+
+namespace velodyne_cloud_separator
+{
+VelodyneCloudSeparator::VelodyneCloudSeparator(const rclcpp::NodeOptions & options)
+: Node("velodyne_cloud_separator", options)
+{
+  // Declare Parameters
+  double update_rate_hz = this->declare_parameter("update_rate_hz", 20.0);
+  double sensor_height = this->declare_parameter("sensor_height_m", 1.0);
+  double radius_coef_close = this->declare_parameter("radius_coef_close", 0.2);
+  double radius_coef_far = this->declare_parameter("radius_coef_far", 0.7);
+  float max_slope_deg = this->declare_parameter("max_slope_deg", 15.0);
+  limiting_ratio_ = tan(max_slope_deg * M_PI / 180.0);
+  gap_threshold_ = this->declare_parameter("gap_threshold_m", 0.1);
+  min_points_num_ = this->declare_parameter("min_points_per_cluster", 2);
+
+  // Declare Subscriptions
+  {
+    pc_sub_ = PointCloud2Subscription::create_subscription(this, "~/input/points");
+  }
+
+  // Declare Publishers
+  {
+    pc_ground_pub_ = this->create_publisher<PointCloud2>("~/output/ground_points", 1);
+    pc_obstacle_pub_ = this->create_publisher<PointCloud2>("~/output/obstacle_points", 1);
+  }
+
+  // Declare timer for update function
+  {
+    auto update_callback = [this]() { this->update(); };
+    auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      std::chrono::duration<double>(1.0 / update_rate_hz));
+    update_timer_ = std::make_shared<rclcpp::GenericTimer<decltype(update_callback)>>(
+      this->get_clock(), period, std::move(update_callback),
+      this->get_node_base_interface()->get_context());
+    this->get_node_timers_interface()->add_timer(update_timer_, nullptr);
+  }
+
+  // Initialize
+  init(sensor_height, radius_coef_close, radius_coef_far);
+}
+
+void VelodyneCloudSeparator::init(
+  const double sensor_height, const double radius_coef_close, const double radius_coef_far)
+{
+  // HDL-32E unique parameters
+  double angle_start = 92.0 / 3.0 * M_PI / 180.0;
+  double angle_diff = 4.0 / 3.0 * M_PI / 180.0;
+
+  radius_array_[0] = std::numeric_limits<double>::max();
+  for (int i = 1; i < height_; i++) {
+    if (i > 20) {
+      radius_array_[i] = radius_array_[20];
+      continue;
+    }
+
+    double angle = angle_start - angle_diff * i;
+    radius_array_[i] = sensor_height * (1.0 / tan(angle) - 1.0 / tan(angle + angle_diff));
+    radius_array_[i] *= (i <= 12 ? radius_coef_close : radius_coef_far);
+  }
+}
+
+void VelodyneCloudSeparator::update()
+{
+  if (!try_subscribe_pc()) return;
+
+  int points_size = pc_->points.size();
+
+  const int width = static_cast<int>(points_size * 1.8 / height_);
+
+  // Create index map
+  std::vector<std::vector<int>> index_map(height_, std::vector<int>(width, -1));
+  for (int i = 0; i < points_size; i++) {
+    const auto & point = pc_->points[i];
+    float angle = atan2(point.y, point.x) * 180.0 / M_PI;
+    if (angle < 0.0) angle += 360.0;
+    int row = height_ - 1 - point.ring;
+    int col = width - 1 - static_cast<int>(static_cast<double>(width) * angle / 360.0);
+    index_map[row][col] = i;
+  }
+
+  // Separate points
+  pcl::PointCloud<pcl::PointXYZ>::Ptr ground_points(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points(new pcl::PointCloud<pcl::PointXYZ>);
+  for (int i = 0; i < width; i++) {
+    PointType point_types[height_] = {PointType::UNKNOWN};
+    int point_indices[height_] = {0};
+    int point_indices_size = 0;
+    int unknown_indices[height_] = {0};
+    int unknown_indices_size = 0;
+    double z_ref = 0.0;
+    double r_ref = 0.0;
+
+    for (int j = height_ - 1; j >= 0; j--) {
+      auto & point = pc_->points[index_map[j][i]];
+      if (index_map[j][i] > -1 && point_types[j] == PointType::UNKNOWN) {
+        const float x0 = point.x;
+        const float y0 = point.y;
+        const float z0 = point.z;
+        const float r0 = sqrt(x0 * x0 + y0 * y0);
+        const float r_diff = r0 - r_ref;
+        const float z_diff = fabs(z0 - z_ref);
+        const float angle = z_diff / r_diff;
+
+        if (
+          !(angle > 0 && angle < limiting_ratio_ && z_diff < gap_threshold_) ||
+          point_indices_size != 0) {
+          for (int k = 0; k < point_indices_size; k++) {
+            int index = index_map[point_indices[k]][i];
+            if (point_indices_size > min_points_num_) {
+              auto & p = pc_->points[index];
+              pcl::PointXYZ p_xyz;
+              p_xyz.x = p.x;
+              p_xyz.y = p.y;
+              p_xyz.z = p.z;
+              ground_points->push_back(p_xyz);
+              point_types[point_indices[k]] = PointType::GROUND;
+            } else {
+              unknown_indices[unknown_indices_size++] = index;
+            }
+          }
+          point_indices_size = 0;
+        }
+        point_indices[point_indices_size++] = j;
+        z_ref = z0;
+        r_ref = r0;
+      }
+      if (j != 0) continue;
+
+      if (point_indices_size != 0) {
+        for (int k = 0; k < point_indices_size; k++) {
+          int index = index_map[point_indices[k]][i];
+          if (point_indices_size > min_points_num_) {
+            auto & p = pc_->points[index];
+            pcl::PointXYZ p_xyz;
+            p_xyz.x = p.x;
+            p_xyz.y = p.y;
+            p_xyz.z = p.z;
+            ground_points->push_back(p_xyz);
+            point_types[point_indices[k]] = PointType::GROUND;
+          } else {
+            unknown_indices[unknown_indices_size++] = index;
+          }
+        }
+        point_indices_size = 0;
+      }
+
+      double centroid = 0;
+      int centroid_ring = 0;
+      int cluster_indices[height_] = {0};
+      int cluster_indices_size = 0;
+      for (int k = unknown_indices_size - 1; k >= 0; k--) {
+        float x0 = pc_->points[unknown_indices[k]].x;
+        float y0 = pc_->points[unknown_indices[k]].y;
+        float r0 = sqrt(x0 * x0 + y0 * y0);
+        float r_diff = fabs(r0 - centroid);
+        float dist = radius_array_[centroid_ring];
+        if (r_diff >= dist && cluster_indices_size != 0) {
+          for (int l = 0; l < cluster_indices_size; l++) {
+            auto & p = pc_->points[cluster_indices[l]];
+            pcl::PointXYZ p_xyz;
+            p_xyz.x = p.x;
+            p_xyz.y = p.y;
+            p_xyz.z = p.z;
+            (cluster_indices_size > 1 ? obstacle_points : ground_points)->push_back(p_xyz);
+          }
+          cluster_indices_size = 0;
+        }
+        cluster_indices[cluster_indices_size++] = unknown_indices[k];
+        centroid = r0;
+        centroid_ring = pc_->points[unknown_indices[k]].ring;
+        if (k != 0) continue;
+        for (int l = 0; l < cluster_indices_size; l++) {
+          auto & p = pc_->points[cluster_indices[l]];
+          pcl::PointXYZ p_xyz;
+          p_xyz.x = p.x;
+          p_xyz.y = p.y;
+          p_xyz.z = p.z;
+          (cluster_indices_size > 1 ? obstacle_points : ground_points)->push_back(p_xyz);
+        }
+        cluster_indices_size = 0;
+      }
+    }
+  }
+
+  // Publish
+  {
+    sensor_msgs::msg::PointCloud2 pc_ground_msg;
+    pcl::toROSMsg(*ground_points, pc_ground_msg);
+    pc_ground_msg.header.frame_id = pc_->header.frame_id;
+    pc_ground_msg.header.stamp = this->now();
+    pc_ground_pub_->publish(pc_ground_msg);
+  }
+  {
+    sensor_msgs::msg::PointCloud2 pc_obstacle_msg;
+    pcl::toROSMsg(*obstacle_points, pc_obstacle_msg);
+    pc_obstacle_msg.header.frame_id = pc_->header.frame_id;
+    pc_obstacle_msg.header.stamp = this->now();
+    pc_obstacle_pub_->publish(pc_obstacle_msg);
+  }
+}
+
+bool VelodyneCloudSeparator::try_subscribe_pc()
+{
+  auto pc_msg = pc_sub_->getData();
+  if (!pc_msg) return false;
+  try {
+    pc_ = pcl::PointCloud<PointXYZIR>::Ptr(new pcl::PointCloud<PointXYZIR>);
+    pcl::fromROSMsg(*pc_msg, *pc_);
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(get_logger(), "Failed to convert PointCloud2 to pcl::PointCloud: %s", e.what());
+    return false;
+  }
+  return true;
+}
+}  // namespace velodyne_cloud_separator
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(velodyne_cloud_separator::VelodyneCloudSeparator)