From b87876bdc00a16129f4a03690ae9b313a9d350e3 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sun, 14 Apr 2024 15:48:11 -0400 Subject: [PATCH 01/12] Update .gitignore --- .gitignore | 219 ++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 217 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 8c61f8b..ee4f0b0 100644 --- a/.gitignore +++ b/.gitignore @@ -1,8 +1,223 @@ .vscode/ build/ - +*/build/ examples/cpp/demo_visualize examples/cpp/demo_sequential -Open3D/ \ No newline at end of file +Open3D/ + +install/ +log/ +results/ +wheelhouse/ +_skbuild/ +.gitlab-ci-local/ + +# Created by https://www.toptal.com/developers/gitignore/api/python,c++ +# Edit at https://www.toptal.com/developers/gitignore?templates=python,c++ +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Python ### +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +cmake-build-release* +cmake-build-debug* +.idea* + +# End of https://www.toptal.com/developers/gitignore/api/python,c++ +*.orig +# Created by https://www.toptal.com/developers/gitignore/api/python +# Edit at https://www.toptal.com/developers/gitignore?templates=python + +### Python ### +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# End of https://www.toptal.com/developers/gitignore/api/python +nn From 0b97848b3274c8e6b09dc87a1ed60197da213b06 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sun, 14 Apr 2024 17:59:08 -0400 Subject: [PATCH 02/12] Update install_open3d.bash (for ubuntu >= 20.04) --- scripts/install_open3d.bash | 32 +++++++++++++++++++++++++++----- 1 file changed, 27 insertions(+), 5 deletions(-) diff --git a/scripts/install_open3d.bash b/scripts/install_open3d.bash index de9da04..7ad63c9 100644 --- a/scripts/install_open3d.bash +++ b/scripts/install_open3d.bash @@ -1,15 +1,37 @@ +#!/usr/bin/env bash set -e -sudo apt-get install libeigen3-dev +SUDO=${SUDO:=sudo} # SUDO=command in docker (running as root, sudo not available) +if [ "$1" == "assume-yes" ]; then + APT_CONFIRM="--assume-yes" +else + APT_CONFIRM="" +fi + +$SUDO apt-get update -y +$SUDO apt-get install git libeigen3-dev python3-pip -y + +# Install CMake > 3.20 for Open3D 0.18.0 version +# Please refer to 'https://apt.kitware.com/' +$SUDO apt-get install ca-certificates gpg wget +test -f /usr/share/doc/kitware-archive-keyring/copyright || +wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | $SUDO tee /usr/share/keyrings/kitware-archive-keyring.gpg >/dev/null + +echo "deb [signed-by=/usr/share/keyrings/kitware-archive-keyring.gpg] https://apt.kitware.com/ubuntu/ $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | $SUDO tee /etc/apt/sources.list.d/kitware.list >/dev/null +$SUDO apt-get update -y +$SUDO apt-get install cmake -y + +# To update the recent version of open3d +python3 -m pip install --upgrade pip pip install numpy -pip install open3d +VERSION=0.18.0 -VERSION=0.15.1 +pip install open3d==$VERSION [ ! -d Open3D ] && git clone https://github.com/isl-org/Open3D.git -b v$VERSION || echo "Skip pulling repo" -cd Open3D && util/install_deps_ubuntu.sh +cd Open3D && bash util/install_deps_ubuntu.sh assume-yes [ ! -d build ] && mkdir build cd build @@ -31,4 +53,4 @@ else fi echo "Applying 'sudo make install'. Enter password" -sudo make install +$SUDO make install From 35e6fe52863df303a4cf88b54188367acbb2940f Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sun, 14 Apr 2024 18:01:00 -0400 Subject: [PATCH 03/12] Update install_open3d.bash (for ubuntu >= 20.04) --- scripts/install_open3d.bash | 32 +++++++++++++++++++++++++++----- 1 file changed, 27 insertions(+), 5 deletions(-) diff --git a/scripts/install_open3d.bash b/scripts/install_open3d.bash index de9da04..7ad63c9 100644 --- a/scripts/install_open3d.bash +++ b/scripts/install_open3d.bash @@ -1,15 +1,37 @@ +#!/usr/bin/env bash set -e -sudo apt-get install libeigen3-dev +SUDO=${SUDO:=sudo} # SUDO=command in docker (running as root, sudo not available) +if [ "$1" == "assume-yes" ]; then + APT_CONFIRM="--assume-yes" +else + APT_CONFIRM="" +fi + +$SUDO apt-get update -y +$SUDO apt-get install git libeigen3-dev python3-pip -y + +# Install CMake > 3.20 for Open3D 0.18.0 version +# Please refer to 'https://apt.kitware.com/' +$SUDO apt-get install ca-certificates gpg wget +test -f /usr/share/doc/kitware-archive-keyring/copyright || +wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | $SUDO tee /usr/share/keyrings/kitware-archive-keyring.gpg >/dev/null + +echo "deb [signed-by=/usr/share/keyrings/kitware-archive-keyring.gpg] https://apt.kitware.com/ubuntu/ $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | $SUDO tee /etc/apt/sources.list.d/kitware.list >/dev/null +$SUDO apt-get update -y +$SUDO apt-get install cmake -y + +# To update the recent version of open3d +python3 -m pip install --upgrade pip pip install numpy -pip install open3d +VERSION=0.18.0 -VERSION=0.15.1 +pip install open3d==$VERSION [ ! -d Open3D ] && git clone https://github.com/isl-org/Open3D.git -b v$VERSION || echo "Skip pulling repo" -cd Open3D && util/install_deps_ubuntu.sh +cd Open3D && bash util/install_deps_ubuntu.sh assume-yes [ ! -d build ] && mkdir build cd build @@ -31,4 +53,4 @@ else fi echo "Applying 'sudo make install'. Enter password" -sudo make install +$SUDO make install From fe6f631000b39cc81bd0a12113c48add22a0d757 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sun, 14 Apr 2024 18:01:40 -0400 Subject: [PATCH 04/12] Reorders files for ROS2 compatibility --- CMakeLists.txt => cpp/CMakeLists.txt | 15 ++-------- cpp/COLCON_IGNORE | 0 {cmake => cpp/cmake}/ListPrepend.cmake | 0 {cmake => cpp/cmake}/pybind11.cmake | 0 .../patchworkpp}/CMakeLists.txt | 0 .../patchworkpp/examples}/demo_sequential.cpp | 0 .../patchworkpp/examples}/demo_visualize.cpp | 0 .../patchworkpp}/include/patchworkpp.h | 0 .../patchworkpp}/src/patchworkpp.cpp | 0 python/CMakeLists.txt | 29 +++++++++++++++++++ python/COLCON_IGNORE | 0 .../examples}/demo_sequential.py | 0 .../examples}/demo_visualize.py | 0 .../patchworkpp}/pybinding.cpp | 0 pyproject.toml => python/pyproject.toml | 0 python_wrapper/CMakeLists.txt | 11 ------- 16 files changed, 31 insertions(+), 24 deletions(-) rename CMakeLists.txt => cpp/CMakeLists.txt (78%) create mode 100644 cpp/COLCON_IGNORE rename {cmake => cpp/cmake}/ListPrepend.cmake (100%) rename {cmake => cpp/cmake}/pybind11.cmake (100%) rename {patchworkpp => cpp/patchworkpp}/CMakeLists.txt (100%) rename {examples/cpp => cpp/patchworkpp/examples}/demo_sequential.cpp (100%) rename {examples/cpp => cpp/patchworkpp/examples}/demo_visualize.cpp (100%) rename {patchworkpp => cpp/patchworkpp}/include/patchworkpp.h (100%) rename {patchworkpp => cpp/patchworkpp}/src/patchworkpp.cpp (100%) create mode 100644 python/CMakeLists.txt create mode 100644 python/COLCON_IGNORE rename {examples/python => python/examples}/demo_sequential.py (100%) rename {examples/python => python/examples}/demo_visualize.py (100%) rename {python_wrapper => python/patchworkpp}/pybinding.cpp (100%) rename pyproject.toml => python/pyproject.toml (100%) delete mode 100644 python_wrapper/CMakeLists.txt diff --git a/CMakeLists.txt b/cpp/CMakeLists.txt similarity index 78% rename from CMakeLists.txt rename to cpp/CMakeLists.txt index 49ced5f..7ff7848 100755 --- a/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -22,19 +22,8 @@ list(APPEND Open3D_LIBRARIES dl) add_subdirectory(patchworkpp) -set(INCLUDE_PYTHON_WRAPPER OFF CACHE BOOL "Build Python wrapper") set(INCLUDE_EXAMPLES ON CACHE BOOL "Build examples") -if (INCLUDE_PYTHON_WRAPPER) - - message(STATUS "Building Python wrapper") - find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module) - include(FetchContent) - include(pybind11) - add_subdirectory(python_wrapper) - -endif() - if (INCLUDE_EXAMPLES) message(STATUS "Building examples for c++") @@ -44,12 +33,12 @@ if (INCLUDE_EXAMPLES) link_directories(${Open3D_LIBRARY_DIRS}) message(STATUS "Found Open3D ${Open3D_VERSION}") - add_executable(demo_visualize examples/cpp/demo_visualize.cpp) + add_executable(demo_visualize ${CMAKE_CURRENT_SOURCE_DIR}/patchworkpp/examples/demo_visualize.cpp) target_link_libraries(demo_visualize PRIVATE PATCHWORK::patchworkpp ${Open3D_LIBRARIES} "stdc++fs") target_include_directories(demo_visualize PUBLIC ${Open3D_INCLUDE_DIRS}) set_target_properties(demo_visualize PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/build/examples/cpp") - add_executable(demo_sequential examples/cpp/demo_sequential.cpp) + add_executable(demo_sequential ${CMAKE_CURRENT_SOURCE_DIR}/patchworkpp/examples/demo_sequential.cpp) target_link_libraries(demo_sequential PRIVATE PATCHWORK::patchworkpp ${Open3D_LIBRARIES} "stdc++fs") target_include_directories(demo_sequential PUBLIC ${Open3D_INCLUDE_DIRS}) set_target_properties(demo_sequential PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/build/examples/cpp") diff --git a/cpp/COLCON_IGNORE b/cpp/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/cmake/ListPrepend.cmake b/cpp/cmake/ListPrepend.cmake similarity index 100% rename from cmake/ListPrepend.cmake rename to cpp/cmake/ListPrepend.cmake diff --git a/cmake/pybind11.cmake b/cpp/cmake/pybind11.cmake similarity index 100% rename from cmake/pybind11.cmake rename to cpp/cmake/pybind11.cmake diff --git a/patchworkpp/CMakeLists.txt b/cpp/patchworkpp/CMakeLists.txt similarity index 100% rename from patchworkpp/CMakeLists.txt rename to cpp/patchworkpp/CMakeLists.txt diff --git a/examples/cpp/demo_sequential.cpp b/cpp/patchworkpp/examples/demo_sequential.cpp similarity index 100% rename from examples/cpp/demo_sequential.cpp rename to cpp/patchworkpp/examples/demo_sequential.cpp diff --git a/examples/cpp/demo_visualize.cpp b/cpp/patchworkpp/examples/demo_visualize.cpp similarity index 100% rename from examples/cpp/demo_visualize.cpp rename to cpp/patchworkpp/examples/demo_visualize.cpp diff --git a/patchworkpp/include/patchworkpp.h b/cpp/patchworkpp/include/patchworkpp.h similarity index 100% rename from patchworkpp/include/patchworkpp.h rename to cpp/patchworkpp/include/patchworkpp.h diff --git a/patchworkpp/src/patchworkpp.cpp b/cpp/patchworkpp/src/patchworkpp.cpp similarity index 100% rename from patchworkpp/src/patchworkpp.cpp rename to cpp/patchworkpp/src/patchworkpp.cpp diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 0000000..af01c64 --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.16...3.26) +project(patchworkpp_python_wrapper LANGUAGES CXX) + +# Set build type +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(Python COMPONENTS Interpreter Development.Module REQUIRED) +find_package(pybind11 CONFIG REQUIRED) + +if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/) + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp ${CMAKE_CURRENT_BINARY_DIR}/patchworkpp_cpp) +else() + cmake_minimum_required(VERSION 3.18) + message(STATUS "Performing out-of-tree build, fetching Patchwork++ v${CMAKE_PROJECT_VERSION} Release from Github") + include(FetchContent) + ## To be update ## +endif() + +pybind11_add_module(pypatchworkpp patchworkpp/pybinding.cpp) + +target_link_libraries(pypatchworkpp PUBLIC PATCHWORK::patchworkpp) + +if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + target_compile_options(pypatchworkpp PUBLIC -fsized-deallocation) +endif() + +install(TARGETS pypatchworkpp DESTINATION .) diff --git a/python/COLCON_IGNORE b/python/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/examples/python/demo_sequential.py b/python/examples/demo_sequential.py similarity index 100% rename from examples/python/demo_sequential.py rename to python/examples/demo_sequential.py diff --git a/examples/python/demo_visualize.py b/python/examples/demo_visualize.py similarity index 100% rename from examples/python/demo_visualize.py rename to python/examples/demo_visualize.py diff --git a/python_wrapper/pybinding.cpp b/python/patchworkpp/pybinding.cpp similarity index 100% rename from python_wrapper/pybinding.cpp rename to python/patchworkpp/pybinding.cpp diff --git a/pyproject.toml b/python/pyproject.toml similarity index 100% rename from pyproject.toml rename to python/pyproject.toml diff --git a/python_wrapper/CMakeLists.txt b/python_wrapper/CMakeLists.txt deleted file mode 100644 index 2559f85..0000000 --- a/python_wrapper/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -project(patchworkpp_python_wrapper LANGUAGES CXX) - -pybind11_add_module(pypatchworkpp pybinding.cpp) - -target_link_libraries(pypatchworkpp PUBLIC PATCHWORK::patchworkpp) - -if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - target_compile_options(pypatchworkpp PUBLIC -fsized-deallocation) -endif() - -install(TARGETS pypatchworkpp DESTINATION .) From 287639e59020083dc8ead738234ddb573a88e1cd Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sun, 14 Apr 2024 18:06:26 -0400 Subject: [PATCH 05/12] (minor) add `-y` option during `apt-get install` --- scripts/install_open3d.bash | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/install_open3d.bash b/scripts/install_open3d.bash index 7ad63c9..3bac3af 100644 --- a/scripts/install_open3d.bash +++ b/scripts/install_open3d.bash @@ -13,7 +13,7 @@ $SUDO apt-get install git libeigen3-dev python3-pip -y # Install CMake > 3.20 for Open3D 0.18.0 version # Please refer to 'https://apt.kitware.com/' -$SUDO apt-get install ca-certificates gpg wget +$SUDO apt-get install ca-certificates gpg wget -y test -f /usr/share/doc/kitware-archive-keyring/copyright || wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | $SUDO tee /usr/share/keyrings/kitware-archive-keyring.gpg >/dev/null From 8c741f2c6c6436c7bf83a49467a89b74e426798b Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sun, 14 Apr 2024 20:41:40 -0400 Subject: [PATCH 06/12] Set appropriate locations of `data` directory in both CMakeLists.txt and cpp files --- cpp/CMakeLists.txt | 6 +++--- cpp/patchworkpp/examples/demo_sequential.cpp | 2 +- cpp/patchworkpp/examples/demo_visualize.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index 7ff7848..e079838 100755 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.11) project(PATCHWORK VERSION 1.0.0) -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 20) set(PYTHON_EXECUTABLE python3) set(CMAKE_BUILD_TYPE Release) @@ -36,11 +36,11 @@ if (INCLUDE_EXAMPLES) add_executable(demo_visualize ${CMAKE_CURRENT_SOURCE_DIR}/patchworkpp/examples/demo_visualize.cpp) target_link_libraries(demo_visualize PRIVATE PATCHWORK::patchworkpp ${Open3D_LIBRARIES} "stdc++fs") target_include_directories(demo_visualize PUBLIC ${Open3D_INCLUDE_DIRS}) - set_target_properties(demo_visualize PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/build/examples/cpp") + set_target_properties(demo_visualize PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/examples") add_executable(demo_sequential ${CMAKE_CURRENT_SOURCE_DIR}/patchworkpp/examples/demo_sequential.cpp) target_link_libraries(demo_sequential PRIVATE PATCHWORK::patchworkpp ${Open3D_LIBRARIES} "stdc++fs") target_include_directories(demo_sequential PUBLIC ${Open3D_INCLUDE_DIRS}) - set_target_properties(demo_sequential PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/build/examples/cpp") + set_target_properties(demo_sequential PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/examples") endif() diff --git a/cpp/patchworkpp/examples/demo_sequential.cpp b/cpp/patchworkpp/examples/demo_sequential.cpp index 7c6c1dd..558c1b6 100644 --- a/cpp/patchworkpp/examples/demo_sequential.cpp +++ b/cpp/patchworkpp/examples/demo_sequential.cpp @@ -11,7 +11,7 @@ using namespace open3d; int filename_length = std::string("demo_sequential.cpp").length(); std::string file_dir = std::string(__FILE__); -std::string data_dir = file_dir.substr(0, file_dir.size()-filename_length) + "../../data/"; +std::string data_dir = file_dir.substr(0, file_dir.size()-filename_length) + "../../../data/"; void read_bin(std::string bin_path, Eigen::MatrixXf &cloud) { diff --git a/cpp/patchworkpp/examples/demo_visualize.cpp b/cpp/patchworkpp/examples/demo_visualize.cpp index ef5ba64..43dee44 100644 --- a/cpp/patchworkpp/examples/demo_visualize.cpp +++ b/cpp/patchworkpp/examples/demo_visualize.cpp @@ -12,7 +12,7 @@ using namespace open3d; int filename_length = std::string("demo_visaulize.cpp").length(); std::string file_dir = std::string(__FILE__); -std::string data_dir = file_dir.substr(0, file_dir.size()-filename_length) + "../../data/"; +std::string data_dir = file_dir.substr(0, file_dir.size()-filename_length) + "../../../data/"; void read_bin(std::string bin_path, Eigen::MatrixXf &cloud) From f88fba51f423126a99aa3994083728457220c885 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sun, 14 Apr 2024 20:42:12 -0400 Subject: [PATCH 07/12] Update README.md about running commands of C++ and Python --- README.md | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index db28226..a86d1e4 100644 --- a/README.md +++ b/README.md @@ -73,13 +73,13 @@ $ sudo make install ### Python ```bash # in patchwork-plusplus directory -$ pip install . +$ cd python && pip install . ``` ### C++ ```bash # in patchwork-plusplus directory -$ mkdir build && cd build +$ mkdir cpp/build && cd cpp/build $ cmake .. $ make ``` @@ -91,22 +91,22 @@ $ make ### Python ```bash # Run patchwork++ and visualize ground points(green) and nonground points(red) -$ python examples/python/demo_visualize.py +$ python examples/demo_visualize.py # Run patchwork++ with sequential point cloud inputs -$ python examples/python/demo_sequential.py +$ python examples/demo_sequential.py ``` ### C++ ```bash # Run patchwork++ and visualize ground points(green) and nonground points(red) -$ ./build/examples/cpp/demo_visualize +$ ./examples/demo_visualize # Run patchwork++ with sequential point cloud inputs -$ ./build/examples/cpp/demo_sequential +$ ./examples/demo_sequential # Run patchwork++ with your point cloud file, example here -$ ./build/examples/cpp/demo_visualize ./data/000000.bin # specify file path +$ ./examples/demo_visualize ./data/000000.bin # specify file path ``` ### Demo Result @@ -143,17 +143,18 @@ In addition, you can also check the paper of our baseline, Patchwork. ([arXiv][p ## :triangular_flag_on_post: Tested Environment -- Ubuntu 18.04 and 20.04 +- Ubuntu ~~18.04 and~~ 20.04 and 22.04 - CMake 3.25.1 (>=3.20, min. Required to install Open3D) -- Open3D 0.15.2 + - In `scripts/install_open3d.bash`, the installation of the higher version of CMake is already implemented. +- Open3D ~~0.15.2~~ 0.18.0 - pybind11 v2.2.3 - Eigen 3.3.7 ## :mailbox: Contact Information If you have any questions, please do not hesitate to contact us -* [Seungjae Lee][sjlink] :envelope: `sj98lee at kaist.ac.kr` -* [Hyungtae Lim][htlink] :envelope: `shapelim at kaist.ac.kr` +* [Seungjae Lee][sjlink] :envelope: sj98lee `at` kaist `ac` kr +* [Hyungtae Lim][htlink] :envelope: shapelim `at` kaist `ac` kr [sjlink]: https://github.com/seungjae24 [htlink]: https://github.com/LimHyungTae From fe276b70b90778bb59885a52f479b660449e28da Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sun, 14 Apr 2024 23:24:00 -0400 Subject: [PATCH 08/12] Init to support ROS2 --- ros/CMakeLists.txt | 46 +++++ ros/LICENSE | 22 +++ ros/README.md | 32 ++++ ros/launch/patchworkpp.launch.py | 76 ++++++++ ros/package.xml | 26 +++ ros/rviz/patchworkpp.rviz | 269 +++++++++++++++++++++++++++ ros/src/GroundSegmentationServer.cpp | 77 ++++++++ ros/src/GroundSegmentationServer.hpp | 41 ++++ ros/src/Utils.hpp | 197 ++++++++++++++++++++ 9 files changed, 786 insertions(+) create mode 100644 ros/CMakeLists.txt create mode 100644 ros/LICENSE create mode 100644 ros/README.md create mode 100644 ros/launch/patchworkpp.launch.py create mode 100644 ros/package.xml create mode 100644 ros/rviz/patchworkpp.rviz create mode 100644 ros/src/GroundSegmentationServer.cpp create mode 100644 ros/src/GroundSegmentationServer.hpp create mode 100644 ros/src/Utils.hpp diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt new file mode 100644 index 0000000..d12fc60 --- /dev/null +++ b/ros/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.16...3.26) +project(patchworkpp VERSION 0.4.0 LANGUAGES CXX) + +set(ignore ${CATKIN_INSTALL_INTO_PREFIX_ROOT}) +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/) + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp ${CMAKE_CURRENT_BINARY_DIR}/patchworkpp_cpp) +else() + cmake_minimum_required(VERSION 3.18) + message(STATUS "Performing out-of-tree build, fetching Patchwork++ v${CMAKE_PROJECT_VERSION} Release from Github") + include(FetchContent) + # Currently, out-of-source build is not supported. + # But, it will be updated soon! +endif() + +find_package(ament_cmake REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rcutils REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +# ROS 2 node. gseg: Abbrev. of `g`round `seg`mentation +add_library(gseg_component SHARED src/GroundSegmentationServer.cpp) +target_compile_features(gseg_component PUBLIC cxx_std_20) +target_include_directories(gseg_component PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_link_libraries(gseg_component PATCHWORK::patchworkpp) +ament_target_dependencies( + gseg_component + rcutils + rclcpp + rclcpp_components + nav_msgs + sensor_msgs + geometry_msgs + tf2_ros) + +rclcpp_components_register_node(gseg_component PLUGIN "patchworkpp_ros::GroundSegmentationServer" EXECUTABLE patchworkpp_node) + +install(TARGETS gseg_component LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}/) + +ament_package() diff --git a/ros/LICENSE b/ros/LICENSE new file mode 100644 index 0000000..d94e830 --- /dev/null +++ b/ros/LICENSE @@ -0,0 +1,22 @@ +MIT License + +Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill +Stachniss. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/ros/README.md b/ros/README.md new file mode 100644 index 0000000..9b638c6 --- /dev/null +++ b/ros/README.md @@ -0,0 +1,32 @@ +# Patchwork++ ROS 2 Wrapper + +### How to build + +You should not need any extra dependency, just clone and build: + +```sh +git clone https://github.com/url-kaist/patchwork-plusplus.git +colcon build --packages-select patchworkpp +source ./install/setup.bash +``` + +### How to run + +The only required argument to provide is the **topic name** so Patchwork++ knows which PointCloud2 to process: + +```sh +ros2 launch kiss_icp odometry.launch.py bagfile:= topic:= +``` + +You can optionally launch the node with any bagfile, and play the bagfiles on a different shell: + +```sh +ros2 launch patchworkpp.launch.py visualize:= use_sim_time:= topic:= +``` + +and then, + +```sh +ros2 bag play *.bag +``` + diff --git a/ros/launch/patchworkpp.launch.py b/ros/launch/patchworkpp.launch.py new file mode 100644 index 0000000..c57c94e --- /dev/null +++ b/ros/launch/patchworkpp.launch.py @@ -0,0 +1,76 @@ +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +# This configuration parameters are not exposed thorugh the launch system, meaning you can't modify +# those throw the ros launch CLI. If you need to change these values, you could write your own +# launch file and modify the 'parameters=' block from the Node class. +class config: + # TBU. Examples are as follows: + max_range: float = 80.0 + # deskew: bool = False + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time", default="true") + + # tf tree configuration, these are the likely 3 parameters to change and nothing else + base_frame = LaunchConfiguration("base_frame", default="base_link") + + # ROS configuration + pointcloud_topic = LaunchConfiguration("topic") + visualize = LaunchConfiguration("visualize", default="true") + + # Optional ros bag play + bagfile = LaunchConfiguration("bagfile", default="") + + # Patchwork++ node + patchworkpp_node = Node( + package="patchworkpp", + executable="patchworkpp_node", + name="patchworkpp_node", + output="screen", + remappings=[ + ("pointcloud_topic", pointcloud_topic), + ], + parameters=[ + { + # ROS node configuration + "base_frame": base_frame, + # Patchwork++ configuration + # TBU + "use_sim_time": use_sim_time, + }, + ], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + output="screen", + arguments=[ + "-d", + PathJoinSubstitution([FindPackageShare("patchworkpp"), "rviz", "patchworkpp.rviz"]), + ], + condition=IfCondition(visualize), + ) + + bagfile_play = ExecuteProcess( + cmd=["ros2", "bag", "play", bagfile], + output="screen", + condition=IfCondition(PythonExpression(["'", bagfile, "' != ''"])), + ) + return LaunchDescription( + [ + patchworkpp_node, + rviz_node, + bagfile_play, + ] + ) diff --git a/ros/package.xml b/ros/package.xml new file mode 100644 index 0000000..1636bc1 --- /dev/null +++ b/ros/package.xml @@ -0,0 +1,26 @@ + + + patchworkpp + 0.1.0 + Patchwork++ ROS 2 Wrapper + Seungjae Lee + GPL-3.0 + + ament_cmake + + rcutils + rclcpp + rclcpp_components + geometry_msgs + nav_msgs + std_msgs + sensor_msgs + tf2 + tf2_ros + + ros2launch + + + ament_cmake + + diff --git a/ros/rviz/patchworkpp.rviz b/ros/rviz/patchworkpp.rviz new file mode 100644 index 0000000..0bf3084 --- /dev/null +++ b/ros/rviz/patchworkpp.rviz @@ -0,0 +1,269 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 752 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 61; 229; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: frame_deskew + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /kiss/frame + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 61; 229; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: kiss_keypoints + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /kiss/keypoints + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 14.369325637817383 + Min Value: 0.20239250361919403 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 102; 102; 102 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: local_map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /kiss/local_map + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: point_clouds + - Class: rviz_common/Group + Displays: + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /kiss/odometry + Value: true + Enabled: false + Name: odometry + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: odom_frame + Radius: 0.10000000149011612 + Reference Frame: odom + Value: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 178.319580078125 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -68.01193237304688 + Y: 258.05126953125 + Z: -27.22064781188965 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.49020177125930786 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 3.340390920639038 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1043 + Hide Left Dock: true + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 0 diff --git a/ros/src/GroundSegmentationServer.cpp b/ros/src/GroundSegmentationServer.cpp new file mode 100644 index 0000000..ee45197 --- /dev/null +++ b/ros/src/GroundSegmentationServer.cpp @@ -0,0 +1,77 @@ +#include +#include +#include +#include + +// Patchwork++-ROS +#include "GroundSegmentationServer.hpp" +#include "Utils.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace patchworkpp_ros { + +using utils::EigenToPointCloud2; +using utils::GetTimestamps; +using utils::PointCloud2ToEigen; + +GroundSegmentationServer::GroundSegmentationServer(const rclcpp::NodeOptions &options) + : rclcpp::Node("patchworkpp_node", options) { + + patchwork::Params patchwork_parameters; + base_frame_ = declare_parameter("base_frame", base_frame_); + + patchwork_parameters.verbose = true; + + // Construct the main Patchwork++ node + Patchworkpp_ = std::make_unique(patchwork_parameters); + + // Initialize subscribers + pointcloud_sub_ = create_subscription( + "pointcloud_topic", rclcpp::SensorDataQoS(), + std::bind(&GroundSegmentationServer::EstimateGround, this, std::placeholders::_1)); + + // Initialize publishers + rclcpp::QoS qos((rclcpp::SystemDefaultsQoS().keep_last(1).durability_volatile())); + ground_publisher_ = create_publisher("/patchworkpp/ground", qos); + nonground_publisher_ = create_publisher("/patchworkpp/non_ground", qos); + + RCLCPP_INFO(this->get_logger(), "Patchwork++ ROS 2 node initialized"); +} + +void GroundSegmentationServer::EstimateGround(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { + const auto cloud_frame_id = msg->header.frame_id; + const auto points = PointCloud2ToEigen(msg); + const auto timestamps = GetTimestamps(msg); + + const auto &cloud = patchworkpp_ros::utils::PointCloud2ToEigenMat(msg); + + // Estimate Ground + Patchworkpp_->estimateGround(cloud); + + // Get Ground and Nonground + Eigen::MatrixX3f ground = Patchworkpp_->getGround(); + Eigen::MatrixX3f nonground = Patchworkpp_->getNonground(); + double time_taken = Patchworkpp_->getTimeTaken(); + PublishClouds(ground, nonground, msg->header); +} + +void GroundSegmentationServer::PublishClouds(const Eigen::MatrixX3f &est_ground, + const Eigen::MatrixX3f &est_nonground, + const std_msgs::msg::Header header_msg) { + + std_msgs::msg::Header header = header_msg; + header.frame_id = base_frame_; + ground_publisher_->publish(std::move(patchworkpp_ros::utils::EigenMatToPointCloud2(est_ground, header))); + nonground_publisher_->publish(std::move(patchworkpp_ros::utils::EigenMatToPointCloud2(est_nonground, header))); +} +} // namespace patchworkpp_ros + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(patchworkpp_ros::GroundSegmentationServer) diff --git a/ros/src/GroundSegmentationServer.hpp b/ros/src/GroundSegmentationServer.hpp new file mode 100644 index 0000000..cd8370c --- /dev/null +++ b/ros/src/GroundSegmentationServer.hpp @@ -0,0 +1,41 @@ +// Patchwork++ +#include "patchworkpp.h" + +// ROS 2 +#include +#include +#include +#include + +namespace patchworkpp_ros { + +class GroundSegmentationServer : public rclcpp::Node { + public: + /// GroundSegmentationServer constructor + GroundSegmentationServer() = delete; + explicit GroundSegmentationServer(const rclcpp::NodeOptions &options); + + private: + /// Register new frame + void EstimateGround(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + + /// Stream the point clouds for visualization + void PublishClouds(const Eigen::MatrixX3f &est_ground, + const Eigen::MatrixX3f &est_nonground, + const std_msgs::msg::Header header_msg); + private: + + /// Data subscribers. + rclcpp::Subscription::SharedPtr pointcloud_sub_; + + /// Data publishers. + rclcpp::Publisher::SharedPtr ground_publisher_; + rclcpp::Publisher::SharedPtr nonground_publisher_; + + /// Patchwork++ + std::unique_ptr Patchworkpp_; + + std::string base_frame_{"base_link"}; +}; + +} // namespace patchworkpp_ros diff --git a/ros/src/Utils.hpp b/ros/src/Utils.hpp new file mode 100644 index 0000000..eef0d53 --- /dev/null +++ b/ros/src/Utils.hpp @@ -0,0 +1,197 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +// ROS 2 +#include +#include +#include +#include + +namespace patchworkpp_ros::utils { + +using PointCloud2 = sensor_msgs::msg::PointCloud2; +using PointField = sensor_msgs::msg::PointField; +using Header = std_msgs::msg::Header; + +inline std::string FixFrameId(const std::string &frame_id) { + return std::regex_replace(frame_id, std::regex("^/"), ""); +} + +inline std::optional GetTimestampField(const PointCloud2::ConstSharedPtr msg) { + PointField timestamp_field; + for (const auto &field : msg->fields) { + if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) { + timestamp_field = field; + } + } + if (timestamp_field.count) return timestamp_field; + RCLCPP_WARN_ONCE(rclcpp::get_logger("patchworkpp_node"), + "Field 't', 'timestamp', or 'time' does not exist. " + "Disabling scan deskewing"); + return {}; +} + +// Normalize timestamps from 0.0 to 1.0 +inline auto NormalizeTimestamps(const std::vector ×tamps) { + const auto [min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); + const double min_timestamp = *min_it; + const double max_timestamp = *max_it; + + std::vector timestamps_normalized(timestamps.size()); + std::transform(timestamps.cbegin(), timestamps.cend(), timestamps_normalized.begin(), + [&](const auto ×tamp) { + return (timestamp - min_timestamp) / (max_timestamp - min_timestamp); + }); + return timestamps_normalized; +} + +inline auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, + const PointField ×tamp_field) { + auto extract_timestamps = + [&msg](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector { + const size_t n_points = msg->height * msg->width; + std::vector timestamps; + timestamps.reserve(n_points); + for (size_t i = 0; i < n_points; ++i, ++it) { + timestamps.emplace_back(static_cast(*it)); + } + return NormalizeTimestamps(timestamps); + }; + + // According to the type of the timestamp == type, return a PointCloud2ConstIterator + using sensor_msgs::PointCloud2ConstIterator; + if (timestamp_field.datatype == PointField::UINT32) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } else if (timestamp_field.datatype == PointField::FLOAT32) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } else if (timestamp_field.datatype == PointField::FLOAT64) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } + + // timestamp type not supported, please open an issue :) + throw std::runtime_error("timestamp field type not supported"); +} + +inline std::unique_ptr CreatePointCloud2Msg(const size_t n_points, + const Header &header, + bool timestamp = false) { + auto cloud_msg = std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*cloud_msg); + cloud_msg->header = header; + cloud_msg->header.frame_id = FixFrameId(cloud_msg->header.frame_id); + cloud_msg->fields.clear(); + int offset = 0; + offset = addPointField(*cloud_msg, "x", 1, PointField::FLOAT32, offset); + offset = addPointField(*cloud_msg, "y", 1, PointField::FLOAT32, offset); + offset = addPointField(*cloud_msg, "z", 1, PointField::FLOAT32, offset); + offset += sizeOfPointField(PointField::FLOAT32); + if (timestamp) { + // assuming timestamp on a velodyne fashion for now (between 0.0 and 1.0) + offset = addPointField(*cloud_msg, "time", 1, PointField::FLOAT64, offset); + offset += sizeOfPointField(PointField::FLOAT64); + } + + // Resize the point cloud accordingly + cloud_msg->point_step = offset; + cloud_msg->row_step = cloud_msg->width * cloud_msg->point_step; + cloud_msg->data.resize(cloud_msg->height * cloud_msg->row_step); + modifier.resize(n_points); + return cloud_msg; +} + +inline void FillPointCloud2XYZ(const std::vector &points, PointCloud2 &msg) { + sensor_msgs::PointCloud2Iterator msg_x(msg, "x"); + sensor_msgs::PointCloud2Iterator msg_y(msg, "y"); + sensor_msgs::PointCloud2Iterator msg_z(msg, "z"); + for (size_t i = 0; i < points.size(); i++, ++msg_x, ++msg_y, ++msg_z) { + const Eigen::Vector3d &point = points[i]; + *msg_x = point.x(); + *msg_y = point.y(); + *msg_z = point.z(); + } +} + +inline void FillPointCloud2XYZ(const Eigen::MatrixX3f &points, PointCloud2 &msg) { + sensor_msgs::PointCloud2Iterator msg_x(msg, "x"); + sensor_msgs::PointCloud2Iterator msg_y(msg, "y"); + sensor_msgs::PointCloud2Iterator msg_z(msg, "z"); + for (size_t i = 0; i < points.size(); ++i, ++msg_x, ++msg_y, ++msg_z) { + *msg_x = points(i, 0); + *msg_y = points(i, 1); + *msg_z = points(i, 2); + } +} + +inline void FillPointCloud2Timestamp(const std::vector ×tamps, PointCloud2 &msg) { + sensor_msgs::PointCloud2Iterator msg_t(msg, "time"); + for (size_t i = 0; i < timestamps.size(); i++, ++msg_t) *msg_t = timestamps[i]; +} + +inline std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg) { + auto timestamp_field = GetTimestampField(msg); + if (!timestamp_field.has_value()) return {}; + + // Extract timestamps from cloud_msg + std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field.value()); + + return timestamps; +} + +inline std::vector PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg) { + std::vector points; + points.reserve(msg->height * msg->width); + sensor_msgs::PointCloud2ConstIterator msg_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator msg_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator msg_z(*msg, "z"); + for (size_t i = 0; i < msg->height * msg->width; ++i, ++msg_x, ++msg_y, ++msg_z) { + points.emplace_back(*msg_x, *msg_y, *msg_z); + } + return points; +} + +inline Eigen::MatrixXf PointCloud2ToEigenMat(const PointCloud2::ConstSharedPtr &msg) { + sensor_msgs::PointCloud2ConstIterator msg_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator msg_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator msg_z(*msg, "z"); + + Eigen::MatrixXf points; + size_t num_points = msg->height * msg->width; + points.resize(num_points, 3); + + for (size_t i = 0; i < num_points; ++i, ++msg_x, ++msg_y, ++msg_z) { + points.row(i) << *msg_x, *msg_y, *msg_z; + } + + return points; +} + +inline std::unique_ptr EigenToPointCloud2(const std::vector &points, + const Header &header) { + auto msg = CreatePointCloud2Msg(points.size(), header); + FillPointCloud2XYZ(points, *msg); + return msg; +} + +inline std::unique_ptr EigenToPointCloud2(const std::vector &points, + const std::vector ×tamps, + const Header &header) { + auto msg = CreatePointCloud2Msg(points.size(), header, true); + FillPointCloud2XYZ(points, *msg); + FillPointCloud2Timestamp(timestamps, *msg); + return msg; +} + +inline std::unique_ptr EigenMatToPointCloud2(const Eigen::MatrixX3f &points, + const Header &header) { + auto msg = CreatePointCloud2Msg(points.size(), header); + FillPointCloud2XYZ(points, *msg); + return msg; +} +} // namespace patchworkpp::utils + From 633346d952fccae329611a02b80bc013bceeb5b9 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sun, 14 Apr 2024 23:24:28 -0400 Subject: [PATCH 09/12] (minor) Update comments and README.md --- README.md | 24 +++++++++++++++++++++--- python/CMakeLists.txt | 3 ++- 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index a86d1e4..c6c4526 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,29 @@ -# Patchwork++ +
+

Patchwork++

+ + + +
+
+ Video +   •   + Install +   •   + ROS 2 +   •   + Paper +   •   + Contact Us +
+
+

animated

+
+ ## :bookmark_tabs: About Patchwork++ (IROS'22) * A fast, robust, and self-adaptive **ground segmentation algorithm** on 3D point cloud. -

animated

- * An extension of [Patchwork][patchworklink] (RA-L'21 with IROS'21). * Please refer our [paper][patchworkppIEEElink] for detailed explanantions and experimental results! diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index af01c64..f4caaf9 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -15,7 +15,8 @@ else() cmake_minimum_required(VERSION 3.18) message(STATUS "Performing out-of-tree build, fetching Patchwork++ v${CMAKE_PROJECT_VERSION} Release from Github") include(FetchContent) - ## To be update ## + # Currently, out-of-source build is not supported. + # But, it will be updated soon! endif() pybind11_add_module(pypatchworkpp patchworkpp/pybinding.cpp) From dc0599eb5a4acc4d146346c71233b01a88c02c4e Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Thu, 30 May 2024 13:25:07 -0400 Subject: [PATCH 10/12] Initialize ROS2 support --- ros/launch/patchworkpp.launch.py | 22 +- ros/rviz/patchworkpp.rviz | 321 +++++++++++++-------------- ros/src/GroundSegmentationServer.cpp | 47 ++-- ros/src/GroundSegmentationServer.hpp | 1 + ros/src/Utils.hpp | 2 +- 5 files changed, 203 insertions(+), 190 deletions(-) diff --git a/ros/launch/patchworkpp.launch.py b/ros/launch/patchworkpp.launch.py index c57c94e..e3be82e 100644 --- a/ros/launch/patchworkpp.launch.py +++ b/ros/launch/patchworkpp.launch.py @@ -26,7 +26,7 @@ def generate_launch_description(): base_frame = LaunchConfiguration("base_frame", default="base_link") # ROS configuration - pointcloud_topic = LaunchConfiguration("topic") + pointcloud_topic = LaunchConfiguration("cloud_topic") visualize = LaunchConfiguration("visualize", default="true") # Optional ros bag play @@ -45,10 +45,24 @@ def generate_launch_description(): { # ROS node configuration "base_frame": base_frame, - # Patchwork++ configuration - # TBU "use_sim_time": use_sim_time, - }, + # Patchwork++ configuration + 'sensor_height': 1.88, + 'num_iter': 3, # Number of iterations for ground plane estimation using PCA. + 'num_lpr': 20, # Maximum number of points to be selected as lowest points representative. + 'num_min_pts': 0, # Minimum number of points to be estimated as ground plane in each patch. + 'th_seeds': 0.3, + # threshold for lowest point representatives using in initial seeds selection of ground points. + 'th_dist': 0.125, # threshold for thickness of ground. + 'th_seeds_v': 0.25, + # threshold for lowest point representatives using in initial seeds selection of vertical structural points. + 'th_dist_v': 0.9, # threshold for thickness of vertical structure. + 'max_range': 80.0, # max_range of ground estimation area + 'min_range': 1.0, # min_range of ground estimation area + 'uprightness_thr': 0.101, + # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. + 'verbose': True # display verbose info + } ], ) rviz_node = Node( diff --git a/ros/rviz/patchworkpp.rviz b/ros/rviz/patchworkpp.rviz index 0bf3084..463361b 100644 --- a/ros/rviz/patchworkpp.rviz +++ b/ros/rviz/patchworkpp.rviz @@ -1,11 +1,19 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 138 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Global Options1 + - /nonground1 + - /nonground1/Topic1 + - /ground1 + - /ground1/Topic1 + - /input1 + - /input1/Topic1 + - /Axes1 Splitter Ratio: 0.5 - Tree Height: 752 + Tree Height: 1652 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -23,170 +31,141 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: nonground Visualization Manager: Class: "" Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 61; 229; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: frame_deskew - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /kiss/frame - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 61; 229; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: kiss_keypoints - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.20000000298023224 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /kiss/keypoints - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 14.369325637817383 - Min Value: 0.20239250361919403 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 102; 102; 102 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: local_map - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /kiss/local_map - Use Fixed Frame: true - Use rainbow: true - Value: true + - Alpha: 0.20000000298023224 + Cell Size: 10 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 Enabled: true - Name: point_clouds - - Class: rviz_common/Group - Displays: - - Angle Tolerance: 0.10000000149011612 - Class: rviz_default_plugins/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 100 - Name: odometry - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /kiss/odometry - Value: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 121; 128 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1680 + Min Color: 0; 0; 0 + Min Intensity: 3 + Name: nonground + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /patchworkpp/nonground + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 166; 255; 206 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1680 + Min Color: 0; 0; 0 + Min Intensity: 3 + Name: ground + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /patchworkpp/ground + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 166; 255; 206 + Color Transformer: Intensity + Decay Time: 0 Enabled: false - Name: odometry + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1670 + Min Color: 0; 0; 0 + Min Intensity: 4 + Name: input + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /lexus3/os_center/points + Use Fixed Frame: true + Use rainbow: true + Value: false - Class: rviz_default_plugins/Axes Enabled: true - Length: 1 - Name: odom_frame - Radius: 0.10000000149011612 - Reference Frame: odom + Length: 3 + Name: Axes + Radius: 0.3499999940395355 + Reference Frame: Value: true Enabled: true Global Options: - Background Color: 0; 0; 0 - Fixed Frame: odom + Background Color: 48; 48; 48 + Fixed Frame: lexus3/os_center_a_laser_data_frame Frame Rate: 30 Name: root Tools: @@ -228,34 +207,34 @@ Visualization Manager: Value: true Views: Current: - Class: rviz_default_plugins/Orbit - Distance: 178.319580078125 + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 62.79574203491211 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -68.01193237304688 - Y: 258.05126953125 - Z: -27.22064781188965 + X: -10.38800048828125 + Y: -10.399812698364258 + Z: -5.2948402299080044e-05 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.49020177125930786 + Pitch: 1.569795846939087 Target Frame: - Value: Orbit (rviz_default_plugins) - Yaw: 3.340390920639038 + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 3.165381669998169 Saved: ~ Window Geometry: Displays: - collapsed: true - Height: 1043 - Hide Left Dock: true + collapsed: false + Height: 2158 + Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003080000076cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000076c0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f0000076cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e0000076c0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000fb40000005afc0100000002fb0000000800540069006d0065010000000000000fb40000058100fffffffb0000000800540069006d0065010000000000000450000000000000000000000b350000076c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -264,6 +243,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 0 + Width: 4020 + X: 370 Y: 0 diff --git a/ros/src/GroundSegmentationServer.cpp b/ros/src/GroundSegmentationServer.cpp index ee45197..8e11bb9 100644 --- a/ros/src/GroundSegmentationServer.cpp +++ b/ros/src/GroundSegmentationServer.cpp @@ -24,38 +24,57 @@ using utils::PointCloud2ToEigen; GroundSegmentationServer::GroundSegmentationServer(const rclcpp::NodeOptions &options) : rclcpp::Node("patchworkpp_node", options) { - patchwork::Params patchwork_parameters; - base_frame_ = declare_parameter("base_frame", base_frame_); + patchwork::Params params; + base_frame_ = declare_parameter("base_frame", base_frame_); - patchwork_parameters.verbose = true; + params.sensor_height = declare_parameter("sensor_height", params.sensor_height); + params.num_iter = declare_parameter("num_iter", params.num_iter); + params.num_lpr = declare_parameter("num_lpr", params.num_lpr); + params.num_min_pts = declare_parameter("num_min_pts", params.num_min_pts); + params.th_seeds = declare_parameter("th_seeds", params.th_seeds); + + params.th_dist = declare_parameter("th_dist", params.th_dist); + params.th_seeds_v = declare_parameter("th_seeds_v", params.th_seeds_v); + params.th_dist_v = declare_parameter("th_dist_v", params.th_dist_v); + + params.max_range = declare_parameter("max_range", params.max_range); + params.min_range = declare_parameter("min_range", params.min_range); + params.uprightness_thr = declare_parameter("uprightness_thr", params.uprightness_thr); + + params.verbose = get_parameter("verbose", params.verbose); // Construct the main Patchwork++ node - Patchworkpp_ = std::make_unique(patchwork_parameters); + Patchworkpp_ = std::make_unique(params); // Initialize subscribers pointcloud_sub_ = create_subscription( "pointcloud_topic", rclcpp::SensorDataQoS(), std::bind(&GroundSegmentationServer::EstimateGround, this, std::placeholders::_1)); - // Initialize publishers - rclcpp::QoS qos((rclcpp::SystemDefaultsQoS().keep_last(1).durability_volatile())); + /* + * We use the following QoS setting for reliable ground segmentation. + * If you want to run Patchwork++ in real-time and real-world operation, + * please change the QoS setting + */ +// rclcpp::QoS qos((rclcpp::SystemDefaultsQoS().keep_last(1).durability_volatile())); + rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); + qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + + cloud_publisher_ = create_publisher("/patchworkpp/cloud", qos); ground_publisher_ = create_publisher("/patchworkpp/ground", qos); - nonground_publisher_ = create_publisher("/patchworkpp/non_ground", qos); + nonground_publisher_ = create_publisher("/patchworkpp/nonground", qos); RCLCPP_INFO(this->get_logger(), "Patchwork++ ROS 2 node initialized"); } void GroundSegmentationServer::EstimateGround(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { - const auto cloud_frame_id = msg->header.frame_id; - const auto points = PointCloud2ToEigen(msg); - const auto timestamps = GetTimestamps(msg); - const auto &cloud = patchworkpp_ros::utils::PointCloud2ToEigenMat(msg); - // Estimate Ground + // Estimate ground Patchworkpp_->estimateGround(cloud); - - // Get Ground and Nonground + cloud_publisher_->publish(patchworkpp_ros::utils::EigenMatToPointCloud2(cloud, msg->header)); + // Get ground and nonground Eigen::MatrixX3f ground = Patchworkpp_->getGround(); Eigen::MatrixX3f nonground = Patchworkpp_->getNonground(); double time_taken = Patchworkpp_->getTimeTaken(); diff --git a/ros/src/GroundSegmentationServer.hpp b/ros/src/GroundSegmentationServer.hpp index cd8370c..7c5b7eb 100644 --- a/ros/src/GroundSegmentationServer.hpp +++ b/ros/src/GroundSegmentationServer.hpp @@ -29,6 +29,7 @@ class GroundSegmentationServer : public rclcpp::Node { rclcpp::Subscription::SharedPtr pointcloud_sub_; /// Data publishers. + rclcpp::Publisher::SharedPtr cloud_publisher_; rclcpp::Publisher::SharedPtr ground_publisher_; rclcpp::Publisher::SharedPtr nonground_publisher_; diff --git a/ros/src/Utils.hpp b/ros/src/Utils.hpp index eef0d53..a3238e1 100644 --- a/ros/src/Utils.hpp +++ b/ros/src/Utils.hpp @@ -121,7 +121,7 @@ inline void FillPointCloud2XYZ(const Eigen::MatrixX3f &points, PointCloud2 &msg) sensor_msgs::PointCloud2Iterator msg_x(msg, "x"); sensor_msgs::PointCloud2Iterator msg_y(msg, "y"); sensor_msgs::PointCloud2Iterator msg_z(msg, "z"); - for (size_t i = 0; i < points.size(); ++i, ++msg_x, ++msg_y, ++msg_z) { + for (size_t i = 0; i < points.rows(); ++i, ++msg_x, ++msg_y, ++msg_z) { *msg_x = points(i, 0); *msg_y = points(i, 1); *msg_z = points(i, 2); From e4dc5bdbb9b08d0e9b87f20c5fc6045ab3804c45 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Thu, 30 May 2024 13:40:30 -0400 Subject: [PATCH 11/12] (minor) Update badges --- README.md | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index c6c4526..c3ff5c1 100644 --- a/README.md +++ b/README.md @@ -1,19 +1,21 @@

Patchwork++

- - - + + + + +

- Video + Video   •   - Install + Install   •   - ROS 2 + ROS2   •   - Paper + Paper   •   - Contact Us + Contact Us

animated

From 4d7612b0dd9cd32d5afd44dc67fb9fbe5eb32fd3 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Thu, 30 May 2024 13:49:16 -0400 Subject: [PATCH 12/12] (minor) Update badges and links --- README.md | 34 +++++++++++++--------------------- 1 file changed, 13 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index c6c4526..622260d 100644 --- a/README.md +++ b/README.md @@ -1,35 +1,27 @@

Patchwork++

- - - + + + + +

- Video + Video   •   - Install + Install   •   - ROS 2 + ROS2   •   - Paper + Paper   •   - Contact Us + Contact Us

animated

-
- - -## :bookmark_tabs: About Patchwork++ (IROS'22) - -* A fast, robust, and self-adaptive **ground segmentation algorithm** on 3D point cloud. -* An extension of [Patchwork][patchworklink] (RA-L'21 with IROS'21). -* Please refer our [paper][patchworkppIEEElink] for detailed explanantions and experimental results! - - * Validated on [SemanticKITTI][SemanticKITTIlink] dataset. The benchmark code is available on [here][benchmarklink]. - -* :bulb: Contents: [YouTube][YouTubeLink], [arXiv][arXivlink], [IEEE *Xplore*][patchworkppIEEElink] + [Patchwork++][arXivlink], an extension of [Patchwork][patchworklink], is **a fast, robust, and self-adaptive ground segmentation algorithm** on 3D point cloud. +
[YouTubeLInk]: https://www.youtube.com/watch?v=fogCM159GRk [arXivlink]: https://arxiv.org/abs/2207.11919 @@ -43,7 +35,7 @@ * Python binding of Patchwork++ using pybind11 ([python_wrapper][wraplink]) * Examples codes, which visualizes a ground segmentation result by Patchwork++ ([examples][examplelink]) :thumbsup: -> If you are familiar with ROS, you can also visit [here][roslink] and try executing ROS-based Patchwork++! +> If you are familiar with ROS1, you can also visit [here][roslink] and try executing ROS1-based Patchwork++! [roslink]: https://github.com/url-kaist/patchwork-plusplus-ros