Skip to content

Commit

Permalink
Patchwork Paper implementation using ROS 2. (#142)
Browse files Browse the repository at this point in the history
* Copy transformer samples to occupancy_seg

* Copied transformer node layout

* Renamed files

* Fix small error

* Clean up

* Add pcl package

* Added eigen to dockerifle

* Update docker-compose.world_modeling.yaml

* Update docker-compose.world_modeling.yaml

* added tbb to dockerfile

* Started working on it

* Finished czm initialization

* Plane

* Mostly done

* Add apt update

* Fixing small bugs

* Build / make error with core.cpp

* Added main node code, still fails build

* Commented some things to get it to compile

* Builds

* Foxglove setup plus error debugging
exit
exit()

* Templated Core file

* lowkey works

* Added headers to publishers

* Copy transformer samples to occupancy_seg

* Copied transformer node layout

* Renamed files

* Fix small error

* Clean up

* Add pcl package

* Added eigen to dockerifle

* Update docker-compose.world_modeling.yaml

* Update docker-compose.world_modeling.yaml

* added tbb to dockerfile

* Started working on it

* Finished czm initialization

* Plane

* Mostly done

* Add apt update

* Fixing small bugs

* Build / make error with core.cpp

* Added main node code, still fails build

* Commented some things to get it to compile

* Builds

* Foxglove setup plus error debugging
exit
exit()

* Templated Core file

* lowkey works

* Added headers to publishers

* Remove watod-config changes

* Cleanup + Timing for patchwork

* Update watod-config.sh

* Update docker_context.sh

* Fix code style issues with clang_format

* added ros params

* Fix code style issues with clang_format

* Removed adaptive seed selection for 1st zone

* Made adaptive seed selection into toggle param

* Put num_zones in params

* Fix code style issues with clang_format

---------

Co-authored-by: VishGit1234 <[email protected]>
Co-authored-by: VishGit1234 <[email protected]>
Co-authored-by: WATonomousAdmin <[email protected]>
  • Loading branch information
4 people authored Sep 27, 2024
1 parent 5b9dd90 commit f121acb
Show file tree
Hide file tree
Showing 9 changed files with 718 additions and 12 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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 && \
Expand All @@ -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}
Expand Down
71 changes: 65 additions & 6 deletions src/world_modeling/occupancy_segmentation/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()

23 changes: 23 additions & 0 deletions src/world_modeling/occupancy_segmentation/config/params.yaml
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
@@ -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_
Original file line number Diff line number Diff line change
@@ -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
])
14 changes: 10 additions & 4 deletions src/world_modeling/occupancy_segmentation/package.xml
Original file line number Diff line number Diff line change
@@ -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>
Expand Down
Loading

0 comments on commit f121acb

Please sign in to comment.