-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Patchwork Paper implementation using ROS 2. (#142)
* 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
1 parent
5b9dd90
commit f121acb
Showing
9 changed files
with
718 additions
and
12 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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
23
src/world_modeling/occupancy_segmentation/config/params.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
129 changes: 129 additions & 0 deletions
129
src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
53 changes: 53 additions & 0 deletions
53
src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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_ |
31 changes: 31 additions & 0 deletions
31
src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 | ||
]) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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> | ||
|
Oops, something went wrong.