From 8b9306a0a3005c858e4f4abe77a012718c9021df Mon Sep 17 00:00:00 2001 From: akilpath Date: Sat, 1 Jun 2024 23:46:34 +0000 Subject: [PATCH 01/59] Copy transformer samples to occupancy_seg --- .../occupancy_segmentation/config/params.yaml | 4 + .../include/transformer_core.hpp | 76 +++++++++++++++ .../include/transformer_node.hpp | 50 ++++++++++ .../launch/transformer.launch.py | 31 ++++++ .../src/transformer_core.cpp | 64 +++++++++++++ .../src/transformer_node.cpp | 51 ++++++++++ .../test/transformer_test.cpp | 94 +++++++++++++++++++ 7 files changed, 370 insertions(+) create mode 100644 src/world_modeling/occupancy_segmentation/config/params.yaml create mode 100644 src/world_modeling/occupancy_segmentation/include/transformer_core.hpp create mode 100644 src/world_modeling/occupancy_segmentation/include/transformer_node.hpp create mode 100755 src/world_modeling/occupancy_segmentation/launch/transformer.launch.py create mode 100644 src/world_modeling/occupancy_segmentation/src/transformer_core.cpp create mode 100644 src/world_modeling/occupancy_segmentation/src/transformer_node.cpp create mode 100644 src/world_modeling/occupancy_segmentation/test/transformer_test.cpp diff --git a/src/world_modeling/occupancy_segmentation/config/params.yaml b/src/world_modeling/occupancy_segmentation/config/params.yaml new file mode 100644 index 00000000..d59e071f --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/config/params.yaml @@ -0,0 +1,4 @@ +transformer_node: + ros__parameters: + version: 1 + compression_method: 0 diff --git a/src/world_modeling/occupancy_segmentation/include/transformer_core.hpp b/src/world_modeling/occupancy_segmentation/include/transformer_core.hpp new file mode 100644 index 00000000..300ca850 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/include/transformer_core.hpp @@ -0,0 +1,76 @@ +#ifndef TRANSFORMER_CORE_HPP_ +#define TRANSFORMER_CORE_HPP_ + +#include + +#include "sample_msgs/msg/filtered.hpp" +#include "sample_msgs/msg/unfiltered.hpp" + +namespace samples { + +/** + * Implementation for the internal logic for the Transformer ROS2 + * node performing data processing and validation. + */ +class TransformerCore { + public: + // Size of buffer before processed messages are published. + static constexpr int BUFFER_CAPACITY = 10; + + public: + /** + * Transformer constructor. + */ + TransformerCore(); + + /** + * Retrieve enqueued messages in buffer. + * + * @returns enqueued processed messages + */ + std::vector buffer_messages() const; + + /** + * Removes all messages in buffer. Called by the transformer + * node after messages have been published to aggregator. + */ + void clear_buffer(); + + /** + * Validates that the 'valid' field of an unfiltered message + * is set to true. + * + * @param unfiltered a raw message + * @returns whether message's 'valid' field is set + */ + bool validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered); + + /** + * Enqueue message into an array of processed messages to "filtered" topic. + * Ignores messages once the buffer capacity is reached. + * + * @param msg a processed message to be published + * @returns whether buffer is full after adding new message + */ + bool enqueue_message(const sample_msgs::msg::Filtered& msg); + + /** + * Deserializes the data field of the unfiltered ROS2 message. + * The data field should be of the form "x:$num1;y:$num2;z:$num3;". + * + * @param[in] unfiltered the raw message containing serialized data + * @param[out] filtered the processed message containing deserialized data + * @returns whether deserialization was successful + */ + bool deserialize_coordinate(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, + sample_msgs::msg::Filtered& filtered); + + private: + // Buffer storing processed messages until BUFFER_CAPACITY. Clear after + // messages are published. + std::vector buffer_; +}; + +} // namespace samples + +#endif // TRANSFORMER_CORE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/include/transformer_node.hpp b/src/world_modeling/occupancy_segmentation/include/transformer_node.hpp new file mode 100644 index 00000000..5dce7b23 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/include/transformer_node.hpp @@ -0,0 +1,50 @@ +#ifndef TRANSFORMER_NODE_HPP_ +#define TRANSFORMER_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "sample_msgs/msg/filtered.hpp" +#include "sample_msgs/msg/filtered_array.hpp" +#include "sample_msgs/msg/unfiltered.hpp" + +#include "transformer_core.hpp" + +/** + * 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 BUFFER_CAPACITY messages it packs + * the processed messages into an array and publishes it to the "filtered" topic. + */ +class TransformerNode : 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. + */ + TransformerNode(); + + private: + /** + * A ROS2 subscription node callback used to process raw data from the + * "unfiltered" topic and publish to the "filtered" topic. + * + * @param msg a raw message from the "unfiltered" topic + */ + void unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg); + + // ROS2 subscriber listening to the unfiltered topic. + rclcpp::Subscription::SharedPtr raw_sub_; + + // ROS2 publisher sending processed messages to the filtered topic. + rclcpp::Publisher::SharedPtr transform_pub_; + + // Object that handles data processing and validation. + samples::TransformerCore transformer_; +}; + +#endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/launch/transformer.launch.py b/src/world_modeling/occupancy_segmentation/launch/transformer.launch.py new file mode 100755 index 00000000..802ca850 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/launch/transformer.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 transformer node.""" + transformer_pkg_prefix = get_package_share_directory('transformer') + transformer_param_file = os.path.join( + transformer_pkg_prefix, 'config', 'params.yaml') + + transformer_param = DeclareLaunchArgument( + 'transformer_param_file', + default_value=transformer_param_file, + description='Path to config file for transformer node' + ) + + transformer_node = Node( + package='transformer', + executable='transformer_node', + parameters=[LaunchConfiguration('transformer_param_file')], + ) + + return LaunchDescription([ + transformer_param, + transformer_node + ]) diff --git a/src/world_modeling/occupancy_segmentation/src/transformer_core.cpp b/src/world_modeling/occupancy_segmentation/src/transformer_core.cpp new file mode 100644 index 00000000..35c3f015 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/src/transformer_core.cpp @@ -0,0 +1,64 @@ +#include +#include + +#include "transformer_core.hpp" + +namespace samples { + +TransformerCore::TransformerCore() {} + +std::vector TransformerCore::buffer_messages() const { return buffer_; } + +void TransformerCore::clear_buffer() { buffer_.clear(); } + +bool TransformerCore::validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered) { + return unfiltered->valid; +} + +bool TransformerCore::enqueue_message(const sample_msgs::msg::Filtered& msg) { + if (buffer_.size() < BUFFER_CAPACITY) { + buffer_.push_back(msg); + } + return buffer_.size() == BUFFER_CAPACITY; +} + +bool TransformerCore::deserialize_coordinate( + const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, + sample_msgs::msg::Filtered& filtered) { + std::string serialized_position = unfiltered->data; + auto start_pos = serialized_position.find("x:"); + auto end_pos = serialized_position.find(";"); + // Validate that the substrings were found + if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { + return false; + } + // Offset index to start of x_pos + start_pos += 2; + // Splice string and convert position to float + float x = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); + + start_pos = serialized_position.find("y:", end_pos + 1); + end_pos = serialized_position.find(";", end_pos + 1); + if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { + return false; + } + // Offset index to start of y_pos + start_pos += 2; + float y = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); + + start_pos = serialized_position.find("z:", end_pos + 1); + end_pos = serialized_position.find(";", end_pos + 1); + if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { + return false; + } + // Offset index to start of z_pos + start_pos += 2; + float z = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); + + filtered.pos_x = x; + filtered.pos_y = y; + filtered.pos_z = z; + return true; +} + +} // namespace samples diff --git a/src/world_modeling/occupancy_segmentation/src/transformer_node.cpp b/src/world_modeling/occupancy_segmentation/src/transformer_node.cpp new file mode 100644 index 00000000..51bd3399 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/src/transformer_node.cpp @@ -0,0 +1,51 @@ +#include + +#include "transformer_node.hpp" + +TransformerNode::TransformerNode() : Node("transformer"), transformer_(samples::TransformerCore()) { + raw_sub_ = this->create_subscription( + "/unfiltered_topic", ADVERTISING_FREQ, + std::bind(&TransformerNode::unfiltered_callback, this, std::placeholders::_1)); + transform_pub_ = + this->create_publisher("/filtered_topic", ADVERTISING_FREQ); + + // Define the default values for parameters if not defined in params.yaml + this->declare_parameter("version", rclcpp::ParameterValue(0)); + this->declare_parameter("compression_method", rclcpp::ParameterValue(0)); +} + +void TransformerNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg) { + if (!transformer_.validate_message(msg)) { + return; + } + + auto filtered = sample_msgs::msg::Filtered(); + if (!transformer_.deserialize_coordinate(msg, filtered)) { + return; + } + + filtered.timestamp = msg->timestamp; + filtered.metadata.version = this->get_parameter("version").as_int(); + filtered.metadata.compression_method = this->get_parameter("compression_method").as_int(); + + if (transformer_.enqueue_message(filtered)) { + RCLCPP_INFO(this->get_logger(), "Buffer Capacity Reached. PUBLISHING..."); + // Publish processed data when the buffer reaches its capacity + sample_msgs::msg::FilteredArray filtered_msgs; + auto buffer = transformer_.buffer_messages(); + transformer_.clear_buffer(); + + // Construct FilteredArray object + for (auto& packet : buffer) { + filtered_msgs.packets.push_back(packet); + } + transform_pub_->publish(filtered_msgs); + } +} + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp b/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp new file mode 100644 index 00000000..0d103350 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp @@ -0,0 +1,94 @@ +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" + +#include "transformer_core.hpp" + +/** + * When writing a large number of tests it is desirable to wrap any common + * setup or cleanup logic in a test fixture. This improves readability and reduces + * the amount of boilerplate code in each test. For more information checkout + * https://google.github.io/googletest/primer.html#same-data-multiple-tests + */ +class TransformerFixtureTest : public ::testing::Test { + public: + void SetUp(int enqueue_count) { + auto filtered = sample_msgs::msg::Filtered(); + for (int i = 0; i < enqueue_count; i++) { + transformer.enqueue_message(filtered); + } + } + + protected: + samples::TransformerCore transformer; +}; + +/** + * Parameterized tests let us test the same logic with different parameters without + * having to write boilerplate for multiple tests. For more information checkout + * https://google.github.io/googletest/advanced.html#value-parameterized-tests + */ +class TransformerParameterizedTest + : public ::testing::TestWithParam> { + protected: + samples::TransformerCore transformer; +}; + +TEST(TransformerTest, FilterInvalidField) { + auto unfiltered = std::make_shared(); + unfiltered->valid = false; + + auto transformer = samples::TransformerCore(); + bool valid = transformer.validate_message(unfiltered); + EXPECT_FALSE(valid); +} + +TEST_F(TransformerFixtureTest, BufferCapacity) { + SetUp(samples::TransformerCore::BUFFER_CAPACITY - 1); + + // Place last message that fits in buffer + auto filtered = sample_msgs::msg::Filtered(); + bool full = transformer.enqueue_message(filtered); + EXPECT_TRUE(full); + int size = transformer.buffer_messages().size(); + EXPECT_EQ(size, 10); + + // Attempting to enqueue message to full buffer should fail + full = transformer.enqueue_message(filtered); + EXPECT_TRUE(full); + size = transformer.buffer_messages().size(); + EXPECT_EQ(size, 10); +} + +TEST_F(TransformerFixtureTest, ClearBuffer) { + // Place 3 messages in buffer + SetUp(3); + + int size = transformer.buffer_messages().size(); + EXPECT_GT(size, 0); + + transformer.clear_buffer(); + size = transformer.buffer_messages().size(); + EXPECT_EQ(size, 0); +} + +TEST_P(TransformerParameterizedTest, SerializationValidation) { + auto [serialized_field, valid] = GetParam(); + auto filtered = sample_msgs::msg::Filtered(); + auto unfiltered = std::make_shared(); + unfiltered->data = serialized_field; + EXPECT_EQ(transformer.deserialize_coordinate(unfiltered, filtered), valid); +} + +// Parameterized testing lets us validate all edge cases for serialization +// using one test case. +INSTANTIATE_TEST_CASE_P(Serialization, TransformerParameterizedTest, + ::testing::Values(std::make_tuple("x:1;y:2;z:3", false), + std::make_tuple("z:1;y:2;x:3;", false), + std::make_tuple("x:1,y:2,z:3", false), + std::make_tuple("x:3;", false), + std::make_tuple("x:3;y:2;z:3;", true), + std::make_tuple("x:3;y:22; z:11;", true))); From 72686d7e910528edf37c4bd8ebaa8b82e68c1771 Mon Sep 17 00:00:00 2001 From: akilpath Date: Tue, 4 Jun 2024 00:04:31 +0000 Subject: [PATCH 02/59] Copied transformer node layout --- .../occupancy_segmentation/CMakeLists.txt | 71 +++++++++++++++++-- .../occupancy_segmentation/package.xml | 17 +++-- watod-config.sh | 2 +- 3 files changed, 76 insertions(+), 14 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/CMakeLists.txt b/src/world_modeling/occupancy_segmentation/CMakeLists.txt index dfa8b8bb..da069a60 100644 --- a/src/world_modeling/occupancy_segmentation/CMakeLists.txt +++ b/src/world_modeling/occupancy_segmentation/CMakeLists.txt @@ -1,14 +1,71 @@ -cmake_minimum_required(VERSION 3.8) -project(occupancy_segmentation) +cmake_minimum_required(VERSION 3.10) +project(transformer) + +# 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( REQUIRED) +# Search for dependencies required for building this package +find_package(ament_cmake REQUIRED) # ROS2 build tool +find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages + +# 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(transformer_lib + src/transformer_core.cpp) +# Indicate to compiler where to search for header files +target_include_directories(transformer_lib + PUBLIC + include) +# Add ROS2 dependencies required by package +ament_target_dependencies(transformer_lib + rclcpp + sample_msgs) + +# 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(transformer_test test/transformer_test.cpp) + # Link to the previously built library to access Transformer classes and methods + target_link_libraries(transformer_test + transformer_lib + gtest_main) + + # Copy executable to installation location + install(TARGETS + transformer_test + DESTINATION lib/${PROJECT_NAME}) +endif() + +# Create ROS2 node executable from source files +add_executable(transformer_node src/transformer_node.cpp) +# Link to the previously built library to access Transformer classes and methods +target_link_libraries(transformer_node transformer_lib) + +# Copy executable to installation location +install(TARGETS + transformer_node + DESTINATION lib/${PROJECT_NAME}) + +# Copy launch and config files to installation location +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME}) ament_package() + diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index 57c0712d..41a28c0f 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -1,16 +1,21 @@ - - occupancy_segmentation + transformer 0.0.0 - TODO: Package description - bolty - TODO: License declaration + A sample ROS package for pubsub communication + Cheran Mahalingam + TODO + + ament_cmake + rclcpp + sample_msgs + ament_cmake_gtest + ament_cmake - + \ No newline at end of file diff --git a/watod-config.sh b/watod-config.sh index 3255dd40..321efd80 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -15,7 +15,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -# ACTIVE_MODULES="" +ACTIVE_MODULES="world_modeling" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. From 55f41ebf06fb296b953679f17537c13c52a23d5c Mon Sep 17 00:00:00 2001 From: akilpath Date: Sat, 8 Jun 2024 20:50:52 +0000 Subject: [PATCH 03/59] Renamed files --- .../occupancy_segmentation/CMakeLists.txt | 52 +++++++++---------- .../occupancy_segmentation/config/params.yaml | 2 +- ...re.hpp => occupancy_segmentation_core.hpp} | 8 +-- ...de.hpp => occupancy_segmentation_node.hpp} | 12 ++--- ...ch.py => occupancy_segmentation.launch.py} | 12 ++--- .../occupancy_segmentation/package.xml | 2 +- ...re.cpp => occupancy_segmentation_core.cpp} | 14 ++--- ...de.cpp => occupancy_segmentation_node.cpp} | 18 +++---- 8 files changed, 60 insertions(+), 60 deletions(-) rename src/world_modeling/occupancy_segmentation/include/{transformer_core.hpp => occupancy_segmentation_core.hpp} (93%) rename src/world_modeling/occupancy_segmentation/include/{transformer_node.hpp => occupancy_segmentation_node.hpp} (84%) rename src/world_modeling/occupancy_segmentation/launch/{transformer.launch.py => occupancy_segmentation.launch.py} (63%) rename src/world_modeling/occupancy_segmentation/src/{transformer_core.cpp => occupancy_segmentation_core.cpp} (75%) rename src/world_modeling/occupancy_segmentation/src/{transformer_node.cpp => occupancy_segmentation_node.cpp} (67%) diff --git a/src/world_modeling/occupancy_segmentation/CMakeLists.txt b/src/world_modeling/occupancy_segmentation/CMakeLists.txt index da069a60..972557da 100644 --- a/src/world_modeling/occupancy_segmentation/CMakeLists.txt +++ b/src/world_modeling/occupancy_segmentation/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10) -project(transformer) +project(occupancy_segmentation) # Set compiler to use C++ 17 standard if(NOT CMAKE_CXX_STANDARD) @@ -20,45 +20,45 @@ find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages # 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(transformer_lib - src/transformer_core.cpp) +add_library(occupancy_segmentation_lib + src/occupancy_segmentation_core.cpp) # Indicate to compiler where to search for header files -target_include_directories(transformer_lib +target_include_directories(occupancy_segmentation_lib PUBLIC include) # Add ROS2 dependencies required by package -ament_target_dependencies(transformer_lib +ament_target_dependencies(occupancy_segmentation_lib rclcpp sample_msgs) -# 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) +# # 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(transformer_test test/transformer_test.cpp) - # Link to the previously built library to access Transformer classes and methods - target_link_libraries(transformer_test - transformer_lib - gtest_main) +# # 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 - transformer_test - DESTINATION lib/${PROJECT_NAME}) -endif() +# # Copy executable to installation location +# install(TARGETS +# occupancy_segmentation_test +# DESTINATION lib/${PROJECT_NAME}) +# endif() # Create ROS2 node executable from source files -add_executable(transformer_node src/transformer_node.cpp) -# Link to the previously built library to access Transformer classes and methods -target_link_libraries(transformer_node transformer_lib) +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 - transformer_node + occupancy_segmentation_node DESTINATION lib/${PROJECT_NAME}) # Copy launch and config files to installation location diff --git a/src/world_modeling/occupancy_segmentation/config/params.yaml b/src/world_modeling/occupancy_segmentation/config/params.yaml index d59e071f..47c10f5b 100644 --- a/src/world_modeling/occupancy_segmentation/config/params.yaml +++ b/src/world_modeling/occupancy_segmentation/config/params.yaml @@ -1,4 +1,4 @@ -transformer_node: +occupancy_segmentation_node: ros__parameters: version: 1 compression_method: 0 diff --git a/src/world_modeling/occupancy_segmentation/include/transformer_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp similarity index 93% rename from src/world_modeling/occupancy_segmentation/include/transformer_core.hpp rename to src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 300ca850..c02d0aad 100644 --- a/src/world_modeling/occupancy_segmentation/include/transformer_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -1,5 +1,5 @@ -#ifndef TRANSFORMER_CORE_HPP_ -#define TRANSFORMER_CORE_HPP_ +#ifndef OCCUPANCY_SEGMENTATION_CORE_HPP_ +#define OCCUPANCY_SEGMENTATION_CORE_HPP_ #include @@ -12,7 +12,7 @@ namespace samples { * Implementation for the internal logic for the Transformer ROS2 * node performing data processing and validation. */ -class TransformerCore { +class OccupancySegmentationCore { public: // Size of buffer before processed messages are published. static constexpr int BUFFER_CAPACITY = 10; @@ -21,7 +21,7 @@ class TransformerCore { /** * Transformer constructor. */ - TransformerCore(); + OccupancySegmentationCore(); /** * Retrieve enqueued messages in buffer. diff --git a/src/world_modeling/occupancy_segmentation/include/transformer_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp similarity index 84% rename from src/world_modeling/occupancy_segmentation/include/transformer_node.hpp rename to src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index 5dce7b23..10970065 100644 --- a/src/world_modeling/occupancy_segmentation/include/transformer_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -1,5 +1,5 @@ -#ifndef TRANSFORMER_NODE_HPP_ -#define TRANSFORMER_NODE_HPP_ +#ifndef OCCUPANCY_SEGMENTATION_NODE_HPP_ +#define OCCUPANCY_SEGMENTATION_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -7,7 +7,7 @@ #include "sample_msgs/msg/filtered_array.hpp" #include "sample_msgs/msg/unfiltered.hpp" -#include "transformer_core.hpp" +#include "occupancy_segmentation_core.hpp" /** * Implementation of a ROS2 node that converts unfiltered messages to filtered_array @@ -17,7 +17,7 @@ * and odd timestamps. Once the node collects BUFFER_CAPACITY messages it packs * the processed messages into an array and publishes it to the "filtered" topic. */ -class TransformerNode : public rclcpp::Node { +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 @@ -26,7 +26,7 @@ class TransformerNode : public rclcpp::Node { /** * Transformer node constructor. */ - TransformerNode(); + OccupancySegmentationNode(); private: /** @@ -44,7 +44,7 @@ class TransformerNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr transform_pub_; // Object that handles data processing and validation. - samples::TransformerCore transformer_; + samples::OccupancySegmentationCore segment_; }; #endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/launch/transformer.launch.py b/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py similarity index 63% rename from src/world_modeling/occupancy_segmentation/launch/transformer.launch.py rename to src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py index 802ca850..197640f5 100755 --- a/src/world_modeling/occupancy_segmentation/launch/transformer.launch.py +++ b/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py @@ -9,20 +9,20 @@ def generate_launch_description(): """Launch transformer node.""" - transformer_pkg_prefix = get_package_share_directory('transformer') + transformer_pkg_prefix = get_package_share_directory('occupancy_segmentation') transformer_param_file = os.path.join( transformer_pkg_prefix, 'config', 'params.yaml') transformer_param = DeclareLaunchArgument( - 'transformer_param_file', + 'occupancy_segmentation_param_file', default_value=transformer_param_file, - description='Path to config file for transformer node' + description='Path to config file for occupancy segmentation node' ) transformer_node = Node( - package='transformer', - executable='transformer_node', - parameters=[LaunchConfiguration('transformer_param_file')], + package='occupancy_segmentation', + executable='occupancy_segmentation_node', + parameters=[LaunchConfiguration('occupancy_segmentation_param_file')], ) return LaunchDescription([ diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index 41a28c0f..dfcf0ef7 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -1,6 +1,6 @@ - transformer + occupancy_segmentation 0.0.0 A sample ROS package for pubsub communication diff --git a/src/world_modeling/occupancy_segmentation/src/transformer_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp similarity index 75% rename from src/world_modeling/occupancy_segmentation/src/transformer_core.cpp rename to src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 35c3f015..20e0e0e5 100644 --- a/src/world_modeling/occupancy_segmentation/src/transformer_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -1,28 +1,28 @@ #include #include -#include "transformer_core.hpp" +#include "occupancy_segmentation_core.hpp" namespace samples { -TransformerCore::TransformerCore() {} +OccupancySegmentationCore::OccupancySegmentationCore() {} -std::vector TransformerCore::buffer_messages() const { return buffer_; } +std::vector OccupancySegmentationCore::buffer_messages() const { return buffer_; } -void TransformerCore::clear_buffer() { buffer_.clear(); } +void OccupancySegmentationCore::clear_buffer() { buffer_.clear(); } -bool TransformerCore::validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered) { +bool OccupancySegmentationCore::validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered) { return unfiltered->valid; } -bool TransformerCore::enqueue_message(const sample_msgs::msg::Filtered& msg) { +bool OccupancySegmentationCore::enqueue_message(const sample_msgs::msg::Filtered& msg) { if (buffer_.size() < BUFFER_CAPACITY) { buffer_.push_back(msg); } return buffer_.size() == BUFFER_CAPACITY; } -bool TransformerCore::deserialize_coordinate( +bool OccupancySegmentationCore::deserialize_coordinate( const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, sample_msgs::msg::Filtered& filtered) { std::string serialized_position = unfiltered->data; diff --git a/src/world_modeling/occupancy_segmentation/src/transformer_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp similarity index 67% rename from src/world_modeling/occupancy_segmentation/src/transformer_node.cpp rename to src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 51bd3399..9dec7c86 100644 --- a/src/world_modeling/occupancy_segmentation/src/transformer_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -1,11 +1,11 @@ #include -#include "transformer_node.hpp" +#include "occupancy_segmentation_node.hpp" -TransformerNode::TransformerNode() : Node("transformer"), transformer_(samples::TransformerCore()) { +OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation"), segment_(samples::OccupancySegmentationCore()) { raw_sub_ = this->create_subscription( "/unfiltered_topic", ADVERTISING_FREQ, - std::bind(&TransformerNode::unfiltered_callback, this, std::placeholders::_1)); + std::bind(&OccupancySegmentationNode::unfiltered_callback, this, std::placeholders::_1)); transform_pub_ = this->create_publisher("/filtered_topic", ADVERTISING_FREQ); @@ -14,13 +14,13 @@ TransformerNode::TransformerNode() : Node("transformer"), transformer_(samples:: this->declare_parameter("compression_method", rclcpp::ParameterValue(0)); } -void TransformerNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg) { - if (!transformer_.validate_message(msg)) { +void OccupancySegmentationNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg) { + if (!segment_.validate_message(msg)) { return; } auto filtered = sample_msgs::msg::Filtered(); - if (!transformer_.deserialize_coordinate(msg, filtered)) { + if (!segment_.deserialize_coordinate(msg, filtered)) { return; } @@ -32,8 +32,8 @@ void TransformerNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::Sh RCLCPP_INFO(this->get_logger(), "Buffer Capacity Reached. PUBLISHING..."); // Publish processed data when the buffer reaches its capacity sample_msgs::msg::FilteredArray filtered_msgs; - auto buffer = transformer_.buffer_messages(); - transformer_.clear_buffer(); + auto buffer = segment_.buffer_messages(); + segment_.clear_buffer(); // Construct FilteredArray object for (auto& packet : buffer) { @@ -45,7 +45,7 @@ void TransformerNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::Sh int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } From 62c740773c5045012b7546d67b348f6d1a6bfe86 Mon Sep 17 00:00:00 2001 From: VishGit1234 Date: Sat, 8 Jun 2024 21:45:08 +0000 Subject: [PATCH 04/59] Fix small error --- .../occupancy_segmentation/src/occupancy_segmentation_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 9dec7c86..b1a0411a 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -28,7 +28,7 @@ void OccupancySegmentationNode::unfiltered_callback(const sample_msgs::msg::Unfi filtered.metadata.version = this->get_parameter("version").as_int(); filtered.metadata.compression_method = this->get_parameter("compression_method").as_int(); - if (transformer_.enqueue_message(filtered)) { + if (segment_.enqueue_message(filtered)) { RCLCPP_INFO(this->get_logger(), "Buffer Capacity Reached. PUBLISHING..."); // Publish processed data when the buffer reaches its capacity sample_msgs::msg::FilteredArray filtered_msgs; From 29ca16c2f911ebbb69428e4b4870fe0b54562f7f Mon Sep 17 00:00:00 2001 From: akilpath Date: Thu, 13 Jun 2024 03:13:58 +0000 Subject: [PATCH 05/59] Clean up --- .../include/occupancy_segmentation_core.hpp | 52 ---------- .../include/occupancy_segmentation_node.hpp | 21 +---- .../launch/occupancy_segmentation.launch.py | 18 ++-- .../src/occupancy_segmentation_core.cpp | 59 +----------- .../src/occupancy_segmentation_node.cpp | 43 ++------- .../test/transformer_test.cpp | 94 ------------------- 6 files changed, 19 insertions(+), 268 deletions(-) delete mode 100644 src/world_modeling/occupancy_segmentation/test/transformer_test.cpp diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index c02d0aad..db1862a0 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -3,10 +3,6 @@ #include -#include "sample_msgs/msg/filtered.hpp" -#include "sample_msgs/msg/unfiltered.hpp" - -namespace samples { /** * Implementation for the internal logic for the Transformer ROS2 @@ -22,55 +18,7 @@ class OccupancySegmentationCore { * Transformer constructor. */ OccupancySegmentationCore(); - - /** - * Retrieve enqueued messages in buffer. - * - * @returns enqueued processed messages - */ - std::vector buffer_messages() const; - - /** - * Removes all messages in buffer. Called by the transformer - * node after messages have been published to aggregator. - */ - void clear_buffer(); - - /** - * Validates that the 'valid' field of an unfiltered message - * is set to true. - * - * @param unfiltered a raw message - * @returns whether message's 'valid' field is set - */ - bool validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered); - - /** - * Enqueue message into an array of processed messages to "filtered" topic. - * Ignores messages once the buffer capacity is reached. - * - * @param msg a processed message to be published - * @returns whether buffer is full after adding new message - */ - bool enqueue_message(const sample_msgs::msg::Filtered& msg); - - /** - * Deserializes the data field of the unfiltered ROS2 message. - * The data field should be of the form "x:$num1;y:$num2;z:$num3;". - * - * @param[in] unfiltered the raw message containing serialized data - * @param[out] filtered the processed message containing deserialized data - * @returns whether deserialization was successful - */ - bool deserialize_coordinate(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, - sample_msgs::msg::Filtered& filtered); - - private: - // Buffer storing processed messages until BUFFER_CAPACITY. Clear after - // messages are published. - std::vector buffer_; }; -} // namespace samples #endif // TRANSFORMER_CORE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index 10970065..580ee4dc 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -3,10 +3,6 @@ #include "rclcpp/rclcpp.hpp" -#include "sample_msgs/msg/filtered.hpp" -#include "sample_msgs/msg/filtered_array.hpp" -#include "sample_msgs/msg/unfiltered.hpp" - #include "occupancy_segmentation_core.hpp" /** @@ -29,22 +25,11 @@ class OccupancySegmentationNode : public rclcpp::Node { OccupancySegmentationNode(); private: - /** - * A ROS2 subscription node callback used to process raw data from the - * "unfiltered" topic and publish to the "filtered" topic. - * - * @param msg a raw message from the "unfiltered" topic - */ - void unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg); - - // ROS2 subscriber listening to the unfiltered topic. - rclcpp::Subscription::SharedPtr raw_sub_; - - // ROS2 publisher sending processed messages to the filtered topic. - rclcpp::Publisher::SharedPtr transform_pub_; // Object that handles data processing and validation. - samples::OccupancySegmentationCore segment_; + + rclcpp::TimerBase::SharedPtr timer_; + void timer_callback(); }; #endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py b/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py index 197640f5..975e8a31 100755 --- a/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py +++ b/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py @@ -8,24 +8,24 @@ def generate_launch_description(): - """Launch transformer node.""" - transformer_pkg_prefix = get_package_share_directory('occupancy_segmentation') - transformer_param_file = os.path.join( - transformer_pkg_prefix, 'config', 'params.yaml') + """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') - transformer_param = DeclareLaunchArgument( + occupancy_seg_param = DeclareLaunchArgument( 'occupancy_segmentation_param_file', - default_value=transformer_param_file, + default_value=occupancy_seg_param_file, description='Path to config file for occupancy segmentation node' ) - transformer_node = Node( + occupancy_seg_node = Node( package='occupancy_segmentation', executable='occupancy_segmentation_node', parameters=[LaunchConfiguration('occupancy_segmentation_param_file')], ) return LaunchDescription([ - transformer_param, - transformer_node + occupancy_seg_param, + occupancy_seg_node ]) diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 20e0e0e5..b8425eea 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -3,62 +3,5 @@ #include "occupancy_segmentation_core.hpp" -namespace samples { -OccupancySegmentationCore::OccupancySegmentationCore() {} - -std::vector OccupancySegmentationCore::buffer_messages() const { return buffer_; } - -void OccupancySegmentationCore::clear_buffer() { buffer_.clear(); } - -bool OccupancySegmentationCore::validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered) { - return unfiltered->valid; -} - -bool OccupancySegmentationCore::enqueue_message(const sample_msgs::msg::Filtered& msg) { - if (buffer_.size() < BUFFER_CAPACITY) { - buffer_.push_back(msg); - } - return buffer_.size() == BUFFER_CAPACITY; -} - -bool OccupancySegmentationCore::deserialize_coordinate( - const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, - sample_msgs::msg::Filtered& filtered) { - std::string serialized_position = unfiltered->data; - auto start_pos = serialized_position.find("x:"); - auto end_pos = serialized_position.find(";"); - // Validate that the substrings were found - if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { - return false; - } - // Offset index to start of x_pos - start_pos += 2; - // Splice string and convert position to float - float x = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); - - start_pos = serialized_position.find("y:", end_pos + 1); - end_pos = serialized_position.find(";", end_pos + 1); - if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { - return false; - } - // Offset index to start of y_pos - start_pos += 2; - float y = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); - - start_pos = serialized_position.find("z:", end_pos + 1); - end_pos = serialized_position.find(";", end_pos + 1); - if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { - return false; - } - // Offset index to start of z_pos - start_pos += 2; - float z = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); - - filtered.pos_x = x; - filtered.pos_y = y; - filtered.pos_z = z; - return true; -} - -} // namespace samples +OccupancySegmentationCore::OccupancySegmentationCore() {} \ No newline at end of file diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index b1a0411a..38d6f2eb 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -1,49 +1,18 @@ #include - +#include #include "occupancy_segmentation_node.hpp" -OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation"), segment_(samples::OccupancySegmentationCore()) { - raw_sub_ = this->create_subscription( - "/unfiltered_topic", ADVERTISING_FREQ, - std::bind(&OccupancySegmentationNode::unfiltered_callback, this, std::placeholders::_1)); - transform_pub_ = - this->create_publisher("/filtered_topic", ADVERTISING_FREQ); +OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { - // Define the default values for parameters if not defined in params.yaml - this->declare_parameter("version", rclcpp::ParameterValue(0)); - this->declare_parameter("compression_method", rclcpp::ParameterValue(0)); + timer_ = this->create_wall_timer(std::chrono::milliseconds(1000), std::bind(&OccupancySegmentationNode::timer_callback, this)); } -void OccupancySegmentationNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg) { - if (!segment_.validate_message(msg)) { - return; - } - - auto filtered = sample_msgs::msg::Filtered(); - if (!segment_.deserialize_coordinate(msg, filtered)) { - return; - } - - filtered.timestamp = msg->timestamp; - filtered.metadata.version = this->get_parameter("version").as_int(); - filtered.metadata.compression_method = this->get_parameter("compression_method").as_int(); - - if (segment_.enqueue_message(filtered)) { - RCLCPP_INFO(this->get_logger(), "Buffer Capacity Reached. PUBLISHING..."); - // Publish processed data when the buffer reaches its capacity - sample_msgs::msg::FilteredArray filtered_msgs; - auto buffer = segment_.buffer_messages(); - segment_.clear_buffer(); - - // Construct FilteredArray object - for (auto& packet : buffer) { - filtered_msgs.packets.push_back(packet); - } - transform_pub_->publish(filtered_msgs); - } +void OccupancySegmentationNode::timer_callback(){ + RCLCPP_INFO(this->get_logger(), "Working"); } int main(int argc, char** argv) { + std::cout << "Huh" << std::endl; rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp b/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp deleted file mode 100644 index 0d103350..00000000 --- a/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp +++ /dev/null @@ -1,94 +0,0 @@ -#include -#include -#include - -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" - -#include "transformer_core.hpp" - -/** - * When writing a large number of tests it is desirable to wrap any common - * setup or cleanup logic in a test fixture. This improves readability and reduces - * the amount of boilerplate code in each test. For more information checkout - * https://google.github.io/googletest/primer.html#same-data-multiple-tests - */ -class TransformerFixtureTest : public ::testing::Test { - public: - void SetUp(int enqueue_count) { - auto filtered = sample_msgs::msg::Filtered(); - for (int i = 0; i < enqueue_count; i++) { - transformer.enqueue_message(filtered); - } - } - - protected: - samples::TransformerCore transformer; -}; - -/** - * Parameterized tests let us test the same logic with different parameters without - * having to write boilerplate for multiple tests. For more information checkout - * https://google.github.io/googletest/advanced.html#value-parameterized-tests - */ -class TransformerParameterizedTest - : public ::testing::TestWithParam> { - protected: - samples::TransformerCore transformer; -}; - -TEST(TransformerTest, FilterInvalidField) { - auto unfiltered = std::make_shared(); - unfiltered->valid = false; - - auto transformer = samples::TransformerCore(); - bool valid = transformer.validate_message(unfiltered); - EXPECT_FALSE(valid); -} - -TEST_F(TransformerFixtureTest, BufferCapacity) { - SetUp(samples::TransformerCore::BUFFER_CAPACITY - 1); - - // Place last message that fits in buffer - auto filtered = sample_msgs::msg::Filtered(); - bool full = transformer.enqueue_message(filtered); - EXPECT_TRUE(full); - int size = transformer.buffer_messages().size(); - EXPECT_EQ(size, 10); - - // Attempting to enqueue message to full buffer should fail - full = transformer.enqueue_message(filtered); - EXPECT_TRUE(full); - size = transformer.buffer_messages().size(); - EXPECT_EQ(size, 10); -} - -TEST_F(TransformerFixtureTest, ClearBuffer) { - // Place 3 messages in buffer - SetUp(3); - - int size = transformer.buffer_messages().size(); - EXPECT_GT(size, 0); - - transformer.clear_buffer(); - size = transformer.buffer_messages().size(); - EXPECT_EQ(size, 0); -} - -TEST_P(TransformerParameterizedTest, SerializationValidation) { - auto [serialized_field, valid] = GetParam(); - auto filtered = sample_msgs::msg::Filtered(); - auto unfiltered = std::make_shared(); - unfiltered->data = serialized_field; - EXPECT_EQ(transformer.deserialize_coordinate(unfiltered, filtered), valid); -} - -// Parameterized testing lets us validate all edge cases for serialization -// using one test case. -INSTANTIATE_TEST_CASE_P(Serialization, TransformerParameterizedTest, - ::testing::Values(std::make_tuple("x:1;y:2;z:3", false), - std::make_tuple("z:1;y:2;x:3;", false), - std::make_tuple("x:1,y:2,z:3", false), - std::make_tuple("x:3;", false), - std::make_tuple("x:3;y:2;z:3;", true), - std::make_tuple("x:3;y:22; z:11;", true))); From d0212019400c35fb884b1357bfc3e296cb8ac43c Mon Sep 17 00:00:00 2001 From: VishGit1234 Date: Sun, 16 Jun 2024 02:56:13 +0000 Subject: [PATCH 06/59] Add pcl package --- src/world_modeling/occupancy_segmentation/CMakeLists.txt | 4 +++- src/world_modeling/occupancy_segmentation/package.xml | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/CMakeLists.txt b/src/world_modeling/occupancy_segmentation/CMakeLists.txt index 972557da..ff5d26bb 100644 --- a/src/world_modeling/occupancy_segmentation/CMakeLists.txt +++ b/src/world_modeling/occupancy_segmentation/CMakeLists.txt @@ -14,6 +14,7 @@ endif() find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages +find_package(pcl_ros REQUIRED) # Compiles source files into a library # A library is not executed, instead other executables can link @@ -29,7 +30,8 @@ target_include_directories(occupancy_segmentation_lib # Add ROS2 dependencies required by package ament_target_dependencies(occupancy_segmentation_lib rclcpp - sample_msgs) + sample_msgs + pcl_ros) # # By default tests are built. This can be overridden by modifying the # # colcon build command to include the -DBUILD_TESTING=OFF flag. diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index dfcf0ef7..a6ed23aa 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -11,6 +11,7 @@ ament_cmake rclcpp sample_msgs + pcl_ros ament_cmake_gtest @@ -18,4 +19,4 @@ ament_cmake - \ No newline at end of file + From 3368d02062afcab9a0c64da9b6f051f27df70239 Mon Sep 17 00:00:00 2001 From: akilpath Date: Mon, 17 Jun 2024 02:37:11 +0000 Subject: [PATCH 07/59] Added eigen to dockerifle --- .../occupancy_segmentation/occupancy_segmentation.Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index 1514079f..74fbe85f 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -16,6 +16,7 @@ RUN apt-get -qq update && rosdep update && \ | awk '{print $3}' \ | sort > /tmp/colcon_install_list +RUN sudo apt-get install libeigen3-dev ################################# Dependencies ################################ FROM ${BASE_IMAGE} as dependencies From 3ccbbfb52a2fcbcdf9c6a57a5a4fc50dc445419c Mon Sep 17 00:00:00 2001 From: VishGit1234 <47926842+VishGit1234@users.noreply.github.com> Date: Wed, 19 Jun 2024 15:58:25 -0400 Subject: [PATCH 08/59] Update docker-compose.world_modeling.yaml --- .../dev_overrides/docker-compose.world_modeling.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/dev_overrides/docker-compose.world_modeling.yaml b/modules/dev_overrides/docker-compose.world_modeling.yaml index 7682c63a..55053d40 100644 --- a/modules/dev_overrides/docker-compose.world_modeling.yaml +++ b/modules/dev_overrides/docker-compose.world_modeling.yaml @@ -13,7 +13,7 @@ services: image: "${WORLD_MODELING_HD_MAP_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/hd_map + - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/hd_map localization: <<: *fixuid @@ -23,7 +23,7 @@ services: image: "${WORLD_MODELING_LOCALIZATION_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/localization + - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/localization occupancy: <<: *fixuid @@ -33,7 +33,7 @@ services: image: "${WORLD_MODELING_OCCUPANCY_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/occupancy + - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/occupancy occupancy_segmentation: <<: *fixuid @@ -43,7 +43,7 @@ services: image: "${WORLD_MODELING_OCCUPANCY_SEGMENTATION_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/occupancy_segmentation + - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/occupancy_segmentation motion_forecasting: <<: *fixuid @@ -53,4 +53,4 @@ services: image: "${WORLD_MODELING_MOTION_FORECASTING_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/motion_forecasting + - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/motion_forecasting From c98364de0b1b363af79f91f43fb6522de7cdc6e2 Mon Sep 17 00:00:00 2001 From: VishGit1234 <47926842+VishGit1234@users.noreply.github.com> Date: Wed, 19 Jun 2024 16:00:33 -0400 Subject: [PATCH 09/59] Update docker-compose.world_modeling.yaml --- modules/dev_overrides/docker-compose.world_modeling.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/dev_overrides/docker-compose.world_modeling.yaml b/modules/dev_overrides/docker-compose.world_modeling.yaml index 55053d40..8cee3f7c 100644 --- a/modules/dev_overrides/docker-compose.world_modeling.yaml +++ b/modules/dev_overrides/docker-compose.world_modeling.yaml @@ -43,7 +43,7 @@ services: image: "${WORLD_MODELING_OCCUPANCY_SEGMENTATION_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/occupancy_segmentation + - ${MONO_DIR}/src/world_modeling/occupancy_segmentation:/home/bolty/ament_ws/src/occupancy_segmentation motion_forecasting: <<: *fixuid From bbd14b9305e6b15d67cf0efd4ae0e9ab8b5eb2a0 Mon Sep 17 00:00:00 2001 From: akilpath Date: Wed, 19 Jun 2024 22:26:29 +0000 Subject: [PATCH 10/59] added tbb to dockerfile --- .../occupancy_segmentation/occupancy_segmentation.Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index 74fbe85f..55d9a87a 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -17,6 +17,7 @@ RUN apt-get -qq update && rosdep update && \ | sort > /tmp/colcon_install_list RUN sudo apt-get install libeigen3-dev +RUN sudo apt-get -y install libtbb-dev ################################# Dependencies ################################ FROM ${BASE_IMAGE} as dependencies From 1db81c728a10ca0be232a9995e341c66eb2f717c Mon Sep 17 00:00:00 2001 From: akilpath Date: Sun, 23 Jun 2024 04:32:03 +0000 Subject: [PATCH 11/59] Started working on it --- .../occupancy_segmentation/CMakeLists.txt | 2 - .../include/occupancy_segmentation_core.hpp | 43 +++++++++++++++---- .../occupancy_segmentation/package.xml | 1 - .../src/occupancy_segmentation_core.cpp | 40 ++++++++++++++++- .../src/occupancy_segmentation_node.cpp | 1 - 5 files changed, 73 insertions(+), 14 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/CMakeLists.txt b/src/world_modeling/occupancy_segmentation/CMakeLists.txt index ff5d26bb..8df7a7cd 100644 --- a/src/world_modeling/occupancy_segmentation/CMakeLists.txt +++ b/src/world_modeling/occupancy_segmentation/CMakeLists.txt @@ -13,7 +13,6 @@ endif() # Search for dependencies required for building this package find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package -find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages find_package(pcl_ros REQUIRED) # Compiles source files into a library @@ -30,7 +29,6 @@ target_include_directories(occupancy_segmentation_lib # Add ROS2 dependencies required by package ament_target_dependencies(occupancy_segmentation_lib rclcpp - sample_msgs pcl_ros) # # By default tests are built. This can be overridden by modifying the diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index db1862a0..01ab8654 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -2,6 +2,17 @@ #define OCCUPANCY_SEGMENTATION_CORE_HPP_ #include +#include +#include +#include + +#include +#include + + +#define NUM_ZONES 4 +#define L_MIN 2.7 +#define L_MAX 80.0 /** @@ -9,15 +20,29 @@ * node performing data processing and validation. */ class OccupancySegmentationCore { - public: - // Size of buffer before processed messages are published. - static constexpr int BUFFER_CAPACITY = 10; - - public: - /** - * Transformer constructor. - */ - OccupancySegmentationCore(); + public: + typedef std::vector> Ring; + typedef std::vector Zone; + // Size of buffer before processed messages are published. + static constexpr int BUFFER_CAPACITY = 10; + + const int ZONE_RINGS [NUM_ZONES] = {2,4,4,4}; + const int ZONE_SECTORS [NUM_ZONES] = {16,32,54,32}; + + + std::vector czm; + + + OccupancySegmentationCore(); + + private: + + void init_czm(); + + void fill_czm(pcl::PointCloud &cloud_in); + + + }; diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index a6ed23aa..e84324c4 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -10,7 +10,6 @@ ament_cmake rclcpp - sample_msgs pcl_ros ament_cmake_gtest diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index b8425eea..e77fbd6a 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -4,4 +4,42 @@ #include "occupancy_segmentation_core.hpp" -OccupancySegmentationCore::OccupancySegmentationCore() {} \ No newline at end of file +OccupancySegmentationCore::OccupancySegmentationCore() {} + +void OccupancySegmentationCore::init_czm(){ + std::vector temp_czm; + czm = temp_czm; + for(int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){ + Zone zone; + for(int r = 0; r < ZONE_RINGS[zone_idx]; r++){ + Ring ring; + for (int s = 0; s < ZONE_SECTORS[zone_idx]; s++){ + pcl::PointCloud patch; + ring.emplace_back(patch); + } + zone.emplace_back(ring); + } + czm.emplace_back(zone); + } +} + +void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in){ + for(pcl::PointXYZ &p : cloud_in.points){ + double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); + double theta = atan2(p.y,p.x); // EDITED! + if (theta < 0){ + theta += 2*M_PI; + } + double lmins[4] = {L_MIN, (7*L_MIN + L_MAX) / 8, (3*L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + double deltal = L_MAX - L_MIN; + + for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){ + + } + + + + } + +} \ No newline at end of file diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 38d6f2eb..baa6475a 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -12,7 +12,6 @@ void OccupancySegmentationNode::timer_callback(){ } int main(int argc, char** argv) { - std::cout << "Huh" << std::endl; rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); From 9d7e70697637df80f8e62a021ffb01f04b44887e Mon Sep 17 00:00:00 2001 From: akilpath Date: Sun, 23 Jun 2024 17:17:14 +0000 Subject: [PATCH 12/59] Finished czm initialization --- .../src/occupancy_segmentation_core.cpp | 28 +++++++++++++------ watod-config.sh | 2 +- 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index e77fbd6a..3c12ab14 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -24,22 +24,34 @@ void OccupancySegmentationCore::init_czm(){ } void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in){ + double lmins[4] = {L_MIN, (7*L_MIN + L_MAX) / 8, (3*L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; for(pcl::PointXYZ &p : cloud_in.points){ double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); - double theta = atan2(p.y,p.x); // EDITED! + + // Out of range for czm + if (r < L_MIN || r > L_MAX){ + continue; + } + + double theta = atan2(p.y,p.x); if (theta < 0){ theta += 2*M_PI; } - double lmins[4] = {L_MIN, (7*L_MIN + L_MAX) / 8, (3*L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + double deltal = L_MAX - L_MIN; for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){ - + int ring_idx, sector_idx; + if (r < lmaxs[zone_idx]){ + double ring_size = deltal / ZONE_RINGS[zone_idx]; + double sector_size = 2*M_PI / ZONE_SECTORS[zone_idx]; + ring_idx = (int) ((r - lmins[zone_idx]) / ring_size); + //TODO: find out why the use min() in the paper, meantime use the top + //ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); + sector_idx = (int) (theta / sector_size); + czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + } } - - - } - } \ No newline at end of file diff --git a/watod-config.sh b/watod-config.sh index 321efd80..dc6a6b1c 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -23,7 +23,7 @@ ACTIVE_MODULES="world_modeling" ## - deploy (default) : runs production-grade containers (non-editable) ## - develop : runs developer containers (editable) -# MODE_OF_OPERATION="" +MODE_OF_OPERATION="develop" ############################## ADVANCED CONFIGURATIONS ############################## ## Name to append to docker containers. DEFAULT = "" From 67b27d8870bd23856da9b94ab35509e7bb8b9a60 Mon Sep 17 00:00:00 2001 From: akilpath Date: Wed, 26 Jun 2024 02:36:30 +0000 Subject: [PATCH 13/59] Plane --- .../include/occupancy_segmentation_core.hpp | 23 +++++++++++++++---- .../src/occupancy_segmentation_core.cpp | 23 +++++++++++++++++++ 2 files changed, 42 insertions(+), 4 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 01ab8654..9a29000b 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -8,17 +8,30 @@ #include #include +#include #define NUM_ZONES 4 #define L_MIN 2.7 #define L_MAX 80.0 +#define N_SEED 20 +#define Z_SEED 0.5 +#define MD 0.3 +#define MH -1.1 + + +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_; +}; -/** - * Implementation for the internal logic for the Transformer ROS2 - * node performing data processing and validation. - */ class OccupancySegmentationCore { public: typedef std::vector> Ring; @@ -41,6 +54,8 @@ class OccupancySegmentationCore { void fill_czm(pcl::PointCloud &cloud_in); + void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); + }; diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 3c12ab14..28cbd4fe 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -54,4 +54,27 @@ void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_i } } } +} + +void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat){ + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = ( feat.singular_values_(0) - feat.singular_values_(1) ) / feat.singular_values_(0); + feat.planarity_ = ( feat.singular_values_(1) - feat.singular_values_(2) ) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; } \ No newline at end of file From 23aafe4f036c59d36cf22e897fabc7ebb6f02d1c Mon Sep 17 00:00:00 2001 From: akilpath Date: Sun, 7 Jul 2024 22:57:54 +0000 Subject: [PATCH 14/59] Mostly done --- .../occupancy_segmentation.Dockerfile | 1 + .../include/occupancy_segmentation_core.hpp | 33 ++- .../src/occupancy_segmentation_core.cpp | 246 +++++++++++++----- 3 files changed, 221 insertions(+), 59 deletions(-) diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index 55d9a87a..ae27a07e 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -6,6 +6,7 @@ FROM ${BASE_IMAGE} as source WORKDIR ${AMENT_WS}/src # Copy in source code +# RUN git clone https://github.com/ros-perception/perception_pcl.git --branch 2.4.3 COPY src/world_modeling/occupancy_segmentation occupancy_segmentation COPY src/wato_msgs/sample_msgs sample_msgs diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 9a29000b..0d2b92a7 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -20,6 +20,11 @@ #define MD 0.3 #define MH -1.1 +#define MIN_NUM_POINTS 10 +#define NUM_SEED_POINTS 20 +#define TH_SEEDS 0.5 +#define UPRIGHTNESS_THRESH 45 + struct PCAFeature { Eigen::Vector3f principal_; @@ -32,6 +37,14 @@ struct PCAFeature { float planarity_; }; +struct Patch_Index { + int idx; + int zone_idx; + int ring_idx; + int sector_idx; + int concentric_idx; +} + class OccupancySegmentationCore { public: typedef std::vector> Ring; @@ -42,8 +55,16 @@ class OccupancySegmentationCore { const int ZONE_RINGS [NUM_ZONES] = {2,4,4,4}; const int ZONE_SECTORS [NUM_ZONES] = {16,32,54,32}; + int num_patches = -1; + + std::vector _czm; + std::vector> _regionwise_ground; + std::vector> _regionwise_nonground; - std::vector czm; + std::vector _patch_indices; + + pcl::PointCloud _ground; + pcl::Pointcloud _non_ground; OccupancySegmentationCore(); @@ -55,6 +76,16 @@ class OccupancySegmentationCore { void fill_czm(pcl::PointCloud &cloud_in); void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); + + void rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat); + + void extract_initial_seeds(pcl::PointCloud &cloud, pcl::PointCloud &seed_cloud, int zone_idx); + + bool ground_likelihood_est(PCAFeature &feat, int concentric_idx); + + void segment_ground(); + + bool point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b); diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 28cbd4fe..c804e9de 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -3,78 +3,208 @@ #include "occupancy_segmentation_core.hpp" - OccupancySegmentationCore::OccupancySegmentationCore() {} -void OccupancySegmentationCore::init_czm(){ - std::vector temp_czm; - czm = temp_czm; - for(int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){ - Zone zone; - for(int r = 0; r < ZONE_RINGS[zone_idx]; r++){ - Ring ring; - for (int s = 0; s < ZONE_SECTORS[zone_idx]; s++){ - pcl::PointCloud patch; - ring.emplace_back(patch); - } - zone.emplace_back(ring); - } - czm.emplace_back(zone); +void OccupancySegmentationCore::init_czm() { + int concentric_count = 0; + int patch_count = 0; + + for (int z = 0; z < NUM_ZONES; z++) { + Zone zone; + Patch_Idx index; + index.zone_idx = z; + + for (int r = 0; r < ZONE_RINGS[z]; r++) { + Ring ring; + index.ring_idx = r; + index.concentric_idx = concentric_count; + + for (int s = 0; s < ZONE_SECTORS[z]; s++) { + index.sector_idx = s; + index.patch_idx = patch_count; + _patch_indices.push_back(index); + pcl::PointCloud patch; + ring.emplace_back(patch); + _regionwise_ground.push_back(pcl::PointCloud>()); + _regionwise_nonground.push_back(pcl::PointCloud>()); + + patch_count++; + } + zone.emplace_back(ring); } + _czm.emplace_back(zone); + } + num_patches = patch_count; } -void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in){ - double lmins[4] = {L_MIN, (7*L_MIN + L_MAX) / 8, (3*L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; - for(pcl::PointXYZ &p : cloud_in.points){ - double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); +void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { + double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + for (pcl::PointXYZ &p : cloud_in.points) { + double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); - // Out of range for czm - if (r < L_MIN || r > L_MAX){ - continue; - } + // Out of range for czm + if (r < L_MIN || r > L_MAX) { + continue; + } - double theta = atan2(p.y,p.x); - if (theta < 0){ - theta += 2*M_PI; - } + double theta = atan2(p.y, p.x); + if (theta < 0) { + theta += 2 * M_PI; + } + + double deltal = L_MAX - L_MIN; + + for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { + int ring_idx, sector_idx; + if (r < lmaxs[zone_idx]) { + double ring_size = deltal / ZONE_RINGS[zone_idx]; + double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; + ring_idx = (int)((r - lmins[zone_idx]) / ring_size); + // TODO: find out why the use min() in the paper, meantime use the top + // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); + sector_idx = (int)(theta / sector_size); + czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + } + } + } +} + +void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, + PCAFeature &feat) { + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = + (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = + (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; +} + +void OccupancySegmentationCore::segment_ground() { + tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { + for (int patch_num = r.begin(); patch_num < r.end(); r++) { + Patch_Index &p_idx = patch_indices[patch_num]; + pcl::PointCloud &patch = czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + PCAFeature features; + if (patch.points.size() > MIN_NUM_POINTS) { + std::sort(patch.points.begin(), patch.points.end()); + pcl::PointCloud ground_temp; + rgpf(patch, p_idx, feat); - double deltal = L_MAX - L_MIN; - - for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){ - int ring_idx, sector_idx; - if (r < lmaxs[zone_idx]){ - double ring_size = deltal / ZONE_RINGS[zone_idx]; - double sector_size = 2*M_PI / ZONE_SECTORS[zone_idx]; - ring_idx = (int) ((r - lmins[zone_idx]) / ring_size); - //TODO: find out why the use min() in the paper, meantime use the top - //ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); - sector_idx = (int) (theta / sector_size); - czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + } + } + }); +} + +void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { + pcl::PointCloud ground_temp; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + + if (!region_ground.empty()) region_ground.clear(); + if (!region_nonground.empty()) region_nonground.clear(); + + size_t N = patch.size(); + + extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); + int num_iters = 3; + for(int i = 0; i < num_iters; i++){ + estimate_plane(ground_temp, feat); + ground_temp.clear(); + Eigen::MatrixXf points(N, 3); + int j = 0; + for (pcl::PointXYZ &p:src.points) { + points.row(j++) = p.getVector3fMap(); + } + + Eigen::VectorXf result = points * feat.normal_; + for (size_t r = 0; r < N; r++) { + if (i < num_iters - 1) { + if (result[r] < feat.th_dist_d_) { + ground_tmp.points.emplace_back(src[r]); + } + } else { // Final stage + if (result[r] < feat.th_dist_d_) { + regionwise_ground.points.emplace_back(src[r]); + } else { + regionwise_nonground.points.emplace_back(src[r]); + } } } + + } +} + +bool OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ + + //uprightness filter + if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ + return false; + } + + const double z_elevation = feat.mean_(2); + const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + + //elevation filter + if (concentric_idx < num_rings_of_interest_) { + if (z_elevation > -sensor_height_ + elevation_thr_[concentric_idx] + && flatness_thr_[concentric_idx] <= surface_variable) { + return false; + } else{ + return true; + } + } else { + if (using_global_thr_ && (z_elevation > global_elevation_thr_)) { + return false; + } else { + return true; } + } + } -void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat){ - Eigen::Matrix3f cov; - Eigen::Vector4f mean; - pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); +void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud &cloud, + pcl::PointCloud &seed_cloud, int zone_idx) { + + //adaptive seed selection for 1st zone + + size_t init_idx = 0; + // if (zone_idx == 0){ - Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - feat.singular_values_ = svd.singularValues(); + // - feat.linearity_ = ( feat.singular_values_(0) - feat.singular_values_(1) ) / feat.singular_values_(0); - feat.planarity_ = ( feat.singular_values_(1) - feat.singular_values_(2) ) / feat.singular_values_(0); + double sum = 0; + int cnt = 0; + for (size_t i = 0; i < cloud.points.size() && cnt < NUM_SEED_POINTS; i++) { + sum += cloud.points[i].z; + cnt++; + } - // use the least singular vector as normal - feat.normal_ = (svd.matrixU().col(2)); - if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - feat.normal_ = -feat.normal_; + double mean_z = sum / cnt; + for (size_t i = init_idx; i < cloud.points.size(); i++) { + if (cloud.points[i].z < mean_z + TH_SEEDS) { + seed_cloud.points.push_back(cloud.points[i]); } - // mean ground seeds value - feat.mean_ = mean.head<3>(); - // according to normal.T*[x,y,z] = -d - feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - feat.th_dist_d_ = MD - feat.d_; -} \ No newline at end of file + } +} + +bool OccupancySegmentationCore::point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b) { return a.z < b.z; } + From ed459ae4c51475b0d2a0900cdf3bf7bea41f548f Mon Sep 17 00:00:00 2001 From: VishGit1234 Date: Tue, 9 Jul 2024 00:24:07 +0000 Subject: [PATCH 15/59] Add apt update --- .../occupancy_segmentation/occupancy_segmentation.Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index ae27a07e..322a1ee8 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -24,7 +24,7 @@ FROM ${BASE_IMAGE} as dependencies # 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} From c67d8217144b2ee4ff62902ff6b75af4d18df243 Mon Sep 17 00:00:00 2001 From: akilpath Date: Thu, 18 Jul 2024 02:19:56 +0000 Subject: [PATCH 16/59] Fixing small bugs --- .../include/occupancy_segmentation_core.hpp | 14 +- .../src/occupancy_segmentation_core.cpp | 327 +++++++++--------- 2 files changed, 177 insertions(+), 164 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 0d2b92a7..1721aec7 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -25,6 +25,11 @@ #define TH_SEEDS 0.5 #define UPRIGHTNESS_THRESH 45 +#define NUM_RINGS_OF_INTEREST 4 +#define SENSOR_HEIGHT 0 +#define GLOBAL_EL_THRESH 0 + + struct PCAFeature { Eigen::Vector3f principal_; @@ -43,7 +48,7 @@ struct Patch_Index { int ring_idx; int sector_idx; int concentric_idx; -} +}; class OccupancySegmentationCore { public: @@ -54,6 +59,9 @@ class OccupancySegmentationCore { const int ZONE_RINGS [NUM_ZONES] = {2,4,4,4}; const int ZONE_SECTORS [NUM_ZONES] = {16,32,54,32}; + const float FLATNESS_THR [NUM_ZONES] = {0.0005, 0.000725, 0.001, 0.001}; + const float ELEVATION_THR [NUM_ZONES] = {0.523, 0.746, 0.879, 1.125}; + int num_patches = -1; @@ -61,10 +69,10 @@ class OccupancySegmentationCore { std::vector> _regionwise_ground; std::vector> _regionwise_nonground; - std::vector _patch_indices; + std::vector _patch_indices; pcl::PointCloud _ground; - pcl::Pointcloud _non_ground; + pcl::PointCloud _non_ground; OccupancySegmentationCore(); diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index c804e9de..bf867542 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -6,108 +6,111 @@ OccupancySegmentationCore::OccupancySegmentationCore() {} void OccupancySegmentationCore::init_czm() { - int concentric_count = 0; - int patch_count = 0; - - for (int z = 0; z < NUM_ZONES; z++) { - Zone zone; - Patch_Idx index; - index.zone_idx = z; - - for (int r = 0; r < ZONE_RINGS[z]; r++) { - Ring ring; - index.ring_idx = r; - index.concentric_idx = concentric_count; - - for (int s = 0; s < ZONE_SECTORS[z]; s++) { - index.sector_idx = s; - index.patch_idx = patch_count; - _patch_indices.push_back(index); - pcl::PointCloud patch; - ring.emplace_back(patch); - _regionwise_ground.push_back(pcl::PointCloud>()); - _regionwise_nonground.push_back(pcl::PointCloud>()); - - patch_count++; - } - zone.emplace_back(ring); - } - _czm.emplace_back(zone); - } - num_patches = patch_count; + // int concentric_count = 0; + // int patch_count = 0; + + // for (int z = 0; z < NUM_ZONES; z++) { + // Zone zone; + // Patch_Index index; + // index.zone_idx = z; + + // for (int r = 0; r < ZONE_RINGS[z]; r++) { + // Ring ring; + // index.ring_idx = r; + // index.concentric_idx = concentric_count; + + // for (int s = 0; s < ZONE_SECTORS[z]; s++) { + // index.sector_idx = s; + // index.idx = patch_count; + // _patch_indices.push_back(index); + // pcl::PointCloud patch; + // ring.emplace_back(patch); + // pcl::PointCloud regionground; + // pcl::PointCloud regionnonground; + // _regionwise_ground.push_back(regionground); + // _regionwise_nonground.push_back(regionnonground); + + // patch_count++; + // } + // zone.emplace_back(ring); + // } + // _czm.emplace_back(zone); + // } + // num_patches = patch_count; } void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { - double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; - for (pcl::PointXYZ &p : cloud_in.points) { - double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); - - // Out of range for czm - if (r < L_MIN || r > L_MAX) { - continue; - } - - double theta = atan2(p.y, p.x); - if (theta < 0) { - theta += 2 * M_PI; - } - - double deltal = L_MAX - L_MIN; - - for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { - int ring_idx, sector_idx; - if (r < lmaxs[zone_idx]) { - double ring_size = deltal / ZONE_RINGS[zone_idx]; - double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; - ring_idx = (int)((r - lmins[zone_idx]) / ring_size); - // TODO: find out why the use min() in the paper, meantime use the top - // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); - sector_idx = (int)(theta / sector_size); - czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); - } - } - } + // double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + // double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + // for (pcl::PointXYZ &p : cloud_in.points) { + // double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); + + // // Out of range for czm + // if (r < L_MIN || r > L_MAX) { + // continue; + // } + + // double theta = atan2(p.y, p.x); + // if (theta < 0) { + // theta += 2 * M_PI; + // } + + // double deltal = L_MAX - L_MIN; + + // for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { + // int ring_idx, sector_idx; + // if (r < lmaxs[zone_idx]) { + // double ring_size = deltal / ZONE_RINGS[zone_idx]; + // double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; + // ring_idx = (int)((r - lmins[zone_idx]) / ring_size); + // // TODO: find out why the use min() in the paper, meantime use the top + // // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); + // sector_idx = (int)(theta / sector_size); + // _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + // } + // } + // } } void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat) { - Eigen::Matrix3f cov; - Eigen::Vector4f mean; - pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); - - Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - feat.singular_values_ = svd.singularValues(); - - feat.linearity_ = - (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - feat.planarity_ = - (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); - - // use the least singular vector as normal - feat.normal_ = (svd.matrixU().col(2)); - if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - feat.normal_ = -feat.normal_; - } - // mean ground seeds value - feat.mean_ = mean.head<3>(); - // according to normal.T*[x,y,z] = -d - feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - feat.th_dist_d_ = MD - feat.d_; + // Eigen::Matrix3f cov; + // Eigen::Vector4f mean; + // pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + // Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + // feat.singular_values_ = svd.singularValues(); + + // feat.linearity_ = + // (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + // feat.planarity_ = + // (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // // use the least singular vector as normal + // feat.normal_ = (svd.matrixU().col(2)); + // if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + // feat.normal_ = -feat.normal_; + // } + // // mean ground seeds value + // feat.mean_ = mean.head<3>(); + // // according to normal.T*[x,y,z] = -d + // feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + // feat.th_dist_d_ = MD - feat.d_; } void OccupancySegmentationCore::segment_ground() { - tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { - for (int patch_num = r.begin(); patch_num < r.end(); r++) { - Patch_Index &p_idx = patch_indices[patch_num]; - pcl::PointCloud &patch = czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + tbb::parallel_for(tbb::blocked_range(0, num_patches), + [&](tbb::blocked_range r) { + for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + Patch_Index &p_idx = _patch_indices[patch_num]; + pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; PCAFeature features; if (patch.points.size() > MIN_NUM_POINTS) { std::sort(patch.points.begin(), patch.points.end()); pcl::PointCloud ground_temp; - rgpf(patch, p_idx, feat); + rgpf(patch, p_idx, features); } } @@ -115,69 +118,69 @@ void OccupancySegmentationCore::segment_ground() { } void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { - pcl::PointCloud ground_temp; - pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - - if (!region_ground.empty()) region_ground.clear(); - if (!region_nonground.empty()) region_nonground.clear(); - - size_t N = patch.size(); - - extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); - int num_iters = 3; - for(int i = 0; i < num_iters; i++){ - estimate_plane(ground_temp, feat); - ground_temp.clear(); - Eigen::MatrixXf points(N, 3); - int j = 0; - for (pcl::PointXYZ &p:src.points) { - points.row(j++) = p.getVector3fMap(); - } - - Eigen::VectorXf result = points * feat.normal_; - for (size_t r = 0; r < N; r++) { - if (i < num_iters - 1) { - if (result[r] < feat.th_dist_d_) { - ground_tmp.points.emplace_back(src[r]); - } - } else { // Final stage - if (result[r] < feat.th_dist_d_) { - regionwise_ground.points.emplace_back(src[r]); - } else { - regionwise_nonground.points.emplace_back(src[r]); - } - } - } - - } + // pcl::PointCloud ground_temp; + // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + + // if (!region_ground.empty()) region_ground.clear(); + // if (!region_nonground.empty()) region_nonground.clear(); + + // size_t N = patch.size(); + + // extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); + // int num_iters = 3; + // for(int i = 0; i < num_iters; i++){ + // estimate_plane(ground_temp, feat); + // ground_temp.clear(); + // Eigen::MatrixXf points(N, 3); + // int j = 0; + // for (pcl::PointXYZ &p:patch.points) { + // points.row(j++) = p.getVector3fMap(); + // } + + // Eigen::VectorXf result = points * feat.normal_; + // for (size_t r = 0; r < N; r++) { + // if (i < num_iters - 1) { + // if (result[r] < feat.th_dist_d_) { + // ground_temp.points.emplace_back(patch.points[r]); + // } + // } else { // Final stage + // if (result[r] < feat.th_dist_d_) { + // region_ground.points.emplace_back(patch.points[r]); + // } else { + // region_nonground.points.emplace_back(patch.points[r]); + // } + // } + // } + + // } } bool OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ - //uprightness filter - if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ - return false; - } - - const double z_elevation = feat.mean_(2); - const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); - - //elevation filter - if (concentric_idx < num_rings_of_interest_) { - if (z_elevation > -sensor_height_ + elevation_thr_[concentric_idx] - && flatness_thr_[concentric_idx] <= surface_variable) { - return false; - } else{ - return true; - } - } else { - if (using_global_thr_ && (z_elevation > global_elevation_thr_)) { - return false; - } else { - return true; - } - } + // //uprightness filter + // if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ + // return false; + // } + + // const double z_elevation = feat.mean_(2); + // const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + + // //elevation filter + // if (concentric_idx < NUM_RINGS_OF_INTEREST) { + // if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx] + // && FLATNESS_THR[concentric_idx] <= surface_variable) { + // return false; + // } else{ + // return true; + // } + // } else { + // if (z_elevation > GLOBAL_EL_THRESH) { + // return false; + // } else { + // return true; + // } + // } } @@ -186,25 +189,27 @@ void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud Date: Sat, 20 Jul 2024 01:04:53 +0000 Subject: [PATCH 17/59] Build / make error with core.cpp --- .../include/occupancy_segmentation_core.hpp | 12 +- .../src/occupancy_segmentation_core.cpp | 378 ++++++++++-------- 2 files changed, 220 insertions(+), 170 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 1721aec7..153f6c38 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -50,6 +50,8 @@ struct Patch_Index { int concentric_idx; }; +enum Status {TOO_TILTED, FLAT_ENOUGH, TOO_HIGH_ELEV, UPRIGHT_ENOUGH, GLOBAL_TOO_HIGH_ELEV, FEW_POINTS}; + class OccupancySegmentationCore { public: typedef std::vector> Ring; @@ -73,6 +75,8 @@ class OccupancySegmentationCore { pcl::PointCloud _ground; pcl::PointCloud _non_ground; + + std::vector _statuses; OccupancySegmentationCore(); @@ -83,17 +87,19 @@ class OccupancySegmentationCore { void fill_czm(pcl::PointCloud &cloud_in); + void clear_czm_and_regionwise(); + void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); void rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat); void extract_initial_seeds(pcl::PointCloud &cloud, pcl::PointCloud &seed_cloud, int zone_idx); - bool ground_likelihood_est(PCAFeature &feat, int concentric_idx); + Status ground_likelihood_est(PCAFeature &feat, int concentric_idx); - void segment_ground(); + void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); - bool point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b); + static bool point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b) { return a.z < b.z; }; diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index bf867542..5942047f 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -3,102 +3,127 @@ #include "occupancy_segmentation_core.hpp" -OccupancySegmentationCore::OccupancySegmentationCore() {} +OccupancySegmentationCore::OccupancySegmentationCore() { + init_czm(); +} void OccupancySegmentationCore::init_czm() { - // int concentric_count = 0; - // int patch_count = 0; - - // for (int z = 0; z < NUM_ZONES; z++) { - // Zone zone; - // Patch_Index index; - // index.zone_idx = z; - - // for (int r = 0; r < ZONE_RINGS[z]; r++) { - // Ring ring; - // index.ring_idx = r; - // index.concentric_idx = concentric_count; - - // for (int s = 0; s < ZONE_SECTORS[z]; s++) { - // index.sector_idx = s; - // index.idx = patch_count; - // _patch_indices.push_back(index); - // pcl::PointCloud patch; - // ring.emplace_back(patch); - // pcl::PointCloud regionground; - // pcl::PointCloud regionnonground; - // _regionwise_ground.push_back(regionground); - // _regionwise_nonground.push_back(regionnonground); - - // patch_count++; - // } - // zone.emplace_back(ring); - // } - // _czm.emplace_back(zone); - // } - // num_patches = patch_count; + int concentric_count = 0; + int patch_count = 0; + + for (int z = 0; z < NUM_ZONES; z++) { + Zone zone; + Patch_Index index; + index.zone_idx = z; + + for (int r = 0; r < ZONE_RINGS[z]; r++) { + Ring ring; + index.ring_idx = r; + index.concentric_idx = concentric_count; + + for (int s = 0; s < ZONE_SECTORS[z]; s++) { + index.sector_idx = s; + index.idx = patch_count; + _patch_indices.push_back(index); + pcl::PointCloud patch; + ring.emplace_back(patch); + + pcl::PointCloud regionground; + pcl::PointCloud regionnonground; + Status status; + _regionwise_ground.push_back(regionground); + _regionwise_nonground.push_back(regionnonground); + + _statuses.push_back(status); + + patch_count++; + } + zone.emplace_back(ring); + } + _czm.emplace_back(zone); + } + num_patches = patch_count; } void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { - // double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - // double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; - // for (pcl::PointXYZ &p : cloud_in.points) { - // double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); - - // // Out of range for czm - // if (r < L_MIN || r > L_MAX) { - // continue; - // } - - // double theta = atan2(p.y, p.x); - // if (theta < 0) { - // theta += 2 * M_PI; - // } - - // double deltal = L_MAX - L_MIN; - - // for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { - // int ring_idx, sector_idx; - // if (r < lmaxs[zone_idx]) { - // double ring_size = deltal / ZONE_RINGS[zone_idx]; - // double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; - // ring_idx = (int)((r - lmins[zone_idx]) / ring_size); - // // TODO: find out why the use min() in the paper, meantime use the top - // // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); - // sector_idx = (int)(theta / sector_size); - // _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); - // } - // } - // } + double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + for (pcl::PointXYZ &p : cloud_in.points) { + double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); + + // Out of range for czm + if (r < L_MIN || r > L_MAX) { + continue; + } + + double theta = atan2(p.y, p.x); + if (theta < 0) { + theta += 2 * M_PI; + } + + double deltal = L_MAX - L_MIN; + + for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { + int ring_idx, sector_idx; + if (r < lmaxs[zone_idx]) { + double ring_size = deltal / ZONE_RINGS[zone_idx]; + double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; + ring_idx = (int)((r - lmins[zone_idx]) / ring_size); + // TODO: find out why the use min() in the paper, meantime use the top + // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); + sector_idx = (int)(theta / sector_size); + _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + } + } + } +} + +void OccupancySegmentationCore::clear_czm_and_regionwise(){ + int i = 0; + for (Zone zone : _czm){ + for (Ring ring : zone){ + for (pcl::PointCloud patch : ring){ + patch.clear(); + _regionwise_ground[i].clear(); + _regionwise_nonground[i].clear(); + i++; + } + } + } } void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat) { - // Eigen::Matrix3f cov; - // Eigen::Vector4f mean; - // pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); - - // Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - // feat.singular_values_ = svd.singularValues(); - - // feat.linearity_ = - // (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - // feat.planarity_ = - // (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); - - // // use the least singular vector as normal - // feat.normal_ = (svd.matrixU().col(2)); - // if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - // feat.normal_ = -feat.normal_; - // } - // // mean ground seeds value - // feat.mean_ = mean.head<3>(); - // // according to normal.T*[x,y,z] = -d - // feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - // feat.th_dist_d_ = MD - feat.d_; + // Code taken directly from repo + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; } -void OccupancySegmentationCore::segment_ground() { +void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground) { + clear_czm_and_regionwise(); + + //TODO error point removal + + fill_czm(unfiltered_cloud); + tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { @@ -107,80 +132,103 @@ void OccupancySegmentationCore::segment_ground() { pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; PCAFeature features; + + region_ground.clear(); + region_nonground.clear(); if (patch.points.size() > MIN_NUM_POINTS) { - std::sort(patch.points.begin(), patch.points.end()); - pcl::PointCloud ground_temp; + std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); rgpf(patch, p_idx, features); + Status status = ground_likelihood_est(features, p_idx.concentric_idx); + _statuses[p_idx.idx] = status; + } else { + region_ground = patch; + _statuses[p_idx.idx] = FEW_POINTS; } } }); + + for (Patch_Index p_idx : _patch_indices){ + Status status = _statuses[p_idx.idx]; + + if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ + ground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } else { + nonground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } + + } } void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { - // pcl::PointCloud ground_temp; - // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - - // if (!region_ground.empty()) region_ground.clear(); - // if (!region_nonground.empty()) region_nonground.clear(); - - // size_t N = patch.size(); - - // extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); - // int num_iters = 3; - // for(int i = 0; i < num_iters; i++){ - // estimate_plane(ground_temp, feat); - // ground_temp.clear(); - // Eigen::MatrixXf points(N, 3); - // int j = 0; - // for (pcl::PointXYZ &p:patch.points) { - // points.row(j++) = p.getVector3fMap(); - // } - - // Eigen::VectorXf result = points * feat.normal_; - // for (size_t r = 0; r < N; r++) { - // if (i < num_iters - 1) { - // if (result[r] < feat.th_dist_d_) { - // ground_temp.points.emplace_back(patch.points[r]); - // } - // } else { // Final stage - // if (result[r] < feat.th_dist_d_) { - // region_ground.points.emplace_back(patch.points[r]); - // } else { - // region_nonground.points.emplace_back(patch.points[r]); - // } - // } - // } - - // } + pcl::PointCloud ground_temp; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + + if (!region_ground.empty()) region_ground.clear(); + if (!region_nonground.empty()) region_nonground.clear(); + + size_t N = patch.size(); + + extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); + int num_iters = 3; + for(int i = 0; i < num_iters; i++){ + estimate_plane(ground_temp, feat); + ground_temp.clear(); + Eigen::MatrixXf points(N, 3); + int j = 0; + for (pcl::PointXYZ &p:patch.points) { + points.row(j++) = p.getVector3fMap(); + } + + Eigen::VectorXf result = points * feat.normal_; + for (size_t r = 0; r < N; r++) { + if (i < num_iters - 1) { + if (result[r] < feat.th_dist_d_) { + ground_temp.points.emplace_back(patch.points[r]); + } + } else { + if (result[r] < feat.th_dist_d_) { + region_ground.points.emplace_back(patch.points[r]); + } else { + region_nonground.points.emplace_back(patch.points[r]); + } + } + } + + } } -bool OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ - - // //uprightness filter - // if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ - // return false; - // } - - // const double z_elevation = feat.mean_(2); - // const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); - - // //elevation filter - // if (concentric_idx < NUM_RINGS_OF_INTEREST) { - // if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx] - // && FLATNESS_THR[concentric_idx] <= surface_variable) { - // return false; - // } else{ - // return true; - // } - // } else { - // if (z_elevation > GLOBAL_EL_THRESH) { - // return false; - // } else { - // return true; - // } - // } +Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ + + //uprightness filter + if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ + return TOO_TILTED; + } + + const double z_elevation = feat.mean_(2); + const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + + //elevation filter + if (concentric_idx < NUM_RINGS_OF_INTEREST) { + if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx]) { + if (FLATNESS_THR[concentric_idx] <= surface_variable){ + return FLAT_ENOUGH; + } else { + return TOO_HIGH_ELEV; + } + } else{ + return UPRIGHT_ENOUGH; + } + } else { + if (z_elevation > GLOBAL_EL_THRESH) { + return GLOBAL_TOO_HIGH_ELEV; + } else { + return UPRIGHT_ENOUGH; + } + } } @@ -189,27 +237,23 @@ void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud Date: Sat, 20 Jul 2024 04:49:20 +0000 Subject: [PATCH 18/59] Added main node code, still fails build --- .../include/occupancy_segmentation_core.hpp | 4 ++-- .../include/occupancy_segmentation_node.hpp | 7 +++++-- .../src/occupancy_segmentation_node.cpp | 19 ++++++++++++++++--- 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 153f6c38..52a17d0d 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -80,6 +80,8 @@ class OccupancySegmentationCore { OccupancySegmentationCore(); + + void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); private: @@ -97,8 +99,6 @@ class OccupancySegmentationCore { Status ground_likelihood_est(PCAFeature &feat, int concentric_idx); - void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); - static bool point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b) { return a.z < b.z; }; diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index 580ee4dc..a9d3b56b 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -4,6 +4,8 @@ #include "rclcpp/rclcpp.hpp" #include "occupancy_segmentation_core.hpp" +#include +#include /** * Implementation of a ROS2 node that converts unfiltered messages to filtered_array @@ -27,9 +29,10 @@ class OccupancySegmentationNode : public rclcpp::Node { private: // Object that handles data processing and validation. + OccupancySegmentationCore _patchwork; + rclcpp::Subscription::SharedPtr _subscriber; - rclcpp::TimerBase::SharedPtr timer_; - void timer_callback(); + void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); }; #endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index baa6475a..d641ff02 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -4,11 +4,24 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { - timer_ = this->create_wall_timer(std::chrono::milliseconds(1000), std::bind(&OccupancySegmentationNode::timer_callback, this)); + _subscriber = this->create_subscription( + "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this)); + } -void OccupancySegmentationNode::timer_callback(){ - RCLCPP_INFO(this->get_logger(), "Working"); +void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ + pcl::PointCloud temp_cloud; + pcl::fromROSMsg(*lidar_cloud, temp_cloud); + + pcl::PointCloud ground; + pcl::PointCloud nonground; + _patchwork.segment_ground(temp_cloud, ground, nonground); + + sensor_msgs::msg::PointCloud2::SharedPtr ground_msg; + sensor_msgs::msg::PointCloud2::SharedPtr nonground_msg; + pcl::toROSMsg(ground, *ground_msg); + pcl::toROSMsg(nonground, *nonground_msg); + } int main(int argc, char** argv) { From bdb8e2f2bb7663e84d521089d84662f33b94792c Mon Sep 17 00:00:00 2001 From: akilpath Date: Mon, 22 Jul 2024 04:08:22 +0000 Subject: [PATCH 19/59] Commented some things to get it to compile --- .../include/occupancy_segmentation_node.hpp | 4 +- .../src/occupancy_segmentation_core.cpp | 124 +++++++++--------- .../src/occupancy_segmentation_node.cpp | 27 ++-- 3 files changed, 77 insertions(+), 78 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index a9d3b56b..77789135 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -30,9 +30,9 @@ class OccupancySegmentationNode : public rclcpp::Node { // Object that handles data processing and validation. OccupancySegmentationCore _patchwork; - rclcpp::Subscription::SharedPtr _subscriber; + // rclcpp::Subscription::SharedPtr _subscriber; - void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); + // void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); }; #endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 5942047f..857e5f48 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -95,71 +95,71 @@ void OccupancySegmentationCore::clear_czm_and_regionwise(){ void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat) { // Code taken directly from repo - Eigen::Matrix3f cov; - Eigen::Vector4f mean; - pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); - - Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - feat.singular_values_ = svd.singularValues(); - - feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); - - // use the least singular vector as normal - feat.normal_ = (svd.matrixU().col(2)); - if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - feat.normal_ = -feat.normal_; - } - // mean ground seeds value - feat.mean_ = mean.head<3>(); - // according to normal.T*[x,y,z] = -d - feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - feat.th_dist_d_ = MD - feat.d_; + // Eigen::Matrix3f cov; + // Eigen::Vector4f mean; + // pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + // Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + // feat.singular_values_ = svd.singularValues(); + + // feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + // feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // // use the least singular vector as normal + // feat.normal_ = (svd.matrixU().col(2)); + // if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + // feat.normal_ = -feat.normal_; + // } + // // mean ground seeds value + // feat.mean_ = mean.head<3>(); + // // according to normal.T*[x,y,z] = -d + // feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + // feat.th_dist_d_ = MD - feat.d_; } void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground) { - clear_czm_and_regionwise(); - - //TODO error point removal - - fill_czm(unfiltered_cloud); - - tbb::parallel_for(tbb::blocked_range(0, num_patches), - [&](tbb::blocked_range r) { - for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { - Patch_Index &p_idx = _patch_indices[patch_num]; - pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; - pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - PCAFeature features; - - region_ground.clear(); - region_nonground.clear(); - if (patch.points.size() > MIN_NUM_POINTS) { - std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); - rgpf(patch, p_idx, features); - - Status status = ground_likelihood_est(features, p_idx.concentric_idx); - _statuses[p_idx.idx] = status; - } else { - region_ground = patch; - _statuses[p_idx.idx] = FEW_POINTS; - } - } - }); - - for (Patch_Index p_idx : _patch_indices){ - Status status = _statuses[p_idx.idx]; - - if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ - ground += _regionwise_ground[p_idx.idx]; - nonground += _regionwise_nonground[p_idx.idx]; - } else { - nonground += _regionwise_ground[p_idx.idx]; - nonground += _regionwise_nonground[p_idx.idx]; - } - - } + // clear_czm_and_regionwise(); + + // //TODO error point removal + + // fill_czm(unfiltered_cloud); + + // tbb::parallel_for(tbb::blocked_range(0, num_patches), + // [&](tbb::blocked_range r) { + // for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + // Patch_Index &p_idx = _patch_indices[patch_num]; + // pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + // PCAFeature features; + + // region_ground.clear(); + // region_nonground.clear(); + // if (patch.points.size() > MIN_NUM_POINTS) { + // std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); + // rgpf(patch, p_idx, features); + + // Status status = ground_likelihood_est(features, p_idx.concentric_idx); + // _statuses[p_idx.idx] = status; + // } else { + // region_ground = patch; + // _statuses[p_idx.idx] = FEW_POINTS; + // } + // } + // }); + + // for (Patch_Index p_idx : _patch_indices){ + // Status status = _statuses[p_idx.idx]; + + // if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ + // ground += _regionwise_ground[p_idx.idx]; + // nonground += _regionwise_nonground[p_idx.idx]; + // } else { + // nonground += _regionwise_ground[p_idx.idx]; + // nonground += _regionwise_nonground[p_idx.idx]; + // } + + // } } void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index d641ff02..eda81010 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -3,26 +3,25 @@ #include "occupancy_segmentation_node.hpp" OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { - - _subscriber = this->create_subscription( - "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this)); + // _subscriber = this->create_subscription( + // "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this)); } -void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ - pcl::PointCloud temp_cloud; - pcl::fromROSMsg(*lidar_cloud, temp_cloud); +// void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ +// // pcl::PointCloud temp_cloud; +// // pcl::fromROSMsg(*lidar_cloud, temp_cloud); - pcl::PointCloud ground; - pcl::PointCloud nonground; - _patchwork.segment_ground(temp_cloud, ground, nonground); +// // pcl::PointCloud ground; +// // pcl::PointCloud nonground; +// // _patchwork.segment_ground(temp_cloud, ground, nonground); - sensor_msgs::msg::PointCloud2::SharedPtr ground_msg; - sensor_msgs::msg::PointCloud2::SharedPtr nonground_msg; - pcl::toROSMsg(ground, *ground_msg); - pcl::toROSMsg(nonground, *nonground_msg); +// // sensor_msgs::msg::PointCloud2::SharedPtr ground_msg; +// // sensor_msgs::msg::PointCloud2::SharedPtr nonground_msg; +// // pcl::toROSMsg(ground, *ground_msg); +// // pcl::toROSMsg(nonground, *nonground_msg); -} +// } int main(int argc, char** argv) { rclcpp::init(argc, argv); From 795bdd996b5a3024d72e8c0555b10772115a3db2 Mon Sep 17 00:00:00 2001 From: akilpath Date: Wed, 24 Jul 2024 00:12:19 +0000 Subject: [PATCH 20/59] Builds --- .../occupancy_segmentation.Dockerfile | 1 - .../occupancy_segmentation/CMakeLists.txt | 2 + .../include/occupancy_segmentation_node.hpp | 6 +- .../occupancy_segmentation/package.xml | 1 + .../src/occupancy_segmentation_core.cpp | 126 +++++++++--------- .../src/occupancy_segmentation_node.cpp | 28 ++-- 6 files changed, 85 insertions(+), 79 deletions(-) diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index 322a1ee8..2168af0f 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -8,7 +8,6 @@ WORKDIR ${AMENT_WS}/src # Copy in source code # RUN git clone https://github.com/ros-perception/perception_pcl.git --branch 2.4.3 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 && \ diff --git a/src/world_modeling/occupancy_segmentation/CMakeLists.txt b/src/world_modeling/occupancy_segmentation/CMakeLists.txt index 8df7a7cd..ed31cbcb 100644 --- a/src/world_modeling/occupancy_segmentation/CMakeLists.txt +++ b/src/world_modeling/occupancy_segmentation/CMakeLists.txt @@ -12,6 +12,7 @@ endif() # 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) @@ -28,6 +29,7 @@ target_include_directories(occupancy_segmentation_lib include) # Add ROS2 dependencies required by package ament_target_dependencies(occupancy_segmentation_lib + sensor_msgs rclcpp pcl_ros) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index 77789135..cd5ead95 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -30,9 +30,11 @@ class OccupancySegmentationNode : public rclcpp::Node { // Object that handles data processing and validation. OccupancySegmentationCore _patchwork; - // rclcpp::Subscription::SharedPtr _subscriber; + rclcpp::Subscription::SharedPtr _subscriber; + rclcpp::Publisher::SharedPtr _nonground_publisher; + rclcpp::Publisher::SharedPtr _ground_publisher; - // void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); + void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); }; #endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index e84324c4..500bbac1 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -9,6 +9,7 @@ ament_cmake + sensor_msgs rclcpp pcl_ros diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 857e5f48..e7e0bc16 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -94,72 +94,72 @@ void OccupancySegmentationCore::clear_czm_and_regionwise(){ void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat) { - // Code taken directly from repo - // Eigen::Matrix3f cov; - // Eigen::Vector4f mean; - // pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); - - // Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - // feat.singular_values_ = svd.singularValues(); - - // feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - // feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); - - // // use the least singular vector as normal - // feat.normal_ = (svd.matrixU().col(2)); - // if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - // feat.normal_ = -feat.normal_; - // } - // // mean ground seeds value - // feat.mean_ = mean.head<3>(); - // // according to normal.T*[x,y,z] = -d - // feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - // feat.th_dist_d_ = MD - feat.d_; + //Code taken directly from repo + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; } void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground) { - // clear_czm_and_regionwise(); - - // //TODO error point removal - - // fill_czm(unfiltered_cloud); - - // tbb::parallel_for(tbb::blocked_range(0, num_patches), - // [&](tbb::blocked_range r) { - // for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { - // Patch_Index &p_idx = _patch_indices[patch_num]; - // pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; - // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - // PCAFeature features; - - // region_ground.clear(); - // region_nonground.clear(); - // if (patch.points.size() > MIN_NUM_POINTS) { - // std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); - // rgpf(patch, p_idx, features); - - // Status status = ground_likelihood_est(features, p_idx.concentric_idx); - // _statuses[p_idx.idx] = status; - // } else { - // region_ground = patch; - // _statuses[p_idx.idx] = FEW_POINTS; - // } - // } - // }); - - // for (Patch_Index p_idx : _patch_indices){ - // Status status = _statuses[p_idx.idx]; - - // if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ - // ground += _regionwise_ground[p_idx.idx]; - // nonground += _regionwise_nonground[p_idx.idx]; - // } else { - // nonground += _regionwise_ground[p_idx.idx]; - // nonground += _regionwise_nonground[p_idx.idx]; - // } - - // } + clear_czm_and_regionwise(); + + //TODO error point removal + + fill_czm(unfiltered_cloud); + + tbb::parallel_for(tbb::blocked_range(0, num_patches), + [&](tbb::blocked_range r) { + for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + Patch_Index &p_idx = _patch_indices[patch_num]; + pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + PCAFeature features; + + region_ground.clear(); + region_nonground.clear(); + if (patch.points.size() > MIN_NUM_POINTS) { + std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); + rgpf(patch, p_idx, features); + + Status status = ground_likelihood_est(features, p_idx.concentric_idx); + _statuses[p_idx.idx] = status; + } else { + region_ground = patch; + _statuses[p_idx.idx] = FEW_POINTS; + } + } + }); + + for (Patch_Index p_idx : _patch_indices){ + Status status = _statuses[p_idx.idx]; + + if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ + ground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } else { + nonground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } + + } } void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index eda81010..9614a5fa 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -3,25 +3,27 @@ #include "occupancy_segmentation_node.hpp" OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { - // _subscriber = this->create_subscription( - // "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this)); + _subscriber = this->create_subscription( + "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this, std::placeholders::_1)); } -// void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ -// // pcl::PointCloud temp_cloud; -// // pcl::fromROSMsg(*lidar_cloud, temp_cloud); +void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ + pcl::PointCloud temp_cloud; + pcl::fromROSMsg(*lidar_cloud, temp_cloud); -// // pcl::PointCloud ground; -// // pcl::PointCloud nonground; -// // _patchwork.segment_ground(temp_cloud, ground, nonground); + pcl::PointCloud ground; + pcl::PointCloud nonground; + _patchwork.segment_ground(temp_cloud, ground, nonground); -// // sensor_msgs::msg::PointCloud2::SharedPtr ground_msg; -// // sensor_msgs::msg::PointCloud2::SharedPtr nonground_msg; -// // pcl::toROSMsg(ground, *ground_msg); -// // pcl::toROSMsg(nonground, *nonground_msg); + sensor_msgs::msg::PointCloud2 ground_msg; + sensor_msgs::msg::PointCloud2 nonground_msg; + pcl::toROSMsg(ground, ground_msg); + pcl::toROSMsg(nonground, nonground_msg); -// } + _ground_publisher -> publish(ground_msg); + _nonground_publisher -> publish(nonground_msg); +} int main(int argc, char** argv) { rclcpp::init(argc, argv); From 5ed8b81ff2835114951bcd21c2654cb181353340 Mon Sep 17 00:00:00 2001 From: akilpath Date: Tue, 30 Jul 2024 03:26:06 +0000 Subject: [PATCH 21/59] Foxglove setup plus error debugging exit exit() --- .../src/occupancy_segmentation_core.cpp | 81 ++++++++++--------- .../src/occupancy_segmentation_node.cpp | 6 +- 2 files changed, 47 insertions(+), 40 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index e7e0bc16..32d43ec6 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -68,11 +68,14 @@ void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_i if (r < lmaxs[zone_idx]) { double ring_size = deltal / ZONE_RINGS[zone_idx]; double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; - ring_idx = (int)((r - lmins[zone_idx]) / ring_size); + //ring_idx = (int)((r - lmins[zone_idx]) / ring_size); // TODO: find out why the use min() in the paper, meantime use the top - // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); - sector_idx = (int)(theta / sector_size); + ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); + sector_idx = std::min((int)(theta / sector_size), ZONE_SECTORS[zone_idx] - 1); + std::cout << "Ring: " << ring_idx << std::endl; + std::cout << "Sector: " << sector_idx << std::endl; _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + break; } } } @@ -124,42 +127,42 @@ void OccupancySegmentationCore::segment_ground(pcl::PointCloud &u fill_czm(unfiltered_cloud); - tbb::parallel_for(tbb::blocked_range(0, num_patches), - [&](tbb::blocked_range r) { - for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { - Patch_Index &p_idx = _patch_indices[patch_num]; - pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; - pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - PCAFeature features; - - region_ground.clear(); - region_nonground.clear(); - if (patch.points.size() > MIN_NUM_POINTS) { - std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); - rgpf(patch, p_idx, features); - - Status status = ground_likelihood_est(features, p_idx.concentric_idx); - _statuses[p_idx.idx] = status; - } else { - region_ground = patch; - _statuses[p_idx.idx] = FEW_POINTS; - } - } - }); - - for (Patch_Index p_idx : _patch_indices){ - Status status = _statuses[p_idx.idx]; - - if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ - ground += _regionwise_ground[p_idx.idx]; - nonground += _regionwise_nonground[p_idx.idx]; - } else { - nonground += _regionwise_ground[p_idx.idx]; - nonground += _regionwise_nonground[p_idx.idx]; - } - - } + // tbb::parallel_for(tbb::blocked_range(0, num_patches), + // [&](tbb::blocked_range r) { + // for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + // Patch_Index &p_idx = _patch_indices[patch_num]; + // pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + // PCAFeature features; + + // region_ground.clear(); + // region_nonground.clear(); + // if (patch.points.size() > MIN_NUM_POINTS) { + // std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); + // rgpf(patch, p_idx, features); + + // Status status = ground_likelihood_est(features, p_idx.concentric_idx); + // _statuses[p_idx.idx] = status; + // } else { + // region_ground = patch; + // _statuses[p_idx.idx] = FEW_POINTS; + // } + // } + // }); + + // for (Patch_Index p_idx : _patch_indices){ + // Status status = _statuses[p_idx.idx]; + + // if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ + // ground += _regionwise_ground[p_idx.idx]; + // nonground += _regionwise_nonground[p_idx.idx]; + // } else { + // nonground += _regionwise_ground[p_idx.idx]; + // nonground += _regionwise_nonground[p_idx.idx]; + // } + + // } } void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 9614a5fa..aad3b26f 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -4,7 +4,10 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { _subscriber = this->create_subscription( - "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this, std::placeholders::_1)); + "/velodyne_points", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this, std::placeholders::_1)); + + _ground_publisher = this -> create_publisher("/ground_points", 10); + _nonground_publisher = this -> create_publisher("/nonground_points", 10); } @@ -18,6 +21,7 @@ void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::P sensor_msgs::msg::PointCloud2 ground_msg; sensor_msgs::msg::PointCloud2 nonground_msg; + pcl::toROSMsg(ground, ground_msg); pcl::toROSMsg(nonground, nonground_msg); From 884156d96482c3ce714f5eb246fe844e957b067f Mon Sep 17 00:00:00 2001 From: akilpath Date: Fri, 2 Aug 2024 03:57:23 +0000 Subject: [PATCH 22/59] Templated Core file --- .../include/occupancy_segmentation_core.hpp | 293 +++++++++++++++++- .../include/occupancy_segmentation_node.hpp | 25 +- .../src/occupancy_segmentation_core.cpp | 257 --------------- .../src/occupancy_segmentation_node.cpp | 13 +- 4 files changed, 316 insertions(+), 272 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 52a17d0d..1a24d697 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -11,6 +11,7 @@ #include + #define NUM_ZONES 4 #define L_MIN 2.7 #define L_MAX 80.0 @@ -52,9 +53,10 @@ struct Patch_Index { enum Status {TOO_TILTED, FLAT_ENOUGH, TOO_HIGH_ELEV, UPRIGHT_ENOUGH, GLOBAL_TOO_HIGH_ELEV, FEW_POINTS}; +template class OccupancySegmentationCore { public: - typedef std::vector> Ring; + typedef std::vector> Ring; typedef std::vector Zone; // Size of buffer before processed messages are published. static constexpr int BUFFER_CAPACITY = 10; @@ -68,42 +70,311 @@ class OccupancySegmentationCore { int num_patches = -1; std::vector _czm; - std::vector> _regionwise_ground; - std::vector> _regionwise_nonground; + std::vector> _regionwise_ground; + std::vector> _regionwise_nonground; std::vector _patch_indices; - pcl::PointCloud _ground; - pcl::PointCloud _non_ground; + pcl::PointCloud _ground; + pcl::PointCloud _non_ground; std::vector _statuses; OccupancySegmentationCore(); - void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); + void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); private: void init_czm(); - void fill_czm(pcl::PointCloud &cloud_in); + void fill_czm(pcl::PointCloud &cloud_in); void clear_czm_and_regionwise(); - void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); + void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); - void rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat); + void rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat); - void extract_initial_seeds(pcl::PointCloud &cloud, pcl::PointCloud &seed_cloud, int zone_idx); + void extract_initial_seeds(pcl::PointCloud &cloud, pcl::PointCloud &seed_cloud, int zone_idx); Status ground_likelihood_est(PCAFeature &feat, int concentric_idx); - static bool point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b) { return a.z < b.z; }; + static bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; }; }; +template +OccupancySegmentationCore::OccupancySegmentationCore() { + init_czm(); +} + +template +void OccupancySegmentationCore::init_czm() { + int concentric_count = 0; + int patch_count = 0; + + for (int z = 0; z < NUM_ZONES; z++) { + Zone zone; + Patch_Index index; + index.zone_idx = z; + + for (int r = 0; r < ZONE_RINGS[z]; r++) { + Ring ring; + index.ring_idx = r; + index.concentric_idx = concentric_count; + + for (int s = 0; s < ZONE_SECTORS[z]; s++) { + index.sector_idx = s; + index.idx = patch_count; + _patch_indices.push_back(index); + pcl::PointCloud patch; + ring.emplace_back(patch); + + pcl::PointCloud regionground; + pcl::PointCloud regionnonground; + Status status; + _regionwise_ground.push_back(regionground); + _regionwise_nonground.push_back(regionnonground); + + _statuses.push_back(status); + + patch_count++; + } + zone.emplace_back(ring); + } + _czm.emplace_back(zone); + } + num_patches = patch_count; +} + +template +void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { + double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + for (PointT &p : cloud_in.points) { + double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); + + // Out of range for czm + if (r < L_MIN || r > L_MAX) { + continue; + } + + double theta = atan2(p.y, p.x); + if (theta < 0) { + theta += 2 * M_PI; + } + + double deltal = L_MAX - L_MIN; + + for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { + int ring_idx, sector_idx; + if (r < lmaxs[zone_idx]) { + double ring_size = deltal / ZONE_RINGS[zone_idx]; + double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; + //ring_idx = (int)((r - lmins[zone_idx]) / ring_size); + // TODO: find out why the use min() in the paper, meantime use the top + ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); + sector_idx = std::min((int)(theta / sector_size), ZONE_SECTORS[zone_idx] - 1); + // std::cout << "Ring: " << ring_idx << std::endl; + // std::cout << "Sector: " << sector_idx << std::endl; + _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + break; + } + } + } +} + +template +void OccupancySegmentationCore::clear_czm_and_regionwise(){ + int i = 0; + for (Zone &zone : _czm){ + for (Ring &ring : zone){ + for (pcl::PointCloud &patch : ring){ + patch.clear(); + _regionwise_ground[i].clear(); + _regionwise_nonground[i].clear(); + i++; + } + } + } +} + +template +void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, + PCAFeature &feat) { + //Code taken directly from repo + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; +} + +template +void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground) { + clear_czm_and_regionwise(); + + //TODO error point removal + + fill_czm(unfiltered_cloud); + std::cout << "Finished filling czm" << std::endl; + tbb::parallel_for(tbb::blocked_range(0, num_patches), + [&](tbb::blocked_range r) { + for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + Patch_Index &p_idx = _patch_indices[patch_num]; + pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + PCAFeature features; + + region_ground.clear(); + region_nonground.clear(); + if (patch.points.size() > MIN_NUM_POINTS) { + std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); + rgpf(patch, p_idx, features); + + Status status = ground_likelihood_est(features, p_idx.concentric_idx); + _statuses[p_idx.idx] = status; + } else { + region_ground = patch; + _statuses[p_idx.idx] = FEW_POINTS; + } + } + }); + + std::cout << "Finished estimation" << std::endl; + + for (Patch_Index p_idx : _patch_indices){ + Status status = _statuses[p_idx.idx]; + + if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ + ground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } else { + nonground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } + + } +} + +template +void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { + pcl::PointCloud ground_temp; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + + if (!region_ground.empty()) region_ground.clear(); + if (!region_nonground.empty()) region_nonground.clear(); + + size_t N = patch.size(); + + extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); + int num_iters = 3; + for(int i = 0; i < num_iters; i++){ + estimate_plane(ground_temp, feat); + ground_temp.clear(); + Eigen::MatrixXf points(N, 3); + int j = 0; + for (PointT &p:patch.points) { + points.row(j++) = p.getVector3fMap(); + } + + Eigen::VectorXf result = points * feat.normal_; + for (size_t r = 0; r < N; r++) { + if (i < num_iters - 1) { + if (result[r] < feat.th_dist_d_) { + ground_temp.points.emplace_back(patch.points[r]); + } + } else { + if (result[r] < feat.th_dist_d_) { + region_ground.points.emplace_back(patch.points[r]); + } else { + region_nonground.points.emplace_back(patch.points[r]); + } + } + } + + } +} + +template +Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ + + //uprightness filter + if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ + return TOO_TILTED; + } + + const double z_elevation = feat.mean_(2); + const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + + //elevation filter + if (concentric_idx < NUM_RINGS_OF_INTEREST) { + if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx]) { + if (FLATNESS_THR[concentric_idx] <= surface_variable){ + return FLAT_ENOUGH; + } else { + return TOO_HIGH_ELEV; + } + } else{ + return UPRIGHT_ENOUGH; + } + } else { + if (z_elevation > GLOBAL_EL_THRESH) { + return GLOBAL_TOO_HIGH_ELEV; + } else { + return UPRIGHT_ENOUGH; + } + } + +} + +template +void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud &cloud, + pcl::PointCloud &seed_cloud, int zone_idx) { + + //adaptive seed selection for 1st zone + + size_t init_idx = 0; + // if (zone_idx == 0){ + + // + + double sum = 0; + int cnt = 0; + for (size_t i = 0; i < cloud.points.size() && cnt < NUM_SEED_POINTS; i++) { + sum += cloud.points[i].z; + cnt++; + } + + double mean_z = sum / cnt; + for (size_t i = init_idx; i < cloud.points.size(); i++) { + if (cloud.points[i].z < mean_z + TH_SEEDS) { + seed_cloud.points.push_back(cloud.points[i]); + } + } +} + + #endif // TRANSFORMER_CORE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index cd5ead95..34ceb661 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -1,11 +1,34 @@ #ifndef OCCUPANCY_SEGMENTATION_NODE_HPP_ #define OCCUPANCY_SEGMENTATION_NODE_HPP_ +#define PCL_NO_PRECOMPILE #include "rclcpp/rclcpp.hpp" #include "occupancy_segmentation_core.hpp" #include #include +#include +#include +#include +#include + +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; + +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 @@ -29,7 +52,7 @@ class OccupancySegmentationNode : public rclcpp::Node { private: // Object that handles data processing and validation. - OccupancySegmentationCore _patchwork; + OccupancySegmentationCore _patchwork; rclcpp::Subscription::SharedPtr _subscriber; rclcpp::Publisher::SharedPtr _nonground_publisher; rclcpp::Publisher::SharedPtr _ground_publisher; diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 32d43ec6..8cca23c3 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -3,260 +3,3 @@ #include "occupancy_segmentation_core.hpp" -OccupancySegmentationCore::OccupancySegmentationCore() { - init_czm(); -} - -void OccupancySegmentationCore::init_czm() { - int concentric_count = 0; - int patch_count = 0; - - for (int z = 0; z < NUM_ZONES; z++) { - Zone zone; - Patch_Index index; - index.zone_idx = z; - - for (int r = 0; r < ZONE_RINGS[z]; r++) { - Ring ring; - index.ring_idx = r; - index.concentric_idx = concentric_count; - - for (int s = 0; s < ZONE_SECTORS[z]; s++) { - index.sector_idx = s; - index.idx = patch_count; - _patch_indices.push_back(index); - pcl::PointCloud patch; - ring.emplace_back(patch); - - pcl::PointCloud regionground; - pcl::PointCloud regionnonground; - Status status; - _regionwise_ground.push_back(regionground); - _regionwise_nonground.push_back(regionnonground); - - _statuses.push_back(status); - - patch_count++; - } - zone.emplace_back(ring); - } - _czm.emplace_back(zone); - } - num_patches = patch_count; -} - -void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { - double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; - for (pcl::PointXYZ &p : cloud_in.points) { - double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); - - // Out of range for czm - if (r < L_MIN || r > L_MAX) { - continue; - } - - double theta = atan2(p.y, p.x); - if (theta < 0) { - theta += 2 * M_PI; - } - - double deltal = L_MAX - L_MIN; - - for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { - int ring_idx, sector_idx; - if (r < lmaxs[zone_idx]) { - double ring_size = deltal / ZONE_RINGS[zone_idx]; - double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; - //ring_idx = (int)((r - lmins[zone_idx]) / ring_size); - // TODO: find out why the use min() in the paper, meantime use the top - ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); - sector_idx = std::min((int)(theta / sector_size), ZONE_SECTORS[zone_idx] - 1); - std::cout << "Ring: " << ring_idx << std::endl; - std::cout << "Sector: " << sector_idx << std::endl; - _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); - break; - } - } - } -} - -void OccupancySegmentationCore::clear_czm_and_regionwise(){ - int i = 0; - for (Zone zone : _czm){ - for (Ring ring : zone){ - for (pcl::PointCloud patch : ring){ - patch.clear(); - _regionwise_ground[i].clear(); - _regionwise_nonground[i].clear(); - i++; - } - } - } -} - -void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, - PCAFeature &feat) { - //Code taken directly from repo - Eigen::Matrix3f cov; - Eigen::Vector4f mean; - pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); - - Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - feat.singular_values_ = svd.singularValues(); - - feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); - - // use the least singular vector as normal - feat.normal_ = (svd.matrixU().col(2)); - if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - feat.normal_ = -feat.normal_; - } - // mean ground seeds value - feat.mean_ = mean.head<3>(); - // according to normal.T*[x,y,z] = -d - feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - feat.th_dist_d_ = MD - feat.d_; -} - -void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground) { - clear_czm_and_regionwise(); - - //TODO error point removal - - fill_czm(unfiltered_cloud); - - // tbb::parallel_for(tbb::blocked_range(0, num_patches), - // [&](tbb::blocked_range r) { - // for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { - // Patch_Index &p_idx = _patch_indices[patch_num]; - // pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; - // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - // PCAFeature features; - - // region_ground.clear(); - // region_nonground.clear(); - // if (patch.points.size() > MIN_NUM_POINTS) { - // std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); - // rgpf(patch, p_idx, features); - - // Status status = ground_likelihood_est(features, p_idx.concentric_idx); - // _statuses[p_idx.idx] = status; - // } else { - // region_ground = patch; - // _statuses[p_idx.idx] = FEW_POINTS; - // } - // } - // }); - - // for (Patch_Index p_idx : _patch_indices){ - // Status status = _statuses[p_idx.idx]; - - // if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ - // ground += _regionwise_ground[p_idx.idx]; - // nonground += _regionwise_nonground[p_idx.idx]; - // } else { - // nonground += _regionwise_ground[p_idx.idx]; - // nonground += _regionwise_nonground[p_idx.idx]; - // } - - // } -} - -void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { - pcl::PointCloud ground_temp; - pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - - if (!region_ground.empty()) region_ground.clear(); - if (!region_nonground.empty()) region_nonground.clear(); - - size_t N = patch.size(); - - extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); - int num_iters = 3; - for(int i = 0; i < num_iters; i++){ - estimate_plane(ground_temp, feat); - ground_temp.clear(); - Eigen::MatrixXf points(N, 3); - int j = 0; - for (pcl::PointXYZ &p:patch.points) { - points.row(j++) = p.getVector3fMap(); - } - - Eigen::VectorXf result = points * feat.normal_; - for (size_t r = 0; r < N; r++) { - if (i < num_iters - 1) { - if (result[r] < feat.th_dist_d_) { - ground_temp.points.emplace_back(patch.points[r]); - } - } else { - if (result[r] < feat.th_dist_d_) { - region_ground.points.emplace_back(patch.points[r]); - } else { - region_nonground.points.emplace_back(patch.points[r]); - } - } - } - - } -} - -Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ - - //uprightness filter - if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ - return TOO_TILTED; - } - - const double z_elevation = feat.mean_(2); - const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); - - //elevation filter - if (concentric_idx < NUM_RINGS_OF_INTEREST) { - if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx]) { - if (FLATNESS_THR[concentric_idx] <= surface_variable){ - return FLAT_ENOUGH; - } else { - return TOO_HIGH_ELEV; - } - } else{ - return UPRIGHT_ENOUGH; - } - } else { - if (z_elevation > GLOBAL_EL_THRESH) { - return GLOBAL_TOO_HIGH_ELEV; - } else { - return UPRIGHT_ENOUGH; - } - } - -} - -void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud &cloud, - pcl::PointCloud &seed_cloud, int zone_idx) { - - //adaptive seed selection for 1st zone - - size_t init_idx = 0; - // if (zone_idx == 0){ - - // - - double sum = 0; - int cnt = 0; - for (size_t i = 0; i < cloud.points.size() && cnt < NUM_SEED_POINTS; i++) { - sum += cloud.points[i].z; - cnt++; - } - - double mean_z = sum / cnt; - for (size_t i = init_idx; i < cloud.points.size(); i++) { - if (cloud.points[i].z < mean_z + TH_SEEDS) { - seed_cloud.points.push_back(cloud.points[i]); - } - } -} - diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index aad3b26f..100e4202 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -12,13 +12,20 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment } void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ - pcl::PointCloud temp_cloud; + pcl::PointCloud temp_cloud; pcl::fromROSMsg(*lidar_cloud, temp_cloud); - pcl::PointCloud ground; - pcl::PointCloud nonground; + pcl::PointCloud ground; + pcl::PointCloud nonground; + + ground.clear(); + nonground.clear(); _patchwork.segment_ground(temp_cloud, ground, nonground); + RCLCPP_INFO(this->get_logger(), "Temp_cloud points %i", static_cast(temp_cloud.size())); + RCLCPP_INFO(this->get_logger(), "Ground points %i", static_cast(ground.size())); + RCLCPP_INFO(this->get_logger(), "Non ground points %i", static_cast(nonground.size())); + sensor_msgs::msg::PointCloud2 ground_msg; sensor_msgs::msg::PointCloud2 nonground_msg; From 34d1641d8e94c59f50ac8a895b9b78a13f363f18 Mon Sep 17 00:00:00 2001 From: akilpath Date: Tue, 6 Aug 2024 02:09:32 +0000 Subject: [PATCH 23/59] lowkey works --- .../include/occupancy_segmentation_core.hpp | 21 ++++++++++++------- .../src/occupancy_segmentation_node.cpp | 2 ++ 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 1a24d697..7613fc87 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -16,7 +16,6 @@ #define L_MIN 2.7 #define L_MAX 80.0 -#define N_SEED 20 #define Z_SEED 0.5 #define MD 0.3 #define MH -1.1 @@ -24,10 +23,10 @@ #define MIN_NUM_POINTS 10 #define NUM_SEED_POINTS 20 #define TH_SEEDS 0.5 -#define UPRIGHTNESS_THRESH 45 +#define UPRIGHTNESS_THRESH 0.5 #define NUM_RINGS_OF_INTEREST 4 -#define SENSOR_HEIGHT 0 +#define SENSOR_HEIGHT 1.7 #define GLOBAL_EL_THRESH 0 @@ -357,9 +356,17 @@ void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud::extract_initial_seeds(pcl::PointCloud temp_cloud; + RCLCPP_INFO(this -> get_logger(), "Header incoming: %s", lidar_cloud -> header.frame_id.c_str()); pcl::fromROSMsg(*lidar_cloud, temp_cloud); pcl::PointCloud ground; @@ -31,6 +32,7 @@ void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::P pcl::toROSMsg(ground, ground_msg); pcl::toROSMsg(nonground, nonground_msg); + RCLCPP_INFO(this -> get_logger(), "Header outgoing: %s", ground_msg.header.frame_id.c_str()); _ground_publisher -> publish(ground_msg); _nonground_publisher -> publish(nonground_msg); From 3f1dadd6affc11133ada40a55f26bb31b6446e77 Mon Sep 17 00:00:00 2001 From: akilpath Date: Wed, 7 Aug 2024 00:00:20 +0000 Subject: [PATCH 24/59] Added headers to publishers --- .../occupancy_segmentation/src/occupancy_segmentation_node.cpp | 2 ++ watod-config.sh | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 7a07eef8..8778bb23 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -21,6 +21,8 @@ void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::P ground.clear(); nonground.clear(); + ground.header = temp_cloud.header; + nonground.header = temp_cloud.header; _patchwork.segment_ground(temp_cloud, ground, nonground); RCLCPP_INFO(this->get_logger(), "Temp_cloud points %i", static_cast(temp_cloud.size())); diff --git a/watod-config.sh b/watod-config.sh index dc6a6b1c..be66ed26 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -15,7 +15,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -ACTIVE_MODULES="world_modeling" +ACTIVE_MODULES="infrastructure world_modeling" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. From 65275702db9c9c36c0f238f43e59e2d7133b12dd Mon Sep 17 00:00:00 2001 From: akilpath Date: Sat, 1 Jun 2024 23:46:34 +0000 Subject: [PATCH 25/59] Copy transformer samples to occupancy_seg --- .../occupancy_segmentation/config/params.yaml | 4 + .../include/transformer_core.hpp | 76 +++++++++++++++ .../include/transformer_node.hpp | 50 ++++++++++ .../launch/transformer.launch.py | 31 ++++++ .../src/transformer_core.cpp | 64 +++++++++++++ .../src/transformer_node.cpp | 51 ++++++++++ .../test/transformer_test.cpp | 94 +++++++++++++++++++ 7 files changed, 370 insertions(+) create mode 100644 src/world_modeling/occupancy_segmentation/config/params.yaml create mode 100644 src/world_modeling/occupancy_segmentation/include/transformer_core.hpp create mode 100644 src/world_modeling/occupancy_segmentation/include/transformer_node.hpp create mode 100755 src/world_modeling/occupancy_segmentation/launch/transformer.launch.py create mode 100644 src/world_modeling/occupancy_segmentation/src/transformer_core.cpp create mode 100644 src/world_modeling/occupancy_segmentation/src/transformer_node.cpp create mode 100644 src/world_modeling/occupancy_segmentation/test/transformer_test.cpp diff --git a/src/world_modeling/occupancy_segmentation/config/params.yaml b/src/world_modeling/occupancy_segmentation/config/params.yaml new file mode 100644 index 00000000..d59e071f --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/config/params.yaml @@ -0,0 +1,4 @@ +transformer_node: + ros__parameters: + version: 1 + compression_method: 0 diff --git a/src/world_modeling/occupancy_segmentation/include/transformer_core.hpp b/src/world_modeling/occupancy_segmentation/include/transformer_core.hpp new file mode 100644 index 00000000..300ca850 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/include/transformer_core.hpp @@ -0,0 +1,76 @@ +#ifndef TRANSFORMER_CORE_HPP_ +#define TRANSFORMER_CORE_HPP_ + +#include + +#include "sample_msgs/msg/filtered.hpp" +#include "sample_msgs/msg/unfiltered.hpp" + +namespace samples { + +/** + * Implementation for the internal logic for the Transformer ROS2 + * node performing data processing and validation. + */ +class TransformerCore { + public: + // Size of buffer before processed messages are published. + static constexpr int BUFFER_CAPACITY = 10; + + public: + /** + * Transformer constructor. + */ + TransformerCore(); + + /** + * Retrieve enqueued messages in buffer. + * + * @returns enqueued processed messages + */ + std::vector buffer_messages() const; + + /** + * Removes all messages in buffer. Called by the transformer + * node after messages have been published to aggregator. + */ + void clear_buffer(); + + /** + * Validates that the 'valid' field of an unfiltered message + * is set to true. + * + * @param unfiltered a raw message + * @returns whether message's 'valid' field is set + */ + bool validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered); + + /** + * Enqueue message into an array of processed messages to "filtered" topic. + * Ignores messages once the buffer capacity is reached. + * + * @param msg a processed message to be published + * @returns whether buffer is full after adding new message + */ + bool enqueue_message(const sample_msgs::msg::Filtered& msg); + + /** + * Deserializes the data field of the unfiltered ROS2 message. + * The data field should be of the form "x:$num1;y:$num2;z:$num3;". + * + * @param[in] unfiltered the raw message containing serialized data + * @param[out] filtered the processed message containing deserialized data + * @returns whether deserialization was successful + */ + bool deserialize_coordinate(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, + sample_msgs::msg::Filtered& filtered); + + private: + // Buffer storing processed messages until BUFFER_CAPACITY. Clear after + // messages are published. + std::vector buffer_; +}; + +} // namespace samples + +#endif // TRANSFORMER_CORE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/include/transformer_node.hpp b/src/world_modeling/occupancy_segmentation/include/transformer_node.hpp new file mode 100644 index 00000000..5dce7b23 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/include/transformer_node.hpp @@ -0,0 +1,50 @@ +#ifndef TRANSFORMER_NODE_HPP_ +#define TRANSFORMER_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "sample_msgs/msg/filtered.hpp" +#include "sample_msgs/msg/filtered_array.hpp" +#include "sample_msgs/msg/unfiltered.hpp" + +#include "transformer_core.hpp" + +/** + * 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 BUFFER_CAPACITY messages it packs + * the processed messages into an array and publishes it to the "filtered" topic. + */ +class TransformerNode : 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. + */ + TransformerNode(); + + private: + /** + * A ROS2 subscription node callback used to process raw data from the + * "unfiltered" topic and publish to the "filtered" topic. + * + * @param msg a raw message from the "unfiltered" topic + */ + void unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg); + + // ROS2 subscriber listening to the unfiltered topic. + rclcpp::Subscription::SharedPtr raw_sub_; + + // ROS2 publisher sending processed messages to the filtered topic. + rclcpp::Publisher::SharedPtr transform_pub_; + + // Object that handles data processing and validation. + samples::TransformerCore transformer_; +}; + +#endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/launch/transformer.launch.py b/src/world_modeling/occupancy_segmentation/launch/transformer.launch.py new file mode 100755 index 00000000..802ca850 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/launch/transformer.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 transformer node.""" + transformer_pkg_prefix = get_package_share_directory('transformer') + transformer_param_file = os.path.join( + transformer_pkg_prefix, 'config', 'params.yaml') + + transformer_param = DeclareLaunchArgument( + 'transformer_param_file', + default_value=transformer_param_file, + description='Path to config file for transformer node' + ) + + transformer_node = Node( + package='transformer', + executable='transformer_node', + parameters=[LaunchConfiguration('transformer_param_file')], + ) + + return LaunchDescription([ + transformer_param, + transformer_node + ]) diff --git a/src/world_modeling/occupancy_segmentation/src/transformer_core.cpp b/src/world_modeling/occupancy_segmentation/src/transformer_core.cpp new file mode 100644 index 00000000..35c3f015 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/src/transformer_core.cpp @@ -0,0 +1,64 @@ +#include +#include + +#include "transformer_core.hpp" + +namespace samples { + +TransformerCore::TransformerCore() {} + +std::vector TransformerCore::buffer_messages() const { return buffer_; } + +void TransformerCore::clear_buffer() { buffer_.clear(); } + +bool TransformerCore::validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered) { + return unfiltered->valid; +} + +bool TransformerCore::enqueue_message(const sample_msgs::msg::Filtered& msg) { + if (buffer_.size() < BUFFER_CAPACITY) { + buffer_.push_back(msg); + } + return buffer_.size() == BUFFER_CAPACITY; +} + +bool TransformerCore::deserialize_coordinate( + const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, + sample_msgs::msg::Filtered& filtered) { + std::string serialized_position = unfiltered->data; + auto start_pos = serialized_position.find("x:"); + auto end_pos = serialized_position.find(";"); + // Validate that the substrings were found + if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { + return false; + } + // Offset index to start of x_pos + start_pos += 2; + // Splice string and convert position to float + float x = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); + + start_pos = serialized_position.find("y:", end_pos + 1); + end_pos = serialized_position.find(";", end_pos + 1); + if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { + return false; + } + // Offset index to start of y_pos + start_pos += 2; + float y = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); + + start_pos = serialized_position.find("z:", end_pos + 1); + end_pos = serialized_position.find(";", end_pos + 1); + if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { + return false; + } + // Offset index to start of z_pos + start_pos += 2; + float z = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); + + filtered.pos_x = x; + filtered.pos_y = y; + filtered.pos_z = z; + return true; +} + +} // namespace samples diff --git a/src/world_modeling/occupancy_segmentation/src/transformer_node.cpp b/src/world_modeling/occupancy_segmentation/src/transformer_node.cpp new file mode 100644 index 00000000..51bd3399 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/src/transformer_node.cpp @@ -0,0 +1,51 @@ +#include + +#include "transformer_node.hpp" + +TransformerNode::TransformerNode() : Node("transformer"), transformer_(samples::TransformerCore()) { + raw_sub_ = this->create_subscription( + "/unfiltered_topic", ADVERTISING_FREQ, + std::bind(&TransformerNode::unfiltered_callback, this, std::placeholders::_1)); + transform_pub_ = + this->create_publisher("/filtered_topic", ADVERTISING_FREQ); + + // Define the default values for parameters if not defined in params.yaml + this->declare_parameter("version", rclcpp::ParameterValue(0)); + this->declare_parameter("compression_method", rclcpp::ParameterValue(0)); +} + +void TransformerNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg) { + if (!transformer_.validate_message(msg)) { + return; + } + + auto filtered = sample_msgs::msg::Filtered(); + if (!transformer_.deserialize_coordinate(msg, filtered)) { + return; + } + + filtered.timestamp = msg->timestamp; + filtered.metadata.version = this->get_parameter("version").as_int(); + filtered.metadata.compression_method = this->get_parameter("compression_method").as_int(); + + if (transformer_.enqueue_message(filtered)) { + RCLCPP_INFO(this->get_logger(), "Buffer Capacity Reached. PUBLISHING..."); + // Publish processed data when the buffer reaches its capacity + sample_msgs::msg::FilteredArray filtered_msgs; + auto buffer = transformer_.buffer_messages(); + transformer_.clear_buffer(); + + // Construct FilteredArray object + for (auto& packet : buffer) { + filtered_msgs.packets.push_back(packet); + } + transform_pub_->publish(filtered_msgs); + } +} + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp b/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp new file mode 100644 index 00000000..0d103350 --- /dev/null +++ b/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp @@ -0,0 +1,94 @@ +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" + +#include "transformer_core.hpp" + +/** + * When writing a large number of tests it is desirable to wrap any common + * setup or cleanup logic in a test fixture. This improves readability and reduces + * the amount of boilerplate code in each test. For more information checkout + * https://google.github.io/googletest/primer.html#same-data-multiple-tests + */ +class TransformerFixtureTest : public ::testing::Test { + public: + void SetUp(int enqueue_count) { + auto filtered = sample_msgs::msg::Filtered(); + for (int i = 0; i < enqueue_count; i++) { + transformer.enqueue_message(filtered); + } + } + + protected: + samples::TransformerCore transformer; +}; + +/** + * Parameterized tests let us test the same logic with different parameters without + * having to write boilerplate for multiple tests. For more information checkout + * https://google.github.io/googletest/advanced.html#value-parameterized-tests + */ +class TransformerParameterizedTest + : public ::testing::TestWithParam> { + protected: + samples::TransformerCore transformer; +}; + +TEST(TransformerTest, FilterInvalidField) { + auto unfiltered = std::make_shared(); + unfiltered->valid = false; + + auto transformer = samples::TransformerCore(); + bool valid = transformer.validate_message(unfiltered); + EXPECT_FALSE(valid); +} + +TEST_F(TransformerFixtureTest, BufferCapacity) { + SetUp(samples::TransformerCore::BUFFER_CAPACITY - 1); + + // Place last message that fits in buffer + auto filtered = sample_msgs::msg::Filtered(); + bool full = transformer.enqueue_message(filtered); + EXPECT_TRUE(full); + int size = transformer.buffer_messages().size(); + EXPECT_EQ(size, 10); + + // Attempting to enqueue message to full buffer should fail + full = transformer.enqueue_message(filtered); + EXPECT_TRUE(full); + size = transformer.buffer_messages().size(); + EXPECT_EQ(size, 10); +} + +TEST_F(TransformerFixtureTest, ClearBuffer) { + // Place 3 messages in buffer + SetUp(3); + + int size = transformer.buffer_messages().size(); + EXPECT_GT(size, 0); + + transformer.clear_buffer(); + size = transformer.buffer_messages().size(); + EXPECT_EQ(size, 0); +} + +TEST_P(TransformerParameterizedTest, SerializationValidation) { + auto [serialized_field, valid] = GetParam(); + auto filtered = sample_msgs::msg::Filtered(); + auto unfiltered = std::make_shared(); + unfiltered->data = serialized_field; + EXPECT_EQ(transformer.deserialize_coordinate(unfiltered, filtered), valid); +} + +// Parameterized testing lets us validate all edge cases for serialization +// using one test case. +INSTANTIATE_TEST_CASE_P(Serialization, TransformerParameterizedTest, + ::testing::Values(std::make_tuple("x:1;y:2;z:3", false), + std::make_tuple("z:1;y:2;x:3;", false), + std::make_tuple("x:1,y:2,z:3", false), + std::make_tuple("x:3;", false), + std::make_tuple("x:3;y:2;z:3;", true), + std::make_tuple("x:3;y:22; z:11;", true))); From cdba3f37fa7e93f519430ee2c04d6d06ca5145d7 Mon Sep 17 00:00:00 2001 From: akilpath Date: Tue, 4 Jun 2024 00:04:31 +0000 Subject: [PATCH 26/59] Copied transformer node layout --- .../occupancy_segmentation/CMakeLists.txt | 71 +++++++++++++++++-- .../occupancy_segmentation/package.xml | 17 +++-- watod-config.sh | 2 +- 3 files changed, 76 insertions(+), 14 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/CMakeLists.txt b/src/world_modeling/occupancy_segmentation/CMakeLists.txt index dfa8b8bb..da069a60 100644 --- a/src/world_modeling/occupancy_segmentation/CMakeLists.txt +++ b/src/world_modeling/occupancy_segmentation/CMakeLists.txt @@ -1,14 +1,71 @@ -cmake_minimum_required(VERSION 3.8) -project(occupancy_segmentation) +cmake_minimum_required(VERSION 3.10) +project(transformer) + +# 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( REQUIRED) +# Search for dependencies required for building this package +find_package(ament_cmake REQUIRED) # ROS2 build tool +find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages + +# 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(transformer_lib + src/transformer_core.cpp) +# Indicate to compiler where to search for header files +target_include_directories(transformer_lib + PUBLIC + include) +# Add ROS2 dependencies required by package +ament_target_dependencies(transformer_lib + rclcpp + sample_msgs) + +# 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(transformer_test test/transformer_test.cpp) + # Link to the previously built library to access Transformer classes and methods + target_link_libraries(transformer_test + transformer_lib + gtest_main) + + # Copy executable to installation location + install(TARGETS + transformer_test + DESTINATION lib/${PROJECT_NAME}) +endif() + +# Create ROS2 node executable from source files +add_executable(transformer_node src/transformer_node.cpp) +# Link to the previously built library to access Transformer classes and methods +target_link_libraries(transformer_node transformer_lib) + +# Copy executable to installation location +install(TARGETS + transformer_node + DESTINATION lib/${PROJECT_NAME}) + +# Copy launch and config files to installation location +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME}) ament_package() + diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index 57c0712d..41a28c0f 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -1,16 +1,21 @@ - - occupancy_segmentation + transformer 0.0.0 - TODO: Package description - bolty - TODO: License declaration + A sample ROS package for pubsub communication + Cheran Mahalingam + TODO + + ament_cmake + rclcpp + sample_msgs + ament_cmake_gtest + ament_cmake - + \ No newline at end of file diff --git a/watod-config.sh b/watod-config.sh index 3255dd40..321efd80 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -15,7 +15,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -# ACTIVE_MODULES="" +ACTIVE_MODULES="world_modeling" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. From 6605e3fc75cd5f08adef179a8eec8bd374f6c127 Mon Sep 17 00:00:00 2001 From: akilpath Date: Sat, 8 Jun 2024 20:50:52 +0000 Subject: [PATCH 27/59] Renamed files --- .../occupancy_segmentation/CMakeLists.txt | 52 +++++++++---------- .../occupancy_segmentation/config/params.yaml | 2 +- ...re.hpp => occupancy_segmentation_core.hpp} | 8 +-- ...de.hpp => occupancy_segmentation_node.hpp} | 12 ++--- ...ch.py => occupancy_segmentation.launch.py} | 12 ++--- .../occupancy_segmentation/package.xml | 2 +- ...re.cpp => occupancy_segmentation_core.cpp} | 14 ++--- ...de.cpp => occupancy_segmentation_node.cpp} | 18 +++---- 8 files changed, 60 insertions(+), 60 deletions(-) rename src/world_modeling/occupancy_segmentation/include/{transformer_core.hpp => occupancy_segmentation_core.hpp} (93%) rename src/world_modeling/occupancy_segmentation/include/{transformer_node.hpp => occupancy_segmentation_node.hpp} (84%) rename src/world_modeling/occupancy_segmentation/launch/{transformer.launch.py => occupancy_segmentation.launch.py} (63%) rename src/world_modeling/occupancy_segmentation/src/{transformer_core.cpp => occupancy_segmentation_core.cpp} (75%) rename src/world_modeling/occupancy_segmentation/src/{transformer_node.cpp => occupancy_segmentation_node.cpp} (67%) diff --git a/src/world_modeling/occupancy_segmentation/CMakeLists.txt b/src/world_modeling/occupancy_segmentation/CMakeLists.txt index da069a60..972557da 100644 --- a/src/world_modeling/occupancy_segmentation/CMakeLists.txt +++ b/src/world_modeling/occupancy_segmentation/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10) -project(transformer) +project(occupancy_segmentation) # Set compiler to use C++ 17 standard if(NOT CMAKE_CXX_STANDARD) @@ -20,45 +20,45 @@ find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages # 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(transformer_lib - src/transformer_core.cpp) +add_library(occupancy_segmentation_lib + src/occupancy_segmentation_core.cpp) # Indicate to compiler where to search for header files -target_include_directories(transformer_lib +target_include_directories(occupancy_segmentation_lib PUBLIC include) # Add ROS2 dependencies required by package -ament_target_dependencies(transformer_lib +ament_target_dependencies(occupancy_segmentation_lib rclcpp sample_msgs) -# 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) +# # 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(transformer_test test/transformer_test.cpp) - # Link to the previously built library to access Transformer classes and methods - target_link_libraries(transformer_test - transformer_lib - gtest_main) +# # 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 - transformer_test - DESTINATION lib/${PROJECT_NAME}) -endif() +# # Copy executable to installation location +# install(TARGETS +# occupancy_segmentation_test +# DESTINATION lib/${PROJECT_NAME}) +# endif() # Create ROS2 node executable from source files -add_executable(transformer_node src/transformer_node.cpp) -# Link to the previously built library to access Transformer classes and methods -target_link_libraries(transformer_node transformer_lib) +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 - transformer_node + occupancy_segmentation_node DESTINATION lib/${PROJECT_NAME}) # Copy launch and config files to installation location diff --git a/src/world_modeling/occupancy_segmentation/config/params.yaml b/src/world_modeling/occupancy_segmentation/config/params.yaml index d59e071f..47c10f5b 100644 --- a/src/world_modeling/occupancy_segmentation/config/params.yaml +++ b/src/world_modeling/occupancy_segmentation/config/params.yaml @@ -1,4 +1,4 @@ -transformer_node: +occupancy_segmentation_node: ros__parameters: version: 1 compression_method: 0 diff --git a/src/world_modeling/occupancy_segmentation/include/transformer_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp similarity index 93% rename from src/world_modeling/occupancy_segmentation/include/transformer_core.hpp rename to src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 300ca850..c02d0aad 100644 --- a/src/world_modeling/occupancy_segmentation/include/transformer_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -1,5 +1,5 @@ -#ifndef TRANSFORMER_CORE_HPP_ -#define TRANSFORMER_CORE_HPP_ +#ifndef OCCUPANCY_SEGMENTATION_CORE_HPP_ +#define OCCUPANCY_SEGMENTATION_CORE_HPP_ #include @@ -12,7 +12,7 @@ namespace samples { * Implementation for the internal logic for the Transformer ROS2 * node performing data processing and validation. */ -class TransformerCore { +class OccupancySegmentationCore { public: // Size of buffer before processed messages are published. static constexpr int BUFFER_CAPACITY = 10; @@ -21,7 +21,7 @@ class TransformerCore { /** * Transformer constructor. */ - TransformerCore(); + OccupancySegmentationCore(); /** * Retrieve enqueued messages in buffer. diff --git a/src/world_modeling/occupancy_segmentation/include/transformer_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp similarity index 84% rename from src/world_modeling/occupancy_segmentation/include/transformer_node.hpp rename to src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index 5dce7b23..10970065 100644 --- a/src/world_modeling/occupancy_segmentation/include/transformer_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -1,5 +1,5 @@ -#ifndef TRANSFORMER_NODE_HPP_ -#define TRANSFORMER_NODE_HPP_ +#ifndef OCCUPANCY_SEGMENTATION_NODE_HPP_ +#define OCCUPANCY_SEGMENTATION_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -7,7 +7,7 @@ #include "sample_msgs/msg/filtered_array.hpp" #include "sample_msgs/msg/unfiltered.hpp" -#include "transformer_core.hpp" +#include "occupancy_segmentation_core.hpp" /** * Implementation of a ROS2 node that converts unfiltered messages to filtered_array @@ -17,7 +17,7 @@ * and odd timestamps. Once the node collects BUFFER_CAPACITY messages it packs * the processed messages into an array and publishes it to the "filtered" topic. */ -class TransformerNode : public rclcpp::Node { +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 @@ -26,7 +26,7 @@ class TransformerNode : public rclcpp::Node { /** * Transformer node constructor. */ - TransformerNode(); + OccupancySegmentationNode(); private: /** @@ -44,7 +44,7 @@ class TransformerNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr transform_pub_; // Object that handles data processing and validation. - samples::TransformerCore transformer_; + samples::OccupancySegmentationCore segment_; }; #endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/launch/transformer.launch.py b/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py similarity index 63% rename from src/world_modeling/occupancy_segmentation/launch/transformer.launch.py rename to src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py index 802ca850..197640f5 100755 --- a/src/world_modeling/occupancy_segmentation/launch/transformer.launch.py +++ b/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py @@ -9,20 +9,20 @@ def generate_launch_description(): """Launch transformer node.""" - transformer_pkg_prefix = get_package_share_directory('transformer') + transformer_pkg_prefix = get_package_share_directory('occupancy_segmentation') transformer_param_file = os.path.join( transformer_pkg_prefix, 'config', 'params.yaml') transformer_param = DeclareLaunchArgument( - 'transformer_param_file', + 'occupancy_segmentation_param_file', default_value=transformer_param_file, - description='Path to config file for transformer node' + description='Path to config file for occupancy segmentation node' ) transformer_node = Node( - package='transformer', - executable='transformer_node', - parameters=[LaunchConfiguration('transformer_param_file')], + package='occupancy_segmentation', + executable='occupancy_segmentation_node', + parameters=[LaunchConfiguration('occupancy_segmentation_param_file')], ) return LaunchDescription([ diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index 41a28c0f..dfcf0ef7 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -1,6 +1,6 @@ - transformer + occupancy_segmentation 0.0.0 A sample ROS package for pubsub communication diff --git a/src/world_modeling/occupancy_segmentation/src/transformer_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp similarity index 75% rename from src/world_modeling/occupancy_segmentation/src/transformer_core.cpp rename to src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 35c3f015..20e0e0e5 100644 --- a/src/world_modeling/occupancy_segmentation/src/transformer_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -1,28 +1,28 @@ #include #include -#include "transformer_core.hpp" +#include "occupancy_segmentation_core.hpp" namespace samples { -TransformerCore::TransformerCore() {} +OccupancySegmentationCore::OccupancySegmentationCore() {} -std::vector TransformerCore::buffer_messages() const { return buffer_; } +std::vector OccupancySegmentationCore::buffer_messages() const { return buffer_; } -void TransformerCore::clear_buffer() { buffer_.clear(); } +void OccupancySegmentationCore::clear_buffer() { buffer_.clear(); } -bool TransformerCore::validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered) { +bool OccupancySegmentationCore::validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered) { return unfiltered->valid; } -bool TransformerCore::enqueue_message(const sample_msgs::msg::Filtered& msg) { +bool OccupancySegmentationCore::enqueue_message(const sample_msgs::msg::Filtered& msg) { if (buffer_.size() < BUFFER_CAPACITY) { buffer_.push_back(msg); } return buffer_.size() == BUFFER_CAPACITY; } -bool TransformerCore::deserialize_coordinate( +bool OccupancySegmentationCore::deserialize_coordinate( const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, sample_msgs::msg::Filtered& filtered) { std::string serialized_position = unfiltered->data; diff --git a/src/world_modeling/occupancy_segmentation/src/transformer_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp similarity index 67% rename from src/world_modeling/occupancy_segmentation/src/transformer_node.cpp rename to src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 51bd3399..9dec7c86 100644 --- a/src/world_modeling/occupancy_segmentation/src/transformer_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -1,11 +1,11 @@ #include -#include "transformer_node.hpp" +#include "occupancy_segmentation_node.hpp" -TransformerNode::TransformerNode() : Node("transformer"), transformer_(samples::TransformerCore()) { +OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation"), segment_(samples::OccupancySegmentationCore()) { raw_sub_ = this->create_subscription( "/unfiltered_topic", ADVERTISING_FREQ, - std::bind(&TransformerNode::unfiltered_callback, this, std::placeholders::_1)); + std::bind(&OccupancySegmentationNode::unfiltered_callback, this, std::placeholders::_1)); transform_pub_ = this->create_publisher("/filtered_topic", ADVERTISING_FREQ); @@ -14,13 +14,13 @@ TransformerNode::TransformerNode() : Node("transformer"), transformer_(samples:: this->declare_parameter("compression_method", rclcpp::ParameterValue(0)); } -void TransformerNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg) { - if (!transformer_.validate_message(msg)) { +void OccupancySegmentationNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg) { + if (!segment_.validate_message(msg)) { return; } auto filtered = sample_msgs::msg::Filtered(); - if (!transformer_.deserialize_coordinate(msg, filtered)) { + if (!segment_.deserialize_coordinate(msg, filtered)) { return; } @@ -32,8 +32,8 @@ void TransformerNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::Sh RCLCPP_INFO(this->get_logger(), "Buffer Capacity Reached. PUBLISHING..."); // Publish processed data when the buffer reaches its capacity sample_msgs::msg::FilteredArray filtered_msgs; - auto buffer = transformer_.buffer_messages(); - transformer_.clear_buffer(); + auto buffer = segment_.buffer_messages(); + segment_.clear_buffer(); // Construct FilteredArray object for (auto& packet : buffer) { @@ -45,7 +45,7 @@ void TransformerNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::Sh int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } From 67bda3913a55b4d067d0623a45487507dd01e8fe Mon Sep 17 00:00:00 2001 From: VishGit1234 Date: Sat, 8 Jun 2024 21:45:08 +0000 Subject: [PATCH 28/59] Fix small error --- .../occupancy_segmentation/src/occupancy_segmentation_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 9dec7c86..b1a0411a 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -28,7 +28,7 @@ void OccupancySegmentationNode::unfiltered_callback(const sample_msgs::msg::Unfi filtered.metadata.version = this->get_parameter("version").as_int(); filtered.metadata.compression_method = this->get_parameter("compression_method").as_int(); - if (transformer_.enqueue_message(filtered)) { + if (segment_.enqueue_message(filtered)) { RCLCPP_INFO(this->get_logger(), "Buffer Capacity Reached. PUBLISHING..."); // Publish processed data when the buffer reaches its capacity sample_msgs::msg::FilteredArray filtered_msgs; From 45767c81cae492b618c8fad3e2bdcc0934d270b3 Mon Sep 17 00:00:00 2001 From: akilpath Date: Thu, 13 Jun 2024 03:13:58 +0000 Subject: [PATCH 29/59] Clean up --- .../include/occupancy_segmentation_core.hpp | 52 ---------- .../include/occupancy_segmentation_node.hpp | 21 +---- .../launch/occupancy_segmentation.launch.py | 18 ++-- .../src/occupancy_segmentation_core.cpp | 59 +----------- .../src/occupancy_segmentation_node.cpp | 43 ++------- .../test/transformer_test.cpp | 94 ------------------- 6 files changed, 19 insertions(+), 268 deletions(-) delete mode 100644 src/world_modeling/occupancy_segmentation/test/transformer_test.cpp diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index c02d0aad..db1862a0 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -3,10 +3,6 @@ #include -#include "sample_msgs/msg/filtered.hpp" -#include "sample_msgs/msg/unfiltered.hpp" - -namespace samples { /** * Implementation for the internal logic for the Transformer ROS2 @@ -22,55 +18,7 @@ class OccupancySegmentationCore { * Transformer constructor. */ OccupancySegmentationCore(); - - /** - * Retrieve enqueued messages in buffer. - * - * @returns enqueued processed messages - */ - std::vector buffer_messages() const; - - /** - * Removes all messages in buffer. Called by the transformer - * node after messages have been published to aggregator. - */ - void clear_buffer(); - - /** - * Validates that the 'valid' field of an unfiltered message - * is set to true. - * - * @param unfiltered a raw message - * @returns whether message's 'valid' field is set - */ - bool validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered); - - /** - * Enqueue message into an array of processed messages to "filtered" topic. - * Ignores messages once the buffer capacity is reached. - * - * @param msg a processed message to be published - * @returns whether buffer is full after adding new message - */ - bool enqueue_message(const sample_msgs::msg::Filtered& msg); - - /** - * Deserializes the data field of the unfiltered ROS2 message. - * The data field should be of the form "x:$num1;y:$num2;z:$num3;". - * - * @param[in] unfiltered the raw message containing serialized data - * @param[out] filtered the processed message containing deserialized data - * @returns whether deserialization was successful - */ - bool deserialize_coordinate(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, - sample_msgs::msg::Filtered& filtered); - - private: - // Buffer storing processed messages until BUFFER_CAPACITY. Clear after - // messages are published. - std::vector buffer_; }; -} // namespace samples #endif // TRANSFORMER_CORE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index 10970065..580ee4dc 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -3,10 +3,6 @@ #include "rclcpp/rclcpp.hpp" -#include "sample_msgs/msg/filtered.hpp" -#include "sample_msgs/msg/filtered_array.hpp" -#include "sample_msgs/msg/unfiltered.hpp" - #include "occupancy_segmentation_core.hpp" /** @@ -29,22 +25,11 @@ class OccupancySegmentationNode : public rclcpp::Node { OccupancySegmentationNode(); private: - /** - * A ROS2 subscription node callback used to process raw data from the - * "unfiltered" topic and publish to the "filtered" topic. - * - * @param msg a raw message from the "unfiltered" topic - */ - void unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg); - - // ROS2 subscriber listening to the unfiltered topic. - rclcpp::Subscription::SharedPtr raw_sub_; - - // ROS2 publisher sending processed messages to the filtered topic. - rclcpp::Publisher::SharedPtr transform_pub_; // Object that handles data processing and validation. - samples::OccupancySegmentationCore segment_; + + rclcpp::TimerBase::SharedPtr timer_; + void timer_callback(); }; #endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py b/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py index 197640f5..975e8a31 100755 --- a/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py +++ b/src/world_modeling/occupancy_segmentation/launch/occupancy_segmentation.launch.py @@ -8,24 +8,24 @@ def generate_launch_description(): - """Launch transformer node.""" - transformer_pkg_prefix = get_package_share_directory('occupancy_segmentation') - transformer_param_file = os.path.join( - transformer_pkg_prefix, 'config', 'params.yaml') + """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') - transformer_param = DeclareLaunchArgument( + occupancy_seg_param = DeclareLaunchArgument( 'occupancy_segmentation_param_file', - default_value=transformer_param_file, + default_value=occupancy_seg_param_file, description='Path to config file for occupancy segmentation node' ) - transformer_node = Node( + occupancy_seg_node = Node( package='occupancy_segmentation', executable='occupancy_segmentation_node', parameters=[LaunchConfiguration('occupancy_segmentation_param_file')], ) return LaunchDescription([ - transformer_param, - transformer_node + occupancy_seg_param, + occupancy_seg_node ]) diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 20e0e0e5..b8425eea 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -3,62 +3,5 @@ #include "occupancy_segmentation_core.hpp" -namespace samples { -OccupancySegmentationCore::OccupancySegmentationCore() {} - -std::vector OccupancySegmentationCore::buffer_messages() const { return buffer_; } - -void OccupancySegmentationCore::clear_buffer() { buffer_.clear(); } - -bool OccupancySegmentationCore::validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered) { - return unfiltered->valid; -} - -bool OccupancySegmentationCore::enqueue_message(const sample_msgs::msg::Filtered& msg) { - if (buffer_.size() < BUFFER_CAPACITY) { - buffer_.push_back(msg); - } - return buffer_.size() == BUFFER_CAPACITY; -} - -bool OccupancySegmentationCore::deserialize_coordinate( - const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, - sample_msgs::msg::Filtered& filtered) { - std::string serialized_position = unfiltered->data; - auto start_pos = serialized_position.find("x:"); - auto end_pos = serialized_position.find(";"); - // Validate that the substrings were found - if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { - return false; - } - // Offset index to start of x_pos - start_pos += 2; - // Splice string and convert position to float - float x = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); - - start_pos = serialized_position.find("y:", end_pos + 1); - end_pos = serialized_position.find(";", end_pos + 1); - if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { - return false; - } - // Offset index to start of y_pos - start_pos += 2; - float y = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); - - start_pos = serialized_position.find("z:", end_pos + 1); - end_pos = serialized_position.find(";", end_pos + 1); - if (start_pos == std::string::npos || end_pos == std::string::npos || end_pos < start_pos) { - return false; - } - // Offset index to start of z_pos - start_pos += 2; - float z = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); - - filtered.pos_x = x; - filtered.pos_y = y; - filtered.pos_z = z; - return true; -} - -} // namespace samples +OccupancySegmentationCore::OccupancySegmentationCore() {} \ No newline at end of file diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index b1a0411a..38d6f2eb 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -1,49 +1,18 @@ #include - +#include #include "occupancy_segmentation_node.hpp" -OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation"), segment_(samples::OccupancySegmentationCore()) { - raw_sub_ = this->create_subscription( - "/unfiltered_topic", ADVERTISING_FREQ, - std::bind(&OccupancySegmentationNode::unfiltered_callback, this, std::placeholders::_1)); - transform_pub_ = - this->create_publisher("/filtered_topic", ADVERTISING_FREQ); +OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { - // Define the default values for parameters if not defined in params.yaml - this->declare_parameter("version", rclcpp::ParameterValue(0)); - this->declare_parameter("compression_method", rclcpp::ParameterValue(0)); + timer_ = this->create_wall_timer(std::chrono::milliseconds(1000), std::bind(&OccupancySegmentationNode::timer_callback, this)); } -void OccupancySegmentationNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg) { - if (!segment_.validate_message(msg)) { - return; - } - - auto filtered = sample_msgs::msg::Filtered(); - if (!segment_.deserialize_coordinate(msg, filtered)) { - return; - } - - filtered.timestamp = msg->timestamp; - filtered.metadata.version = this->get_parameter("version").as_int(); - filtered.metadata.compression_method = this->get_parameter("compression_method").as_int(); - - if (segment_.enqueue_message(filtered)) { - RCLCPP_INFO(this->get_logger(), "Buffer Capacity Reached. PUBLISHING..."); - // Publish processed data when the buffer reaches its capacity - sample_msgs::msg::FilteredArray filtered_msgs; - auto buffer = segment_.buffer_messages(); - segment_.clear_buffer(); - - // Construct FilteredArray object - for (auto& packet : buffer) { - filtered_msgs.packets.push_back(packet); - } - transform_pub_->publish(filtered_msgs); - } +void OccupancySegmentationNode::timer_callback(){ + RCLCPP_INFO(this->get_logger(), "Working"); } int main(int argc, char** argv) { + std::cout << "Huh" << std::endl; rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp b/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp deleted file mode 100644 index 0d103350..00000000 --- a/src/world_modeling/occupancy_segmentation/test/transformer_test.cpp +++ /dev/null @@ -1,94 +0,0 @@ -#include -#include -#include - -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" - -#include "transformer_core.hpp" - -/** - * When writing a large number of tests it is desirable to wrap any common - * setup or cleanup logic in a test fixture. This improves readability and reduces - * the amount of boilerplate code in each test. For more information checkout - * https://google.github.io/googletest/primer.html#same-data-multiple-tests - */ -class TransformerFixtureTest : public ::testing::Test { - public: - void SetUp(int enqueue_count) { - auto filtered = sample_msgs::msg::Filtered(); - for (int i = 0; i < enqueue_count; i++) { - transformer.enqueue_message(filtered); - } - } - - protected: - samples::TransformerCore transformer; -}; - -/** - * Parameterized tests let us test the same logic with different parameters without - * having to write boilerplate for multiple tests. For more information checkout - * https://google.github.io/googletest/advanced.html#value-parameterized-tests - */ -class TransformerParameterizedTest - : public ::testing::TestWithParam> { - protected: - samples::TransformerCore transformer; -}; - -TEST(TransformerTest, FilterInvalidField) { - auto unfiltered = std::make_shared(); - unfiltered->valid = false; - - auto transformer = samples::TransformerCore(); - bool valid = transformer.validate_message(unfiltered); - EXPECT_FALSE(valid); -} - -TEST_F(TransformerFixtureTest, BufferCapacity) { - SetUp(samples::TransformerCore::BUFFER_CAPACITY - 1); - - // Place last message that fits in buffer - auto filtered = sample_msgs::msg::Filtered(); - bool full = transformer.enqueue_message(filtered); - EXPECT_TRUE(full); - int size = transformer.buffer_messages().size(); - EXPECT_EQ(size, 10); - - // Attempting to enqueue message to full buffer should fail - full = transformer.enqueue_message(filtered); - EXPECT_TRUE(full); - size = transformer.buffer_messages().size(); - EXPECT_EQ(size, 10); -} - -TEST_F(TransformerFixtureTest, ClearBuffer) { - // Place 3 messages in buffer - SetUp(3); - - int size = transformer.buffer_messages().size(); - EXPECT_GT(size, 0); - - transformer.clear_buffer(); - size = transformer.buffer_messages().size(); - EXPECT_EQ(size, 0); -} - -TEST_P(TransformerParameterizedTest, SerializationValidation) { - auto [serialized_field, valid] = GetParam(); - auto filtered = sample_msgs::msg::Filtered(); - auto unfiltered = std::make_shared(); - unfiltered->data = serialized_field; - EXPECT_EQ(transformer.deserialize_coordinate(unfiltered, filtered), valid); -} - -// Parameterized testing lets us validate all edge cases for serialization -// using one test case. -INSTANTIATE_TEST_CASE_P(Serialization, TransformerParameterizedTest, - ::testing::Values(std::make_tuple("x:1;y:2;z:3", false), - std::make_tuple("z:1;y:2;x:3;", false), - std::make_tuple("x:1,y:2,z:3", false), - std::make_tuple("x:3;", false), - std::make_tuple("x:3;y:2;z:3;", true), - std::make_tuple("x:3;y:22; z:11;", true))); From b74770ca2496750b083bdc0131037942b866138c Mon Sep 17 00:00:00 2001 From: VishGit1234 Date: Sun, 16 Jun 2024 02:56:13 +0000 Subject: [PATCH 30/59] Add pcl package --- src/world_modeling/occupancy_segmentation/CMakeLists.txt | 4 +++- src/world_modeling/occupancy_segmentation/package.xml | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/CMakeLists.txt b/src/world_modeling/occupancy_segmentation/CMakeLists.txt index 972557da..ff5d26bb 100644 --- a/src/world_modeling/occupancy_segmentation/CMakeLists.txt +++ b/src/world_modeling/occupancy_segmentation/CMakeLists.txt @@ -14,6 +14,7 @@ endif() find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages +find_package(pcl_ros REQUIRED) # Compiles source files into a library # A library is not executed, instead other executables can link @@ -29,7 +30,8 @@ target_include_directories(occupancy_segmentation_lib # Add ROS2 dependencies required by package ament_target_dependencies(occupancy_segmentation_lib rclcpp - sample_msgs) + sample_msgs + pcl_ros) # # By default tests are built. This can be overridden by modifying the # # colcon build command to include the -DBUILD_TESTING=OFF flag. diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index dfcf0ef7..a6ed23aa 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -11,6 +11,7 @@ ament_cmake rclcpp sample_msgs + pcl_ros ament_cmake_gtest @@ -18,4 +19,4 @@ ament_cmake - \ No newline at end of file + From f947fd59df69f74226a4b9272da71fb697b237dd Mon Sep 17 00:00:00 2001 From: akilpath Date: Mon, 17 Jun 2024 02:37:11 +0000 Subject: [PATCH 31/59] Added eigen to dockerifle --- .../occupancy_segmentation/occupancy_segmentation.Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index 1514079f..74fbe85f 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -16,6 +16,7 @@ RUN apt-get -qq update && rosdep update && \ | awk '{print $3}' \ | sort > /tmp/colcon_install_list +RUN sudo apt-get install libeigen3-dev ################################# Dependencies ################################ FROM ${BASE_IMAGE} as dependencies From 49076c9ec156f5760a1590c4a6a1af3591652880 Mon Sep 17 00:00:00 2001 From: VishGit1234 <47926842+VishGit1234@users.noreply.github.com> Date: Wed, 19 Jun 2024 15:58:25 -0400 Subject: [PATCH 32/59] Update docker-compose.world_modeling.yaml --- .../dev_overrides/docker-compose.world_modeling.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/dev_overrides/docker-compose.world_modeling.yaml b/modules/dev_overrides/docker-compose.world_modeling.yaml index 7682c63a..55053d40 100644 --- a/modules/dev_overrides/docker-compose.world_modeling.yaml +++ b/modules/dev_overrides/docker-compose.world_modeling.yaml @@ -13,7 +13,7 @@ services: image: "${WORLD_MODELING_HD_MAP_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/hd_map + - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/hd_map localization: <<: *fixuid @@ -23,7 +23,7 @@ services: image: "${WORLD_MODELING_LOCALIZATION_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/localization + - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/localization occupancy: <<: *fixuid @@ -33,7 +33,7 @@ services: image: "${WORLD_MODELING_OCCUPANCY_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/occupancy + - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/occupancy occupancy_segmentation: <<: *fixuid @@ -43,7 +43,7 @@ services: image: "${WORLD_MODELING_OCCUPANCY_SEGMENTATION_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/occupancy_segmentation + - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/occupancy_segmentation motion_forecasting: <<: *fixuid @@ -53,4 +53,4 @@ services: image: "${WORLD_MODELING_MOTION_FORECASTING_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/ament_ws/src/motion_forecasting + - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/motion_forecasting From 75a1c9e1e20a3dba0d81c4f74a2fada82f54ade8 Mon Sep 17 00:00:00 2001 From: VishGit1234 <47926842+VishGit1234@users.noreply.github.com> Date: Wed, 19 Jun 2024 16:00:33 -0400 Subject: [PATCH 33/59] Update docker-compose.world_modeling.yaml --- modules/dev_overrides/docker-compose.world_modeling.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/dev_overrides/docker-compose.world_modeling.yaml b/modules/dev_overrides/docker-compose.world_modeling.yaml index 55053d40..8cee3f7c 100644 --- a/modules/dev_overrides/docker-compose.world_modeling.yaml +++ b/modules/dev_overrides/docker-compose.world_modeling.yaml @@ -43,7 +43,7 @@ services: image: "${WORLD_MODELING_OCCUPANCY_SEGMENTATION_IMAGE}:build_${TAG}" command: tail -F anything volumes: - - ${MONO_DIR}/src/world_modeling:/home/bolty/ament_ws/src/occupancy_segmentation + - ${MONO_DIR}/src/world_modeling/occupancy_segmentation:/home/bolty/ament_ws/src/occupancy_segmentation motion_forecasting: <<: *fixuid From b36ebecdadf823ee302b619f2ca482ffef48614f Mon Sep 17 00:00:00 2001 From: akilpath Date: Wed, 19 Jun 2024 22:26:29 +0000 Subject: [PATCH 34/59] added tbb to dockerfile --- .../occupancy_segmentation/occupancy_segmentation.Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index 74fbe85f..55d9a87a 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -17,6 +17,7 @@ RUN apt-get -qq update && rosdep update && \ | sort > /tmp/colcon_install_list RUN sudo apt-get install libeigen3-dev +RUN sudo apt-get -y install libtbb-dev ################################# Dependencies ################################ FROM ${BASE_IMAGE} as dependencies From f9e0f33176b2f9395b9c963d1162b613f64184b0 Mon Sep 17 00:00:00 2001 From: akilpath Date: Sun, 23 Jun 2024 04:32:03 +0000 Subject: [PATCH 35/59] Started working on it --- .../occupancy_segmentation/CMakeLists.txt | 2 - .../include/occupancy_segmentation_core.hpp | 43 +++++++++++++++---- .../occupancy_segmentation/package.xml | 1 - .../src/occupancy_segmentation_core.cpp | 40 ++++++++++++++++- .../src/occupancy_segmentation_node.cpp | 1 - 5 files changed, 73 insertions(+), 14 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/CMakeLists.txt b/src/world_modeling/occupancy_segmentation/CMakeLists.txt index ff5d26bb..8df7a7cd 100644 --- a/src/world_modeling/occupancy_segmentation/CMakeLists.txt +++ b/src/world_modeling/occupancy_segmentation/CMakeLists.txt @@ -13,7 +13,6 @@ endif() # Search for dependencies required for building this package find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package -find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages find_package(pcl_ros REQUIRED) # Compiles source files into a library @@ -30,7 +29,6 @@ target_include_directories(occupancy_segmentation_lib # Add ROS2 dependencies required by package ament_target_dependencies(occupancy_segmentation_lib rclcpp - sample_msgs pcl_ros) # # By default tests are built. This can be overridden by modifying the diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index db1862a0..01ab8654 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -2,6 +2,17 @@ #define OCCUPANCY_SEGMENTATION_CORE_HPP_ #include +#include +#include +#include + +#include +#include + + +#define NUM_ZONES 4 +#define L_MIN 2.7 +#define L_MAX 80.0 /** @@ -9,15 +20,29 @@ * node performing data processing and validation. */ class OccupancySegmentationCore { - public: - // Size of buffer before processed messages are published. - static constexpr int BUFFER_CAPACITY = 10; - - public: - /** - * Transformer constructor. - */ - OccupancySegmentationCore(); + public: + typedef std::vector> Ring; + typedef std::vector Zone; + // Size of buffer before processed messages are published. + static constexpr int BUFFER_CAPACITY = 10; + + const int ZONE_RINGS [NUM_ZONES] = {2,4,4,4}; + const int ZONE_SECTORS [NUM_ZONES] = {16,32,54,32}; + + + std::vector czm; + + + OccupancySegmentationCore(); + + private: + + void init_czm(); + + void fill_czm(pcl::PointCloud &cloud_in); + + + }; diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index a6ed23aa..e84324c4 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -10,7 +10,6 @@ ament_cmake rclcpp - sample_msgs pcl_ros ament_cmake_gtest diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index b8425eea..e77fbd6a 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -4,4 +4,42 @@ #include "occupancy_segmentation_core.hpp" -OccupancySegmentationCore::OccupancySegmentationCore() {} \ No newline at end of file +OccupancySegmentationCore::OccupancySegmentationCore() {} + +void OccupancySegmentationCore::init_czm(){ + std::vector temp_czm; + czm = temp_czm; + for(int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){ + Zone zone; + for(int r = 0; r < ZONE_RINGS[zone_idx]; r++){ + Ring ring; + for (int s = 0; s < ZONE_SECTORS[zone_idx]; s++){ + pcl::PointCloud patch; + ring.emplace_back(patch); + } + zone.emplace_back(ring); + } + czm.emplace_back(zone); + } +} + +void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in){ + for(pcl::PointXYZ &p : cloud_in.points){ + double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); + double theta = atan2(p.y,p.x); // EDITED! + if (theta < 0){ + theta += 2*M_PI; + } + double lmins[4] = {L_MIN, (7*L_MIN + L_MAX) / 8, (3*L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + double deltal = L_MAX - L_MIN; + + for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){ + + } + + + + } + +} \ No newline at end of file diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 38d6f2eb..baa6475a 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -12,7 +12,6 @@ void OccupancySegmentationNode::timer_callback(){ } int main(int argc, char** argv) { - std::cout << "Huh" << std::endl; rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); From 0a4d547a368e97b9c1ba725d33ff518e5e74943e Mon Sep 17 00:00:00 2001 From: akilpath Date: Sun, 23 Jun 2024 17:17:14 +0000 Subject: [PATCH 36/59] Finished czm initialization --- .../src/occupancy_segmentation_core.cpp | 28 +++++++++++++------ watod-config.sh | 2 +- 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index e77fbd6a..3c12ab14 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -24,22 +24,34 @@ void OccupancySegmentationCore::init_czm(){ } void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in){ + double lmins[4] = {L_MIN, (7*L_MIN + L_MAX) / 8, (3*L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; for(pcl::PointXYZ &p : cloud_in.points){ double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); - double theta = atan2(p.y,p.x); // EDITED! + + // Out of range for czm + if (r < L_MIN || r > L_MAX){ + continue; + } + + double theta = atan2(p.y,p.x); if (theta < 0){ theta += 2*M_PI; } - double lmins[4] = {L_MIN, (7*L_MIN + L_MAX) / 8, (3*L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + double deltal = L_MAX - L_MIN; for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){ - + int ring_idx, sector_idx; + if (r < lmaxs[zone_idx]){ + double ring_size = deltal / ZONE_RINGS[zone_idx]; + double sector_size = 2*M_PI / ZONE_SECTORS[zone_idx]; + ring_idx = (int) ((r - lmins[zone_idx]) / ring_size); + //TODO: find out why the use min() in the paper, meantime use the top + //ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); + sector_idx = (int) (theta / sector_size); + czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + } } - - - } - } \ No newline at end of file diff --git a/watod-config.sh b/watod-config.sh index 321efd80..dc6a6b1c 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -23,7 +23,7 @@ ACTIVE_MODULES="world_modeling" ## - deploy (default) : runs production-grade containers (non-editable) ## - develop : runs developer containers (editable) -# MODE_OF_OPERATION="" +MODE_OF_OPERATION="develop" ############################## ADVANCED CONFIGURATIONS ############################## ## Name to append to docker containers. DEFAULT = "" From 7ade2d1082f4a1c05b0b8f02668e6a2508e360d0 Mon Sep 17 00:00:00 2001 From: akilpath Date: Wed, 26 Jun 2024 02:36:30 +0000 Subject: [PATCH 37/59] Plane --- .../include/occupancy_segmentation_core.hpp | 23 +++++++++++++++---- .../src/occupancy_segmentation_core.cpp | 23 +++++++++++++++++++ 2 files changed, 42 insertions(+), 4 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 01ab8654..9a29000b 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -8,17 +8,30 @@ #include #include +#include #define NUM_ZONES 4 #define L_MIN 2.7 #define L_MAX 80.0 +#define N_SEED 20 +#define Z_SEED 0.5 +#define MD 0.3 +#define MH -1.1 + + +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_; +}; -/** - * Implementation for the internal logic for the Transformer ROS2 - * node performing data processing and validation. - */ class OccupancySegmentationCore { public: typedef std::vector> Ring; @@ -41,6 +54,8 @@ class OccupancySegmentationCore { void fill_czm(pcl::PointCloud &cloud_in); + void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); + }; diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 3c12ab14..28cbd4fe 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -54,4 +54,27 @@ void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_i } } } +} + +void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat){ + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = ( feat.singular_values_(0) - feat.singular_values_(1) ) / feat.singular_values_(0); + feat.planarity_ = ( feat.singular_values_(1) - feat.singular_values_(2) ) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; } \ No newline at end of file From 70fd5981f115e1965bbf6bc54004855b4971ee4c Mon Sep 17 00:00:00 2001 From: akilpath Date: Sun, 7 Jul 2024 22:57:54 +0000 Subject: [PATCH 38/59] Mostly done --- .../occupancy_segmentation.Dockerfile | 1 + .../include/occupancy_segmentation_core.hpp | 33 ++- .../src/occupancy_segmentation_core.cpp | 246 +++++++++++++----- 3 files changed, 221 insertions(+), 59 deletions(-) diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index 55d9a87a..ae27a07e 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -6,6 +6,7 @@ FROM ${BASE_IMAGE} as source WORKDIR ${AMENT_WS}/src # Copy in source code +# RUN git clone https://github.com/ros-perception/perception_pcl.git --branch 2.4.3 COPY src/world_modeling/occupancy_segmentation occupancy_segmentation COPY src/wato_msgs/sample_msgs sample_msgs diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 9a29000b..0d2b92a7 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -20,6 +20,11 @@ #define MD 0.3 #define MH -1.1 +#define MIN_NUM_POINTS 10 +#define NUM_SEED_POINTS 20 +#define TH_SEEDS 0.5 +#define UPRIGHTNESS_THRESH 45 + struct PCAFeature { Eigen::Vector3f principal_; @@ -32,6 +37,14 @@ struct PCAFeature { float planarity_; }; +struct Patch_Index { + int idx; + int zone_idx; + int ring_idx; + int sector_idx; + int concentric_idx; +} + class OccupancySegmentationCore { public: typedef std::vector> Ring; @@ -42,8 +55,16 @@ class OccupancySegmentationCore { const int ZONE_RINGS [NUM_ZONES] = {2,4,4,4}; const int ZONE_SECTORS [NUM_ZONES] = {16,32,54,32}; + int num_patches = -1; + + std::vector _czm; + std::vector> _regionwise_ground; + std::vector> _regionwise_nonground; - std::vector czm; + std::vector _patch_indices; + + pcl::PointCloud _ground; + pcl::Pointcloud _non_ground; OccupancySegmentationCore(); @@ -55,6 +76,16 @@ class OccupancySegmentationCore { void fill_czm(pcl::PointCloud &cloud_in); void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); + + void rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat); + + void extract_initial_seeds(pcl::PointCloud &cloud, pcl::PointCloud &seed_cloud, int zone_idx); + + bool ground_likelihood_est(PCAFeature &feat, int concentric_idx); + + void segment_ground(); + + bool point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b); diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 28cbd4fe..c804e9de 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -3,78 +3,208 @@ #include "occupancy_segmentation_core.hpp" - OccupancySegmentationCore::OccupancySegmentationCore() {} -void OccupancySegmentationCore::init_czm(){ - std::vector temp_czm; - czm = temp_czm; - for(int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){ - Zone zone; - for(int r = 0; r < ZONE_RINGS[zone_idx]; r++){ - Ring ring; - for (int s = 0; s < ZONE_SECTORS[zone_idx]; s++){ - pcl::PointCloud patch; - ring.emplace_back(patch); - } - zone.emplace_back(ring); - } - czm.emplace_back(zone); +void OccupancySegmentationCore::init_czm() { + int concentric_count = 0; + int patch_count = 0; + + for (int z = 0; z < NUM_ZONES; z++) { + Zone zone; + Patch_Idx index; + index.zone_idx = z; + + for (int r = 0; r < ZONE_RINGS[z]; r++) { + Ring ring; + index.ring_idx = r; + index.concentric_idx = concentric_count; + + for (int s = 0; s < ZONE_SECTORS[z]; s++) { + index.sector_idx = s; + index.patch_idx = patch_count; + _patch_indices.push_back(index); + pcl::PointCloud patch; + ring.emplace_back(patch); + _regionwise_ground.push_back(pcl::PointCloud>()); + _regionwise_nonground.push_back(pcl::PointCloud>()); + + patch_count++; + } + zone.emplace_back(ring); } + _czm.emplace_back(zone); + } + num_patches = patch_count; } -void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in){ - double lmins[4] = {L_MIN, (7*L_MIN + L_MAX) / 8, (3*L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; - for(pcl::PointXYZ &p : cloud_in.points){ - double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); +void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { + double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + for (pcl::PointXYZ &p : cloud_in.points) { + double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); - // Out of range for czm - if (r < L_MIN || r > L_MAX){ - continue; - } + // Out of range for czm + if (r < L_MIN || r > L_MAX) { + continue; + } - double theta = atan2(p.y,p.x); - if (theta < 0){ - theta += 2*M_PI; - } + double theta = atan2(p.y, p.x); + if (theta < 0) { + theta += 2 * M_PI; + } + + double deltal = L_MAX - L_MIN; + + for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { + int ring_idx, sector_idx; + if (r < lmaxs[zone_idx]) { + double ring_size = deltal / ZONE_RINGS[zone_idx]; + double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; + ring_idx = (int)((r - lmins[zone_idx]) / ring_size); + // TODO: find out why the use min() in the paper, meantime use the top + // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); + sector_idx = (int)(theta / sector_size); + czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + } + } + } +} + +void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, + PCAFeature &feat) { + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = + (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = + (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; +} + +void OccupancySegmentationCore::segment_ground() { + tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { + for (int patch_num = r.begin(); patch_num < r.end(); r++) { + Patch_Index &p_idx = patch_indices[patch_num]; + pcl::PointCloud &patch = czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + PCAFeature features; + if (patch.points.size() > MIN_NUM_POINTS) { + std::sort(patch.points.begin(), patch.points.end()); + pcl::PointCloud ground_temp; + rgpf(patch, p_idx, feat); - double deltal = L_MAX - L_MIN; - - for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++){ - int ring_idx, sector_idx; - if (r < lmaxs[zone_idx]){ - double ring_size = deltal / ZONE_RINGS[zone_idx]; - double sector_size = 2*M_PI / ZONE_SECTORS[zone_idx]; - ring_idx = (int) ((r - lmins[zone_idx]) / ring_size); - //TODO: find out why the use min() in the paper, meantime use the top - //ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); - sector_idx = (int) (theta / sector_size); - czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + } + } + }); +} + +void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { + pcl::PointCloud ground_temp; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + + if (!region_ground.empty()) region_ground.clear(); + if (!region_nonground.empty()) region_nonground.clear(); + + size_t N = patch.size(); + + extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); + int num_iters = 3; + for(int i = 0; i < num_iters; i++){ + estimate_plane(ground_temp, feat); + ground_temp.clear(); + Eigen::MatrixXf points(N, 3); + int j = 0; + for (pcl::PointXYZ &p:src.points) { + points.row(j++) = p.getVector3fMap(); + } + + Eigen::VectorXf result = points * feat.normal_; + for (size_t r = 0; r < N; r++) { + if (i < num_iters - 1) { + if (result[r] < feat.th_dist_d_) { + ground_tmp.points.emplace_back(src[r]); + } + } else { // Final stage + if (result[r] < feat.th_dist_d_) { + regionwise_ground.points.emplace_back(src[r]); + } else { + regionwise_nonground.points.emplace_back(src[r]); + } } } + + } +} + +bool OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ + + //uprightness filter + if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ + return false; + } + + const double z_elevation = feat.mean_(2); + const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + + //elevation filter + if (concentric_idx < num_rings_of_interest_) { + if (z_elevation > -sensor_height_ + elevation_thr_[concentric_idx] + && flatness_thr_[concentric_idx] <= surface_variable) { + return false; + } else{ + return true; + } + } else { + if (using_global_thr_ && (z_elevation > global_elevation_thr_)) { + return false; + } else { + return true; } + } + } -void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat){ - Eigen::Matrix3f cov; - Eigen::Vector4f mean; - pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); +void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud &cloud, + pcl::PointCloud &seed_cloud, int zone_idx) { + + //adaptive seed selection for 1st zone + + size_t init_idx = 0; + // if (zone_idx == 0){ - Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - feat.singular_values_ = svd.singularValues(); + // - feat.linearity_ = ( feat.singular_values_(0) - feat.singular_values_(1) ) / feat.singular_values_(0); - feat.planarity_ = ( feat.singular_values_(1) - feat.singular_values_(2) ) / feat.singular_values_(0); + double sum = 0; + int cnt = 0; + for (size_t i = 0; i < cloud.points.size() && cnt < NUM_SEED_POINTS; i++) { + sum += cloud.points[i].z; + cnt++; + } - // use the least singular vector as normal - feat.normal_ = (svd.matrixU().col(2)); - if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - feat.normal_ = -feat.normal_; + double mean_z = sum / cnt; + for (size_t i = init_idx; i < cloud.points.size(); i++) { + if (cloud.points[i].z < mean_z + TH_SEEDS) { + seed_cloud.points.push_back(cloud.points[i]); } - // mean ground seeds value - feat.mean_ = mean.head<3>(); - // according to normal.T*[x,y,z] = -d - feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - feat.th_dist_d_ = MD - feat.d_; -} \ No newline at end of file + } +} + +bool OccupancySegmentationCore::point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b) { return a.z < b.z; } + From 310e9d23e895a5cecf1dff11b96378fa300fe7b3 Mon Sep 17 00:00:00 2001 From: VishGit1234 Date: Tue, 9 Jul 2024 00:24:07 +0000 Subject: [PATCH 39/59] Add apt update --- .../occupancy_segmentation/occupancy_segmentation.Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index ae27a07e..322a1ee8 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -24,7 +24,7 @@ FROM ${BASE_IMAGE} as dependencies # 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} From 957ead8d957cb092dcf53ee269be8e0258716970 Mon Sep 17 00:00:00 2001 From: akilpath Date: Thu, 18 Jul 2024 02:19:56 +0000 Subject: [PATCH 40/59] Fixing small bugs --- .../include/occupancy_segmentation_core.hpp | 14 +- .../src/occupancy_segmentation_core.cpp | 327 +++++++++--------- 2 files changed, 177 insertions(+), 164 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 0d2b92a7..1721aec7 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -25,6 +25,11 @@ #define TH_SEEDS 0.5 #define UPRIGHTNESS_THRESH 45 +#define NUM_RINGS_OF_INTEREST 4 +#define SENSOR_HEIGHT 0 +#define GLOBAL_EL_THRESH 0 + + struct PCAFeature { Eigen::Vector3f principal_; @@ -43,7 +48,7 @@ struct Patch_Index { int ring_idx; int sector_idx; int concentric_idx; -} +}; class OccupancySegmentationCore { public: @@ -54,6 +59,9 @@ class OccupancySegmentationCore { const int ZONE_RINGS [NUM_ZONES] = {2,4,4,4}; const int ZONE_SECTORS [NUM_ZONES] = {16,32,54,32}; + const float FLATNESS_THR [NUM_ZONES] = {0.0005, 0.000725, 0.001, 0.001}; + const float ELEVATION_THR [NUM_ZONES] = {0.523, 0.746, 0.879, 1.125}; + int num_patches = -1; @@ -61,10 +69,10 @@ class OccupancySegmentationCore { std::vector> _regionwise_ground; std::vector> _regionwise_nonground; - std::vector _patch_indices; + std::vector _patch_indices; pcl::PointCloud _ground; - pcl::Pointcloud _non_ground; + pcl::PointCloud _non_ground; OccupancySegmentationCore(); diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index c804e9de..bf867542 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -6,108 +6,111 @@ OccupancySegmentationCore::OccupancySegmentationCore() {} void OccupancySegmentationCore::init_czm() { - int concentric_count = 0; - int patch_count = 0; - - for (int z = 0; z < NUM_ZONES; z++) { - Zone zone; - Patch_Idx index; - index.zone_idx = z; - - for (int r = 0; r < ZONE_RINGS[z]; r++) { - Ring ring; - index.ring_idx = r; - index.concentric_idx = concentric_count; - - for (int s = 0; s < ZONE_SECTORS[z]; s++) { - index.sector_idx = s; - index.patch_idx = patch_count; - _patch_indices.push_back(index); - pcl::PointCloud patch; - ring.emplace_back(patch); - _regionwise_ground.push_back(pcl::PointCloud>()); - _regionwise_nonground.push_back(pcl::PointCloud>()); - - patch_count++; - } - zone.emplace_back(ring); - } - _czm.emplace_back(zone); - } - num_patches = patch_count; + // int concentric_count = 0; + // int patch_count = 0; + + // for (int z = 0; z < NUM_ZONES; z++) { + // Zone zone; + // Patch_Index index; + // index.zone_idx = z; + + // for (int r = 0; r < ZONE_RINGS[z]; r++) { + // Ring ring; + // index.ring_idx = r; + // index.concentric_idx = concentric_count; + + // for (int s = 0; s < ZONE_SECTORS[z]; s++) { + // index.sector_idx = s; + // index.idx = patch_count; + // _patch_indices.push_back(index); + // pcl::PointCloud patch; + // ring.emplace_back(patch); + // pcl::PointCloud regionground; + // pcl::PointCloud regionnonground; + // _regionwise_ground.push_back(regionground); + // _regionwise_nonground.push_back(regionnonground); + + // patch_count++; + // } + // zone.emplace_back(ring); + // } + // _czm.emplace_back(zone); + // } + // num_patches = patch_count; } void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { - double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; - for (pcl::PointXYZ &p : cloud_in.points) { - double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); - - // Out of range for czm - if (r < L_MIN || r > L_MAX) { - continue; - } - - double theta = atan2(p.y, p.x); - if (theta < 0) { - theta += 2 * M_PI; - } - - double deltal = L_MAX - L_MIN; - - for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { - int ring_idx, sector_idx; - if (r < lmaxs[zone_idx]) { - double ring_size = deltal / ZONE_RINGS[zone_idx]; - double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; - ring_idx = (int)((r - lmins[zone_idx]) / ring_size); - // TODO: find out why the use min() in the paper, meantime use the top - // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); - sector_idx = (int)(theta / sector_size); - czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); - } - } - } + // double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + // double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + // for (pcl::PointXYZ &p : cloud_in.points) { + // double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); + + // // Out of range for czm + // if (r < L_MIN || r > L_MAX) { + // continue; + // } + + // double theta = atan2(p.y, p.x); + // if (theta < 0) { + // theta += 2 * M_PI; + // } + + // double deltal = L_MAX - L_MIN; + + // for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { + // int ring_idx, sector_idx; + // if (r < lmaxs[zone_idx]) { + // double ring_size = deltal / ZONE_RINGS[zone_idx]; + // double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; + // ring_idx = (int)((r - lmins[zone_idx]) / ring_size); + // // TODO: find out why the use min() in the paper, meantime use the top + // // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); + // sector_idx = (int)(theta / sector_size); + // _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + // } + // } + // } } void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat) { - Eigen::Matrix3f cov; - Eigen::Vector4f mean; - pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); - - Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - feat.singular_values_ = svd.singularValues(); - - feat.linearity_ = - (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - feat.planarity_ = - (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); - - // use the least singular vector as normal - feat.normal_ = (svd.matrixU().col(2)); - if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - feat.normal_ = -feat.normal_; - } - // mean ground seeds value - feat.mean_ = mean.head<3>(); - // according to normal.T*[x,y,z] = -d - feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - feat.th_dist_d_ = MD - feat.d_; + // Eigen::Matrix3f cov; + // Eigen::Vector4f mean; + // pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + // Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + // feat.singular_values_ = svd.singularValues(); + + // feat.linearity_ = + // (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + // feat.planarity_ = + // (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // // use the least singular vector as normal + // feat.normal_ = (svd.matrixU().col(2)); + // if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + // feat.normal_ = -feat.normal_; + // } + // // mean ground seeds value + // feat.mean_ = mean.head<3>(); + // // according to normal.T*[x,y,z] = -d + // feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + // feat.th_dist_d_ = MD - feat.d_; } void OccupancySegmentationCore::segment_ground() { - tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { - for (int patch_num = r.begin(); patch_num < r.end(); r++) { - Patch_Index &p_idx = patch_indices[patch_num]; - pcl::PointCloud &patch = czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + tbb::parallel_for(tbb::blocked_range(0, num_patches), + [&](tbb::blocked_range r) { + for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + Patch_Index &p_idx = _patch_indices[patch_num]; + pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; PCAFeature features; if (patch.points.size() > MIN_NUM_POINTS) { std::sort(patch.points.begin(), patch.points.end()); pcl::PointCloud ground_temp; - rgpf(patch, p_idx, feat); + rgpf(patch, p_idx, features); } } @@ -115,69 +118,69 @@ void OccupancySegmentationCore::segment_ground() { } void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { - pcl::PointCloud ground_temp; - pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - - if (!region_ground.empty()) region_ground.clear(); - if (!region_nonground.empty()) region_nonground.clear(); - - size_t N = patch.size(); - - extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); - int num_iters = 3; - for(int i = 0; i < num_iters; i++){ - estimate_plane(ground_temp, feat); - ground_temp.clear(); - Eigen::MatrixXf points(N, 3); - int j = 0; - for (pcl::PointXYZ &p:src.points) { - points.row(j++) = p.getVector3fMap(); - } - - Eigen::VectorXf result = points * feat.normal_; - for (size_t r = 0; r < N; r++) { - if (i < num_iters - 1) { - if (result[r] < feat.th_dist_d_) { - ground_tmp.points.emplace_back(src[r]); - } - } else { // Final stage - if (result[r] < feat.th_dist_d_) { - regionwise_ground.points.emplace_back(src[r]); - } else { - regionwise_nonground.points.emplace_back(src[r]); - } - } - } - - } + // pcl::PointCloud ground_temp; + // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + + // if (!region_ground.empty()) region_ground.clear(); + // if (!region_nonground.empty()) region_nonground.clear(); + + // size_t N = patch.size(); + + // extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); + // int num_iters = 3; + // for(int i = 0; i < num_iters; i++){ + // estimate_plane(ground_temp, feat); + // ground_temp.clear(); + // Eigen::MatrixXf points(N, 3); + // int j = 0; + // for (pcl::PointXYZ &p:patch.points) { + // points.row(j++) = p.getVector3fMap(); + // } + + // Eigen::VectorXf result = points * feat.normal_; + // for (size_t r = 0; r < N; r++) { + // if (i < num_iters - 1) { + // if (result[r] < feat.th_dist_d_) { + // ground_temp.points.emplace_back(patch.points[r]); + // } + // } else { // Final stage + // if (result[r] < feat.th_dist_d_) { + // region_ground.points.emplace_back(patch.points[r]); + // } else { + // region_nonground.points.emplace_back(patch.points[r]); + // } + // } + // } + + // } } bool OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ - //uprightness filter - if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ - return false; - } - - const double z_elevation = feat.mean_(2); - const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); - - //elevation filter - if (concentric_idx < num_rings_of_interest_) { - if (z_elevation > -sensor_height_ + elevation_thr_[concentric_idx] - && flatness_thr_[concentric_idx] <= surface_variable) { - return false; - } else{ - return true; - } - } else { - if (using_global_thr_ && (z_elevation > global_elevation_thr_)) { - return false; - } else { - return true; - } - } + // //uprightness filter + // if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ + // return false; + // } + + // const double z_elevation = feat.mean_(2); + // const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + + // //elevation filter + // if (concentric_idx < NUM_RINGS_OF_INTEREST) { + // if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx] + // && FLATNESS_THR[concentric_idx] <= surface_variable) { + // return false; + // } else{ + // return true; + // } + // } else { + // if (z_elevation > GLOBAL_EL_THRESH) { + // return false; + // } else { + // return true; + // } + // } } @@ -186,25 +189,27 @@ void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud Date: Sat, 20 Jul 2024 01:04:53 +0000 Subject: [PATCH 41/59] Build / make error with core.cpp --- .../include/occupancy_segmentation_core.hpp | 12 +- .../src/occupancy_segmentation_core.cpp | 378 ++++++++++-------- 2 files changed, 220 insertions(+), 170 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 1721aec7..153f6c38 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -50,6 +50,8 @@ struct Patch_Index { int concentric_idx; }; +enum Status {TOO_TILTED, FLAT_ENOUGH, TOO_HIGH_ELEV, UPRIGHT_ENOUGH, GLOBAL_TOO_HIGH_ELEV, FEW_POINTS}; + class OccupancySegmentationCore { public: typedef std::vector> Ring; @@ -73,6 +75,8 @@ class OccupancySegmentationCore { pcl::PointCloud _ground; pcl::PointCloud _non_ground; + + std::vector _statuses; OccupancySegmentationCore(); @@ -83,17 +87,19 @@ class OccupancySegmentationCore { void fill_czm(pcl::PointCloud &cloud_in); + void clear_czm_and_regionwise(); + void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); void rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat); void extract_initial_seeds(pcl::PointCloud &cloud, pcl::PointCloud &seed_cloud, int zone_idx); - bool ground_likelihood_est(PCAFeature &feat, int concentric_idx); + Status ground_likelihood_est(PCAFeature &feat, int concentric_idx); - void segment_ground(); + void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); - bool point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b); + static bool point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b) { return a.z < b.z; }; diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index bf867542..5942047f 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -3,102 +3,127 @@ #include "occupancy_segmentation_core.hpp" -OccupancySegmentationCore::OccupancySegmentationCore() {} +OccupancySegmentationCore::OccupancySegmentationCore() { + init_czm(); +} void OccupancySegmentationCore::init_czm() { - // int concentric_count = 0; - // int patch_count = 0; - - // for (int z = 0; z < NUM_ZONES; z++) { - // Zone zone; - // Patch_Index index; - // index.zone_idx = z; - - // for (int r = 0; r < ZONE_RINGS[z]; r++) { - // Ring ring; - // index.ring_idx = r; - // index.concentric_idx = concentric_count; - - // for (int s = 0; s < ZONE_SECTORS[z]; s++) { - // index.sector_idx = s; - // index.idx = patch_count; - // _patch_indices.push_back(index); - // pcl::PointCloud patch; - // ring.emplace_back(patch); - // pcl::PointCloud regionground; - // pcl::PointCloud regionnonground; - // _regionwise_ground.push_back(regionground); - // _regionwise_nonground.push_back(regionnonground); - - // patch_count++; - // } - // zone.emplace_back(ring); - // } - // _czm.emplace_back(zone); - // } - // num_patches = patch_count; + int concentric_count = 0; + int patch_count = 0; + + for (int z = 0; z < NUM_ZONES; z++) { + Zone zone; + Patch_Index index; + index.zone_idx = z; + + for (int r = 0; r < ZONE_RINGS[z]; r++) { + Ring ring; + index.ring_idx = r; + index.concentric_idx = concentric_count; + + for (int s = 0; s < ZONE_SECTORS[z]; s++) { + index.sector_idx = s; + index.idx = patch_count; + _patch_indices.push_back(index); + pcl::PointCloud patch; + ring.emplace_back(patch); + + pcl::PointCloud regionground; + pcl::PointCloud regionnonground; + Status status; + _regionwise_ground.push_back(regionground); + _regionwise_nonground.push_back(regionnonground); + + _statuses.push_back(status); + + patch_count++; + } + zone.emplace_back(ring); + } + _czm.emplace_back(zone); + } + num_patches = patch_count; } void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { - // double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - // double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; - // for (pcl::PointXYZ &p : cloud_in.points) { - // double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); - - // // Out of range for czm - // if (r < L_MIN || r > L_MAX) { - // continue; - // } - - // double theta = atan2(p.y, p.x); - // if (theta < 0) { - // theta += 2 * M_PI; - // } - - // double deltal = L_MAX - L_MIN; - - // for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { - // int ring_idx, sector_idx; - // if (r < lmaxs[zone_idx]) { - // double ring_size = deltal / ZONE_RINGS[zone_idx]; - // double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; - // ring_idx = (int)((r - lmins[zone_idx]) / ring_size); - // // TODO: find out why the use min() in the paper, meantime use the top - // // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); - // sector_idx = (int)(theta / sector_size); - // _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); - // } - // } - // } + double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + for (pcl::PointXYZ &p : cloud_in.points) { + double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); + + // Out of range for czm + if (r < L_MIN || r > L_MAX) { + continue; + } + + double theta = atan2(p.y, p.x); + if (theta < 0) { + theta += 2 * M_PI; + } + + double deltal = L_MAX - L_MIN; + + for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { + int ring_idx, sector_idx; + if (r < lmaxs[zone_idx]) { + double ring_size = deltal / ZONE_RINGS[zone_idx]; + double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; + ring_idx = (int)((r - lmins[zone_idx]) / ring_size); + // TODO: find out why the use min() in the paper, meantime use the top + // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); + sector_idx = (int)(theta / sector_size); + _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + } + } + } +} + +void OccupancySegmentationCore::clear_czm_and_regionwise(){ + int i = 0; + for (Zone zone : _czm){ + for (Ring ring : zone){ + for (pcl::PointCloud patch : ring){ + patch.clear(); + _regionwise_ground[i].clear(); + _regionwise_nonground[i].clear(); + i++; + } + } + } } void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat) { - // Eigen::Matrix3f cov; - // Eigen::Vector4f mean; - // pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); - - // Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - // feat.singular_values_ = svd.singularValues(); - - // feat.linearity_ = - // (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - // feat.planarity_ = - // (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); - - // // use the least singular vector as normal - // feat.normal_ = (svd.matrixU().col(2)); - // if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - // feat.normal_ = -feat.normal_; - // } - // // mean ground seeds value - // feat.mean_ = mean.head<3>(); - // // according to normal.T*[x,y,z] = -d - // feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - // feat.th_dist_d_ = MD - feat.d_; + // Code taken directly from repo + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; } -void OccupancySegmentationCore::segment_ground() { +void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground) { + clear_czm_and_regionwise(); + + //TODO error point removal + + fill_czm(unfiltered_cloud); + tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { @@ -107,80 +132,103 @@ void OccupancySegmentationCore::segment_ground() { pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; PCAFeature features; + + region_ground.clear(); + region_nonground.clear(); if (patch.points.size() > MIN_NUM_POINTS) { - std::sort(patch.points.begin(), patch.points.end()); - pcl::PointCloud ground_temp; + std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); rgpf(patch, p_idx, features); + Status status = ground_likelihood_est(features, p_idx.concentric_idx); + _statuses[p_idx.idx] = status; + } else { + region_ground = patch; + _statuses[p_idx.idx] = FEW_POINTS; } } }); + + for (Patch_Index p_idx : _patch_indices){ + Status status = _statuses[p_idx.idx]; + + if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ + ground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } else { + nonground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } + + } } void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { - // pcl::PointCloud ground_temp; - // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - - // if (!region_ground.empty()) region_ground.clear(); - // if (!region_nonground.empty()) region_nonground.clear(); - - // size_t N = patch.size(); - - // extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); - // int num_iters = 3; - // for(int i = 0; i < num_iters; i++){ - // estimate_plane(ground_temp, feat); - // ground_temp.clear(); - // Eigen::MatrixXf points(N, 3); - // int j = 0; - // for (pcl::PointXYZ &p:patch.points) { - // points.row(j++) = p.getVector3fMap(); - // } - - // Eigen::VectorXf result = points * feat.normal_; - // for (size_t r = 0; r < N; r++) { - // if (i < num_iters - 1) { - // if (result[r] < feat.th_dist_d_) { - // ground_temp.points.emplace_back(patch.points[r]); - // } - // } else { // Final stage - // if (result[r] < feat.th_dist_d_) { - // region_ground.points.emplace_back(patch.points[r]); - // } else { - // region_nonground.points.emplace_back(patch.points[r]); - // } - // } - // } - - // } + pcl::PointCloud ground_temp; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + + if (!region_ground.empty()) region_ground.clear(); + if (!region_nonground.empty()) region_nonground.clear(); + + size_t N = patch.size(); + + extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); + int num_iters = 3; + for(int i = 0; i < num_iters; i++){ + estimate_plane(ground_temp, feat); + ground_temp.clear(); + Eigen::MatrixXf points(N, 3); + int j = 0; + for (pcl::PointXYZ &p:patch.points) { + points.row(j++) = p.getVector3fMap(); + } + + Eigen::VectorXf result = points * feat.normal_; + for (size_t r = 0; r < N; r++) { + if (i < num_iters - 1) { + if (result[r] < feat.th_dist_d_) { + ground_temp.points.emplace_back(patch.points[r]); + } + } else { + if (result[r] < feat.th_dist_d_) { + region_ground.points.emplace_back(patch.points[r]); + } else { + region_nonground.points.emplace_back(patch.points[r]); + } + } + } + + } } -bool OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ - - // //uprightness filter - // if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ - // return false; - // } - - // const double z_elevation = feat.mean_(2); - // const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); - - // //elevation filter - // if (concentric_idx < NUM_RINGS_OF_INTEREST) { - // if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx] - // && FLATNESS_THR[concentric_idx] <= surface_variable) { - // return false; - // } else{ - // return true; - // } - // } else { - // if (z_elevation > GLOBAL_EL_THRESH) { - // return false; - // } else { - // return true; - // } - // } +Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ + + //uprightness filter + if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ + return TOO_TILTED; + } + + const double z_elevation = feat.mean_(2); + const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + + //elevation filter + if (concentric_idx < NUM_RINGS_OF_INTEREST) { + if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx]) { + if (FLATNESS_THR[concentric_idx] <= surface_variable){ + return FLAT_ENOUGH; + } else { + return TOO_HIGH_ELEV; + } + } else{ + return UPRIGHT_ENOUGH; + } + } else { + if (z_elevation > GLOBAL_EL_THRESH) { + return GLOBAL_TOO_HIGH_ELEV; + } else { + return UPRIGHT_ENOUGH; + } + } } @@ -189,27 +237,23 @@ void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud Date: Sat, 20 Jul 2024 04:49:20 +0000 Subject: [PATCH 42/59] Added main node code, still fails build --- .../include/occupancy_segmentation_core.hpp | 4 ++-- .../include/occupancy_segmentation_node.hpp | 7 +++++-- .../src/occupancy_segmentation_node.cpp | 19 ++++++++++++++++--- 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 153f6c38..52a17d0d 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -80,6 +80,8 @@ class OccupancySegmentationCore { OccupancySegmentationCore(); + + void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); private: @@ -97,8 +99,6 @@ class OccupancySegmentationCore { Status ground_likelihood_est(PCAFeature &feat, int concentric_idx); - void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); - static bool point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b) { return a.z < b.z; }; diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index 580ee4dc..a9d3b56b 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -4,6 +4,8 @@ #include "rclcpp/rclcpp.hpp" #include "occupancy_segmentation_core.hpp" +#include +#include /** * Implementation of a ROS2 node that converts unfiltered messages to filtered_array @@ -27,9 +29,10 @@ class OccupancySegmentationNode : public rclcpp::Node { private: // Object that handles data processing and validation. + OccupancySegmentationCore _patchwork; + rclcpp::Subscription::SharedPtr _subscriber; - rclcpp::TimerBase::SharedPtr timer_; - void timer_callback(); + void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); }; #endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index baa6475a..d641ff02 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -4,11 +4,24 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { - timer_ = this->create_wall_timer(std::chrono::milliseconds(1000), std::bind(&OccupancySegmentationNode::timer_callback, this)); + _subscriber = this->create_subscription( + "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this)); + } -void OccupancySegmentationNode::timer_callback(){ - RCLCPP_INFO(this->get_logger(), "Working"); +void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ + pcl::PointCloud temp_cloud; + pcl::fromROSMsg(*lidar_cloud, temp_cloud); + + pcl::PointCloud ground; + pcl::PointCloud nonground; + _patchwork.segment_ground(temp_cloud, ground, nonground); + + sensor_msgs::msg::PointCloud2::SharedPtr ground_msg; + sensor_msgs::msg::PointCloud2::SharedPtr nonground_msg; + pcl::toROSMsg(ground, *ground_msg); + pcl::toROSMsg(nonground, *nonground_msg); + } int main(int argc, char** argv) { From e0bd23d9cd5ba09717bf0990b521c1891ada735a Mon Sep 17 00:00:00 2001 From: akilpath Date: Mon, 22 Jul 2024 04:08:22 +0000 Subject: [PATCH 43/59] Commented some things to get it to compile --- .../include/occupancy_segmentation_node.hpp | 4 +- .../src/occupancy_segmentation_core.cpp | 124 +++++++++--------- .../src/occupancy_segmentation_node.cpp | 27 ++-- 3 files changed, 77 insertions(+), 78 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index a9d3b56b..77789135 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -30,9 +30,9 @@ class OccupancySegmentationNode : public rclcpp::Node { // Object that handles data processing and validation. OccupancySegmentationCore _patchwork; - rclcpp::Subscription::SharedPtr _subscriber; + // rclcpp::Subscription::SharedPtr _subscriber; - void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); + // void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); }; #endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 5942047f..857e5f48 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -95,71 +95,71 @@ void OccupancySegmentationCore::clear_czm_and_regionwise(){ void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat) { // Code taken directly from repo - Eigen::Matrix3f cov; - Eigen::Vector4f mean; - pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); - - Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - feat.singular_values_ = svd.singularValues(); - - feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); - - // use the least singular vector as normal - feat.normal_ = (svd.matrixU().col(2)); - if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - feat.normal_ = -feat.normal_; - } - // mean ground seeds value - feat.mean_ = mean.head<3>(); - // according to normal.T*[x,y,z] = -d - feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - feat.th_dist_d_ = MD - feat.d_; + // Eigen::Matrix3f cov; + // Eigen::Vector4f mean; + // pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + // Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + // feat.singular_values_ = svd.singularValues(); + + // feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + // feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // // use the least singular vector as normal + // feat.normal_ = (svd.matrixU().col(2)); + // if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + // feat.normal_ = -feat.normal_; + // } + // // mean ground seeds value + // feat.mean_ = mean.head<3>(); + // // according to normal.T*[x,y,z] = -d + // feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + // feat.th_dist_d_ = MD - feat.d_; } void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground) { - clear_czm_and_regionwise(); - - //TODO error point removal - - fill_czm(unfiltered_cloud); - - tbb::parallel_for(tbb::blocked_range(0, num_patches), - [&](tbb::blocked_range r) { - for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { - Patch_Index &p_idx = _patch_indices[patch_num]; - pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; - pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - PCAFeature features; - - region_ground.clear(); - region_nonground.clear(); - if (patch.points.size() > MIN_NUM_POINTS) { - std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); - rgpf(patch, p_idx, features); - - Status status = ground_likelihood_est(features, p_idx.concentric_idx); - _statuses[p_idx.idx] = status; - } else { - region_ground = patch; - _statuses[p_idx.idx] = FEW_POINTS; - } - } - }); - - for (Patch_Index p_idx : _patch_indices){ - Status status = _statuses[p_idx.idx]; - - if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ - ground += _regionwise_ground[p_idx.idx]; - nonground += _regionwise_nonground[p_idx.idx]; - } else { - nonground += _regionwise_ground[p_idx.idx]; - nonground += _regionwise_nonground[p_idx.idx]; - } - - } + // clear_czm_and_regionwise(); + + // //TODO error point removal + + // fill_czm(unfiltered_cloud); + + // tbb::parallel_for(tbb::blocked_range(0, num_patches), + // [&](tbb::blocked_range r) { + // for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + // Patch_Index &p_idx = _patch_indices[patch_num]; + // pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + // PCAFeature features; + + // region_ground.clear(); + // region_nonground.clear(); + // if (patch.points.size() > MIN_NUM_POINTS) { + // std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); + // rgpf(patch, p_idx, features); + + // Status status = ground_likelihood_est(features, p_idx.concentric_idx); + // _statuses[p_idx.idx] = status; + // } else { + // region_ground = patch; + // _statuses[p_idx.idx] = FEW_POINTS; + // } + // } + // }); + + // for (Patch_Index p_idx : _patch_indices){ + // Status status = _statuses[p_idx.idx]; + + // if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ + // ground += _regionwise_ground[p_idx.idx]; + // nonground += _regionwise_nonground[p_idx.idx]; + // } else { + // nonground += _regionwise_ground[p_idx.idx]; + // nonground += _regionwise_nonground[p_idx.idx]; + // } + + // } } void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index d641ff02..eda81010 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -3,26 +3,25 @@ #include "occupancy_segmentation_node.hpp" OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { - - _subscriber = this->create_subscription( - "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this)); + // _subscriber = this->create_subscription( + // "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this)); } -void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ - pcl::PointCloud temp_cloud; - pcl::fromROSMsg(*lidar_cloud, temp_cloud); +// void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ +// // pcl::PointCloud temp_cloud; +// // pcl::fromROSMsg(*lidar_cloud, temp_cloud); - pcl::PointCloud ground; - pcl::PointCloud nonground; - _patchwork.segment_ground(temp_cloud, ground, nonground); +// // pcl::PointCloud ground; +// // pcl::PointCloud nonground; +// // _patchwork.segment_ground(temp_cloud, ground, nonground); - sensor_msgs::msg::PointCloud2::SharedPtr ground_msg; - sensor_msgs::msg::PointCloud2::SharedPtr nonground_msg; - pcl::toROSMsg(ground, *ground_msg); - pcl::toROSMsg(nonground, *nonground_msg); +// // sensor_msgs::msg::PointCloud2::SharedPtr ground_msg; +// // sensor_msgs::msg::PointCloud2::SharedPtr nonground_msg; +// // pcl::toROSMsg(ground, *ground_msg); +// // pcl::toROSMsg(nonground, *nonground_msg); -} +// } int main(int argc, char** argv) { rclcpp::init(argc, argv); From fdf562ce0fbf14fee7933a1c8dae92274e4f17ac Mon Sep 17 00:00:00 2001 From: akilpath Date: Wed, 24 Jul 2024 00:12:19 +0000 Subject: [PATCH 44/59] Builds --- .../occupancy_segmentation.Dockerfile | 1 - .../occupancy_segmentation/CMakeLists.txt | 2 + .../include/occupancy_segmentation_node.hpp | 6 +- .../occupancy_segmentation/package.xml | 1 + .../src/occupancy_segmentation_core.cpp | 126 +++++++++--------- .../src/occupancy_segmentation_node.cpp | 28 ++-- 6 files changed, 85 insertions(+), 79 deletions(-) diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index 322a1ee8..2168af0f 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -8,7 +8,6 @@ WORKDIR ${AMENT_WS}/src # Copy in source code # RUN git clone https://github.com/ros-perception/perception_pcl.git --branch 2.4.3 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 && \ diff --git a/src/world_modeling/occupancy_segmentation/CMakeLists.txt b/src/world_modeling/occupancy_segmentation/CMakeLists.txt index 8df7a7cd..ed31cbcb 100644 --- a/src/world_modeling/occupancy_segmentation/CMakeLists.txt +++ b/src/world_modeling/occupancy_segmentation/CMakeLists.txt @@ -12,6 +12,7 @@ endif() # 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) @@ -28,6 +29,7 @@ target_include_directories(occupancy_segmentation_lib include) # Add ROS2 dependencies required by package ament_target_dependencies(occupancy_segmentation_lib + sensor_msgs rclcpp pcl_ros) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index 77789135..cd5ead95 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -30,9 +30,11 @@ class OccupancySegmentationNode : public rclcpp::Node { // Object that handles data processing and validation. OccupancySegmentationCore _patchwork; - // rclcpp::Subscription::SharedPtr _subscriber; + rclcpp::Subscription::SharedPtr _subscriber; + rclcpp::Publisher::SharedPtr _nonground_publisher; + rclcpp::Publisher::SharedPtr _ground_publisher; - // void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); + void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); }; #endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index e84324c4..500bbac1 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -9,6 +9,7 @@ ament_cmake + sensor_msgs rclcpp pcl_ros diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 857e5f48..e7e0bc16 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -94,72 +94,72 @@ void OccupancySegmentationCore::clear_czm_and_regionwise(){ void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat) { - // Code taken directly from repo - // Eigen::Matrix3f cov; - // Eigen::Vector4f mean; - // pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); - - // Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - // feat.singular_values_ = svd.singularValues(); - - // feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - // feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); - - // // use the least singular vector as normal - // feat.normal_ = (svd.matrixU().col(2)); - // if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - // feat.normal_ = -feat.normal_; - // } - // // mean ground seeds value - // feat.mean_ = mean.head<3>(); - // // according to normal.T*[x,y,z] = -d - // feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - // feat.th_dist_d_ = MD - feat.d_; + //Code taken directly from repo + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; } void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground) { - // clear_czm_and_regionwise(); - - // //TODO error point removal - - // fill_czm(unfiltered_cloud); - - // tbb::parallel_for(tbb::blocked_range(0, num_patches), - // [&](tbb::blocked_range r) { - // for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { - // Patch_Index &p_idx = _patch_indices[patch_num]; - // pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; - // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - // PCAFeature features; - - // region_ground.clear(); - // region_nonground.clear(); - // if (patch.points.size() > MIN_NUM_POINTS) { - // std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); - // rgpf(patch, p_idx, features); - - // Status status = ground_likelihood_est(features, p_idx.concentric_idx); - // _statuses[p_idx.idx] = status; - // } else { - // region_ground = patch; - // _statuses[p_idx.idx] = FEW_POINTS; - // } - // } - // }); - - // for (Patch_Index p_idx : _patch_indices){ - // Status status = _statuses[p_idx.idx]; - - // if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ - // ground += _regionwise_ground[p_idx.idx]; - // nonground += _regionwise_nonground[p_idx.idx]; - // } else { - // nonground += _regionwise_ground[p_idx.idx]; - // nonground += _regionwise_nonground[p_idx.idx]; - // } - - // } + clear_czm_and_regionwise(); + + //TODO error point removal + + fill_czm(unfiltered_cloud); + + tbb::parallel_for(tbb::blocked_range(0, num_patches), + [&](tbb::blocked_range r) { + for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + Patch_Index &p_idx = _patch_indices[patch_num]; + pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + PCAFeature features; + + region_ground.clear(); + region_nonground.clear(); + if (patch.points.size() > MIN_NUM_POINTS) { + std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); + rgpf(patch, p_idx, features); + + Status status = ground_likelihood_est(features, p_idx.concentric_idx); + _statuses[p_idx.idx] = status; + } else { + region_ground = patch; + _statuses[p_idx.idx] = FEW_POINTS; + } + } + }); + + for (Patch_Index p_idx : _patch_indices){ + Status status = _statuses[p_idx.idx]; + + if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ + ground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } else { + nonground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } + + } } void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index eda81010..9614a5fa 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -3,25 +3,27 @@ #include "occupancy_segmentation_node.hpp" OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { - // _subscriber = this->create_subscription( - // "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this)); + _subscriber = this->create_subscription( + "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this, std::placeholders::_1)); } -// void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ -// // pcl::PointCloud temp_cloud; -// // pcl::fromROSMsg(*lidar_cloud, temp_cloud); +void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ + pcl::PointCloud temp_cloud; + pcl::fromROSMsg(*lidar_cloud, temp_cloud); -// // pcl::PointCloud ground; -// // pcl::PointCloud nonground; -// // _patchwork.segment_ground(temp_cloud, ground, nonground); + pcl::PointCloud ground; + pcl::PointCloud nonground; + _patchwork.segment_ground(temp_cloud, ground, nonground); -// // sensor_msgs::msg::PointCloud2::SharedPtr ground_msg; -// // sensor_msgs::msg::PointCloud2::SharedPtr nonground_msg; -// // pcl::toROSMsg(ground, *ground_msg); -// // pcl::toROSMsg(nonground, *nonground_msg); + sensor_msgs::msg::PointCloud2 ground_msg; + sensor_msgs::msg::PointCloud2 nonground_msg; + pcl::toROSMsg(ground, ground_msg); + pcl::toROSMsg(nonground, nonground_msg); -// } + _ground_publisher -> publish(ground_msg); + _nonground_publisher -> publish(nonground_msg); +} int main(int argc, char** argv) { rclcpp::init(argc, argv); From c78f67c1ed3e274911bfa6bebe86515f46827e31 Mon Sep 17 00:00:00 2001 From: akilpath Date: Tue, 30 Jul 2024 03:26:06 +0000 Subject: [PATCH 45/59] Foxglove setup plus error debugging exit exit() --- .../src/occupancy_segmentation_core.cpp | 81 ++++++++++--------- .../src/occupancy_segmentation_node.cpp | 6 +- 2 files changed, 47 insertions(+), 40 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index e7e0bc16..32d43ec6 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -68,11 +68,14 @@ void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_i if (r < lmaxs[zone_idx]) { double ring_size = deltal / ZONE_RINGS[zone_idx]; double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; - ring_idx = (int)((r - lmins[zone_idx]) / ring_size); + //ring_idx = (int)((r - lmins[zone_idx]) / ring_size); // TODO: find out why the use min() in the paper, meantime use the top - // ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size),); - sector_idx = (int)(theta / sector_size); + ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); + sector_idx = std::min((int)(theta / sector_size), ZONE_SECTORS[zone_idx] - 1); + std::cout << "Ring: " << ring_idx << std::endl; + std::cout << "Sector: " << sector_idx << std::endl; _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + break; } } } @@ -124,42 +127,42 @@ void OccupancySegmentationCore::segment_ground(pcl::PointCloud &u fill_czm(unfiltered_cloud); - tbb::parallel_for(tbb::blocked_range(0, num_patches), - [&](tbb::blocked_range r) { - for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { - Patch_Index &p_idx = _patch_indices[patch_num]; - pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; - pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - PCAFeature features; - - region_ground.clear(); - region_nonground.clear(); - if (patch.points.size() > MIN_NUM_POINTS) { - std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); - rgpf(patch, p_idx, features); - - Status status = ground_likelihood_est(features, p_idx.concentric_idx); - _statuses[p_idx.idx] = status; - } else { - region_ground = patch; - _statuses[p_idx.idx] = FEW_POINTS; - } - } - }); - - for (Patch_Index p_idx : _patch_indices){ - Status status = _statuses[p_idx.idx]; - - if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ - ground += _regionwise_ground[p_idx.idx]; - nonground += _regionwise_nonground[p_idx.idx]; - } else { - nonground += _regionwise_ground[p_idx.idx]; - nonground += _regionwise_nonground[p_idx.idx]; - } - - } + // tbb::parallel_for(tbb::blocked_range(0, num_patches), + // [&](tbb::blocked_range r) { + // for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + // Patch_Index &p_idx = _patch_indices[patch_num]; + // pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + // PCAFeature features; + + // region_ground.clear(); + // region_nonground.clear(); + // if (patch.points.size() > MIN_NUM_POINTS) { + // std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); + // rgpf(patch, p_idx, features); + + // Status status = ground_likelihood_est(features, p_idx.concentric_idx); + // _statuses[p_idx.idx] = status; + // } else { + // region_ground = patch; + // _statuses[p_idx.idx] = FEW_POINTS; + // } + // } + // }); + + // for (Patch_Index p_idx : _patch_indices){ + // Status status = _statuses[p_idx.idx]; + + // if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ + // ground += _regionwise_ground[p_idx.idx]; + // nonground += _regionwise_nonground[p_idx.idx]; + // } else { + // nonground += _regionwise_ground[p_idx.idx]; + // nonground += _regionwise_nonground[p_idx.idx]; + // } + + // } } void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 9614a5fa..aad3b26f 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -4,7 +4,10 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { _subscriber = this->create_subscription( - "topic", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this, std::placeholders::_1)); + "/velodyne_points", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this, std::placeholders::_1)); + + _ground_publisher = this -> create_publisher("/ground_points", 10); + _nonground_publisher = this -> create_publisher("/nonground_points", 10); } @@ -18,6 +21,7 @@ void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::P sensor_msgs::msg::PointCloud2 ground_msg; sensor_msgs::msg::PointCloud2 nonground_msg; + pcl::toROSMsg(ground, ground_msg); pcl::toROSMsg(nonground, nonground_msg); From ffcb771bbe7f29b7b4eb492e9aa3f5154a82c278 Mon Sep 17 00:00:00 2001 From: akilpath Date: Fri, 2 Aug 2024 03:57:23 +0000 Subject: [PATCH 46/59] Templated Core file --- .../include/occupancy_segmentation_core.hpp | 293 +++++++++++++++++- .../include/occupancy_segmentation_node.hpp | 25 +- .../src/occupancy_segmentation_core.cpp | 257 --------------- .../src/occupancy_segmentation_node.cpp | 13 +- 4 files changed, 316 insertions(+), 272 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 52a17d0d..1a24d697 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -11,6 +11,7 @@ #include + #define NUM_ZONES 4 #define L_MIN 2.7 #define L_MAX 80.0 @@ -52,9 +53,10 @@ struct Patch_Index { enum Status {TOO_TILTED, FLAT_ENOUGH, TOO_HIGH_ELEV, UPRIGHT_ENOUGH, GLOBAL_TOO_HIGH_ELEV, FEW_POINTS}; +template class OccupancySegmentationCore { public: - typedef std::vector> Ring; + typedef std::vector> Ring; typedef std::vector Zone; // Size of buffer before processed messages are published. static constexpr int BUFFER_CAPACITY = 10; @@ -68,42 +70,311 @@ class OccupancySegmentationCore { int num_patches = -1; std::vector _czm; - std::vector> _regionwise_ground; - std::vector> _regionwise_nonground; + std::vector> _regionwise_ground; + std::vector> _regionwise_nonground; std::vector _patch_indices; - pcl::PointCloud _ground; - pcl::PointCloud _non_ground; + pcl::PointCloud _ground; + pcl::PointCloud _non_ground; std::vector _statuses; OccupancySegmentationCore(); - void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); + void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); private: void init_czm(); - void fill_czm(pcl::PointCloud &cloud_in); + void fill_czm(pcl::PointCloud &cloud_in); void clear_czm_and_regionwise(); - void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); + void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); - void rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat); + void rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat); - void extract_initial_seeds(pcl::PointCloud &cloud, pcl::PointCloud &seed_cloud, int zone_idx); + void extract_initial_seeds(pcl::PointCloud &cloud, pcl::PointCloud &seed_cloud, int zone_idx); Status ground_likelihood_est(PCAFeature &feat, int concentric_idx); - static bool point_z_cmp(pcl::PointXYZ a, pcl::PointXYZ b) { return a.z < b.z; }; + static bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; }; }; +template +OccupancySegmentationCore::OccupancySegmentationCore() { + init_czm(); +} + +template +void OccupancySegmentationCore::init_czm() { + int concentric_count = 0; + int patch_count = 0; + + for (int z = 0; z < NUM_ZONES; z++) { + Zone zone; + Patch_Index index; + index.zone_idx = z; + + for (int r = 0; r < ZONE_RINGS[z]; r++) { + Ring ring; + index.ring_idx = r; + index.concentric_idx = concentric_count; + + for (int s = 0; s < ZONE_SECTORS[z]; s++) { + index.sector_idx = s; + index.idx = patch_count; + _patch_indices.push_back(index); + pcl::PointCloud patch; + ring.emplace_back(patch); + + pcl::PointCloud regionground; + pcl::PointCloud regionnonground; + Status status; + _regionwise_ground.push_back(regionground); + _regionwise_nonground.push_back(regionnonground); + + _statuses.push_back(status); + + patch_count++; + } + zone.emplace_back(ring); + } + _czm.emplace_back(zone); + } + num_patches = patch_count; +} + +template +void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { + double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + for (PointT &p : cloud_in.points) { + double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); + + // Out of range for czm + if (r < L_MIN || r > L_MAX) { + continue; + } + + double theta = atan2(p.y, p.x); + if (theta < 0) { + theta += 2 * M_PI; + } + + double deltal = L_MAX - L_MIN; + + for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { + int ring_idx, sector_idx; + if (r < lmaxs[zone_idx]) { + double ring_size = deltal / ZONE_RINGS[zone_idx]; + double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; + //ring_idx = (int)((r - lmins[zone_idx]) / ring_size); + // TODO: find out why the use min() in the paper, meantime use the top + ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); + sector_idx = std::min((int)(theta / sector_size), ZONE_SECTORS[zone_idx] - 1); + // std::cout << "Ring: " << ring_idx << std::endl; + // std::cout << "Sector: " << sector_idx << std::endl; + _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + break; + } + } + } +} + +template +void OccupancySegmentationCore::clear_czm_and_regionwise(){ + int i = 0; + for (Zone &zone : _czm){ + for (Ring &ring : zone){ + for (pcl::PointCloud &patch : ring){ + patch.clear(); + _regionwise_ground[i].clear(); + _regionwise_nonground[i].clear(); + i++; + } + } + } +} + +template +void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, + PCAFeature &feat) { + //Code taken directly from repo + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; +} + +template +void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground) { + clear_czm_and_regionwise(); + + //TODO error point removal + + fill_czm(unfiltered_cloud); + std::cout << "Finished filling czm" << std::endl; + tbb::parallel_for(tbb::blocked_range(0, num_patches), + [&](tbb::blocked_range r) { + for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + Patch_Index &p_idx = _patch_indices[patch_num]; + pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + PCAFeature features; + + region_ground.clear(); + region_nonground.clear(); + if (patch.points.size() > MIN_NUM_POINTS) { + std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); + rgpf(patch, p_idx, features); + + Status status = ground_likelihood_est(features, p_idx.concentric_idx); + _statuses[p_idx.idx] = status; + } else { + region_ground = patch; + _statuses[p_idx.idx] = FEW_POINTS; + } + } + }); + + std::cout << "Finished estimation" << std::endl; + + for (Patch_Index p_idx : _patch_indices){ + Status status = _statuses[p_idx.idx]; + + if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ + ground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } else { + nonground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } + + } +} + +template +void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { + pcl::PointCloud ground_temp; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + + if (!region_ground.empty()) region_ground.clear(); + if (!region_nonground.empty()) region_nonground.clear(); + + size_t N = patch.size(); + + extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); + int num_iters = 3; + for(int i = 0; i < num_iters; i++){ + estimate_plane(ground_temp, feat); + ground_temp.clear(); + Eigen::MatrixXf points(N, 3); + int j = 0; + for (PointT &p:patch.points) { + points.row(j++) = p.getVector3fMap(); + } + + Eigen::VectorXf result = points * feat.normal_; + for (size_t r = 0; r < N; r++) { + if (i < num_iters - 1) { + if (result[r] < feat.th_dist_d_) { + ground_temp.points.emplace_back(patch.points[r]); + } + } else { + if (result[r] < feat.th_dist_d_) { + region_ground.points.emplace_back(patch.points[r]); + } else { + region_nonground.points.emplace_back(patch.points[r]); + } + } + } + + } +} + +template +Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ + + //uprightness filter + if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ + return TOO_TILTED; + } + + const double z_elevation = feat.mean_(2); + const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + + //elevation filter + if (concentric_idx < NUM_RINGS_OF_INTEREST) { + if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx]) { + if (FLATNESS_THR[concentric_idx] <= surface_variable){ + return FLAT_ENOUGH; + } else { + return TOO_HIGH_ELEV; + } + } else{ + return UPRIGHT_ENOUGH; + } + } else { + if (z_elevation > GLOBAL_EL_THRESH) { + return GLOBAL_TOO_HIGH_ELEV; + } else { + return UPRIGHT_ENOUGH; + } + } + +} + +template +void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud &cloud, + pcl::PointCloud &seed_cloud, int zone_idx) { + + //adaptive seed selection for 1st zone + + size_t init_idx = 0; + // if (zone_idx == 0){ + + // + + double sum = 0; + int cnt = 0; + for (size_t i = 0; i < cloud.points.size() && cnt < NUM_SEED_POINTS; i++) { + sum += cloud.points[i].z; + cnt++; + } + + double mean_z = sum / cnt; + for (size_t i = init_idx; i < cloud.points.size(); i++) { + if (cloud.points[i].z < mean_z + TH_SEEDS) { + seed_cloud.points.push_back(cloud.points[i]); + } + } +} + + #endif // TRANSFORMER_CORE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index cd5ead95..34ceb661 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -1,11 +1,34 @@ #ifndef OCCUPANCY_SEGMENTATION_NODE_HPP_ #define OCCUPANCY_SEGMENTATION_NODE_HPP_ +#define PCL_NO_PRECOMPILE #include "rclcpp/rclcpp.hpp" #include "occupancy_segmentation_core.hpp" #include #include +#include +#include +#include +#include + +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; + +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 @@ -29,7 +52,7 @@ class OccupancySegmentationNode : public rclcpp::Node { private: // Object that handles data processing and validation. - OccupancySegmentationCore _patchwork; + OccupancySegmentationCore _patchwork; rclcpp::Subscription::SharedPtr _subscriber; rclcpp::Publisher::SharedPtr _nonground_publisher; rclcpp::Publisher::SharedPtr _ground_publisher; diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 32d43ec6..8cca23c3 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -3,260 +3,3 @@ #include "occupancy_segmentation_core.hpp" -OccupancySegmentationCore::OccupancySegmentationCore() { - init_czm(); -} - -void OccupancySegmentationCore::init_czm() { - int concentric_count = 0; - int patch_count = 0; - - for (int z = 0; z < NUM_ZONES; z++) { - Zone zone; - Patch_Index index; - index.zone_idx = z; - - for (int r = 0; r < ZONE_RINGS[z]; r++) { - Ring ring; - index.ring_idx = r; - index.concentric_idx = concentric_count; - - for (int s = 0; s < ZONE_SECTORS[z]; s++) { - index.sector_idx = s; - index.idx = patch_count; - _patch_indices.push_back(index); - pcl::PointCloud patch; - ring.emplace_back(patch); - - pcl::PointCloud regionground; - pcl::PointCloud regionnonground; - Status status; - _regionwise_ground.push_back(regionground); - _regionwise_nonground.push_back(regionnonground); - - _statuses.push_back(status); - - patch_count++; - } - zone.emplace_back(ring); - } - _czm.emplace_back(zone); - } - num_patches = patch_count; -} - -void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { - double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; - for (pcl::PointXYZ &p : cloud_in.points) { - double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); - - // Out of range for czm - if (r < L_MIN || r > L_MAX) { - continue; - } - - double theta = atan2(p.y, p.x); - if (theta < 0) { - theta += 2 * M_PI; - } - - double deltal = L_MAX - L_MIN; - - for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { - int ring_idx, sector_idx; - if (r < lmaxs[zone_idx]) { - double ring_size = deltal / ZONE_RINGS[zone_idx]; - double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; - //ring_idx = (int)((r - lmins[zone_idx]) / ring_size); - // TODO: find out why the use min() in the paper, meantime use the top - ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); - sector_idx = std::min((int)(theta / sector_size), ZONE_SECTORS[zone_idx] - 1); - std::cout << "Ring: " << ring_idx << std::endl; - std::cout << "Sector: " << sector_idx << std::endl; - _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); - break; - } - } - } -} - -void OccupancySegmentationCore::clear_czm_and_regionwise(){ - int i = 0; - for (Zone zone : _czm){ - for (Ring ring : zone){ - for (pcl::PointCloud patch : ring){ - patch.clear(); - _regionwise_ground[i].clear(); - _regionwise_nonground[i].clear(); - i++; - } - } - } -} - -void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, - PCAFeature &feat) { - //Code taken directly from repo - Eigen::Matrix3f cov; - Eigen::Vector4f mean; - pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); - - Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - feat.singular_values_ = svd.singularValues(); - - feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); - - // use the least singular vector as normal - feat.normal_ = (svd.matrixU().col(2)); - if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - feat.normal_ = -feat.normal_; - } - // mean ground seeds value - feat.mean_ = mean.head<3>(); - // according to normal.T*[x,y,z] = -d - feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - feat.th_dist_d_ = MD - feat.d_; -} - -void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground) { - clear_czm_and_regionwise(); - - //TODO error point removal - - fill_czm(unfiltered_cloud); - - // tbb::parallel_for(tbb::blocked_range(0, num_patches), - // [&](tbb::blocked_range r) { - // for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { - // Patch_Index &p_idx = _patch_indices[patch_num]; - // pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; - // pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - // pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - // PCAFeature features; - - // region_ground.clear(); - // region_nonground.clear(); - // if (patch.points.size() > MIN_NUM_POINTS) { - // std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); - // rgpf(patch, p_idx, features); - - // Status status = ground_likelihood_est(features, p_idx.concentric_idx); - // _statuses[p_idx.idx] = status; - // } else { - // region_ground = patch; - // _statuses[p_idx.idx] = FEW_POINTS; - // } - // } - // }); - - // for (Patch_Index p_idx : _patch_indices){ - // Status status = _statuses[p_idx.idx]; - - // if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ - // ground += _regionwise_ground[p_idx.idx]; - // nonground += _regionwise_nonground[p_idx.idx]; - // } else { - // nonground += _regionwise_ground[p_idx.idx]; - // nonground += _regionwise_nonground[p_idx.idx]; - // } - - // } -} - -void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { - pcl::PointCloud ground_temp; - pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - - if (!region_ground.empty()) region_ground.clear(); - if (!region_nonground.empty()) region_nonground.clear(); - - size_t N = patch.size(); - - extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); - int num_iters = 3; - for(int i = 0; i < num_iters; i++){ - estimate_plane(ground_temp, feat); - ground_temp.clear(); - Eigen::MatrixXf points(N, 3); - int j = 0; - for (pcl::PointXYZ &p:patch.points) { - points.row(j++) = p.getVector3fMap(); - } - - Eigen::VectorXf result = points * feat.normal_; - for (size_t r = 0; r < N; r++) { - if (i < num_iters - 1) { - if (result[r] < feat.th_dist_d_) { - ground_temp.points.emplace_back(patch.points[r]); - } - } else { - if (result[r] < feat.th_dist_d_) { - region_ground.points.emplace_back(patch.points[r]); - } else { - region_nonground.points.emplace_back(patch.points[r]); - } - } - } - - } -} - -Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ - - //uprightness filter - if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ - return TOO_TILTED; - } - - const double z_elevation = feat.mean_(2); - const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); - - //elevation filter - if (concentric_idx < NUM_RINGS_OF_INTEREST) { - if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx]) { - if (FLATNESS_THR[concentric_idx] <= surface_variable){ - return FLAT_ENOUGH; - } else { - return TOO_HIGH_ELEV; - } - } else{ - return UPRIGHT_ENOUGH; - } - } else { - if (z_elevation > GLOBAL_EL_THRESH) { - return GLOBAL_TOO_HIGH_ELEV; - } else { - return UPRIGHT_ENOUGH; - } - } - -} - -void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud &cloud, - pcl::PointCloud &seed_cloud, int zone_idx) { - - //adaptive seed selection for 1st zone - - size_t init_idx = 0; - // if (zone_idx == 0){ - - // - - double sum = 0; - int cnt = 0; - for (size_t i = 0; i < cloud.points.size() && cnt < NUM_SEED_POINTS; i++) { - sum += cloud.points[i].z; - cnt++; - } - - double mean_z = sum / cnt; - for (size_t i = init_idx; i < cloud.points.size(); i++) { - if (cloud.points[i].z < mean_z + TH_SEEDS) { - seed_cloud.points.push_back(cloud.points[i]); - } - } -} - diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index aad3b26f..100e4202 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -12,13 +12,20 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment } void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ - pcl::PointCloud temp_cloud; + pcl::PointCloud temp_cloud; pcl::fromROSMsg(*lidar_cloud, temp_cloud); - pcl::PointCloud ground; - pcl::PointCloud nonground; + pcl::PointCloud ground; + pcl::PointCloud nonground; + + ground.clear(); + nonground.clear(); _patchwork.segment_ground(temp_cloud, ground, nonground); + RCLCPP_INFO(this->get_logger(), "Temp_cloud points %i", static_cast(temp_cloud.size())); + RCLCPP_INFO(this->get_logger(), "Ground points %i", static_cast(ground.size())); + RCLCPP_INFO(this->get_logger(), "Non ground points %i", static_cast(nonground.size())); + sensor_msgs::msg::PointCloud2 ground_msg; sensor_msgs::msg::PointCloud2 nonground_msg; From 808946d8909db0698d011eb24087616a46217280 Mon Sep 17 00:00:00 2001 From: akilpath Date: Tue, 6 Aug 2024 02:09:32 +0000 Subject: [PATCH 47/59] lowkey works --- .../include/occupancy_segmentation_core.hpp | 21 ++++++++++++------- .../src/occupancy_segmentation_node.cpp | 2 ++ 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 1a24d697..7613fc87 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -16,7 +16,6 @@ #define L_MIN 2.7 #define L_MAX 80.0 -#define N_SEED 20 #define Z_SEED 0.5 #define MD 0.3 #define MH -1.1 @@ -24,10 +23,10 @@ #define MIN_NUM_POINTS 10 #define NUM_SEED_POINTS 20 #define TH_SEEDS 0.5 -#define UPRIGHTNESS_THRESH 45 +#define UPRIGHTNESS_THRESH 0.5 #define NUM_RINGS_OF_INTEREST 4 -#define SENSOR_HEIGHT 0 +#define SENSOR_HEIGHT 1.7 #define GLOBAL_EL_THRESH 0 @@ -357,9 +356,17 @@ void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud::extract_initial_seeds(pcl::PointCloud temp_cloud; + RCLCPP_INFO(this -> get_logger(), "Header incoming: %s", lidar_cloud -> header.frame_id.c_str()); pcl::fromROSMsg(*lidar_cloud, temp_cloud); pcl::PointCloud ground; @@ -31,6 +32,7 @@ void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::P pcl::toROSMsg(ground, ground_msg); pcl::toROSMsg(nonground, nonground_msg); + RCLCPP_INFO(this -> get_logger(), "Header outgoing: %s", ground_msg.header.frame_id.c_str()); _ground_publisher -> publish(ground_msg); _nonground_publisher -> publish(nonground_msg); From fb9409e6d3eb4b4fa037a0d7d70b76e7ccd34c14 Mon Sep 17 00:00:00 2001 From: akilpath Date: Wed, 7 Aug 2024 00:00:20 +0000 Subject: [PATCH 48/59] Added headers to publishers --- .../occupancy_segmentation/src/occupancy_segmentation_node.cpp | 2 ++ watod-config.sh | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 7a07eef8..8778bb23 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -21,6 +21,8 @@ void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::P ground.clear(); nonground.clear(); + ground.header = temp_cloud.header; + nonground.header = temp_cloud.header; _patchwork.segment_ground(temp_cloud, ground, nonground); RCLCPP_INFO(this->get_logger(), "Temp_cloud points %i", static_cast(temp_cloud.size())); diff --git a/watod-config.sh b/watod-config.sh index dc6a6b1c..be66ed26 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -15,7 +15,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -ACTIVE_MODULES="world_modeling" +ACTIVE_MODULES="infrastructure world_modeling" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. From cee8212377c2eace5f2fc7e823228ca69f28ad5b Mon Sep 17 00:00:00 2001 From: VishGit1234 Date: Sun, 11 Aug 2024 01:32:50 +0000 Subject: [PATCH 49/59] Remove watod-config changes --- watod-config.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/watod-config.sh b/watod-config.sh index be66ed26..3255dd40 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -15,7 +15,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -ACTIVE_MODULES="infrastructure world_modeling" +# ACTIVE_MODULES="" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. @@ -23,7 +23,7 @@ ACTIVE_MODULES="infrastructure world_modeling" ## - deploy (default) : runs production-grade containers (non-editable) ## - develop : runs developer containers (editable) -MODE_OF_OPERATION="develop" +# MODE_OF_OPERATION="" ############################## ADVANCED CONFIGURATIONS ############################## ## Name to append to docker containers. DEFAULT = "" From 8beed8ff05a55379202896c4c9c5a27f19caef14 Mon Sep 17 00:00:00 2001 From: akilpath Date: Tue, 13 Aug 2024 00:46:50 +0000 Subject: [PATCH 50/59] Cleanup + Timing for patchwork --- .../include/occupancy_segmentation_core.hpp | 19 +++++-------------- .../include/occupancy_segmentation_node.hpp | 4 ++++ .../src/occupancy_segmentation_node.cpp | 15 ++++++++++----- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 7613fc87..c17da4a9 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -64,6 +64,8 @@ class OccupancySegmentationCore { const int ZONE_SECTORS [NUM_ZONES] = {16,32,54,32}; const float FLATNESS_THR [NUM_ZONES] = {0.0005, 0.000725, 0.001, 0.001}; const float ELEVATION_THR [NUM_ZONES] = {0.523, 0.746, 0.879, 1.125}; + const double lmins[NUM_ZONES] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; + const double lmaxs[NUM_ZONES] = {lmins[1], lmins[2], lmins[3], L_MAX}; int num_patches = -1; @@ -153,8 +155,7 @@ void OccupancySegmentationCore::init_czm() { template void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { - double lmins[4] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - double lmaxs[4] = {lmins[1], lmins[2], lmins[3], L_MAX}; + for (PointT &p : cloud_in.points) { double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); @@ -175,12 +176,9 @@ void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_ if (r < lmaxs[zone_idx]) { double ring_size = deltal / ZONE_RINGS[zone_idx]; double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; - //ring_idx = (int)((r - lmins[zone_idx]) / ring_size); - // TODO: find out why the use min() in the paper, meantime use the top + ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); sector_idx = std::min((int)(theta / sector_size), ZONE_SECTORS[zone_idx] - 1); - // std::cout << "Ring: " << ring_idx << std::endl; - // std::cout << "Sector: " << sector_idx << std::endl; _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); break; } @@ -190,14 +188,10 @@ void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_ template void OccupancySegmentationCore::clear_czm_and_regionwise(){ - int i = 0; for (Zone &zone : _czm){ for (Ring &ring : zone){ for (pcl::PointCloud &patch : ring){ patch.clear(); - _regionwise_ground[i].clear(); - _regionwise_nonground[i].clear(); - i++; } } } @@ -283,9 +277,6 @@ void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Pat pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - if (!region_ground.empty()) region_ground.clear(); - if (!region_nonground.empty()) region_nonground.clear(); - size_t N = patch.size(); extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); @@ -384,4 +375,4 @@ void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud #include #include +#include + +typedef std::chrono::high_resolution_clock Clock; + struct PointXYZIRT { diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 8778bb23..4fa53ef2 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -13,7 +13,7 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ pcl::PointCloud temp_cloud; - RCLCPP_INFO(this -> get_logger(), "Header incoming: %s", lidar_cloud -> header.frame_id.c_str()); + //RCLCPP_INFO(this -> get_logger(), "Header incoming: %s", lidar_cloud -> header.frame_id.c_str()); pcl::fromROSMsg(*lidar_cloud, temp_cloud); pcl::PointCloud ground; @@ -23,18 +23,23 @@ void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::P nonground.clear(); ground.header = temp_cloud.header; nonground.header = temp_cloud.header; + + auto begin = Clock::now(); _patchwork.segment_ground(temp_cloud, ground, nonground); + auto end = Clock::now(); - RCLCPP_INFO(this->get_logger(), "Temp_cloud points %i", static_cast(temp_cloud.size())); - RCLCPP_INFO(this->get_logger(), "Ground points %i", static_cast(ground.size())); - RCLCPP_INFO(this->get_logger(), "Non ground points %i", static_cast(nonground.size())); + int duration = std::chrono::duration_cast(end - begin).count(); + RCLCPP_INFO(this -> get_logger(), "Runtime for Patchwork: %i ms", duration); + // RCLCPP_INFO(this->get_logger(), "Temp_cloud points %i", static_cast(temp_cloud.size())); + // RCLCPP_INFO(this->get_logger(), "Ground points %i", static_cast(ground.size())); + // RCLCPP_INFO(this->get_logger(), "Non ground points %i", static_cast(nonground.size())); sensor_msgs::msg::PointCloud2 ground_msg; sensor_msgs::msg::PointCloud2 nonground_msg; pcl::toROSMsg(ground, ground_msg); pcl::toROSMsg(nonground, nonground_msg); - RCLCPP_INFO(this -> get_logger(), "Header outgoing: %s", ground_msg.header.frame_id.c_str()); + //RCLCPP_INFO(this -> get_logger(), "Header outgoing: %s", ground_msg.header.frame_id.c_str()); _ground_publisher -> publish(ground_msg); _nonground_publisher -> publish(nonground_msg); From ca3feef97dae571eac51304f47278643a452ca20 Mon Sep 17 00:00:00 2001 From: VishGit1234 <47926842+VishGit1234@users.noreply.github.com> Date: Mon, 12 Aug 2024 21:06:22 -0400 Subject: [PATCH 51/59] Update watod-config.sh --- watod-config.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/watod-config.sh b/watod-config.sh index be66ed26..3255dd40 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -15,7 +15,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -ACTIVE_MODULES="infrastructure world_modeling" +# ACTIVE_MODULES="" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. @@ -23,7 +23,7 @@ ACTIVE_MODULES="infrastructure world_modeling" ## - deploy (default) : runs production-grade containers (non-editable) ## - develop : runs developer containers (editable) -MODE_OF_OPERATION="develop" +# MODE_OF_OPERATION="" ############################## ADVANCED CONFIGURATIONS ############################## ## Name to append to docker containers. DEFAULT = "" From 14760409101bb7b92392e9e5fc4beb95901a9b64 Mon Sep 17 00:00:00 2001 From: VishGit1234 <47926842+VishGit1234@users.noreply.github.com> Date: Mon, 12 Aug 2024 21:21:57 -0400 Subject: [PATCH 52/59] Update docker_context.sh --- .github/templates/docker_context/docker_context.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/templates/docker_context/docker_context.sh b/.github/templates/docker_context/docker_context.sh index e8dc0d1f..3ced31a0 100755 --- a/.github/templates/docker_context/docker_context.sh +++ b/.github/templates/docker_context/docker_context.sh @@ -21,7 +21,7 @@ fi while read -r module; do # Retrieve docker compose service names - services=$(docker-compose -f "$module" config --services) + services=$(docker compose -f "$module" config --services) module_out=$(echo "$module" | sed -n 's/modules\/docker-compose\.\(.*\)\.yaml/\1/p') # Skip simulation module From 2aa306de946f158daa39cd908b66a9257b3ffe60 Mon Sep 17 00:00:00 2001 From: WATonomousAdmin Date: Tue, 13 Aug 2024 01:26:24 +0000 Subject: [PATCH 53/59] Fix code style issues with clang_format --- .../include/occupancy_segmentation_core.hpp | 219 +++++++++--------- .../include/occupancy_segmentation_node.hpp | 36 ++- .../src/occupancy_segmentation_core.cpp | 1 - .../src/occupancy_segmentation_node.cpp | 31 +-- 4 files changed, 141 insertions(+), 146 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index c17da4a9..1dfae4eb 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -1,17 +1,15 @@ #ifndef OCCUPANCY_SEGMENTATION_CORE_HPP_ #define OCCUPANCY_SEGMENTATION_CORE_HPP_ -#include -#include #include #include +#include +#include -#include #include +#include #include - - #define NUM_ZONES 4 #define L_MIN 2.7 #define L_MAX 80.0 @@ -29,17 +27,15 @@ #define SENSOR_HEIGHT 1.7 #define GLOBAL_EL_THRESH 0 - - 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_; + 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 { @@ -50,65 +46,68 @@ struct Patch_Index { int concentric_idx; }; -enum Status {TOO_TILTED, FLAT_ENOUGH, TOO_HIGH_ELEV, UPRIGHT_ENOUGH, GLOBAL_TOO_HIGH_ELEV, FEW_POINTS}; +enum Status { + TOO_TILTED, + FLAT_ENOUGH, + TOO_HIGH_ELEV, + UPRIGHT_ENOUGH, + GLOBAL_TOO_HIGH_ELEV, + FEW_POINTS +}; -template +template class OccupancySegmentationCore { - public: - typedef std::vector> Ring; - typedef std::vector Zone; - // Size of buffer before processed messages are published. - static constexpr int BUFFER_CAPACITY = 10; + public: + typedef std::vector> Ring; + typedef std::vector Zone; + // Size of buffer before processed messages are published. + static constexpr int BUFFER_CAPACITY = 10; - const int ZONE_RINGS [NUM_ZONES] = {2,4,4,4}; - const int ZONE_SECTORS [NUM_ZONES] = {16,32,54,32}; - const float FLATNESS_THR [NUM_ZONES] = {0.0005, 0.000725, 0.001, 0.001}; - const float ELEVATION_THR [NUM_ZONES] = {0.523, 0.746, 0.879, 1.125}; - const double lmins[NUM_ZONES] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - const double lmaxs[NUM_ZONES] = {lmins[1], lmins[2], lmins[3], L_MAX}; + const int ZONE_RINGS[NUM_ZONES] = {2, 4, 4, 4}; + const int ZONE_SECTORS[NUM_ZONES] = {16, 32, 54, 32}; + const float FLATNESS_THR[NUM_ZONES] = {0.0005, 0.000725, 0.001, 0.001}; + const float ELEVATION_THR[NUM_ZONES] = {0.523, 0.746, 0.879, 1.125}; + const double lmins[NUM_ZONES] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, + (L_MIN + L_MAX) / 2}; + const double lmaxs[NUM_ZONES] = {lmins[1], lmins[2], lmins[3], L_MAX}; + int num_patches = -1; - int num_patches = -1; + std::vector _czm; + std::vector> _regionwise_ground; + std::vector> _regionwise_nonground; - std::vector _czm; - std::vector> _regionwise_ground; - std::vector> _regionwise_nonground; + std::vector _patch_indices; - std::vector _patch_indices; + pcl::PointCloud _ground; + pcl::PointCloud _non_ground; - pcl::PointCloud _ground; - pcl::PointCloud _non_ground; - - std::vector _statuses; + std::vector _statuses; + OccupancySegmentationCore(); - OccupancySegmentationCore(); + void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, + pcl::PointCloud &nonground); - void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); - - private: + private: + void init_czm(); - void init_czm(); + void fill_czm(pcl::PointCloud &cloud_in); - void fill_czm(pcl::PointCloud &cloud_in); + void clear_czm_and_regionwise(); - void clear_czm_and_regionwise(); + void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); - void estimate_plane(pcl::PointCloud &cloud, PCAFeature &feat); - - void rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat); - - void extract_initial_seeds(pcl::PointCloud &cloud, pcl::PointCloud &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; }; + void rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat); + void extract_initial_seeds(pcl::PointCloud &cloud, pcl::PointCloud &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; }; }; - template OccupancySegmentationCore::OccupancySegmentationCore() { init_czm(); @@ -141,7 +140,7 @@ void OccupancySegmentationCore::init_czm() { Status status; _regionwise_ground.push_back(regionground); _regionwise_nonground.push_back(regionnonground); - + _statuses.push_back(status); patch_count++; @@ -155,7 +154,6 @@ void OccupancySegmentationCore::init_czm() { template void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { - for (PointT &p : cloud_in.points) { double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); @@ -177,7 +175,7 @@ void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_ double ring_size = deltal / ZONE_RINGS[zone_idx]; double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; - ring_idx = std::min((int) ((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); + ring_idx = std::min((int)((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); sector_idx = std::min((int)(theta / sector_size), ZONE_SECTORS[zone_idx] - 1); _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); break; @@ -187,10 +185,10 @@ void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_ } template -void OccupancySegmentationCore::clear_czm_and_regionwise(){ - for (Zone &zone : _czm){ - for (Ring &ring : zone){ - for (pcl::PointCloud &patch : ring){ +void OccupancySegmentationCore::clear_czm_and_regionwise() { + for (Zone &zone : _czm) { + for (Ring &ring : zone) { + for (pcl::PointCloud &patch : ring) { patch.clear(); } } @@ -199,8 +197,8 @@ void OccupancySegmentationCore::clear_czm_and_regionwise(){ template void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, - PCAFeature &feat) { - //Code taken directly from repo + PCAFeature &feat) { + // Code taken directly from repo Eigen::Matrix3f cov; Eigen::Vector4f mean; pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); @@ -208,8 +206,10 @@ void OccupancySegmentationCore::estimate_plane(pcl::PointCloud & Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); feat.singular_values_ = svd.singularValues(); - feat.linearity_ = (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - feat.planarity_ = (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + feat.linearity_ = + (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = + (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); // use the least singular vector as normal feat.normal_ = (svd.matrixU().col(2)); @@ -224,15 +224,16 @@ void OccupancySegmentationCore::estimate_plane(pcl::PointCloud & } template -void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground) { +void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, + pcl::PointCloud &ground, + pcl::PointCloud &nonground) { clear_czm_and_regionwise(); - //TODO error point removal + // TODO error point removal fill_czm(unfiltered_cloud); std::cout << "Finished filling czm" << std::endl; - tbb::parallel_for(tbb::blocked_range(0, num_patches), - [&](tbb::blocked_range r) { + tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { Patch_Index &p_idx = _patch_indices[patch_num]; pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; @@ -257,22 +258,22 @@ void OccupancySegmentationCore::segment_ground(pcl::PointCloud & std::cout << "Finished estimation" << std::endl; - for (Patch_Index p_idx : _patch_indices){ + for (Patch_Index p_idx : _patch_indices) { Status status = _statuses[p_idx.idx]; - if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH){ + if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH) { ground += _regionwise_ground[p_idx.idx]; nonground += _regionwise_nonground[p_idx.idx]; } else { nonground += _regionwise_ground[p_idx.idx]; nonground += _regionwise_nonground[p_idx.idx]; } - } } template -void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, PCAFeature &feat) { +void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, + PCAFeature &feat) { pcl::PointCloud ground_temp; pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; @@ -281,53 +282,54 @@ void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Pat extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); int num_iters = 3; - for(int i = 0; i < num_iters; i++){ + for (int i = 0; i < num_iters; i++) { estimate_plane(ground_temp, feat); ground_temp.clear(); Eigen::MatrixXf points(N, 3); int j = 0; - for (PointT &p:patch.points) { - points.row(j++) = p.getVector3fMap(); + for (PointT &p : patch.points) { + points.row(j++) = p.getVector3fMap(); } Eigen::VectorXf result = points * feat.normal_; for (size_t r = 0; r < N; r++) { - if (i < num_iters - 1) { - if (result[r] < feat.th_dist_d_) { - ground_temp.points.emplace_back(patch.points[r]); - } - } else { - if (result[r] < feat.th_dist_d_) { - region_ground.points.emplace_back(patch.points[r]); - } else { - region_nonground.points.emplace_back(patch.points[r]); - } - } + if (i < num_iters - 1) { + if (result[r] < feat.th_dist_d_) { + ground_temp.points.emplace_back(patch.points[r]); } - + } else { + if (result[r] < feat.th_dist_d_) { + region_ground.points.emplace_back(patch.points[r]); + } else { + region_nonground.points.emplace_back(patch.points[r]); + } + } + } } } template -Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, int concentric_idx){ - - //uprightness filter - if(std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH){ +Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, + int concentric_idx) { + // uprightness filter + if (std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH) { return TOO_TILTED; } const double z_elevation = feat.mean_(2); - const double surface_variable = feat.singular_values_.minCoeff() / (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + const double surface_variable = + feat.singular_values_.minCoeff() / + (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); - //elevation filter + // elevation filter if (concentric_idx < NUM_RINGS_OF_INTEREST) { if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx]) { - if (FLATNESS_THR[concentric_idx] <= surface_variable){ - return FLAT_ENOUGH; - } else { - return TOO_HIGH_ELEV; - } - } else{ + if (FLATNESS_THR[concentric_idx] <= surface_variable) { + return FLAT_ENOUGH; + } else { + return TOO_HIGH_ELEV; + } + } else { return UPRIGHT_ENOUGH; } } else { @@ -337,27 +339,25 @@ Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat return UPRIGHT_ENOUGH; } } - } template void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud &cloud, - pcl::PointCloud &seed_cloud, int zone_idx) { - - //adaptive seed selection for 1st zone + pcl::PointCloud &seed_cloud, + int zone_idx) { + // adaptive seed selection for 1st zone size_t init_idx = 0; - if (zone_idx == 0){ + if (zone_idx == 0) { double adaptive_seed_selection_margin = MH * SENSOR_HEIGHT; - for (size_t i = 0; i < cloud.points.size(); i++){ - if (cloud.points[i].z < adaptive_seed_selection_margin){ + for (size_t i = 0; i < cloud.points.size(); i++) { + if (cloud.points[i].z < adaptive_seed_selection_margin) { init_idx++; - } else{ + } else { break; } } } - double sum = 0; int cnt = 0; @@ -374,5 +374,4 @@ void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud -#include +#include #include -#include #include -#include +#include +#include #include +#include +#include "occupancy_segmentation_core.hpp" typedef std::chrono::high_resolution_clock Clock; - -struct PointXYZIRT -{ - PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding +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; + PCL_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned +} EIGEN_ALIGN16; -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) -) +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 @@ -54,14 +49,13 @@ class OccupancySegmentationNode : public rclcpp::Node { OccupancySegmentationNode(); private: - // Object that handles data processing and validation. OccupancySegmentationCore _patchwork; rclcpp::Subscription::SharedPtr _subscriber; rclcpp::Publisher::SharedPtr _nonground_publisher; rclcpp::Publisher::SharedPtr _ground_publisher; - void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); + void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); }; #endif // TRANSFORMER_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 8cca23c3..bfc60873 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -2,4 +2,3 @@ #include #include "occupancy_segmentation_core.hpp" - diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 4fa53ef2..7b750160 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -1,19 +1,22 @@ -#include -#include #include "occupancy_segmentation_node.hpp" +#include +#include OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { - _subscriber = this->create_subscription( - "/velodyne_points", 10, std::bind(&OccupancySegmentationNode::subscription_callback, this, std::placeholders::_1)); - - _ground_publisher = this -> create_publisher("/ground_points", 10); - _nonground_publisher = this -> create_publisher("/nonground_points", 10); + _subscriber = this->create_subscription( + "/velodyne_points", 10, + std::bind(&OccupancySegmentationNode::subscription_callback, this, std::placeholders::_1)); + _ground_publisher = this->create_publisher("/ground_points", 10); + _nonground_publisher = + this->create_publisher("/nonground_points", 10); } -void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud){ +void OccupancySegmentationNode::subscription_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud) { pcl::PointCloud temp_cloud; - //RCLCPP_INFO(this -> get_logger(), "Header incoming: %s", lidar_cloud -> header.frame_id.c_str()); + // RCLCPP_INFO(this -> get_logger(), "Header incoming: %s", lidar_cloud -> + // header.frame_id.c_str()); pcl::fromROSMsg(*lidar_cloud, temp_cloud); pcl::PointCloud ground; @@ -29,20 +32,20 @@ void OccupancySegmentationNode::subscription_callback(const sensor_msgs::msg::P auto end = Clock::now(); int duration = std::chrono::duration_cast(end - begin).count(); - RCLCPP_INFO(this -> get_logger(), "Runtime for Patchwork: %i ms", duration); + RCLCPP_INFO(this->get_logger(), "Runtime for Patchwork: %i ms", duration); // RCLCPP_INFO(this->get_logger(), "Temp_cloud points %i", static_cast(temp_cloud.size())); // RCLCPP_INFO(this->get_logger(), "Ground points %i", static_cast(ground.size())); // RCLCPP_INFO(this->get_logger(), "Non ground points %i", static_cast(nonground.size())); sensor_msgs::msg::PointCloud2 ground_msg; sensor_msgs::msg::PointCloud2 nonground_msg; - + pcl::toROSMsg(ground, ground_msg); pcl::toROSMsg(nonground, nonground_msg); - //RCLCPP_INFO(this -> get_logger(), "Header outgoing: %s", ground_msg.header.frame_id.c_str()); + // RCLCPP_INFO(this -> get_logger(), "Header outgoing: %s", ground_msg.header.frame_id.c_str()); - _ground_publisher -> publish(ground_msg); - _nonground_publisher -> publish(nonground_msg); + _ground_publisher->publish(ground_msg); + _nonground_publisher->publish(nonground_msg); } int main(int argc, char** argv) { From 9b6fff381c54a7a97f8da83c649901f6812059ab Mon Sep 17 00:00:00 2001 From: VishGit1234 Date: Thu, 12 Sep 2024 18:48:49 +0000 Subject: [PATCH 54/59] added ros params --- .../occupancy_segmentation.Dockerfile | 7 +- .../occupancy_segmentation/config/params.yaml | 21 +- .../include/occupancy_segmentation_core.hpp | 333 +++--------------- .../include/occupancy_segmentation_node.hpp | 10 +- .../occupancy_segmentation/package.xml | 4 +- .../src/occupancy_segmentation_core.cpp | 309 +++++++++++++++- .../src/occupancy_segmentation_node.cpp | 61 +++- 7 files changed, 430 insertions(+), 315 deletions(-) diff --git a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile index 2168af0f..088351e9 100644 --- a/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile +++ b/docker/world_modeling/occupancy_segmentation/occupancy_segmentation.Dockerfile @@ -6,7 +6,6 @@ FROM ${BASE_IMAGE} as source WORKDIR ${AMENT_WS}/src # Copy in source code -# RUN git clone https://github.com/ros-perception/perception_pcl.git --branch 2.4.3 COPY src/world_modeling/occupancy_segmentation occupancy_segmentation # Scan for rosdeps @@ -16,11 +15,13 @@ RUN apt-get -qq update && rosdep update && \ | awk '{print $3}' \ | sort > /tmp/colcon_install_list -RUN sudo apt-get install libeigen3-dev -RUN sudo apt-get -y install libtbb-dev ################################# 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-get -qq update && apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) diff --git a/src/world_modeling/occupancy_segmentation/config/params.yaml b/src/world_modeling/occupancy_segmentation/config/params.yaml index 47c10f5b..22b22ed4 100644 --- a/src/world_modeling/occupancy_segmentation/config/params.yaml +++ b/src/world_modeling/occupancy_segmentation/config/params.yaml @@ -1,4 +1,21 @@ -occupancy_segmentation_node: +occupancy_segmentation: ros__parameters: version: 1 - compression_method: 0 + lidar_input_topic: "/velodyne_points" + ground_output_topic: "/ground_points" + nonground_output_topic: "/nonground_points" + 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] diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 1dfae4eb..4dc8eeb8 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -5,27 +5,21 @@ #include #include #include +#include #include #include #include #define NUM_ZONES 4 -#define L_MIN 2.7 -#define L_MAX 80.0 -#define Z_SEED 0.5 -#define MD 0.3 -#define MH -1.1 - -#define MIN_NUM_POINTS 10 -#define NUM_SEED_POINTS 20 -#define TH_SEEDS 0.5 -#define UPRIGHTNESS_THRESH 0.5 - -#define NUM_RINGS_OF_INTEREST 4 -#define SENSOR_HEIGHT 1.7 -#define GLOBAL_EL_THRESH 0 +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_; @@ -63,13 +57,29 @@ class OccupancySegmentationCore { // Size of buffer before processed messages are published. static constexpr int BUFFER_CAPACITY = 10; - const int ZONE_RINGS[NUM_ZONES] = {2, 4, 4, 4}; - const int ZONE_SECTORS[NUM_ZONES] = {16, 32, 54, 32}; - const float FLATNESS_THR[NUM_ZONES] = {0.0005, 0.000725, 0.001, 0.001}; - const float ELEVATION_THR[NUM_ZONES] = {0.523, 0.746, 0.879, 1.125}; - const double lmins[NUM_ZONES] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, + 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; + + int ZONE_RINGS[NUM_ZONES]; + int ZONE_SECTORS[NUM_ZONES]; + float FLATNESS_THR[NUM_ZONES]; + float ELEVATION_THR[NUM_ZONES]; + double lmins[NUM_ZONES] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - const double lmaxs[NUM_ZONES] = {lmins[1], lmins[2], lmins[3], L_MAX}; + double lmaxs[NUM_ZONES] = {lmins[1], lmins[2], lmins[3], L_MAX}; int num_patches = -1; @@ -85,6 +95,23 @@ class OccupancySegmentationCore { std::vector _statuses; OccupancySegmentationCore(); + OccupancySegmentationCore( + 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 >& zone_rings, + std::vector >& zone_sectors, + std::vector& flatness_thr, + std::vector& elevation_thr + ); void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); @@ -108,270 +135,4 @@ class OccupancySegmentationCore { static bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; }; }; -template -OccupancySegmentationCore::OccupancySegmentationCore() { - init_czm(); -} - -template -void OccupancySegmentationCore::init_czm() { - int concentric_count = 0; - int patch_count = 0; - - for (int z = 0; z < NUM_ZONES; z++) { - Zone zone; - Patch_Index index; - index.zone_idx = z; - - for (int r = 0; r < ZONE_RINGS[z]; r++) { - Ring ring; - index.ring_idx = r; - index.concentric_idx = concentric_count; - - for (int s = 0; s < ZONE_SECTORS[z]; s++) { - index.sector_idx = s; - index.idx = patch_count; - _patch_indices.push_back(index); - pcl::PointCloud patch; - ring.emplace_back(patch); - - pcl::PointCloud regionground; - pcl::PointCloud regionnonground; - Status status; - _regionwise_ground.push_back(regionground); - _regionwise_nonground.push_back(regionnonground); - - _statuses.push_back(status); - - patch_count++; - } - zone.emplace_back(ring); - } - _czm.emplace_back(zone); - } - num_patches = patch_count; -} - -template -void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { - for (PointT &p : cloud_in.points) { - double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); - - // Out of range for czm - if (r < L_MIN || r > L_MAX) { - continue; - } - - double theta = atan2(p.y, p.x); - if (theta < 0) { - theta += 2 * M_PI; - } - - double deltal = L_MAX - L_MIN; - - for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { - int ring_idx, sector_idx; - if (r < lmaxs[zone_idx]) { - double ring_size = deltal / ZONE_RINGS[zone_idx]; - double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; - - ring_idx = std::min((int)((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); - sector_idx = std::min((int)(theta / sector_size), ZONE_SECTORS[zone_idx] - 1); - _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); - break; - } - } - } -} - -template -void OccupancySegmentationCore::clear_czm_and_regionwise() { - for (Zone &zone : _czm) { - for (Ring &ring : zone) { - for (pcl::PointCloud &patch : ring) { - patch.clear(); - } - } - } -} - -template -void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, - PCAFeature &feat) { - // Code taken directly from repo - Eigen::Matrix3f cov; - Eigen::Vector4f mean; - pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); - - Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); - feat.singular_values_ = svd.singularValues(); - - feat.linearity_ = - (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); - feat.planarity_ = - (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); - - // use the least singular vector as normal - feat.normal_ = (svd.matrixU().col(2)); - if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive - feat.normal_ = -feat.normal_; - } - // mean ground seeds value - feat.mean_ = mean.head<3>(); - // according to normal.T*[x,y,z] = -d - feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); - feat.th_dist_d_ = MD - feat.d_; -} - -template -void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, - pcl::PointCloud &ground, - pcl::PointCloud &nonground) { - clear_czm_and_regionwise(); - - // TODO error point removal - - fill_czm(unfiltered_cloud); - std::cout << "Finished filling czm" << std::endl; - tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { - for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { - Patch_Index &p_idx = _patch_indices[patch_num]; - pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; - pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - PCAFeature features; - - region_ground.clear(); - region_nonground.clear(); - if (patch.points.size() > MIN_NUM_POINTS) { - std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); - rgpf(patch, p_idx, features); - - Status status = ground_likelihood_est(features, p_idx.concentric_idx); - _statuses[p_idx.idx] = status; - } else { - region_ground = patch; - _statuses[p_idx.idx] = FEW_POINTS; - } - } - }); - - std::cout << "Finished estimation" << std::endl; - - for (Patch_Index p_idx : _patch_indices) { - Status status = _statuses[p_idx.idx]; - - if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH) { - ground += _regionwise_ground[p_idx.idx]; - nonground += _regionwise_nonground[p_idx.idx]; - } else { - nonground += _regionwise_ground[p_idx.idx]; - nonground += _regionwise_nonground[p_idx.idx]; - } - } -} - -template -void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, - PCAFeature &feat) { - pcl::PointCloud ground_temp; - pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; - pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; - - size_t N = patch.size(); - - extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); - int num_iters = 3; - for (int i = 0; i < num_iters; i++) { - estimate_plane(ground_temp, feat); - ground_temp.clear(); - Eigen::MatrixXf points(N, 3); - int j = 0; - for (PointT &p : patch.points) { - points.row(j++) = p.getVector3fMap(); - } - - Eigen::VectorXf result = points * feat.normal_; - for (size_t r = 0; r < N; r++) { - if (i < num_iters - 1) { - if (result[r] < feat.th_dist_d_) { - ground_temp.points.emplace_back(patch.points[r]); - } - } else { - if (result[r] < feat.th_dist_d_) { - region_ground.points.emplace_back(patch.points[r]); - } else { - region_nonground.points.emplace_back(patch.points[r]); - } - } - } - } -} - -template -Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, - int concentric_idx) { - // uprightness filter - if (std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH) { - return TOO_TILTED; - } - - const double z_elevation = feat.mean_(2); - const double surface_variable = - feat.singular_values_.minCoeff() / - (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); - - // elevation filter - if (concentric_idx < NUM_RINGS_OF_INTEREST) { - if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx]) { - if (FLATNESS_THR[concentric_idx] <= surface_variable) { - return FLAT_ENOUGH; - } else { - return TOO_HIGH_ELEV; - } - } else { - return UPRIGHT_ENOUGH; - } - } else { - if (z_elevation > GLOBAL_EL_THRESH) { - return GLOBAL_TOO_HIGH_ELEV; - } else { - return UPRIGHT_ENOUGH; - } - } -} - -template -void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud &cloud, - pcl::PointCloud &seed_cloud, - int zone_idx) { - // adaptive seed selection for 1st zone - - size_t init_idx = 0; - if (zone_idx == 0) { - double adaptive_seed_selection_margin = MH * SENSOR_HEIGHT; - for (size_t i = 0; i < cloud.points.size(); i++) { - if (cloud.points[i].z < adaptive_seed_selection_margin) { - init_idx++; - } else { - break; - } - } - } - - double sum = 0; - int cnt = 0; - for (size_t i = 0; i < cloud.points.size() && cnt < NUM_SEED_POINTS; i++) { - sum += cloud.points[i].z; - cnt++; - } - - double mean_z = (cnt != 0) ? sum / cnt : 0; - for (size_t i = init_idx; i < cloud.points.size(); i++) { - if (cloud.points[i].z < mean_z + TH_SEEDS) { - seed_cloud.points.push_back(cloud.points[i]); - } - } -} - #endif diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index a88cb691..5301d7bd 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -15,14 +15,6 @@ typedef std::chrono::high_resolution_clock Clock; -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; - 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, @@ -58,4 +50,4 @@ class OccupancySegmentationNode : public rclcpp::Node { void subscription_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud); }; -#endif // TRANSFORMER_NODE_HPP_ +#endif // OCCUPANCY_SEGMENTATION_NODE_HPP_ diff --git a/src/world_modeling/occupancy_segmentation/package.xml b/src/world_modeling/occupancy_segmentation/package.xml index 500bbac1..14ec454f 100644 --- a/src/world_modeling/occupancy_segmentation/package.xml +++ b/src/world_modeling/occupancy_segmentation/package.xml @@ -2,9 +2,9 @@ occupancy_segmentation 0.0.0 - A sample ROS package for pubsub communication + A ROS2 package to handle LiDAR ground segmentation using Patchworks - Cheran Mahalingam + Akil Pathiranage TODO diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index bfc60873..952c0a3e 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -1,4 +1,307 @@ -#include -#include - #include "occupancy_segmentation_core.hpp" + +// Explicitly instantiate template constructor for the type we will use +template class OccupancySegmentationCore; + +template +OccupancySegmentationCore::OccupancySegmentationCore( + 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 >& zone_rings, + std::vector >& zone_sectors, + std::vector& flatness_thr, + std::vector& elevation_thr + ) : + L_MIN{l_min}, + L_MAX{l_max}, + MD{md}, + MH{mh}, + MIN_NUM_POINTS{min_num_points}, + NUM_SEED_POINTS{num_seed_points}, + TH_SEEDS{th_seeds}, + UPRIGHTNESS_THRESH{uprightness_thresh}, + NUM_RINGS_OF_INTEREST{num_rings_of_interest}, + SENSOR_HEIGHT{sensor_height}, + GLOBAL_EL_THRESH{global_el_thresh}, + ZONE_RINGS{zone_rings[0], zone_rings[1], zone_rings[2], zone_rings[3]}, + ZONE_SECTORS{zone_sectors[0], zone_sectors[1], zone_sectors[2], zone_sectors[3]}, + FLATNESS_THR{flatness_thr[0], flatness_thr[1], flatness_thr[2], flatness_thr[3]}, + ELEVATION_THR{elevation_thr[0], elevation_thr[1], elevation_thr[2], elevation_thr[3]}, + lmins{L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}, + lmaxs{lmins[1], lmins[2], lmins[3], L_MAX} +{ + init_czm(); +} + +template +OccupancySegmentationCore::OccupancySegmentationCore() {} + +template +void OccupancySegmentationCore::init_czm() { + int concentric_count = 0; + int patch_count = 0; + + for (int z = 0; z < NUM_ZONES; z++) { + Zone zone; + Patch_Index index; + index.zone_idx = z; + + for (int r = 0; r < ZONE_RINGS[z]; r++) { + Ring ring; + index.ring_idx = r; + index.concentric_idx = concentric_count; + + for (int s = 0; s < ZONE_SECTORS[z]; s++) { + index.sector_idx = s; + index.idx = patch_count; + _patch_indices.push_back(index); + pcl::PointCloud patch; + ring.emplace_back(patch); + + pcl::PointCloud regionground; + pcl::PointCloud regionnonground; + Status status; + _regionwise_ground.push_back(regionground); + _regionwise_nonground.push_back(regionnonground); + + _statuses.push_back(status); + + patch_count++; + } + zone.emplace_back(ring); + } + _czm.emplace_back(zone); + } + num_patches = patch_count; +} + +template +void OccupancySegmentationCore::fill_czm(pcl::PointCloud &cloud_in) { + for (PointT &p : cloud_in.points) { + double r = sqrt(pow(p.x, 2) + pow(p.y, 2)); + + // Out of range for czm + if (r < L_MIN || r > L_MAX) { + continue; + } + + double theta = atan2(p.y, p.x); + if (theta < 0) { + theta += 2 * M_PI; + } + + double deltal = L_MAX - L_MIN; + + for (int zone_idx = 0; zone_idx < NUM_ZONES; zone_idx++) { + int ring_idx, sector_idx; + if (r < lmaxs[zone_idx]) { + double ring_size = deltal / ZONE_RINGS[zone_idx]; + double sector_size = 2 * M_PI / ZONE_SECTORS[zone_idx]; + + ring_idx = std::min((int)((r - lmins[zone_idx]) / ring_size), ZONE_RINGS[zone_idx] - 1); + sector_idx = std::min((int)(theta / sector_size), ZONE_SECTORS[zone_idx] - 1); + _czm[zone_idx][ring_idx][sector_idx].points.emplace_back(p); + break; + } + } + } +} + +template +void OccupancySegmentationCore::clear_czm_and_regionwise() { + for (Zone &zone : _czm) { + for (Ring &ring : zone) { + for (pcl::PointCloud &patch : ring) { + patch.clear(); + } + } + } +} + +template +void OccupancySegmentationCore::estimate_plane(pcl::PointCloud &cloud, + PCAFeature &feat) { + // Code taken directly from repo + Eigen::Matrix3f cov; + Eigen::Vector4f mean; + pcl::computeMeanAndCovarianceMatrix(cloud, cov, mean); + + Eigen::JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); + feat.singular_values_ = svd.singularValues(); + + feat.linearity_ = + (feat.singular_values_(0) - feat.singular_values_(1)) / feat.singular_values_(0); + feat.planarity_ = + (feat.singular_values_(1) - feat.singular_values_(2)) / feat.singular_values_(0); + + // use the least singular vector as normal + feat.normal_ = (svd.matrixU().col(2)); + if (feat.normal_(2) < 0) { // z-direction of the normal vector should be positive + feat.normal_ = -feat.normal_; + } + // mean ground seeds value + feat.mean_ = mean.head<3>(); + // according to normal.T*[x,y,z] = -d + feat.d_ = -(feat.normal_.transpose() * feat.mean_)(0, 0); + feat.th_dist_d_ = MD - feat.d_; +} + +template +void OccupancySegmentationCore::segment_ground(pcl::PointCloud &unfiltered_cloud, + pcl::PointCloud &ground, + pcl::PointCloud &nonground) { + clear_czm_and_regionwise(); + + // TODO error point removal + + fill_czm(unfiltered_cloud); + std::cout << "Finished filling czm" << std::endl; + tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { + for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { + Patch_Index &p_idx = _patch_indices[patch_num]; + pcl::PointCloud &patch = _czm[p_idx.zone_idx][p_idx.ring_idx][p_idx.sector_idx]; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + PCAFeature features; + + region_ground.clear(); + region_nonground.clear(); + if (patch.points.size() > MIN_NUM_POINTS) { + std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); + rgpf(patch, p_idx, features); + + Status status = ground_likelihood_est(features, p_idx.concentric_idx); + _statuses[p_idx.idx] = status; + } else { + region_ground = patch; + _statuses[p_idx.idx] = FEW_POINTS; + } + } + }); + + std::cout << "Finished estimation" << std::endl; + + for (Patch_Index p_idx : _patch_indices) { + Status status = _statuses[p_idx.idx]; + + if (status == FEW_POINTS || status == FLAT_ENOUGH || status == UPRIGHT_ENOUGH) { + ground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } else { + nonground += _regionwise_ground[p_idx.idx]; + nonground += _regionwise_nonground[p_idx.idx]; + } + } +} + +template +void OccupancySegmentationCore::rgpf(pcl::PointCloud &patch, Patch_Index &p_idx, + PCAFeature &feat) { + pcl::PointCloud ground_temp; + pcl::PointCloud ®ion_ground = _regionwise_ground[p_idx.idx]; + pcl::PointCloud ®ion_nonground = _regionwise_nonground[p_idx.idx]; + + size_t N = patch.size(); + + extract_initial_seeds(patch, ground_temp, p_idx.zone_idx); + int num_iters = 3; + for (int i = 0; i < num_iters; i++) { + estimate_plane(ground_temp, feat); + ground_temp.clear(); + Eigen::MatrixXf points(N, 3); + int j = 0; + for (PointT &p : patch.points) { + points.row(j++) = p.getVector3fMap(); + } + + Eigen::VectorXf result = points * feat.normal_; + for (size_t r = 0; r < N; r++) { + if (i < num_iters - 1) { + if (result[r] < feat.th_dist_d_) { + ground_temp.points.emplace_back(patch.points[r]); + } + } else { + if (result[r] < feat.th_dist_d_) { + region_ground.points.emplace_back(patch.points[r]); + } else { + region_nonground.points.emplace_back(patch.points[r]); + } + } + } + } +} + +template +Status OccupancySegmentationCore::ground_likelihood_est(PCAFeature &feat, + int concentric_idx) { + // uprightness filter + if (std::abs(feat.normal_(2)) < UPRIGHTNESS_THRESH) { + return TOO_TILTED; + } + + const double z_elevation = feat.mean_(2); + const double surface_variable = + feat.singular_values_.minCoeff() / + (feat.singular_values_(0) + feat.singular_values_(1) + feat.singular_values_(2)); + + // elevation filter + if (concentric_idx < NUM_RINGS_OF_INTEREST) { + if (z_elevation > -SENSOR_HEIGHT + ELEVATION_THR[concentric_idx]) { + if (FLATNESS_THR[concentric_idx] <= surface_variable) { + return FLAT_ENOUGH; + } else { + return TOO_HIGH_ELEV; + } + } else { + return UPRIGHT_ENOUGH; + } + } else { + if (z_elevation > GLOBAL_EL_THRESH) { + return GLOBAL_TOO_HIGH_ELEV; + } else { + return UPRIGHT_ENOUGH; + } + } +} + +template +void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud &cloud, + pcl::PointCloud &seed_cloud, + int zone_idx) { + // adaptive seed selection for 1st zone + + size_t init_idx = 0; + if (zone_idx == 0) { + double adaptive_seed_selection_margin = MH * SENSOR_HEIGHT; + for (size_t i = 0; i < cloud.points.size(); i++) { + if (cloud.points[i].z < adaptive_seed_selection_margin) { + init_idx++; + } else { + break; + } + } + } + + double sum = 0; + int cnt = 0; + for (size_t i = 0; i < cloud.points.size() && cnt < NUM_SEED_POINTS; i++) { + sum += cloud.points[i].z; + cnt++; + } + + double mean_z = (cnt != 0) ? sum / cnt : 0; + for (size_t i = init_idx; i < cloud.points.size(); i++) { + if (cloud.points[i].z < mean_z + TH_SEEDS) { + seed_cloud.points.push_back(cloud.points[i]); + } + } +} diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 7b750160..b72fe8f9 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -3,20 +3,61 @@ #include OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { + // Declare parameters + this->declare_parameter("l_min", 2.7); + this->declare_parameter("l_max", 80.0); + this->declare_parameter("md", 0.3); + this->declare_parameter("mh", -1.1); + this->declare_parameter("min_num_points", 10); + this->declare_parameter("num_seed_points", 20); + this->declare_parameter("th_seeds", 20); + this->declare_parameter("uprightness_thresh", 0.5); + this->declare_parameter("num_rings_of_interest", 4); + this->declare_parameter("sensor_height", 1.7); + this->declare_parameter("global_el_thresh", 0.0); + this->declare_parameter>("zone_rings", std::vector{2, 4, 4, 4}); + this->declare_parameter>("zone_sectors", std::vector{16, 32, 54, 32}); + this->declare_parameter>("flatness_thr", std::vector{0.0005, 0.000725, 0.001, 0.001}); + this->declare_parameter>("elevation_thr", std::vector{0.523, 0.746, 0.879, 1.125}); + this->declare_parameter("lidar_input_topic", std::string("/velodyne_points")); + this->declare_parameter("ground_output_topic", std::string("/ground_points")); + this->declare_parameter("nonground_output_topic", std::string("/nonground_points")); + + // Retrieve parameters + double l_min = this->get_parameter("l_min").as_double(); + double l_max = this->get_parameter("l_max").as_double(); + double md = this->get_parameter("md").as_double(); + double mh = this->get_parameter("mh").as_double(); + int min_num_points = this->get_parameter("min_num_points").as_int(); + int num_seed_points = this->get_parameter("num_seed_points").as_int(); + double th_seeds = this->get_parameter("th_seeds").as_double(); + double uprightness_thresh = this->get_parameter("uprightness_thresh").as_double(); + int num_rings_of_interest = this->get_parameter("num_rings_of_interest").as_int(); + double sensor_height = this->get_parameter("sensor_height").as_double(); + double global_el_thresh = this->get_parameter("global_el_thresh").as_double(); + auto zone_rings = this->get_parameter("zone_rings").as_integer_array(); + auto zone_sectors = this->get_parameter("zone_sectors").as_integer_array(); + auto flatness_thr = this->get_parameter("flatness_thr").as_double_array(); + auto elevation_thr = this->get_parameter("elevation_thr").as_double_array(); + std::string lidar_input_topic = this->get_parameter("lidar_input_topic").as_string(); + std::string ground_output_topic = this->get_parameter("ground_output_topic").as_string(); + std::string nonground_output_topic = this->get_parameter("nonground_output_topic").as_string(); + + _patchwork = OccupancySegmentationCore(l_min, l_max, md, mh, min_num_points, num_seed_points, th_seeds, uprightness_thresh, num_rings_of_interest, sensor_height, global_el_thresh, zone_rings, zone_sectors, flatness_thr, elevation_thr); + _subscriber = this->create_subscription( - "/velodyne_points", 10, + lidar_input_topic, 10, std::bind(&OccupancySegmentationNode::subscription_callback, this, std::placeholders::_1)); - _ground_publisher = this->create_publisher("/ground_points", 10); + _ground_publisher = this->create_publisher(ground_output_topic, 10); _nonground_publisher = - this->create_publisher("/nonground_points", 10); + this->create_publisher(nonground_output_topic, 10); } void OccupancySegmentationNode::subscription_callback( const sensor_msgs::msg::PointCloud2::SharedPtr lidar_cloud) { pcl::PointCloud temp_cloud; - // RCLCPP_INFO(this -> get_logger(), "Header incoming: %s", lidar_cloud -> - // header.frame_id.c_str()); + RCLCPP_DEBUG(this->get_logger(), "Header incoming: %s", lidar_cloud->header.frame_id.c_str()); pcl::fromROSMsg(*lidar_cloud, temp_cloud); pcl::PointCloud ground; @@ -32,17 +73,17 @@ void OccupancySegmentationNode::subscription_callback( auto end = Clock::now(); int duration = std::chrono::duration_cast(end - begin).count(); - RCLCPP_INFO(this->get_logger(), "Runtime for Patchwork: %i ms", duration); - // RCLCPP_INFO(this->get_logger(), "Temp_cloud points %i", static_cast(temp_cloud.size())); - // RCLCPP_INFO(this->get_logger(), "Ground points %i", static_cast(ground.size())); - // RCLCPP_INFO(this->get_logger(), "Non ground points %i", static_cast(nonground.size())); + RCLCPP_DEBUG(this->get_logger(), "Runtime for Patchwork: %i ms", duration); + RCLCPP_DEBUG(this->get_logger(), "Temp_cloud points %i", static_cast(temp_cloud.size())); + RCLCPP_DEBUG(this->get_logger(), "Ground points %i", static_cast(ground.size())); + RCLCPP_DEBUG(this->get_logger(), "Non ground points %i", static_cast(nonground.size())); sensor_msgs::msg::PointCloud2 ground_msg; sensor_msgs::msg::PointCloud2 nonground_msg; pcl::toROSMsg(ground, ground_msg); pcl::toROSMsg(nonground, nonground_msg); - // RCLCPP_INFO(this -> get_logger(), "Header outgoing: %s", ground_msg.header.frame_id.c_str()); + RCLCPP_DEBUG(this->get_logger(), "Header outgoing: %s", ground_msg.header.frame_id.c_str()); _ground_publisher->publish(ground_msg); _nonground_publisher->publish(nonground_msg); From 00340a7724c3bb43eb376a7d384819521f4402a5 Mon Sep 17 00:00:00 2001 From: WATonomousAdmin Date: Thu, 12 Sep 2024 18:55:26 +0000 Subject: [PATCH 55/59] Fix code style issues with clang_format --- .../include/occupancy_segmentation_core.hpp | 27 +++------ .../src/occupancy_segmentation_core.cpp | 58 ++++++++----------- .../src/occupancy_segmentation_node.cpp | 14 +++-- 3 files changed, 41 insertions(+), 58 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 4dc8eeb8..084b36db 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -4,8 +4,8 @@ #include #include #include -#include #include +#include #include #include @@ -78,7 +78,7 @@ class OccupancySegmentationCore { float FLATNESS_THR[NUM_ZONES]; float ELEVATION_THR[NUM_ZONES]; double lmins[NUM_ZONES] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, - (L_MIN + L_MAX) / 2}; + (L_MIN + L_MAX) / 2}; double lmaxs[NUM_ZONES] = {lmins[1], lmins[2], lmins[3], L_MAX}; int num_patches = -1; @@ -95,23 +95,12 @@ class OccupancySegmentationCore { std::vector _statuses; OccupancySegmentationCore(); - OccupancySegmentationCore( - 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 >& zone_rings, - std::vector >& zone_sectors, - std::vector& flatness_thr, - std::vector& elevation_thr - ); + OccupancySegmentationCore(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> &zone_rings, + std::vector> &zone_sectors, + std::vector &flatness_thr, std::vector &elevation_thr); void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 952c0a3e..256dfa36 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -3,42 +3,30 @@ // Explicitly instantiate template constructor for the type we will use template class OccupancySegmentationCore; -template +template OccupancySegmentationCore::OccupancySegmentationCore( - 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 >& zone_rings, - std::vector >& zone_sectors, - std::vector& flatness_thr, - std::vector& elevation_thr - ) : - L_MIN{l_min}, - L_MAX{l_max}, - MD{md}, - MH{mh}, - MIN_NUM_POINTS{min_num_points}, - NUM_SEED_POINTS{num_seed_points}, - TH_SEEDS{th_seeds}, - UPRIGHTNESS_THRESH{uprightness_thresh}, - NUM_RINGS_OF_INTEREST{num_rings_of_interest}, - SENSOR_HEIGHT{sensor_height}, - GLOBAL_EL_THRESH{global_el_thresh}, - ZONE_RINGS{zone_rings[0], zone_rings[1], zone_rings[2], zone_rings[3]}, - ZONE_SECTORS{zone_sectors[0], zone_sectors[1], zone_sectors[2], zone_sectors[3]}, - FLATNESS_THR{flatness_thr[0], flatness_thr[1], flatness_thr[2], flatness_thr[3]}, - ELEVATION_THR{elevation_thr[0], elevation_thr[1], elevation_thr[2], elevation_thr[3]}, - lmins{L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}, - lmaxs{lmins[1], lmins[2], lmins[3], L_MAX} -{ + 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 > &zone_rings, + std::vector > &zone_sectors, + std::vector &flatness_thr, std::vector &elevation_thr) + : L_MIN{l_min}, + L_MAX{l_max}, + MD{md}, + MH{mh}, + MIN_NUM_POINTS{min_num_points}, + NUM_SEED_POINTS{num_seed_points}, + TH_SEEDS{th_seeds}, + UPRIGHTNESS_THRESH{uprightness_thresh}, + NUM_RINGS_OF_INTEREST{num_rings_of_interest}, + SENSOR_HEIGHT{sensor_height}, + GLOBAL_EL_THRESH{global_el_thresh}, + ZONE_RINGS{zone_rings[0], zone_rings[1], zone_rings[2], zone_rings[3]}, + ZONE_SECTORS{zone_sectors[0], zone_sectors[1], zone_sectors[2], zone_sectors[3]}, + FLATNESS_THR{flatness_thr[0], flatness_thr[1], flatness_thr[2], flatness_thr[3]}, + ELEVATION_THR{elevation_thr[0], elevation_thr[1], elevation_thr[2], elevation_thr[3]}, + lmins{L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}, + lmaxs{lmins[1], lmins[2], lmins[3], L_MAX} { init_czm(); } diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index b72fe8f9..b9eacf59 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -17,8 +17,10 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment this->declare_parameter("global_el_thresh", 0.0); this->declare_parameter>("zone_rings", std::vector{2, 4, 4, 4}); this->declare_parameter>("zone_sectors", std::vector{16, 32, 54, 32}); - this->declare_parameter>("flatness_thr", std::vector{0.0005, 0.000725, 0.001, 0.001}); - this->declare_parameter>("elevation_thr", std::vector{0.523, 0.746, 0.879, 1.125}); + this->declare_parameter>("flatness_thr", + std::vector{0.0005, 0.000725, 0.001, 0.001}); + this->declare_parameter>("elevation_thr", + std::vector{0.523, 0.746, 0.879, 1.125}); this->declare_parameter("lidar_input_topic", std::string("/velodyne_points")); this->declare_parameter("ground_output_topic", std::string("/ground_points")); this->declare_parameter("nonground_output_topic", std::string("/nonground_points")); @@ -43,13 +45,17 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment std::string ground_output_topic = this->get_parameter("ground_output_topic").as_string(); std::string nonground_output_topic = this->get_parameter("nonground_output_topic").as_string(); - _patchwork = OccupancySegmentationCore(l_min, l_max, md, mh, min_num_points, num_seed_points, th_seeds, uprightness_thresh, num_rings_of_interest, sensor_height, global_el_thresh, zone_rings, zone_sectors, flatness_thr, elevation_thr); + _patchwork = OccupancySegmentationCore( + l_min, l_max, md, mh, min_num_points, num_seed_points, th_seeds, uprightness_thresh, + num_rings_of_interest, sensor_height, global_el_thresh, zone_rings, zone_sectors, + flatness_thr, elevation_thr); _subscriber = this->create_subscription( lidar_input_topic, 10, std::bind(&OccupancySegmentationNode::subscription_callback, this, std::placeholders::_1)); - _ground_publisher = this->create_publisher(ground_output_topic, 10); + _ground_publisher = + this->create_publisher(ground_output_topic, 10); _nonground_publisher = this->create_publisher(nonground_output_topic, 10); } From 06c9902809585fc0ab888bf51788d7433e77b292 Mon Sep 17 00:00:00 2001 From: akilpath Date: Tue, 17 Sep 2024 01:08:54 +0000 Subject: [PATCH 56/59] Removed adaptive seed selection for 1st zone --- .../src/occupancy_segmentation_core.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 256dfa36..6eee352a 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -166,7 +166,7 @@ void OccupancySegmentationCore::segment_ground(pcl::PointCloud & if (patch.points.size() > MIN_NUM_POINTS) { std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); rgpf(patch, p_idx, features); - + Status status = ground_likelihood_est(features, p_idx.concentric_idx); _statuses[p_idx.idx] = status; } else { @@ -268,16 +268,16 @@ void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud Date: Tue, 17 Sep 2024 18:09:23 +0000 Subject: [PATCH 57/59] Made adaptive seed selection into toggle param --- .../occupancy_segmentation/config/params.yaml | 1 + .../include/occupancy_segmentation_core.hpp | 4 ++- .../src/occupancy_segmentation_core.cpp | 25 +++++++++---------- .../src/occupancy_segmentation_node.cpp | 4 ++- 4 files changed, 19 insertions(+), 15 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/config/params.yaml b/src/world_modeling/occupancy_segmentation/config/params.yaml index 22b22ed4..dcf6448f 100644 --- a/src/world_modeling/occupancy_segmentation/config/params.yaml +++ b/src/world_modeling/occupancy_segmentation/config/params.yaml @@ -19,3 +19,4 @@ occupancy_segmentation: 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 diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 084b36db..e93e03d5 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -83,6 +83,8 @@ class OccupancySegmentationCore { int num_patches = -1; + bool ADAPTIVE_SELECTION_EN; + std::vector _czm; std::vector> _regionwise_ground; std::vector> _regionwise_nonground; @@ -100,7 +102,7 @@ class OccupancySegmentationCore { int num_rings_of_interest, float sensor_height, float global_el_thresh, std::vector> &zone_rings, std::vector> &zone_sectors, - std::vector &flatness_thr, std::vector &elevation_thr); + std::vector &flatness_thr, std::vector &elevation_thr, bool adaptive_selection_en); void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 6eee352a..635913b6 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -9,7 +9,7 @@ OccupancySegmentationCore::OccupancySegmentationCore( float th_seeds, float uprightness_thresh, int num_rings_of_interest, float sensor_height, float global_el_thresh, std::vector > &zone_rings, std::vector > &zone_sectors, - std::vector &flatness_thr, std::vector &elevation_thr) + std::vector &flatness_thr, std::vector &elevation_thr, bool adaptive_selection_en) : L_MIN{l_min}, L_MAX{l_max}, MD{md}, @@ -26,7 +26,7 @@ OccupancySegmentationCore::OccupancySegmentationCore( FLATNESS_THR{flatness_thr[0], flatness_thr[1], flatness_thr[2], flatness_thr[3]}, ELEVATION_THR{elevation_thr[0], elevation_thr[1], elevation_thr[2], elevation_thr[3]}, lmins{L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}, - lmaxs{lmins[1], lmins[2], lmins[3], L_MAX} { + lmaxs{lmins[1], lmins[2], lmins[3], L_MAX}, ADAPTIVE_SELECTION_EN{adaptive_selection_en}{ init_czm(); } @@ -266,18 +266,17 @@ void OccupancySegmentationCore::extract_initial_seeds(pcl::PointCloud &seed_cloud, int zone_idx) { // adaptive seed selection for 1st zone - size_t init_idx = 0; - // if (zone_idx == 0) { - // double adaptive_seed_selection_margin = MH * SENSOR_HEIGHT; - // for (size_t i = 0; i < cloud.points.size(); i++) { - // if (cloud.points[i].z < adaptive_seed_selection_margin) { - // init_idx++; - // } else { - // break; - // } - // } - // } + if (ADAPTIVE_SELECTION_EN && zone_idx == 0) { + double adaptive_seed_selection_margin = MH * SENSOR_HEIGHT; + for (size_t i = 0; i < cloud.points.size(); i++) { + if (cloud.points[i].z < adaptive_seed_selection_margin) { + init_idx++; + } else { + break; + } + } + } double sum = 0; int cnt = 0; diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index b9eacf59..b7d52865 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -21,6 +21,7 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment std::vector{0.0005, 0.000725, 0.001, 0.001}); this->declare_parameter>("elevation_thr", std::vector{0.523, 0.746, 0.879, 1.125}); + this->declare_parameter("adaptive_selection_en", bool(false)); this->declare_parameter("lidar_input_topic", std::string("/velodyne_points")); this->declare_parameter("ground_output_topic", std::string("/ground_points")); this->declare_parameter("nonground_output_topic", std::string("/nonground_points")); @@ -41,6 +42,7 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment auto zone_sectors = this->get_parameter("zone_sectors").as_integer_array(); auto flatness_thr = this->get_parameter("flatness_thr").as_double_array(); auto elevation_thr = this->get_parameter("elevation_thr").as_double_array(); + bool adaptive_selection_en = this->get_parameter("adaptive_selection_en").as_bool(); std::string lidar_input_topic = this->get_parameter("lidar_input_topic").as_string(); std::string ground_output_topic = this->get_parameter("ground_output_topic").as_string(); std::string nonground_output_topic = this->get_parameter("nonground_output_topic").as_string(); @@ -48,7 +50,7 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment _patchwork = OccupancySegmentationCore( l_min, l_max, md, mh, min_num_points, num_seed_points, th_seeds, uprightness_thresh, num_rings_of_interest, sensor_height, global_el_thresh, zone_rings, zone_sectors, - flatness_thr, elevation_thr); + flatness_thr, elevation_thr, adaptive_selection_en); _subscriber = this->create_subscription( lidar_input_topic, 10, From 47193cf19061e4f25411bbfe30954d56cfae8c03 Mon Sep 17 00:00:00 2001 From: VishGit1234 Date: Thu, 19 Sep 2024 21:06:10 +0000 Subject: [PATCH 58/59] Put num_zones in params --- .../occupancy_segmentation/config/params.yaml | 1 + .../include/occupancy_segmentation_core.hpp | 20 +++++++++---------- .../include/occupancy_segmentation_node.hpp | 2 +- .../src/occupancy_segmentation_core.cpp | 16 +++++++-------- .../src/occupancy_segmentation_node.cpp | 4 +++- 5 files changed, 21 insertions(+), 22 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/config/params.yaml b/src/world_modeling/occupancy_segmentation/config/params.yaml index dcf6448f..ba955edc 100644 --- a/src/world_modeling/occupancy_segmentation/config/params.yaml +++ b/src/world_modeling/occupancy_segmentation/config/params.yaml @@ -4,6 +4,7 @@ occupancy_segmentation: 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 diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index e93e03d5..316908f1 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -11,8 +11,6 @@ #include #include -#define NUM_ZONES 4 - struct PointXYZIRT { PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding float intensity; @@ -54,8 +52,8 @@ class OccupancySegmentationCore { public: typedef std::vector> Ring; typedef std::vector Zone; - // Size of buffer before processed messages are published. - static constexpr int BUFFER_CAPACITY = 10; + + int NUM_ZONES; float L_MIN; float L_MAX; @@ -73,13 +71,13 @@ class OccupancySegmentationCore { float SENSOR_HEIGHT; float GLOBAL_EL_THRESH; - int ZONE_RINGS[NUM_ZONES]; - int ZONE_SECTORS[NUM_ZONES]; - float FLATNESS_THR[NUM_ZONES]; - float ELEVATION_THR[NUM_ZONES]; - double lmins[NUM_ZONES] = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, + std::vector ZONE_RINGS; + std::vector ZONE_SECTORS; + std::vector FLATNESS_THR; + std::vector ELEVATION_THR; + std::vector lmins = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}; - double lmaxs[NUM_ZONES] = {lmins[1], lmins[2], lmins[3], L_MAX}; + std::vector lmaxs = {lmins[1], lmins[2], lmins[3], L_MAX}; int num_patches = -1; @@ -97,7 +95,7 @@ class OccupancySegmentationCore { std::vector _statuses; OccupancySegmentationCore(); - OccupancySegmentationCore(float l_min, float l_max, float md, float mh, int min_num_points, + 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> &zone_rings, diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp index 5301d7bd..ed71ef67 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_node.hpp @@ -26,7 +26,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT, // here we assume a XYZ + "test" * messages. * * Listens to the "unfiltered" topic and filters out data with invalid fields - * and odd timestamps. Once the node collects BUFFER_CAPACITY messages it packs + * 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 { diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index 635913b6..d6cf81a6 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -5,12 +5,13 @@ template class OccupancySegmentationCore; template OccupancySegmentationCore::OccupancySegmentationCore( - float l_min, float l_max, float md, float mh, int min_num_points, int num_seed_points, + 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 > &zone_rings, std::vector > &zone_sectors, std::vector &flatness_thr, std::vector &elevation_thr, bool adaptive_selection_en) - : L_MIN{l_min}, + : NUM_ZONES{num_zones}, + L_MIN{l_min}, L_MAX{l_max}, MD{md}, MH{mh}, @@ -21,10 +22,10 @@ OccupancySegmentationCore::OccupancySegmentationCore( NUM_RINGS_OF_INTEREST{num_rings_of_interest}, SENSOR_HEIGHT{sensor_height}, GLOBAL_EL_THRESH{global_el_thresh}, - ZONE_RINGS{zone_rings[0], zone_rings[1], zone_rings[2], zone_rings[3]}, - ZONE_SECTORS{zone_sectors[0], zone_sectors[1], zone_sectors[2], zone_sectors[3]}, - FLATNESS_THR{flatness_thr[0], flatness_thr[1], flatness_thr[2], flatness_thr[3]}, - ELEVATION_THR{elevation_thr[0], elevation_thr[1], elevation_thr[2], elevation_thr[3]}, + ZONE_RINGS{zone_rings.begin(), zone_rings.end()}, + ZONE_SECTORS{zone_rings.begin(), zone_sectors.end()}, + FLATNESS_THR{flatness_thr.begin(), flatness_thr.end()}, + ELEVATION_THR{elevation_thr.begin(), elevation_thr.end()}, lmins{L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}, lmaxs{lmins[1], lmins[2], lmins[3], L_MAX}, ADAPTIVE_SELECTION_EN{adaptive_selection_en}{ init_czm(); @@ -152,7 +153,6 @@ void OccupancySegmentationCore::segment_ground(pcl::PointCloud & // TODO error point removal fill_czm(unfiltered_cloud); - std::cout << "Finished filling czm" << std::endl; tbb::parallel_for(tbb::blocked_range(0, num_patches), [&](tbb::blocked_range r) { for (auto patch_num = r.begin(); patch_num != r.end(); patch_num++) { Patch_Index &p_idx = _patch_indices[patch_num]; @@ -176,8 +176,6 @@ void OccupancySegmentationCore::segment_ground(pcl::PointCloud & } }); - std::cout << "Finished estimation" << std::endl; - for (Patch_Index p_idx : _patch_indices) { Status status = _statuses[p_idx.idx]; diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index b7d52865..3ae1715c 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -4,6 +4,7 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segmentation") { // Declare parameters + this->declare_parameter("num_zones", 4); this->declare_parameter("l_min", 2.7); this->declare_parameter("l_max", 80.0); this->declare_parameter("md", 0.3); @@ -27,6 +28,7 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment this->declare_parameter("nonground_output_topic", std::string("/nonground_points")); // Retrieve parameters + int num_zones = this->get_parameter("num_zones").as_int(); double l_min = this->get_parameter("l_min").as_double(); double l_max = this->get_parameter("l_max").as_double(); double md = this->get_parameter("md").as_double(); @@ -48,7 +50,7 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment std::string nonground_output_topic = this->get_parameter("nonground_output_topic").as_string(); _patchwork = OccupancySegmentationCore( - l_min, l_max, md, mh, min_num_points, num_seed_points, th_seeds, uprightness_thresh, + num_zones, l_min, l_max, md, mh, min_num_points, num_seed_points, th_seeds, uprightness_thresh, num_rings_of_interest, sensor_height, global_el_thresh, zone_rings, zone_sectors, flatness_thr, elevation_thr, adaptive_selection_en); From 5915c67f5eebabf332bd12cfeb01c46c81add9ca Mon Sep 17 00:00:00 2001 From: WATonomousAdmin Date: Fri, 20 Sep 2024 17:06:08 +0000 Subject: [PATCH 59/59] Fix code style issues with clang_format --- .../include/occupancy_segmentation_core.hpp | 12 +++++++----- .../src/occupancy_segmentation_core.cpp | 15 +++++++++------ .../src/occupancy_segmentation_node.cpp | 6 +++--- 3 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp index 316908f1..749b4ad9 100644 --- a/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp +++ b/src/world_modeling/occupancy_segmentation/include/occupancy_segmentation_core.hpp @@ -76,7 +76,7 @@ class OccupancySegmentationCore { std::vector FLATNESS_THR; std::vector ELEVATION_THR; std::vector lmins = {L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, - (L_MIN + L_MAX) / 2}; + (L_MIN + L_MAX) / 2}; std::vector lmaxs = {lmins[1], lmins[2], lmins[3], L_MAX}; int num_patches = -1; @@ -95,12 +95,14 @@ class OccupancySegmentationCore { std::vector _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, + 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> &zone_rings, std::vector> &zone_sectors, - std::vector &flatness_thr, std::vector &elevation_thr, bool adaptive_selection_en); + std::vector &flatness_thr, std::vector &elevation_thr, + bool adaptive_selection_en); void segment_ground(pcl::PointCloud &unfiltered_cloud, pcl::PointCloud &ground, pcl::PointCloud &nonground); diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp index d6cf81a6..b590a17c 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_core.cpp @@ -5,11 +5,13 @@ template class OccupancySegmentationCore; template 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 > &zone_rings, + 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 > &zone_rings, std::vector > &zone_sectors, - std::vector &flatness_thr, std::vector &elevation_thr, bool adaptive_selection_en) + std::vector &flatness_thr, std::vector &elevation_thr, + bool adaptive_selection_en) : NUM_ZONES{num_zones}, L_MIN{l_min}, L_MAX{l_max}, @@ -27,7 +29,8 @@ OccupancySegmentationCore::OccupancySegmentationCore( FLATNESS_THR{flatness_thr.begin(), flatness_thr.end()}, ELEVATION_THR{elevation_thr.begin(), elevation_thr.end()}, lmins{L_MIN, (7 * L_MIN + L_MAX) / 8, (3 * L_MIN + L_MAX) / 4, (L_MIN + L_MAX) / 2}, - lmaxs{lmins[1], lmins[2], lmins[3], L_MAX}, ADAPTIVE_SELECTION_EN{adaptive_selection_en}{ + lmaxs{lmins[1], lmins[2], lmins[3], L_MAX}, + ADAPTIVE_SELECTION_EN{adaptive_selection_en} { init_czm(); } @@ -166,7 +169,7 @@ void OccupancySegmentationCore::segment_ground(pcl::PointCloud & if (patch.points.size() > MIN_NUM_POINTS) { std::sort(patch.points.begin(), patch.points.end(), point_z_cmp); rgpf(patch, p_idx, features); - + Status status = ground_likelihood_est(features, p_idx.concentric_idx); _statuses[p_idx.idx] = status; } else { diff --git a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp index 3ae1715c..ebe01238 100644 --- a/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp +++ b/src/world_modeling/occupancy_segmentation/src/occupancy_segmentation_node.cpp @@ -50,9 +50,9 @@ OccupancySegmentationNode::OccupancySegmentationNode() : Node("occupancy_segment std::string nonground_output_topic = this->get_parameter("nonground_output_topic").as_string(); _patchwork = OccupancySegmentationCore( - num_zones, l_min, l_max, md, mh, min_num_points, num_seed_points, th_seeds, uprightness_thresh, - num_rings_of_interest, sensor_height, global_el_thresh, zone_rings, zone_sectors, - flatness_thr, elevation_thr, adaptive_selection_en); + num_zones, l_min, l_max, md, mh, min_num_points, num_seed_points, th_seeds, + uprightness_thresh, num_rings_of_interest, sensor_height, global_el_thresh, zone_rings, + zone_sectors, flatness_thr, elevation_thr, adaptive_selection_en); _subscriber = this->create_subscription( lidar_input_topic, 10,