Patchwork Paper implementation using ROS 2.

docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile

@@ -7,7 +7,6 @@ WORKDIR ${AMENT_WS}/src
 
 # Copy in source code 
 COPY src/world_modeling/occupancy_segmentation occupancy_segmentation
-COPY src/wato_msgs/sample_msgs sample_msgs
 
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
@@ -19,9 +18,13 @@ RUN apt-get -qq update && rosdep update && \
 ################################# Dependencies ################################
 FROM ${BASE_IMAGE} as dependencies
 
+# Install some patchwork dependencies
+RUN sudo apt-get install libeigen3-dev
+RUN sudo apt-get -y install libtbb-dev
+
 # Install Rosdep requirements
 COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list
-RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
+RUN apt-get -qq update && apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
 
 # Copy in source code from source stage
 WORKDIR ${AMENT_WS}

src/world_modeling/occupancy_segmentation/CMakeLists.txt

@@ -1,14 +1,73 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.10)
 project(occupancy_segmentation)
 
+# Set compiler to use C++ 17 standard
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
-find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+# Search for dependencies required for building this package
+find_package(ament_cmake REQUIRED) # ROS2 build tool
+find_package(sensor_msgs REQUIRED)
+find_package(rclcpp REQUIRED)      # ROS2 C++ package
+find_package(pcl_ros REQUIRED)
+
+# Compiles source files into a library
+# A library is not executed, instead other executables can link
+# against it to access defined methods and classes.
+# We build a library so that the methods defined can be used by
+# both the unit test and ROS2 node executables.
+add_library(occupancy_segmentation_lib
+  src/occupancy_segmentation_core.cpp)
+# Indicate to compiler where to search for header files
+target_include_directories(occupancy_segmentation_lib
+  PUBLIC
+  include)
+# Add ROS2 dependencies required by package
+ament_target_dependencies(occupancy_segmentation_lib
+  sensor_msgs
+  rclcpp
+  pcl_ros)
+
+# # By default tests are built. This can be overridden by modifying the
+# # colcon build command to include the -DBUILD_TESTING=OFF flag.
+# option(BUILD_TESTING "Build tests" ON)
+# if(BUILD_TESTING)
+#   # Search for dependencies required for building tests + linting
+#   find_package(ament_cmake_gtest REQUIRED)
+
+#   # Create test executable from source files
+#   ament_add_gtest(occupancy_segmentation_test test/occupancy_segmentation_test.cpp)
+#   # Link to the previously built library to access occupancy_segmentation classes and methods
+#   target_link_libraries(occupancy_segmentation_test
+#     occupancy_segmentation_lib
+#     gtest_main)
+
+#   # Copy executable to installation location
+#   install(TARGETS
+#     occupancy_segmentation_test
+#     DESTINATION lib/${PROJECT_NAME})
+# endif()
+
+# Create ROS2 node executable from source files
+add_executable(occupancy_segmentation_node src/occupancy_segmentation_node.cpp)
+# Link to the previously built library to access occupancy_segmentation classes and methods
+target_link_libraries(occupancy_segmentation_node occupancy_segmentation_lib)
+
+# Copy executable to installation location
+install(TARGETS
+  occupancy_segmentation_node
+  DESTINATION lib/${PROJECT_NAME})
+
+# Copy launch and config files to installation location
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME})
 
 ament_package()
+

src/world_modeling/occupancy_segmentation/config/params.yaml

@@ -0,0 +1,23 @@
+occupancy_segmentation:
+  ros__parameters:
+    version: 1
+    lidar_input_topic: "/velodyne_points"
+    ground_output_topic: "/ground_points"
+    nonground_output_topic: "/nonground_points"
+    num_zones: 4
+    l_min: 2.7
+    l_max: 80.0
+    md: 0.3
+    mh: -1.1
+    min_num_points: 10
+    num_seed_points: 20
+    th_seeds: 0.5
+    uprightness_thresh: 0.5
+    num_rings_of_interest: 4
+    sensor_height: 1.7
+    global_el_thresh: 0.0
+    zone_rings: [2, 4, 4, 4]
+    zone_sectors: [16, 32, 54, 32]
+    flatness_thr: [0.0005, 0.000725, 0.001, 0.001]
+    elevation_thr: [0.523, 0.746, 0.879, 1.125]
+    adaptive_selection_en: false

src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp

@@ -0,0 +1,129 @@
+#ifndef OCCUPANCY_SEGMENTATION_CORE_HPP_
+#define OCCUPANCY_SEGMENTATION_CORE_HPP_
+
+#include <pcl/common/centroid.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <string>
+#include <vector>
+
+#include <tbb/blocked_range.h>
+#include <tbb/parallel_for.h>
+#include <Eigen/Dense>
+
+struct PointXYZIRT {
+  PCL_ADD_POINT4D;  // preferred way of adding a XYZ+padding
+  float intensity;
+  u_int16_t ring;
+  float time;
+  PCL_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+
+struct PCAFeature {
+  Eigen::Vector3f principal_;
+  Eigen::Vector3f normal_;
+  Eigen::Vector3f singular_values_;
+  Eigen::Vector3f mean_;
+  float d_;
+  float th_dist_d_;
+  float linearity_;
+  float planarity_;
+};
+
+struct Patch_Index {
+  int idx;
+  int zone_idx;
+  int ring_idx;
+  int sector_idx;
+  int concentric_idx;
+};
+
+enum Status {
+  TOO_TILTED,
+  FLAT_ENOUGH,
+  TOO_HIGH_ELEV,
+  UPRIGHT_ENOUGH,
+  GLOBAL_TOO_HIGH_ELEV,
+  FEW_POINTS
+};
+
+template <typename PointT>
+class OccupancySegmentationCore {
+ public:
+  typedef std::vector<pcl::PointCloud<PointT>> Ring;
+  typedef std::vector<Ring> Zone;
+
+  int NUM_ZONES;
+
+  float L_MIN;
+  float L_MAX;
+
+  float MD;
+  float MH;
+
+  int MIN_NUM_POINTS;
+  int NUM_SEED_POINTS;
+
+  float TH_SEEDS;
+  float UPRIGHTNESS_THRESH;
+
+  int NUM_RINGS_OF_INTEREST;
+  float SENSOR_HEIGHT;
+  float GLOBAL_EL_THRESH;
+
+  std::vector<int> ZONE_RINGS;
+  std::vector<int> ZONE_SECTORS;
+  std::vector<float> FLATNESS_THR;
+  std::vector<float> ELEVATION_THR;
+  std::vector<double> lmins = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4,
+                               (L_MIN + L_MAX) / 2};
+  std::vector<double> lmaxs = {lmins[1], lmins[2], lmins[3], L_MAX};
+
+  int num_patches = -1;
+
+  bool ADAPTIVE_SELECTION_EN;
+
+  std::vector<Zone> _czm;
+  std::vector<pcl::PointCloud<PointT>> _regionwise_ground;
+  std::vector<pcl::PointCloud<PointT>> _regionwise_nonground;
+
+  std::vector<Patch_Index> _patch_indices;
+
+  pcl::PointCloud<PointT> _ground;
+  pcl::PointCloud<PointT> _non_ground;
+
+  std::vector<Status> _statuses;
+
+  OccupancySegmentationCore();
+  OccupancySegmentationCore(int num_zones, float l_min, float l_max, float md, float mh,
+                            int min_num_points, int num_seed_points, float th_seeds,
+                            float uprightness_thresh, int num_rings_of_interest,
+                            float sensor_height, float global_el_thresh,
+                            std::vector<long int, std::allocator<long int>> &zone_rings,
+                            std::vector<long int, std::allocator<long int>> &zone_sectors,
+                            std::vector<double> &flatness_thr, std::vector<double> &elevation_thr,
+                            bool adaptive_selection_en);
+
+  void segment_ground(pcl::PointCloud<PointT> &unfiltered_cloud, pcl::PointCloud<PointT> &ground,
+                      pcl::PointCloud<PointT> &nonground);
+
+ private:
+  void init_czm();
+
+  void fill_czm(pcl::PointCloud<PointT> &cloud_in);
+
+  void clear_czm_and_regionwise();
+
+  void estimate_plane(pcl::PointCloud<PointT> &cloud, PCAFeature &feat);
+
+  void rgpf(pcl::PointCloud<PointT> &patch, Patch_Index &p_idx, PCAFeature &feat);
+
+  void extract_initial_seeds(pcl::PointCloud<PointT> &cloud, pcl::PointCloud<PointT> &seed_cloud,
+                             int zone_idx);
+
+  Status ground_likelihood_est(PCAFeature &feat, int concentric_idx);
+
+  static bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; };
+};
+
+#endif

src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp

@@ -0,0 +1,53 @@
+#ifndef OCCUPANCY_SEGMENTATION_NODE_HPP_
+#define OCCUPANCY_SEGMENTATION_NODE_HPP_
+#define PCL_NO_PRECOMPILE
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <chrono>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include "occupancy_segmentation_core.hpp"
+
+typedef std::chrono::high_resolution_clock Clock;
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT,  // here we assume a XYZ + "test" (as fields)
+                                  (float, x, x)(float, y, y)(float, z,
+                                                             z)(float, intensity,
+                                                                intensity)(u_int16_t, ring,
+                                                                           ring)(float, time, time))
+
+/**
+ * Implementation of a ROS2 node that converts unfiltered messages to filtered_array
+ * messages.
+ *
+ * Listens to the "unfiltered" topic and filters out data with invalid fields
+ * and odd timestamps. Once the node collects  messages it packs
+ * the processed messages into an array and publishes it to the "filtered" topic.
+ */
+class OccupancySegmentationNode : public rclcpp::Node {
+ public:
+  // Configure pubsub nodes to keep last 20 messages.
+  // https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html
+  static constexpr int ADVERTISING_FREQ = 20;
+
+  /**
+   * Transformer node constructor.
+   */
+  OccupancySegmentationNode();
+
+ private:
+  // Object that handles data processing and validation.
+  OccupancySegmentationCore<PointXYZIRT> _patchwork;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr _subscriber;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr _nonground_publisher;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr _ground_publisher;
+
+  void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud);
+};
+
+#endif  // OCCUPANCY_SEGMENTATION_NODE_HPP_

src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py

@@ -0,0 +1,31 @@
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+import os
+
+
+def generate_launch_description():
+    """Launch occupancy_seg node."""
+    occupancy_seg_pkg_prefix = get_package_share_directory('occupancy_segmentation')
+    occupancy_seg_param_file = os.path.join(
+        occupancy_seg_pkg_prefix, 'config', 'params.yaml')
+
+    occupancy_seg_param = DeclareLaunchArgument(
+        'occupancy_segmentation_param_file',
+        default_value=occupancy_seg_param_file,
+        description='Path to config file for occupancy segmentation node'
+    )
+
+    occupancy_seg_node = Node(
+        package='occupancy_segmentation',
+        executable='occupancy_segmentation_node',
+        parameters=[LaunchConfiguration('occupancy_segmentation_param_file')],
+    )
+
+    return LaunchDescription([
+        occupancy_seg_param,
+        occupancy_seg_node
+    ])

src/world_modeling/occupancy_segmentation/package.xml

@@ -1,15 +1,21 @@
 <?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>occupancy_segmentation</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="[email protected]">bolty</maintainer>
-  <license>TODO: License declaration</license>
+  <description>A ROS2 package to handle LiDAR ground segmentation using Patchworks</description>
 
+  <maintainer email="[email protected]">Akil Pathiranage</maintainer>
+  <license>TODO</license>
+
+  <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>sensor_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>pcl_ros</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
 
+  <!--https://www.ros.org/reps/rep-0149.html#export-->
   <export>
     <build_type>ament_cmake</build_type>
   </export>