linefit. initial working version

Signed-off-by: amc-nu <[email protected]>

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(ros2_composable_template)
+project(ros2_ground_segmentation)
 
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
@@ -10,22 +10,26 @@ ament_auto_find_build_dependencies()
 
 find_package (PCL REQUIRED)
 
-# composable_sample - Composable Nodes are libraries loaded at runtime
-ament_auto_add_library(composable_sample SHARED
-        src/composable_sample.cpp
+# line_fit_ground_segmentation - Composable Nodes are libraries loaded at runtime
+ament_auto_add_library(linefit_ground_segmentation SHARED
+        src/line_fit_ground_segmentation.cpp
+        src/linefit/linefit_seg.cpp
+        src/linefit/bin.cpp
+        src/linefit/segment.cpp
         )
 
-target_link_libraries(composable_sample
+target_link_libraries(linefit_ground_segmentation
         ${PCL_LIBRARIES}
         )
 
-target_include_directories(composable_sample PUBLIC
+target_include_directories(linefit_ground_segmentation PUBLIC
+        include
         ${PCL_INCLUDE_DIRS}
         )
 
-rclcpp_components_register_node(composable_sample
-        PLUGIN "Ros2ComposableTemplate"
-        EXECUTABLE composable_sample_node
+rclcpp_components_register_node(linefit_ground_segmentation
+        PLUGIN "LinefitGroundSegmentation"
+        EXECUTABLE linefit_ground_segmentation_node
         )
 
 ament_auto_package(INSTALL_TO_SHARE

README.md

@@ -1,10 +1,11 @@
-# composable_sample
+# ground_segmentation
 
-ROS2 Composable Sample node. Subscribes to a PointCloud2 topic and publishes a PointCloud2 Topic.
+ROS2 Composable Node for Ground Segmentation. 
+Subscribes to a PointCloud2 topic and publishes a PointCloud2 Topic.
 
 How to launch:
 ```
-ros2 launch ros2_composable_template composable_sample.launch.xml \
+ros2 launch ros2_ground_segmentation ground_segmentation.launch.xml \
     input/topic:=/lidar/pointcloud \
     output/topic:=/processed/pointcloud
 ```

include/ros2_ground_segmentation/line_fit_ground_segmentation.hpp

@@ -0,0 +1,86 @@
+/*
+ *  Copyright (c) 2023, MAP IV, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef LINEFIT_GROUND_SEGMENTATION_H
+#define LINEFIT_GROUND_SEGMENTATION_H
+
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/node_interfaces/node_parameters_interface.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.hpp>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <ros2_ground_segmentation/point_type.h>
+#include <ros2_ground_segmentation/linefit/linefit_params.h>
+#include <ros2_ground_segmentation/linefit/linefit_seg.h>
+
+
+template <typename T>
+bool get_param(const std::vector<rclcpp::Parameter> & p, const std::string & name, T & value)
+{
+  auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) {
+    return parameter.get_name() == name;
+  });
+  if (it != p.cend()) {
+    value = it->template get_value<T>();
+    return true;
+  }
+  return false;
+}
+
+class LinefitGroundSegmentation : public rclcpp::Node
+{
+public:
+  explicit LinefitGroundSegmentation(const rclcpp::NodeOptions & options);
+
+private:
+  void CloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg);
+  rcl_interfaces::msg::SetParametersResult ReconfigureCallback(const std::vector<rclcpp::Parameter> & p);
+  void transformPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & in,
+                           sensor_msgs::msg::PointCloud2::SharedPtr & out);
+  void transformPointCloud(
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & in,
+    sensor_msgs::msg::PointCloud2::SharedPtr & out,
+    const std::string & target_frame);
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
+  std::shared_ptr<LinefitGroundSeg> linefit_segmentation_ptr_;
+
+  LinefitSegParams linefit_params_;
+  LinefitSegParams GetParameters();
+  std::string output_frame_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+};
+
+#endif //LINEFIT_GROUND_SEGMENTATION_H

src/composable_sample.cpp → include/ros2_ground_segmentation/linefit/bin.h

@@ -25,40 +25,50 @@
  *
  */
 
-#include "ros2_composable_template/composable_sample.hpp"
+#ifndef LINEFIT_BIN_H
+#define LINEFIT_BIN_H
 
-Ros2ComposableTemplate::Ros2ComposableTemplate(const rclcpp::NodeOptions & options) : Node("composable_sample", options) {
+#include <atomic>
+#include <limits>
+#include <cmath>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
 
-  pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
-      "input/topic", rclcpp::SensorDataQoS().keep_last(1),
-      std::bind(&Ros2ComposableTemplate::CloudCallback, this, std::placeholders::_1));
+class Bin {
+public:
+  struct MinZPoint {
+    MinZPoint() : z(0), d(0) {
+    }
 
-  pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("output/topic", rclcpp::SensorDataQoS());
-}
+    MinZPoint(const double &d, const double &z) : z(z), d(d) {
+    }
 
-void Ros2ComposableTemplate::CloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg)
-{
-  if (
-    pointcloud_pub_->get_subscription_count() > 0 ||
-    pointcloud_pub_->get_intra_process_subscription_count() > 0) { //subscription guards
+    bool operator==(const MinZPoint &comp) const {
+      return z == comp.z && d == comp.d;
+    }
 
-    sensor_msgs::msg::PointCloud2 cloud_result;
-    RCLCPP_INFO_STREAM(get_logger(), "Point cloud received");
+    double z;  // height
+    double d;  // range
+  };
 
-    // convert ros to pcl
-    pcl::PointCloud<pcl::PointXYZ>::Ptr raw_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr);
+  Bin();
 
-    // do something
+  Bin(const Bin &segment);
 
-    // convert pcl to ros
-    auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
-    pcl::toROSMsg(*raw_pointcloud_ptr, *ros_pc_msg_ptr);
-  }
+  void addPoint(const pcl::PointXYZ &point);
+
+  void addPoint(const double &d, const double &z);
 
-}
+  MinZPoint getMinZPoint();
 
-#include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(Ros2ComposableTemplate)
+  inline bool hasPoint() {
+    return has_point_;
+  }
 
+private:
+  std::atomic<bool> has_point_;
+  std::atomic<double> min_z_;
+  std::atomic<double> min_z_d_;
+};
 
+#endif  // LINEFIT_BIN_H

include/ros2_ground_segmentation/linefit/linefit_params.h

@@ -0,0 +1,80 @@
+/*
+ *  Copyright (c) 2023, MAP IV, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef ROS2_LINEFIT_PARAMS_H
+#define ROS2_LINEFIT_PARAMS_H
+
+struct LinefitSegParams {
+  LinefitSegParams()
+    : r_min(0.1), r_max(20), bins_num(100), segs_num(360), max_dist_to_line(0.15), max_slope(1), threads_num(4),
+      max_error(0.01), long_thres(2.0), max_long_height(0.1), max_start_height(0.2), sensor_height(0.5),
+      line_search_angle(0.2) {
+  }
+/*
+ *
+ *  r_min: 2
+    r_max: 100
+    bins_num: 300
+    segs_num: 360
+    threads_num: 4
+    max_slope: 0.3
+    max_dist_to_line: 0.1
+    long_thres: 5
+    max_long_height: 0.1
+    max_start_height: 0.5
+    line_search_angle: 0.5
+    max_fit_error: 0.02
+ */
+  // Minimum range of segmentation.
+  double r_min;
+  // Maximum range of segmentation.
+  double r_max;
+  // Number of radial bins.
+  int bins_num;
+  // Number of angular segments.
+  int segs_num;
+  // Maximum distance to a ground line to be classified as ground.
+  double max_dist_to_line;
+  // Max slope to be considered ground line.
+  double max_slope;
+  // Max error for line fit.
+  double max_error;
+  // Distance at which points are considered far from each other.
+  double long_thres;
+  // Maximum slope for
+  double max_long_height;
+  // Maximum heigh of starting line to be labelled ground.
+  double max_start_height;
+  // Height of sensor above ground.
+  double sensor_height;
+  // How far to search for a line in angular direction [rad].
+  double line_search_angle;
+  // Number of threads.
+  int threads_num;
+};
+
+#endif //ROS2_LINEFIT_PARAMS_H

include/ros2_ground_segmentation/linefit/linefit_seg.h

@@ -0,0 +1,89 @@
+/*
+ *  Copyright (c) 2023, MAP IV, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef LINEFIT_SEG_H
+#define LINEFIT_SEG_H
+
+#include <chrono>
+#include <cmath>
+#include <iostream>
+#include <list>
+#include <memory>
+#include <mutex>
+#include <thread>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <yaml-cpp/yaml.h>
+
+#include <ros2_ground_segmentation/point_type.h>
+#include <ros2_ground_segmentation/linefit/segment.h>
+#include <ros2_ground_segmentation/linefit/linefit_params.h>
+
+class LinefitGroundSeg {
+  enum class PointCategory {
+    OBJECT = 0,
+    GROUND = 1,
+  };
+public:
+  LinefitGroundSeg(const LinefitSegParams& linefitseg_params );
+
+  void setParameters(const LinefitSegParams& linefitseg_params);
+
+  void segmentGround(const pcl::PointCloud<PointXYZIR>::ConstPtr &in_cloud_msg,
+                     pcl::PointCloud<pcl::PointXYZI> &out_groundless_points,
+                     pcl::PointCloud<pcl::PointXYZI> &out_ground_points);
+  void displayParams() const;
+private:
+  std::mutex params_mutex_;
+  LinefitSegParams params_;
+  // Access with segments_[segment][bin].
+  std::vector<Segment> segments_;
+  // Bin index of every point.
+  std::vector<std::pair<int, int>> bin_index_;
+  // 2D coordinates (d, z) of every point in its respective segment.
+  std::vector<Bin::MinZPoint> segment_coordinates_;
+
+  void initializeSegments();
+
+  void segment(const pcl::PointCloud<pcl::PointXYZ> &cloud, std::vector<PointCategory> *labels);
+
+  void assignCluster(std::vector<PointCategory> *labels);
+
+  void assignClusterThread(const unsigned int &start_index, const unsigned int &end_index,
+                           std::vector<PointCategory> *labels);
+
+  void insertPoints(const pcl::PointCloud<pcl::PointXYZ> &cloud);
+
+  void insertionThread(const pcl::PointCloud<pcl::PointXYZ> &cloud, const size_t start_index, const size_t end_index);
+
+  void lineFitting();
+
+  void lineFitThread(const unsigned int start_index, const unsigned int end_index, std::mutex *lines_mutex);
+};
+
+#endif  // LINEFIT_SEG_H