diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..bfce055 --- /dev/null +++ b/.clang-format @@ -0,0 +1,10 @@ +BasedOnStyle: Google +UseTab: Never +IndentWidth: 4 +AccessModifierOffset: -4 +ColumnLimit: 100 +BinPackParameters: false +SortIncludes: true +Standard: c++17 +DerivePointerAlignment: false +PointerAlignment: Right diff --git a/.cmake-format.yaml b/.cmake-format.yaml new file mode 100644 index 0000000..36140f3 --- /dev/null +++ b/.cmake-format.yaml @@ -0,0 +1,4 @@ +enable_markup: false +line_width: 120 +format: + max_subgroups_hwrap: 5 diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 0000000..36c9760 --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1 @@ +* @nachovizzo @benemer @tizianoGuadagnino diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml new file mode 100644 index 0000000..7984caa --- /dev/null +++ b/.github/workflows/cpp.yml @@ -0,0 +1,56 @@ +name: C++ API + +on: + push: + branches: ["main"] + pull_request: + branches: ["main"] + +env: + BUILD_TYPE: Release + +jobs: + cpp_api: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04] + steps: + - uses: actions/checkout@v3 + - name: Setup cmake + uses: jwlawson/actions-setup-cmake@v1.13 + with: + cmake-version: "3.25.x" + - name: Configure CMake + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kinematic_icp + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + # As the previous job will always install the dependencies from cmake, and this is guaranteed to + # work, we also want to support dev sandboxes where the main dependencies are already + # pre-installed in the system. For now, we only support dev machines under a GNU/Linux + # environmnets. If you are reading this and need the same functionallity in Windows/macOS please + # open a ticket. + cpp_api_dev: + runs-on: ${{ matrix.os }} + strategy: + matrix: + os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04] + + steps: + - uses: actions/checkout@v3 + - name: Cache dependencies + uses: actions/cache@v2 + with: + path: ~/.apt/cache + key: ${{ runner.os }}-apt-${{ hashFiles('**/ubuntu_dependencies.yml') }} + restore-keys: | + ${{ runner.os }}-apt- + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libeigen3-dev libtbb-dev + - name: Configure CMake + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kinematic_icp + - name: Build + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 0000000..87740ba --- /dev/null +++ b/.github/workflows/pre-commit.yml @@ -0,0 +1,15 @@ +name: Style Check + +on: + push: + branches: ["main"] + pull_request: + branches: ["main"] + +jobs: + pre-commit: + name: Pre-commit checks + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: pre-commit/action@v3.0.0 diff --git a/.github/workflows/ros.yml b/.github/workflows/ros.yml new file mode 100644 index 0000000..60c7b28 --- /dev/null +++ b/.github/workflows/ros.yml @@ -0,0 +1,24 @@ +name: ROS nodes + +on: + push: + branches: ["main"] + pull_request: + branches: ["main"] + +jobs: + ros2_node: + runs-on: ubuntu-latest + strategy: + matrix: + release: [humble, iron, jazzy, rolling] + container: osrf/ros:${{ matrix.release }}-desktop + steps: + - name: Setup cmake + uses: jwlawson/actions-setup-cmake@v1.13 + with: + cmake-version: "3.25.x" + - uses: actions/checkout@v3 + - name: Run colcon + run: source /opt/ros/${{ matrix.release }}/setup.bash && colcon build --event-handlers console_direct+ + shell: bash diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ca717a6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,117 @@ +# Created by https://www.toptal.com/developers/gitignore/api/c++ +# Edit at https://www.toptal.com/developers/gitignore?templates=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 + +# End of https://www.toptal.com/developers/gitignore/api/c++ +n# Created by https://www.toptal.com/developers/gitignore/api/cmake +# Edit at https://www.toptal.com/developers/gitignore?templates=cmake + +### CMake ### +CMakeLists.txt.user +CMakeCache.txt +CMakeFiles +CMakeScripts +Testing +Makefile +cmake_install.cmake +install_manifest.txt +compile_commands.json +CTestTestfile.cmake +_deps + +### CMake Patch ### +# External projects +*-prefix/ + +# End of https://www.toptal.com/developers/gitignore/api/cmake +n# Created by https://www.toptal.com/developers/gitignore/api/ros +# Edit at https://www.toptal.com/developers/gitignore?templates=ros + +### ROS ### +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE + +# End of https://www.toptal.com/developers/gitignore/api/ros +n diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..37cfe77 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,18 @@ +repos: +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.4.0 + hooks: + - id: trailing-whitespace + exclude: \.patch$ + - id: end-of-file-fixer + exclude: \.patch$ + - id: check-yaml + - id: check-added-large-files +- repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.0 + hooks: + - id: clang-format +- repo: https://github.com/ahans/cmake-format-precommit + rev: 8e52fb6506f169dddfaa87f88600d765fca48386 + hooks: + - id: cmake-format diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..66982c8 --- /dev/null +++ b/LICENSE @@ -0,0 +1,22 @@ +MIT License + +Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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/README.md b/README.md new file mode 100644 index 0000000..58a34ff --- /dev/null +++ b/README.md @@ -0,0 +1,106 @@ +
+

Kinematic-ICP

+ + + +
+
+ Paper +   •   + Contact Us +
+
+ +[Kinematic-ICP](https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/kissteam2025icra.pdf) is a LiDAR odometry approach that explicitly incorporates the kinematic constraints of mobile robots into the classic point-to-point ICP algorithm. + +Kinematic-ICP + +
+ +# How to Build + +Our system operates on ROS2, supporting **ROS Humble**, **Iron**, and **Jazzy**. To build and run Kinematic-ICP, follow these steps: + +1. **Clone the Repository**: + ```sh + cd /src + git clone https://github.com/PRBonn/kinematic-icp + cd .. + ``` + +2. **Ensure all Dependencies are Installed**: + ```sh + rosdep install --from-paths src --ignore-src -r -y + ``` + +3. **Build the Workspace**: + ```sh + colcon build + ``` + +4. **Source the Setup Script**: + ```sh + source ./install/setup.bash + ``` + + +# TF Requirements + +Kinematic ICP can enhance existing odometry using a 3D LiDAR. However, there are specific requirements regarding motion and transformations due to the assumption that the robot operates on a unicycle kinematic model. Below are the key requirements: + +1. **Planar Movement**: The robot is expected to move primarily on a planar surface. + +2. **Existing Odometry**: An existing odometry source must be provided, such as the platform's wheel odometry. In the ROS ecosystem, this means that another node must publish the `tf` transformation between `base_link` and `odom`. (Note: The names may vary and can be adjusted in the pipeline parameters.) + +3. **Static Transform for LiDAR**: To utilize the platform's motion model effectively, the system needs to compute the pose in `base_link`. Therefore, a static `tf` transform between `base_link` and the LiDAR frame (extrinsic calibration) is required. If this calibration is significantly inaccurate, it may compromise system performance. + +Finally, Kinematic ICP will publish a new `tf` transformation between `base_link` and `odom_lidar`. + +# Running the System + +This system offers two entry points for deployment, depending on your use case: one for real-time operation and one for offline processing. + +## 1. Real-Time Deployment: `online_node` + +Use the `online_node` to run the system on a robotics platform. The only required parameter is the **lidar_topic**. You can start the system using the following command: + +```sh +ros2 launch kinematic_icp online_node.launch.py lidar_topic:= +``` + +To enable simultaneous visualization through RViz, use the `visualize` flag set to `true`: + +```sh +ros2 launch kinematic_icp online_node.launch.py lidar_topic:= visualize:=true +``` + +## 2. Offline Processing: `offline_node` + +For post-processing and analysis, the `offline_node` processes a ROS bag file at CPU speed, ensuring no frames are dropped. This mode is ideal for reviewing trajectory results, debugging, and speeding up bag file processing. You can launch the offline node with the following command: + +```sh +ros2 launch kinematic_icp offline_node.launch.py lidar_topic:= bag_filename:= +``` + +RViz can also be used in this mode by setting the `visualize` flag to `true`. Additionally, the system will output a file in TUM format containing the estimated poses, named **_kinematic_poses_tum.txt**. This file is saved in the same directory as the ROS bag file by default. + +To specify a custom directory for the output file, use the `output_dir` parameter: + +```sh +ros2 launch kinematic_icp offline_node.launch.py lidar_topic:= bag_filename:= output_dir:= +``` + + +## Citation + +If you use this library for any academic work, please cite our original [paper](https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/kissteam2025icra.pdf). + +```bibtex +@article{kissteam2024arxiv, + author = {Guadagnino ,Tiziano and Mersch, Benedikt and Vizzo, Ignacio and Gupta, Saurabh and Malladi, Meher V.R. and Lobefaro, Luca and Doisy, Guillaume and Stachniss, Cyrill} + title = {{Kinematic-ICP: Enhancing LiDAR Odometry with Kinematic Constraints for Wheeled Mobile Robots Moving on Planar Surfaces + journal = arXiv Preprint, + year = {2024}, + codeurl = {https://github.com/PRBonn/kinematic-icp}, +} +``` diff --git a/cpp/COLCON_IGNORE b/cpp/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/cpp/kinematic_icp/CMakeLists.txt b/cpp/kinematic_icp/CMakeLists.txt new file mode 100644 index 0000000..718f731 --- /dev/null +++ b/cpp/kinematic_icp/CMakeLists.txt @@ -0,0 +1,58 @@ +# MIT License +# +# Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +cmake_minimum_required(VERSION 3.16...3.26) +project(kinematic_icp_cpp VERSION 0.0.1 LANGUAGES CXX) + +# Setup build options for the underlying kiss dependency +option(USE_CCACHE "Build using Ccache if found on the path" ON) +option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) +option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" OFF) +option(USE_SYSTEM_TSL-ROBIN-MAP "Use system pre-installed tsl_robin" ON) +option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON) +include(kiss_icp/kiss-icp.cmake) + +# KISS dependecies that are also used in Kinematic. +# If the internal FetchContent from kiss is in use (through the above options) +# find_package will use those dependencies, otherwise it will search system-wise +find_package(Eigen3 QUIET NO_MODULE) +find_package(Sophus QUIET NO_MODULE) +find_package(TBB QUIET NO_MODULE) +# ccache setup +if(USE_CCACHE) + find_program(CCACHE_PATH ccache) + if(CCACHE_PATH) + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) + message(STATUS "Using ccache: ${CCACHE_PATH}") + endif() +endif() + +# Set build type (repeat here for C++ only consumers) +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +include(cmake/CompilerOptions.cmake) + +add_subdirectory(registration) +add_subdirectory(pipeline) diff --git a/cpp/kinematic_icp/LICENSE b/cpp/kinematic_icp/LICENSE new file mode 100644 index 0000000..66982c8 --- /dev/null +++ b/cpp/kinematic_icp/LICENSE @@ -0,0 +1,22 @@ +MIT License + +Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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/cpp/kinematic_icp/cmake/CompilerOptions.cmake b/cpp/kinematic_icp/cmake/CompilerOptions.cmake new file mode 100644 index 0000000..ffe7fee --- /dev/null +++ b/cpp/kinematic_icp/cmake/CompilerOptions.cmake @@ -0,0 +1,52 @@ +# We ackowledge the original license for this file +# 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. +function(set_global_target_properties target) + target_compile_features(${target} PUBLIC cxx_std_17) + target_compile_definitions(${target} PUBLIC $<$:_USE_MATH_DEFINES>) + target_compile_options( + ${target} + PRIVATE # MSVC + $<$:/W4> + $<$:/WX> + # Clang/AppleClang + $<$:-fcolor-diagnostics> + $<$:-Werror> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wconversion> + $<$:-Wno-sign-conversion> + # GNU + $<$:-fdiagnostics-color=always> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wcast-align> + $<$:-Wcast-qual> + $<$:-Wconversion> + $<$:-Wdisabled-optimization> + $<$:-Woverloaded-virtual>) + set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}) + get_filename_component(INCLUDE_DIRS ${INCLUDE_DIRS} PATH) + target_include_directories(${target} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC $ $) +endfunction() diff --git a/cpp/kinematic_icp/kiss_icp/LICENSE b/cpp/kinematic_icp/kiss_icp/LICENSE new file mode 100644 index 0000000..d94e830 --- /dev/null +++ b/cpp/kinematic_icp/kiss_icp/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/cpp/kinematic_icp/kiss_icp/kiss-icp.cmake b/cpp/kinematic_icp/kiss_icp/kiss-icp.cmake new file mode 100644 index 0000000..11bfe59 --- /dev/null +++ b/cpp/kinematic_icp/kiss_icp/kiss-icp.cmake @@ -0,0 +1,31 @@ +# MIT License +# +# Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +# Silence timestamp warning +if(CMAKE_VERSION VERSION_GREATER 3.24) + cmake_policy(SET CMP0135 OLD) +endif() + +include(FetchContent) +FetchContent_Declare(kiss_icp URL https://github.com/PRBonn/kiss-icp/archive/refs/tags/v1.1.0.tar.gz SOURCE_SUBDIR + cpp/kiss_icp) +FetchContent_MakeAvailable(kiss_icp) diff --git a/cpp/kinematic_icp/pipeline/CMakeLists.txt b/cpp/kinematic_icp/pipeline/CMakeLists.txt new file mode 100644 index 0000000..25fa5b3 --- /dev/null +++ b/cpp/kinematic_icp/pipeline/CMakeLists.txt @@ -0,0 +1,26 @@ +# MIT License +# +# Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +add_library(kinematic_icp_pipeline STATIC) +target_sources(kinematic_icp_pipeline PRIVATE KinematicICP.cpp) +target_link_libraries(kinematic_icp_pipeline PUBLIC kinematic_icp_registration kiss_icp_pipeline) +set_global_target_properties(kinematic_icp_pipeline) diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.cpp b/cpp/kinematic_icp/pipeline/KinematicICP.cpp new file mode 100644 index 0000000..c93e878 --- /dev/null +++ b/cpp/kinematic_icp/pipeline/KinematicICP.cpp @@ -0,0 +1,90 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#include "KinematicICP.hpp" + +#include +#include +#include +#include +#include +#include + +namespace { +auto transform_points(const std::vector &points, const Sophus::SE3d &pose) { + std::vector points_transformed(points.size()); + std::transform(points.cbegin(), points.cend(), points_transformed.begin(), + [&](const auto &point) { return pose * point; }); + return points_transformed; +} + +auto Voxelize(const std::vector &frame, const double voxel_size) { + const std::vector &frame_downsample = + kiss_icp::VoxelDownsample(frame, voxel_size * 0.5); + const std::vector &source = + kiss_icp::VoxelDownsample(frame_downsample, voxel_size * 1.5); + return std::make_tuple(source, frame_downsample); +} +} // namespace +namespace kinematic_icp::pipeline { + +KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame( + const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &lidar_to_base, + const Sophus::SE3d &relative_odometry) { + const auto &deskew_frame = [&]() -> std::vector { + if (!config_.deskew || timestamps.empty()) return frame; + return kiss_icp::DeSkewScan(frame, timestamps, + lidar_to_base.inverse() * relative_odometry * lidar_to_base); + }(); + const auto &deskew_frame_in_base = transform_points(deskew_frame, lidar_to_base); + // Preprocess the input cloud + const auto &cropped_frame = + kiss_icp::Preprocess(deskew_frame_in_base, config_.max_range, config_.min_range); + + // Voxelize + const auto &[source, frame_downsample] = Voxelize(cropped_frame, config_.voxel_size); + + // Get adaptive_threshold + const double sigma = adaptive_threshold_.ComputeThreshold(); + + // Run ICP + const auto new_pose = registration_.ComputeRobotMotion(source, // frame + local_map_, // voxel_map + last_pose_, // last_pose + relative_odometry, // robot_motion + 3 * sigma); // max_correspondence_dist + + // Compute the difference between the prediction and the actual estimate + const auto model_deviation = (last_pose_ * relative_odometry).inverse() * new_pose; + + // Update step: threshold, local map and the last pose + adaptive_threshold_.UpdateModelDeviation(model_deviation); + local_map_.Update(frame_downsample, new_pose); + last_pose_ = new_pose; + + // Return the (deskew) input raw scan (frame) and the points used for + // registration (source) + return {deskew_frame_in_base, source}; +} +} // namespace kinematic_icp::pipeline diff --git a/cpp/kinematic_icp/pipeline/KinematicICP.hpp b/cpp/kinematic_icp/pipeline/KinematicICP.hpp new file mode 100644 index 0000000..2aff12e --- /dev/null +++ b/cpp/kinematic_icp/pipeline/KinematicICP.hpp @@ -0,0 +1,81 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "kinematic_icp/registration/Registration.hpp" + +namespace kinematic_icp::pipeline { + +using Config = kiss_icp::pipeline::KISSConfig; + +class KinematicICP { +public: + using Vector3dVector = std::vector; + using Vector3dVectorTuple = std::tuple; + + explicit KinematicICP(const kiss_icp::pipeline::KISSConfig &config) + : registration_( + config.max_num_iterations, config.convergence_criterion, config.max_num_threads), + config_(config), + local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel), + adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {} + + Vector3dVectorTuple RegisterFrame(const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &lidar_to_base, + const Sophus::SE3d &relative_odometry); + + inline void SetPose(const Sophus::SE3d &pose) { + last_pose_ = pose; + local_map_.Clear(); + adaptive_threshold_ = kiss_icp::AdaptiveThreshold(config_.initial_threshold, + config_.min_motion_th, config_.max_range); + }; + + std::vector LocalMap() const { return local_map_.Pointcloud(); }; + + const kiss_icp::VoxelHashMap &VoxelMap() const { return local_map_; }; + kiss_icp::VoxelHashMap &VoxelMap() { return local_map_; }; + + const Sophus::SE3d &pose() const { return last_pose_; } + Sophus::SE3d &pose() { return last_pose_; } + +private: + Sophus::SE3d last_pose_; + // Kinematic module + KinematicRegistration registration_; + // KISS-ICP pipeline modules + kiss_icp::pipeline::KISSConfig config_; + kiss_icp::VoxelHashMap local_map_; + kiss_icp::AdaptiveThreshold adaptive_threshold_; +}; + +} // namespace kinematic_icp::pipeline diff --git a/cpp/kinematic_icp/registration/CMakeLists.txt b/cpp/kinematic_icp/registration/CMakeLists.txt new file mode 100644 index 0000000..25d98cf --- /dev/null +++ b/cpp/kinematic_icp/registration/CMakeLists.txt @@ -0,0 +1,25 @@ +# MIT License +# +# Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +add_library(kinematic_icp_registration Registration.cpp) +target_link_libraries(kinematic_icp_registration kiss_icp_core Eigen3::Eigen TBB::tbb Sophus::Sophus) +set_global_target_properties(kinematic_icp_registration) diff --git a/cpp/kinematic_icp/registration/Registration.cpp b/cpp/kinematic_icp/registration/Registration.cpp new file mode 100644 index 0000000..1cc95b0 --- /dev/null +++ b/cpp/kinematic_icp/registration/Registration.cpp @@ -0,0 +1,193 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#include "Registration.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using LinearSystem = std::pair; +using Correspondences = std::vector>; + +namespace { +constexpr double epsilon = std::numeric_limits::min(); + +double ComputeOdometryRegularization(const Correspondences &associations, + const Sophus::SE3d &odometry_initial_guess) { + const double sum_of_squared_residuals = + std::transform_reduce(associations.cbegin(), associations.cend(), 0.0, std::plus(), + [&](const auto &association) { + const auto &[source, target] = association; + return (odometry_initial_guess * source - target).squaredNorm(); + }); + const double N = static_cast(associations.size()); + const double mean_squared_residual = sum_of_squared_residuals / N; + const double beta = 1.0 / (mean_squared_residual + epsilon); + return beta; +} + +Correspondences DataAssociation(const std::vector &points, + const kiss_icp::VoxelHashMap &voxel_map, + const Sophus::SE3d &T, + const double max_correspondance_distance) { + using points_iterator = std::vector::const_iterator; + Correspondences associations; + associations.reserve(points.size()); + associations = tbb::parallel_reduce( + // Range + tbb::blocked_range{points.cbegin(), points.cend()}, + // Identity + associations, + // 1st lambda: Parallel computation + [&](const tbb::blocked_range &r, Correspondences res) -> Correspondences { + res.reserve(r.size()); + std::for_each(r.begin(), r.end(), [&](const auto &point) { + const auto &[closest_neighbor, distance] = voxel_map.GetClosestNeighbor(T * point); + if (distance < max_correspondance_distance) { + res.emplace_back(point, closest_neighbor); + } + }); + return res; + }, + // 2nd lambda: Parallel reduction + [](Correspondences a, const Correspondences &b) -> Correspondences { + a.insert(a.end(), // + std::make_move_iterator(b.cbegin()), // + std::make_move_iterator(b.cend())); + return a; + }); + + return associations; +} + +Eigen::Vector2d ComputePerturbation(const Correspondences &correspondences, + const Sophus::SE3d ¤t_estimate, + const double beta) { + auto compute_jacobian_and_residual = [&](const auto &correspondence) { + const auto &[source, target] = correspondence; + const Eigen::Vector3d residual = current_estimate * source - target; + Eigen::Matrix J; + J.col(0) = current_estimate.so3() * Eigen::Vector3d::UnitX(); + J.col(1) = current_estimate.so3() * Eigen::Vector3d(-source.y(), source.x(), 0.0); + return std::make_tuple(J, residual); + }; + + auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) { + a.first += b.first; + a.second += b.second; + return a; + }; + + using correspondence_iterator = Correspondences::const_iterator; + auto [JTJ, JTr] = tbb::parallel_reduce( + // Range + tbb::blocked_range{correspondences.cbegin(), + correspondences.cend()}, + // Identity + LinearSystem(Eigen::Matrix2d::Zero(), Eigen::Vector2d::Zero()), + // 1st Lambda: Parallel computation + [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { + return std::transform_reduce( + r.begin(), r.end(), J, sum_linear_systems, [&](const auto &correspondence) { + const auto &[J_r, residual] = compute_jacobian_and_residual(correspondence); + return LinearSystem(J_r.transpose() * J_r, // JTJ + J_r.transpose() * residual); // JTr + }); + }, + // 2nd Lambda: Parallel reduction of the private Jacboians + sum_linear_systems); + const double num_correspondences = static_cast(correspondences.size()); + + const Eigen::Matrix2d Omega = Eigen::Vector2d(beta, 0).asDiagonal(); + JTJ /= num_correspondences; + JTr /= num_correspondences; + JTJ += Omega; + return -(JTJ.inverse() * JTr); +} + +} // namespace + +namespace kinematic_icp { + +KinematicRegistration::KinematicRegistration(int max_num_iteration, + double convergence_criterion, + int max_num_threads) + : max_num_iterations_(max_num_iteration), + convergence_criterion_(convergence_criterion), + // Only manipulate the number of threads if the user specifies something + // greater than 0 + max_num_threads_(max_num_threads > 0 ? max_num_threads + : tbb::this_task_arena::max_concurrency()) { + // This global variable requires static duration storage to be able to + // manipulate the max concurrency from TBB across the entire class + static const auto tbb_control_settings = tbb::global_control( + tbb::global_control::max_allowed_parallelism, static_cast(max_num_threads_)); +} + +Sophus::SE3d KinematicRegistration::ComputeRobotMotion(const std::vector &frame, + const kiss_icp::VoxelHashMap &voxel_map, + const Sophus::SE3d &last_robot_pose, + const Sophus::SE3d &relative_wheel_odometry, + const double max_correspondence_distance) { + Sophus::SE3d current_estimate = last_robot_pose * relative_wheel_odometry; + if (voxel_map.Empty()) return current_estimate; + + auto motion_model = [](const Eigen::Vector2d &integrated_controls) { + Sophus::SE3d::Tangent dx = Sophus::SE3d::Tangent::Zero(); + const double &displacement = integrated_controls(0); + const double &theta = integrated_controls(1); + dx(0) = displacement * std::sin(theta) / (theta + epsilon); + dx(1) = displacement * (1.0 - std::cos(theta)) / (theta + epsilon); + dx(5) = theta; + return Sophus::SE3d::exp(dx); + }; + auto correspondences = + DataAssociation(frame, voxel_map, current_estimate, max_correspondence_distance); + + const double regularization_term = + ComputeOdometryRegularization(correspondences, current_estimate); + // ICP-loop + for (int j = 0; j < max_num_iterations_; ++j) { + const auto dx = ComputePerturbation(correspondences, current_estimate, regularization_term); + const auto delta_motion = motion_model(dx); + current_estimate = current_estimate * delta_motion; + // Break loop + if (dx.norm() < convergence_criterion_) break; + correspondences = + DataAssociation(frame, voxel_map, current_estimate, max_correspondence_distance); + } + // Spit the final transformation + return current_estimate; +} +} // namespace kinematic_icp diff --git a/cpp/kinematic_icp/registration/Registration.hpp b/cpp/kinematic_icp/registration/Registration.hpp new file mode 100644 index 0000000..e67d105 --- /dev/null +++ b/cpp/kinematic_icp/registration/Registration.hpp @@ -0,0 +1,47 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#pragma once + +#include +#include +#include +#include + +namespace kinematic_icp { + +struct KinematicRegistration { + explicit KinematicRegistration(int max_num_iteration, + double convergence_criterion, + int max_num_threads); + + Sophus::SE3d ComputeRobotMotion(const std::vector &frame, + const kiss_icp::VoxelHashMap &voxel_map, + const Sophus::SE3d &last_robot_pose, + const Sophus::SE3d &relative_wheel_odometry, + const double max_correspondence_distance); + + int max_num_iterations_; + double convergence_criterion_; + int max_num_threads_; +}; +} // namespace kinematic_icp diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt new file mode 100644 index 0000000..d4e2678 --- /dev/null +++ b/ros/CMakeLists.txt @@ -0,0 +1,79 @@ +# MIT License +# +# Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +cmake_minimum_required(VERSION 3.16...3.26) +project(kinematic_icp VERSION 0.0.1 LANGUAGES CXX) + +set(CMAKE_BUILD_TYPE Release) + +add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kinematic_icp ${CMAKE_CURRENT_BINARY_DIR}/kinematic_icp) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rcutils REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(rosbag2_storage REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) + +set(dependencies + geometry_msgs + nav_msgs + rclcpp + rclcpp_components + rcutils + rosbag2_cpp + rosbag2_storage + sensor_msgs + std_msgs + std_srvs + tf2_ros + visualization_msgs) + +add_library( + kinematic_icp_ros SHARED + src/kinematic_icp_ros/server/LidarOdometryServer.cpp src/kinematic_icp_ros/utils/RosUtils.cpp + src/kinematic_icp_ros/utils/RosbagUtils.cpp src/kinematic_icp_ros/nodes/online_node.cpp +)# Adding it here for composition +target_compile_features(kinematic_icp_ros PUBLIC cxx_std_17) +target_include_directories(kinematic_icp_ros PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_link_libraries(kinematic_icp_ros kinematic_icp_pipeline "${cpp_typesupport_target}") +ament_target_dependencies(kinematic_icp_ros ${dependencies}) + +add_executable(kinematic_icp_offline_node src/kinematic_icp_ros/nodes/offline_node.cpp) +target_link_libraries(kinematic_icp_offline_node PUBLIC kinematic_icp_ros) + +rclcpp_components_register_node(kinematic_icp_ros PLUGIN "kinematic_icp_ros::OnlineNode" EXECUTABLE + kinematic_icp_online_node) + +install(TARGETS kinematic_icp_ros kinematic_icp_online_node kinematic_icp_offline_node LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY config launch rviz DESTINATION share/${PROJECT_NAME}/) + +ament_package() diff --git a/ros/LICENSE b/ros/LICENSE new file mode 100644 index 0000000..66982c8 --- /dev/null +++ b/ros/LICENSE @@ -0,0 +1,22 @@ +MIT License + +Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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/config/kinematic_icp_ros.yaml b/ros/config/kinematic_icp_ros.yaml new file mode 100644 index 0000000..6d4e0b6 --- /dev/null +++ b/ros/config/kinematic_icp_ros.yaml @@ -0,0 +1,23 @@ +/**: + ros__parameters: + # Preprocessing + max_range: 100.0 + min_range: 0.0 + deskew: true + + # Mapping parameters + voxel_size: 1.0 + max_points_per_voxel: 20 + + # Adaptive threshold + initial_threshold: 2.0 + min_motion_th: 0.1 + + # Registration + max_num_iterations: 10 + convergence_criterion: 0.001 + max_num_threads: 1 + + # Covariance diagonal values + orientation_covariance: 0.1 + position_covariance: 0.1 diff --git a/ros/include/kinematic_icp_ros/nodes/offline_node.hpp b/ros/include/kinematic_icp_ros/nodes/offline_node.hpp new file mode 100644 index 0000000..186a1f8 --- /dev/null +++ b/ros/include/kinematic_icp_ros/nodes/offline_node.hpp @@ -0,0 +1,61 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#pragma once + +#include +#include + +// ROS +#include +#include +#include + +#include "kinematic_icp_ros/server/LidarOdometryServer.hpp" +#include "kinematic_icp_ros/utils/RosbagUtils.hpp" + +namespace kinematic_icp_ros { + +class OfflineNode { +public: + OfflineNode() = delete; + explicit OfflineNode(const rclcpp::NodeOptions &options); + + void writePosesInTumFormat(); + void Run(); + +private: + // Common for offline/online nodes + std::string pcl_topic_; + std::shared_ptr odometry_server_; + rclcpp::Node::SharedPtr node_; + // store data for the experiments + using PoseWithStamp = std::pair; + std::vector poses_with_timestamps_; + std::string output_dir_; + std::string poses_filename_; + + // Offline node specifics + BagMultiplexer bag_multiplexer_; +}; + +} // namespace kinematic_icp_ros diff --git a/ros/include/kinematic_icp_ros/nodes/online_node.hpp b/ros/include/kinematic_icp_ros/nodes/online_node.hpp new file mode 100644 index 0000000..d23932b --- /dev/null +++ b/ros/include/kinematic_icp_ros/nodes/online_node.hpp @@ -0,0 +1,54 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#pragma once + +#include +#include + +// ROS +#include +#include + +#include "kinematic_icp_ros/server/LidarOdometryServer.hpp" + +namespace kinematic_icp_ros { + +class OnlineNode { +public: + OnlineNode() = delete; + explicit OnlineNode(const rclcpp::NodeOptions &options); + + // Neccesary for ROS 2 composition + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); + +private: + // Common for offline/online nodes + std::string pcl_topic_; + std::shared_ptr odometry_server_; + rclcpp::Node::SharedPtr node_; + + // Online node specifics + rclcpp::Subscription::SharedPtr pointcloud_sub_; +}; + +} // namespace kinematic_icp_ros diff --git a/ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp b/ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp new file mode 100644 index 0000000..3fe8fab --- /dev/null +++ b/ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp @@ -0,0 +1,107 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#pragma once + +#include "kinematic_icp/pipeline/KinematicICP.hpp" + +// ROS 2 C +#include +#include +#include + +// ROS 2 C++ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// STL +#include +#include +#include +#include + +namespace kinematic_icp_ros { + +class LidarOdometryServer { +public: + /// LidarOdometryServer constructor + LidarOdometryServer() = delete; + explicit LidarOdometryServer(rclcpp::Node::SharedPtr node); + + /// Register new frame + void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + std::unique_ptr kinematic_icp_; + builtin_interfaces::msg::Time current_stamp_; + +private: + /// Stream the estimated pose to ROS + void PublishOdometryMsg(const Sophus::SE3d &pose, const Sophus::SE3d::Tangent &velocity); + + /// Stream the debugging point clouds for visualization (if required) + void PublishClouds(const std::vector frame, + const std::vector keypoints); + + // Temporal initializaiton strattegy until we convert the odometry server to life cycle + void InitializePoseAndExtrinsic(const std::string &lidar_frame_id); + + /// Tools for broadcasting TFs. + std::unique_ptr tf_broadcaster_; + std::unique_ptr tf2_buffer_; + std::unique_ptr tf2_listener_; + std::chrono::milliseconds tf_timeout_; + bool publish_odom_tf_; + bool invert_odom_tf_; + bool publish_debug_clouds_; + Sophus::SE3d sensor_to_base_footprint_; + + /// Data publishers. + rclcpp::Publisher::SharedPtr odom_publisher_; + rclcpp::Publisher::SharedPtr frame_publisher_; + rclcpp::Publisher::SharedPtr kpoints_publisher_; + rclcpp::Publisher::SharedPtr map_publisher_; + rclcpp::Publisher::SharedPtr voxel_grid_pub_; + + /// SetPose service + rclcpp::Service::SharedPtr set_pose_srv_; + + /// Internal messages, models the current state that will be published by this node. + nav_msgs::msg::Odometry odom_msg_; + geometry_msgs::msg::TransformStamped tf_msg_; + + /// Global/map coordinate frame. + std::string lidar_odom_frame_{"odom_lidar"}; + std::string wheel_odom_frame_{"odom"}; + std::string base_frame_{"base_link"}; + // TF frame initialization flag + bool initialize_odom_node; + + rclcpp::Node::SharedPtr node_; +}; + +} // namespace kinematic_icp_ros diff --git a/ros/include/kinematic_icp_ros/utils/RosUtils.hpp b/ros/include/kinematic_icp_ros/utils/RosUtils.hpp new file mode 100644 index 0000000..5001fba --- /dev/null +++ b/ros/include/kinematic_icp_ros/utils/RosUtils.hpp @@ -0,0 +1,150 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#pragma once + +// tf2 +#include + +#include +#include +#include +#include +#include +#include + +// ROS 2 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace tf2 { + +inline geometry_msgs::msg::Transform sophusToTransform(const Sophus::SE3d &T) { + geometry_msgs::msg::Transform t; + t.translation.x = T.translation().x(); + t.translation.y = T.translation().y(); + t.translation.z = T.translation().z(); + + Eigen::Quaterniond q(T.so3().unit_quaternion()); + t.rotation.x = q.x(); + t.rotation.y = q.y(); + t.rotation.z = q.z(); + t.rotation.w = q.w(); + + return t; +} + +inline geometry_msgs::msg::Pose sophusToPose(const Sophus::SE3d &T) { + geometry_msgs::msg::Pose t; + t.position.x = T.translation().x(); + t.position.y = T.translation().y(); + t.position.z = T.translation().z(); + + Eigen::Quaterniond q(T.so3().unit_quaternion()); + t.orientation.x = q.x(); + t.orientation.y = q.y(); + t.orientation.z = q.z(); + t.orientation.w = q.w(); + + return t; +} + +inline Sophus::SE3d transformToSophus(const geometry_msgs::msg::TransformStamped &transform) { + const auto &t = transform.transform; + return Sophus::SE3d( + Sophus::SE3d::QuaternionType(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z), + Sophus::SE3d::Point(t.translation.x, t.translation.y, t.translation.z)); +} + +inline Sophus::SE3d poseToSophus(const geometry_msgs::msg::Pose &pose) { + const auto &T = pose; + return Sophus::SE3d(Sophus::SE3d::QuaternionType(T.orientation.w, T.orientation.x, + T.orientation.y, T.orientation.z), + Sophus::SE3d::Point(T.position.x, T.position.y, T.position.z)); +} + +} // namespace tf2 + +namespace kinematic_icp_ros::utils { +using PointCloud2 = sensor_msgs::msg::PointCloud2; +using PointField = sensor_msgs::msg::PointField; +using Header = std_msgs::msg::Header; + +inline Sophus::SE3d LookupTransform( + const std::string &target_frame, + const std::string &source_frame, + const std::unique_ptr &tf2_buffer, + const rclcpp::Time &time = tf2_ros::toRclcpp(tf2::TimePointZero)) { + try { + auto tf = tf2_buffer->lookupTransform(target_frame, source_frame, time); + return tf2::transformToSophus(tf); + } catch (tf2::TransformException &ex) { + RCLCPP_WARN(rclcpp::get_logger("LookupTransform"), "%s", ex.what()); + return {}; + } +} + +inline Sophus::SE3d LookupDeltaTransform(const std::string &target_frame, + const rclcpp::Time &target_time, + const std::string &source_frame, + const rclcpp::Time &source_time, + const std::string &fixed_frame, + const rclcpp::Duration timeout, + const std::unique_ptr &tf2_buffer) { + try { + auto tf = tf2_buffer->lookupTransform(target_frame, target_time, source_frame, source_time, + fixed_frame, timeout); + return tf2::transformToSophus(tf); + } catch (tf2::TransformException &ex) { + RCLCPP_WARN(rclcpp::get_logger("LookupTransform"), "%s", ex.what()); + return {}; + } +} + +std::optional GetTimestampField(const PointCloud2::ConstSharedPtr msg); + +std::vector NormalizeTimestamps(const std::vector ×tamps); + +auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, + const PointField ×tamp_field); + +std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg); + +std::vector PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg, + const Sophus::SE3d &T = {}); + +std::unique_ptr EigenToPointCloud2(const std::vector &points, + const Header &header); + +visualization_msgs::msg::Marker VoxelsToMarker(const std::vector &voxels, + const double voxel_size, + const Header &header); +} // namespace kinematic_icp_ros::utils diff --git a/ros/include/kinematic_icp_ros/utils/RosbagUtils.hpp b/ros/include/kinematic_icp_ros/utils/RosbagUtils.hpp new file mode 100644 index 0000000..bd7102d --- /dev/null +++ b/ros/include/kinematic_icp_ros/utils/RosbagUtils.hpp @@ -0,0 +1,89 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#pragma once + +#include +#include +#include +#include +#include +#include + +// tf2 +#include +#include + +// ROS +#include +#include + +// rosbag headers +#include +#include + +class BufferableBag { +public: + // Wrapper node to process the transforamtions present in the bagfile + struct TFBridge { + TFBridge(rclcpp::Node::SharedPtr node); + void ProcessTFMessage(std::shared_ptr msg) const; + std::unique_ptr tf_broadcaster; + std::unique_ptr tf_static_broadcaster; + rclcpp::Serialization serializer; + }; + + BufferableBag(const std::string &bag_filename, + const std::shared_ptr tf_bridge, + const std::string &topic, + const std::chrono::seconds buffer_size = std::chrono::seconds(1)); + + std::size_t message_count() const; + void BufferMessages(); + rosbag2_storage::SerializedBagMessage PopNextMessage(); + bool finished() const; + +private: + std::shared_ptr tf_bridge_; + std::unique_ptr bag_reader_; + std::queue buffer_; + std::chrono::seconds buffer_size_; + std::string topic_; + std::size_t message_count_{0}; +}; + +class BagMultiplexer { +public: + // assume we feed this bags in ordered fashion, plesae behave + void AddBag(BufferableBag &&bag); + + // How many messages in total in all the bagfiles + std::size_t message_count() const; + + rosbag2_storage::SerializedBagMessage GetNextMessage(); + bool IsMessageAvailable() const; + +private: + std::vector bags_; + std::size_t current_index_{0}; + std::size_t message_count_{0}; +}; diff --git a/ros/include/kinematic_icp_ros/utils/indicators.hpp b/ros/include/kinematic_icp_ros/utils/indicators.hpp new file mode 100644 index 0000000..7f7a729 --- /dev/null +++ b/ros/include/kinematic_icp_ros/utils/indicators.hpp @@ -0,0 +1,3037 @@ + +#ifndef INDICATORS_COLOR +#define INDICATORS_COLOR + +namespace indicators { +enum class Color { grey, red, green, yellow, blue, magenta, cyan, white, unspecified }; +} + +#endif + +#ifndef INDICATORS_FONT_STYLE +#define INDICATORS_FONT_STYLE + +namespace indicators { +enum class FontStyle { bold, dark, italic, underline, blink, reverse, concealed, crossed }; +} + +#endif + +#ifndef INDICATORS_PROGRESS_TYPE +#define INDICATORS_PROGRESS_TYPE + +namespace indicators { +enum class ProgressType { incremental, decremental }; +} + +#endif + +//! +//! termcolor +//! ~~~~~~~~~ +//! +//! termcolor is a header-only c++ library for printing colored messages +//! to the terminal. Written just for fun with a help of the Force. +//! +//! :copyright: (c) 2013 by Ihor Kalnytskyi +//! :license: BSD, see LICENSE for details +//! + +#ifndef TERMCOLOR_HPP_ +#define TERMCOLOR_HPP_ + +#include +#include +#include + +// Detect target's platform and set some macros in order to wrap platform +// specific code this library depends on. +#if defined(_WIN32) || defined(_WIN64) +#define TERMCOLOR_TARGET_WINDOWS +#elif defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)) +#define TERMCOLOR_TARGET_POSIX +#endif + +// If implementation has not been explicitly set, try to choose one based on +// target platform. +#if !defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) && !defined(TERMCOLOR_USE_WINDOWS_API) && \ + !defined(TERMCOLOR_USE_NOOP) +#if defined(TERMCOLOR_TARGET_POSIX) +#define TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES +#define TERMCOLOR_AUTODETECTED_IMPLEMENTATION +#elif defined(TERMCOLOR_TARGET_WINDOWS) +#define TERMCOLOR_USE_WINDOWS_API +#define TERMCOLOR_AUTODETECTED_IMPLEMENTATION +#endif +#endif + +// These headers provide isatty()/fileno() functions, which are used for +// testing whether a standard stream refers to the terminal. +#if defined(TERMCOLOR_TARGET_POSIX) +#include +#elif defined(TERMCOLOR_TARGET_WINDOWS) +#if defined(_MSC_VER) +#if !defined(NOMINMAX) +#define NOMINMAX +#endif +#endif +#include +#include +#endif + +namespace termcolor { +// Forward declaration of the `_internal` namespace. +// All comments are below. +namespace _internal { +inline int colorize_index(); +inline FILE *get_standard_stream(const std::ostream &stream); +inline bool is_colorized(std::ostream &stream); +inline bool is_atty(const std::ostream &stream); + +#if defined(TERMCOLOR_TARGET_WINDOWS) +inline void win_change_attributes(std::ostream &stream, int foreground, int background = -1); +#endif +} // namespace _internal + +inline std::ostream &colorize(std::ostream &stream) { + stream.iword(_internal::colorize_index()) = 1L; + return stream; +} + +inline std::ostream &nocolorize(std::ostream &stream) { + stream.iword(_internal::colorize_index()) = 0L; + return stream; +} + +inline std::ostream &reset(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[00m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, -1); +#endif + } + return stream; +} + +inline std::ostream &bold(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[1m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) +#endif + } + return stream; +} + +inline std::ostream &dark(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[2m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) +#endif + } + return stream; +} + +inline std::ostream &italic(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[3m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) +#endif + } + return stream; +} + +inline std::ostream &underline(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[4m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, COMMON_LVB_UNDERSCORE); +#endif + } + return stream; +} + +inline std::ostream &blink(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[5m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) +#endif + } + return stream; +} + +inline std::ostream &reverse(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[7m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) +#endif + } + return stream; +} + +inline std::ostream &concealed(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[8m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) +#endif + } + return stream; +} + +inline std::ostream &crossed(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[9m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) +#endif + } + return stream; +} + +template +inline std::ostream &color(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + char command[12]; + std::snprintf(command, sizeof(command), "\033[38;5;%dm", code); + stream << command; +#elif defined(TERMCOLOR_USE_WINDOWS_API) +#endif + } + return stream; +} + +template +inline std::ostream &on_color(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + char command[12]; + std::snprintf(command, sizeof(command), "\033[48;5;%dm", code); + stream << command; +#elif defined(TERMCOLOR_USE_WINDOWS_API) +#endif + } + return stream; +} + +template +inline std::ostream &color(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + char command[20]; + std::snprintf(command, sizeof(command), "\033[38;2;%d;%d;%dm", r, g, b); + stream << command; +#elif defined(TERMCOLOR_USE_WINDOWS_API) +#endif + } + return stream; +} + +template +inline std::ostream &on_color(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + char command[20]; + std::snprintf(command, sizeof(command), "\033[48;2;%d;%d;%dm", r, g, b); + stream << command; +#elif defined(TERMCOLOR_USE_WINDOWS_API) +#endif + } + return stream; +} + +inline std::ostream &grey(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[30m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, + 0 // grey (black) + ); +#endif + } + return stream; +} + +inline std::ostream &red(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[31m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, FOREGROUND_RED); +#endif + } + return stream; +} + +inline std::ostream &green(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[32m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, FOREGROUND_GREEN); +#endif + } + return stream; +} + +inline std::ostream &yellow(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[33m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, FOREGROUND_GREEN | FOREGROUND_RED); +#endif + } + return stream; +} + +inline std::ostream &blue(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[34m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, FOREGROUND_BLUE); +#endif + } + return stream; +} + +inline std::ostream &magenta(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[35m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, FOREGROUND_BLUE | FOREGROUND_RED); +#endif + } + return stream; +} + +inline std::ostream &cyan(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[36m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, FOREGROUND_BLUE | FOREGROUND_GREEN); +#endif + } + return stream; +} + +inline std::ostream &white(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[37m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, + FOREGROUND_BLUE | FOREGROUND_GREEN | FOREGROUND_RED); +#endif + } + return stream; +} + +inline std::ostream &bright_grey(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[90m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, + 0 | FOREGROUND_INTENSITY // grey (black) + ); +#endif + } + return stream; +} + +inline std::ostream &bright_red(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[91m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, FOREGROUND_RED | FOREGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &bright_green(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[92m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, FOREGROUND_GREEN | FOREGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &bright_yellow(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[93m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, + FOREGROUND_GREEN | FOREGROUND_RED | FOREGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &bright_blue(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[94m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, FOREGROUND_BLUE | FOREGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &bright_magenta(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[95m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, + FOREGROUND_BLUE | FOREGROUND_RED | FOREGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &bright_cyan(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[96m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, + FOREGROUND_BLUE | FOREGROUND_GREEN | FOREGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &bright_white(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[97m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes( + stream, FOREGROUND_BLUE | FOREGROUND_GREEN | FOREGROUND_RED | FOREGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &on_grey(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[40m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, + 0 // grey (black) + ); +#endif + } + return stream; +} + +inline std::ostream &on_red(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[41m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, BACKGROUND_RED); +#endif + } + return stream; +} + +inline std::ostream &on_green(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[42m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, BACKGROUND_GREEN); +#endif + } + return stream; +} + +inline std::ostream &on_yellow(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[43m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, BACKGROUND_GREEN | BACKGROUND_RED); +#endif + } + return stream; +} + +inline std::ostream &on_blue(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[44m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, BACKGROUND_BLUE); +#endif + } + return stream; +} + +inline std::ostream &on_magenta(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[45m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, BACKGROUND_BLUE | BACKGROUND_RED); +#endif + } + return stream; +} + +inline std::ostream &on_cyan(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[46m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, BACKGROUND_GREEN | BACKGROUND_BLUE); +#endif + } + return stream; +} + +inline std::ostream &on_white(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[47m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, + BACKGROUND_GREEN | BACKGROUND_BLUE | BACKGROUND_RED); +#endif + } + + return stream; +} + +inline std::ostream &on_bright_grey(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[100m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, + 0 | BACKGROUND_INTENSITY // grey (black) + ); +#endif + } + return stream; +} + +inline std::ostream &on_bright_red(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[101m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, BACKGROUND_RED | BACKGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &on_bright_green(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[102m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, BACKGROUND_GREEN | BACKGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &on_bright_yellow(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[103m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, + BACKGROUND_GREEN | BACKGROUND_RED | BACKGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &on_bright_blue(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[104m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, BACKGROUND_BLUE | BACKGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &on_bright_magenta(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[105m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, + BACKGROUND_BLUE | BACKGROUND_RED | BACKGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &on_bright_cyan(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[106m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes(stream, -1, + BACKGROUND_GREEN | BACKGROUND_BLUE | BACKGROUND_INTENSITY); +#endif + } + return stream; +} + +inline std::ostream &on_bright_white(std::ostream &stream) { + if (_internal::is_colorized(stream)) { +#if defined(TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES) + stream << "\033[107m"; +#elif defined(TERMCOLOR_USE_WINDOWS_API) + _internal::win_change_attributes( + stream, -1, BACKGROUND_GREEN | BACKGROUND_BLUE | BACKGROUND_RED | BACKGROUND_INTENSITY); +#endif + } + + return stream; +} + +//! Since C++ hasn't a way to hide something in the header from +//! the outer access, I have to introduce this namespace which +//! is used for internal purpose and should't be access from +//! the user code. +namespace _internal { +// An index to be used to access a private storage of I/O streams. See +// colorize / nocolorize I/O manipulators for details. Due to the fact +// that static variables ain't shared between translation units, inline +// function with local static variable is used to do the trick and share +// the variable value between translation units. +inline int colorize_index() { + static int colorize_index = std::ios_base::xalloc(); + return colorize_index; +} + +//! Since C++ hasn't a true way to extract stream handler +//! from the a given `std::ostream` object, I have to write +//! this kind of hack. +inline FILE *get_standard_stream(const std::ostream &stream) { + if (&stream == &std::cout) + return stdout; + else if ((&stream == &std::cerr) || (&stream == &std::clog)) + return stderr; + + return nullptr; +} + +// Say whether a given stream should be colorized or not. It's always +// true for ATTY streams and may be true for streams marked with +// colorize flag. +inline bool is_colorized(std::ostream &stream) { + return is_atty(stream) || static_cast(stream.iword(colorize_index())); +} + +//! Test whether a given `std::ostream` object refers to +//! a terminal. +inline bool is_atty(const std::ostream &stream) { + FILE *std_stream = get_standard_stream(stream); + + // Unfortunately, fileno() ends with segmentation fault + // if invalid file descriptor is passed. So we need to + // handle this case gracefully and assume it's not a tty + // if standard stream is not detected, and 0 is returned. + if (!std_stream) return false; + +#if defined(TERMCOLOR_TARGET_POSIX) + return ::isatty(fileno(std_stream)); +#elif defined(TERMCOLOR_TARGET_WINDOWS) + return ::_isatty(_fileno(std_stream)); +#else + return false; +#endif +} + +#if defined(TERMCOLOR_TARGET_WINDOWS) +//! Change Windows Terminal colors attribute. If some +//! parameter is `-1` then attribute won't changed. +inline void win_change_attributes(std::ostream &stream, int foreground, int background) { + // yeah, i know.. it's ugly, it's windows. + static WORD defaultAttributes = 0; + + // Windows doesn't have ANSI escape sequences and so we use special + // API to change Terminal output color. That means we can't + // manipulate colors by means of "std::stringstream" and hence + // should do nothing in this case. + if (!_internal::is_atty(stream)) return; + + // get terminal handle + HANDLE hTerminal = INVALID_HANDLE_VALUE; + if (&stream == &std::cout) + hTerminal = GetStdHandle(STD_OUTPUT_HANDLE); + else if (&stream == &std::cerr) + hTerminal = GetStdHandle(STD_ERROR_HANDLE); + + // save default terminal attributes if it unsaved + if (!defaultAttributes) { + CONSOLE_SCREEN_BUFFER_INFO info; + if (!GetConsoleScreenBufferInfo(hTerminal, &info)) return; + defaultAttributes = info.wAttributes; + } + + // restore all default settings + if (foreground == -1 && background == -1) { + SetConsoleTextAttribute(hTerminal, defaultAttributes); + return; + } + + // get current settings + CONSOLE_SCREEN_BUFFER_INFO info; + if (!GetConsoleScreenBufferInfo(hTerminal, &info)) return; + + if (foreground != -1) { + info.wAttributes &= ~(info.wAttributes & 0x0F); + info.wAttributes |= static_cast(foreground); + } + + if (background != -1) { + info.wAttributes &= ~(info.wAttributes & 0xF0); + info.wAttributes |= static_cast(background); + } + + SetConsoleTextAttribute(hTerminal, info.wAttributes); +} +#endif // TERMCOLOR_TARGET_WINDOWS + +} // namespace _internal + +} // namespace termcolor + +#undef TERMCOLOR_TARGET_POSIX +#undef TERMCOLOR_TARGET_WINDOWS + +#if defined(TERMCOLOR_AUTODETECTED_IMPLEMENTATION) +#undef TERMCOLOR_USE_ANSI_ESCAPE_SEQUENCES +#undef TERMCOLOR_USE_WINDOWS_API +#endif + +#endif // TERMCOLOR_HPP_ + +#ifndef INDICATORS_TERMINAL_SIZE +#define INDICATORS_TERMINAL_SIZE +#include + +#if defined(_WIN32) +#include + +namespace indicators { + +static inline std::pair terminal_size() { + CONSOLE_SCREEN_BUFFER_INFO csbi; + int cols, rows; + GetConsoleScreenBufferInfo(GetStdHandle(STD_OUTPUT_HANDLE), &csbi); + cols = csbi.srWindow.Right - csbi.srWindow.Left + 1; + rows = csbi.srWindow.Bottom - csbi.srWindow.Top + 1; + return {static_cast(rows), static_cast(cols)}; +} + +static inline size_t terminal_width() { return terminal_size().second; } + +} // namespace indicators + +#else + +#include //ioctl() and TIOCGWINSZ +#include // for STDOUT_FILENO + +namespace indicators { + +static inline std::pair terminal_size() { + struct winsize size {}; + ioctl(STDOUT_FILENO, TIOCGWINSZ, &size); + return {static_cast(size.ws_row), static_cast(size.ws_col)}; +} + +static inline size_t terminal_width() { return terminal_size().second; } + +} // namespace indicators + +#endif + +#endif + +/* +Activity Indicators for Modern C++ +https://github.com/p-ranav/indicators + +Licensed under the MIT License . +SPDX-License-Identifier: MIT +Copyright (c) 2019 Dawid Pilarski . + +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. +*/ +#ifndef INDICATORS_SETTING +#define INDICATORS_SETTING + +#include +// #include +// #include +// #include +#include +#include +#include +#include +#include + +namespace indicators { + +namespace details { + +template +struct if_else; + +template <> +struct if_else { + using type = std::true_type; +}; + +template <> +struct if_else { + using type = std::false_type; +}; + +template +struct if_else_type; + +template +struct if_else_type { + using type = True; +}; + +template +struct if_else_type { + using type = False; +}; + +template +struct conjuction; + +template <> +struct conjuction<> : std::true_type {}; + +template +struct conjuction + : if_else_type>::type {}; + +template +struct disjunction; + +template <> +struct disjunction<> : std::false_type {}; + +template +struct disjunction + : if_else_type>::type {}; + +enum class ProgressBarOption { + bar_width = 0, + prefix_text, + postfix_text, + start, + end, + fill, + lead, + remainder, + max_postfix_text_len, + completed, + show_percentage, + show_elapsed_time, + show_remaining_time, + saved_start_time, + foreground_color, + spinner_show, + spinner_states, + font_styles, + hide_bar_when_complete, + min_progress, + max_progress, + progress_type, + stream +}; + +template +struct Setting { + template ::value>::type> + explicit Setting(Args &&...args) : value(std::forward(args)...) {} + Setting(const Setting &) = default; + Setting(Setting &&) = default; + + static constexpr auto id = Id; + using type = T; + + T value{}; +}; + +template +struct is_setting : std::false_type {}; + +template +struct is_setting> : std::true_type {}; + +template +struct are_settings : if_else...>::value>::type {}; + +template <> +struct are_settings<> : std::true_type {}; + +template +struct is_setting_from_tuple; + +template +struct is_setting_from_tuple> : std::true_type {}; + +template +struct is_setting_from_tuple> + : if_else...>::value>::type {}; + +template +struct are_settings_from_tuple + : if_else...>::value>::type {}; + +template +struct always_true { + static constexpr auto value = true; +}; + +template +Default &&get_impl(Default &&def) { + return std::forward(def); +} + +template +auto get_impl(Default && /*def*/, T &&first, Args &&.../*tail*/) -> + typename std::enable_if<(std::decay::type::id == Id), + decltype(std::forward(first))>::type { + return std::forward(first); +} + +template +auto get_impl(Default &&def, T && /*first*/, Args &&...tail) -> + typename std::enable_if<(std::decay::type::id != Id), + decltype(get_impl(std::forward(def), + std::forward(tail)...))>::type { + return get_impl(std::forward(def), std::forward(tail)...); +} + +template ::value, void>::type> +auto get(Default &&def, Args &&...args) + -> decltype(details::get_impl(std::forward(def), std::forward(args)...)) { + return details::get_impl(std::forward(def), std::forward(args)...); +} + +template +using StringSetting = Setting; + +template +using IntegerSetting = Setting; + +template +using BooleanSetting = Setting; + +template +struct option_idx; + +template +struct option_idx, counter> + : if_else_type<(Id == T::id), + std::integral_constant, + option_idx, counter + 1>>::type {}; + +template +struct option_idx, counter> { + static_assert(always_true<(ProgressBarOption)Id>::value, "No such option was found"); +}; + +template +auto get_value(Settings &&settings) + -> decltype((std::get::type>::value>( + std::declval()))) { + return std::get::type>::value>( + std::forward(settings)); +} + +} // namespace details + +namespace option { +using BarWidth = details::IntegerSetting; +using PrefixText = details::StringSetting; +using PostfixText = details::StringSetting; +using Start = details::StringSetting; +using End = details::StringSetting; +using Fill = details::StringSetting; +using Lead = details::StringSetting; +using Remainder = details::StringSetting; +using MaxPostfixTextLen = details::IntegerSetting; +using Completed = details::BooleanSetting; +using ShowPercentage = details::BooleanSetting; +using ShowElapsedTime = details::BooleanSetting; +using ShowRemainingTime = details::BooleanSetting; +using SavedStartTime = details::BooleanSetting; +using ForegroundColor = details::Setting; +using ShowSpinner = details::BooleanSetting; +using SpinnerStates = + details::Setting, details::ProgressBarOption::spinner_states>; +using HideBarWhenComplete = + details::BooleanSetting; +using FontStyles = + details::Setting, details::ProgressBarOption::font_styles>; +using MinProgress = details::IntegerSetting; +using MaxProgress = details::IntegerSetting; +using ProgressType = details::Setting; +using Stream = details::Setting; +} // namespace option +} // namespace indicators + +#endif + +#ifndef INDICATORS_CURSOR_CONTROL +#define INDICATORS_CURSOR_CONTROL + +#if defined(_MSC_VER) +#if !defined(NOMINMAX) +#define NOMINMAX +#endif +#include +#include +#else +#include +#endif + +namespace indicators { + +#if defined(_MSC_VER) + +static inline void show_console_cursor(bool const show) { + HANDLE out = GetStdHandle(STD_OUTPUT_HANDLE); + + CONSOLE_CURSOR_INFO cursorInfo; + + GetConsoleCursorInfo(out, &cursorInfo); + cursorInfo.bVisible = show; // set the cursor visibility + SetConsoleCursorInfo(out, &cursorInfo); +} + +static inline void erase_line() { + auto hStdout = GetStdHandle(STD_OUTPUT_HANDLE); + if (!hStdout) return; + + CONSOLE_SCREEN_BUFFER_INFO csbiInfo; + GetConsoleScreenBufferInfo(hStdout, &csbiInfo); + + COORD cursor; + + cursor.X = 0; + cursor.Y = csbiInfo.dwCursorPosition.Y; + + DWORD count = 0; + + FillConsoleOutputCharacterA(hStdout, ' ', csbiInfo.dwSize.X, cursor, &count); + + FillConsoleOutputAttribute(hStdout, csbiInfo.wAttributes, csbiInfo.dwSize.X, cursor, &count); + + SetConsoleCursorPosition(hStdout, cursor); +} + +#else + +static inline void show_console_cursor(bool const show) { + std::fputs(show ? "\033[?25h" : "\033[?25l", stdout); +} + +static inline void erase_line() { std::fputs("\r\033[K", stdout); } + +#endif + +} // namespace indicators + +#endif + +#ifndef INDICATORS_CURSOR_MOVEMENT +#define INDICATORS_CURSOR_MOVEMENT + +#if defined(_MSC_VER) +#if !defined(NOMINMAX) +#define NOMINMAX +#endif +#include +#include +#else +#include +#endif + +namespace indicators { + +#ifdef _MSC_VER + +static inline void move(int x, int y) { + auto hStdout = GetStdHandle(STD_OUTPUT_HANDLE); + if (!hStdout) return; + + CONSOLE_SCREEN_BUFFER_INFO csbiInfo; + GetConsoleScreenBufferInfo(hStdout, &csbiInfo); + + COORD cursor; + + cursor.X = csbiInfo.dwCursorPosition.X + x; + cursor.Y = csbiInfo.dwCursorPosition.Y + y; + SetConsoleCursorPosition(hStdout, cursor); +} + +static inline void move_up(int lines) { move(0, -lines); } +static inline void move_down(int lines) { move(0, -lines); } +static inline void move_right(int cols) { move(cols, 0); } +static inline void move_left(int cols) { move(-cols, 0); } + +#else + +static inline void move_up(int lines) { std::cout << "\033[" << lines << "A"; } +static inline void move_down(int lines) { std::cout << "\033[" << lines << "B"; } +static inline void move_right(int cols) { std::cout << "\033[" << cols << "C"; } +static inline void move_left(int cols) { std::cout << "\033[" << cols << "D"; } + +#endif + +} // namespace indicators + +#endif + +#ifndef INDICATORS_STREAM_HELPER +#define INDICATORS_STREAM_HELPER + +// #include +#ifndef INDICATORS_DISPLAY_WIDTH +#define INDICATORS_DISPLAY_WIDTH + +#include + +#include +#include +#include +#include + +namespace unicode { + +namespace details { + +/* + * This is an implementation of wcwidth() and wcswidth() (defined in + * IEEE Std 1002.1-2001) for Unicode. + * + * http://www.opengroup.org/onlinepubs/007904975/functions/wcwidth.html + * http://www.opengroup.org/onlinepubs/007904975/functions/wcswidth.html + * + * In fixed-width output devices, Latin characters all occupy a single + * "cell" position of equal width, whereas ideographic CJK characters + * occupy two such cells. Interoperability between terminal-line + * applications and (teletype-style) character terminals using the + * UTF-8 encoding requires agreement on which character should advance + * the cursor by how many cell positions. No established formal + * standards exist at present on which Unicode character shall occupy + * how many cell positions on character terminals. These routines are + * a first attempt of defining such behavior based on simple rules + * applied to data provided by the Unicode Consortium. + * + * For some graphical characters, the Unicode standard explicitly + * defines a character-cell width via the definition of the East Asian + * FullWidth (F), Wide (W), Half-width (H), and Narrow (Na) classes. + * In all these cases, there is no ambiguity about which width a + * terminal shall use. For characters in the East Asian Ambiguous (A) + * class, the width choice depends purely on a preference of backward + * compatibility with either historic CJK or Western practice. + * Choosing single-width for these characters is easy to justify as + * the appropriate long-term solution, as the CJK practice of + * displaying these characters as double-width comes from historic + * implementation simplicity (8-bit encoded characters were displayed + * single-width and 16-bit ones double-width, even for Greek, + * Cyrillic, etc.) and not any typographic considerations. + * + * Much less clear is the choice of width for the Not East Asian + * (Neutral) class. Existing practice does not dictate a width for any + * of these characters. It would nevertheless make sense + * typographically to allocate two character cells to characters such + * as for instance EM SPACE or VOLUME INTEGRAL, which cannot be + * represented adequately with a single-width glyph. The following + * routines at present merely assign a single-cell width to all + * neutral characters, in the interest of simplicity. This is not + * entirely satisfactory and should be reconsidered before + * establishing a formal standard in this area. At the moment, the + * decision which Not East Asian (Neutral) characters should be + * represented by double-width glyphs cannot yet be answered by + * applying a simple rule from the Unicode database content. Setting + * up a proper standard for the behavior of UTF-8 character terminals + * will require a careful analysis not only of each Unicode character, + * but also of each presentation form, something the author of these + * routines has avoided to do so far. + * + * http://www.unicode.org/unicode/reports/tr11/ + * + * Markus Kuhn -- 2007-05-26 (Unicode 5.0) + * + * Permission to use, copy, modify, and distribute this software + * for any purpose and without fee is hereby granted. The author + * disclaims all warranties with regard to this software. + * + * Latest version: http://www.cl.cam.ac.uk/~mgk25/ucs/wcwidth.c + */ + +struct interval { + int first; + int last; +}; + +/* auxiliary function for binary search in interval table */ +static inline int bisearch(wchar_t ucs, const struct interval *table, int max) { + int min = 0; + int mid; + + if (ucs < table[0].first || ucs > table[max].last) return 0; + while (max >= min) { + mid = (min + max) / 2; + if (ucs > table[mid].last) + min = mid + 1; + else if (ucs < table[mid].first) + max = mid - 1; + else + return 1; + } + + return 0; +} + +/* The following two functions define the column width of an ISO 10646 + * character as follows: + * + * - The null character (U+0000) has a column width of 0. + * + * - Other C0/C1 control characters and DEL will lead to a return + * value of -1. + * + * - Non-spacing and enclosing combining characters (general + * category code Mn or Me in the Unicode database) have a + * column width of 0. + * + * - SOFT HYPHEN (U+00AD) has a column width of 1. + * + * - Other format characters (general category code Cf in the Unicode + * database) and ZERO WIDTH SPACE (U+200B) have a column width of 0. + * + * - Hangul Jamo medial vowels and final consonants (U+1160-U+11FF) + * have a column width of 0. + * + * - Spacing characters in the East Asian Wide (W) or East Asian + * Full-width (F) category as defined in Unicode Technical + * Report #11 have a column width of 2. + * + * - All remaining characters (including all printable + * ISO 8859-1 and WGL4 characters, Unicode control characters, + * etc.) have a column width of 1. + * + * This implementation assumes that wchar_t characters are encoded + * in ISO 10646. + */ + +static inline int mk_wcwidth(wchar_t ucs) { + /* sorted list of non-overlapping intervals of non-spacing characters */ + /* generated by "uniset +cat=Me +cat=Mn +cat=Cf -00AD +1160-11FF +200B c" */ + static const struct interval combining[] = { + {0x0300, 0x036F}, {0x0483, 0x0486}, {0x0488, 0x0489}, {0x0591, 0x05BD}, + {0x05BF, 0x05BF}, {0x05C1, 0x05C2}, {0x05C4, 0x05C5}, {0x05C7, 0x05C7}, + {0x0600, 0x0603}, {0x0610, 0x0615}, {0x064B, 0x065E}, {0x0670, 0x0670}, + {0x06D6, 0x06E4}, {0x06E7, 0x06E8}, {0x06EA, 0x06ED}, {0x070F, 0x070F}, + {0x0711, 0x0711}, {0x0730, 0x074A}, {0x07A6, 0x07B0}, {0x07EB, 0x07F3}, + {0x0901, 0x0902}, {0x093C, 0x093C}, {0x0941, 0x0948}, {0x094D, 0x094D}, + {0x0951, 0x0954}, {0x0962, 0x0963}, {0x0981, 0x0981}, {0x09BC, 0x09BC}, + {0x09C1, 0x09C4}, {0x09CD, 0x09CD}, {0x09E2, 0x09E3}, {0x0A01, 0x0A02}, + {0x0A3C, 0x0A3C}, {0x0A41, 0x0A42}, {0x0A47, 0x0A48}, {0x0A4B, 0x0A4D}, + {0x0A70, 0x0A71}, {0x0A81, 0x0A82}, {0x0ABC, 0x0ABC}, {0x0AC1, 0x0AC5}, + {0x0AC7, 0x0AC8}, {0x0ACD, 0x0ACD}, {0x0AE2, 0x0AE3}, {0x0B01, 0x0B01}, + {0x0B3C, 0x0B3C}, {0x0B3F, 0x0B3F}, {0x0B41, 0x0B43}, {0x0B4D, 0x0B4D}, + {0x0B56, 0x0B56}, {0x0B82, 0x0B82}, {0x0BC0, 0x0BC0}, {0x0BCD, 0x0BCD}, + {0x0C3E, 0x0C40}, {0x0C46, 0x0C48}, {0x0C4A, 0x0C4D}, {0x0C55, 0x0C56}, + {0x0CBC, 0x0CBC}, {0x0CBF, 0x0CBF}, {0x0CC6, 0x0CC6}, {0x0CCC, 0x0CCD}, + {0x0CE2, 0x0CE3}, {0x0D41, 0x0D43}, {0x0D4D, 0x0D4D}, {0x0DCA, 0x0DCA}, + {0x0DD2, 0x0DD4}, {0x0DD6, 0x0DD6}, {0x0E31, 0x0E31}, {0x0E34, 0x0E3A}, + {0x0E47, 0x0E4E}, {0x0EB1, 0x0EB1}, {0x0EB4, 0x0EB9}, {0x0EBB, 0x0EBC}, + {0x0EC8, 0x0ECD}, {0x0F18, 0x0F19}, {0x0F35, 0x0F35}, {0x0F37, 0x0F37}, + {0x0F39, 0x0F39}, {0x0F71, 0x0F7E}, {0x0F80, 0x0F84}, {0x0F86, 0x0F87}, + {0x0F90, 0x0F97}, {0x0F99, 0x0FBC}, {0x0FC6, 0x0FC6}, {0x102D, 0x1030}, + {0x1032, 0x1032}, {0x1036, 0x1037}, {0x1039, 0x1039}, {0x1058, 0x1059}, + {0x1160, 0x11FF}, {0x135F, 0x135F}, {0x1712, 0x1714}, {0x1732, 0x1734}, + {0x1752, 0x1753}, {0x1772, 0x1773}, {0x17B4, 0x17B5}, {0x17B7, 0x17BD}, + {0x17C6, 0x17C6}, {0x17C9, 0x17D3}, {0x17DD, 0x17DD}, {0x180B, 0x180D}, + {0x18A9, 0x18A9}, {0x1920, 0x1922}, {0x1927, 0x1928}, {0x1932, 0x1932}, + {0x1939, 0x193B}, {0x1A17, 0x1A18}, {0x1B00, 0x1B03}, {0x1B34, 0x1B34}, + {0x1B36, 0x1B3A}, {0x1B3C, 0x1B3C}, {0x1B42, 0x1B42}, {0x1B6B, 0x1B73}, + {0x1DC0, 0x1DCA}, {0x1DFE, 0x1DFF}, {0x200B, 0x200F}, {0x202A, 0x202E}, + {0x2060, 0x2063}, {0x206A, 0x206F}, {0x20D0, 0x20EF}, {0x302A, 0x302F}, + {0x3099, 0x309A}, {0xA806, 0xA806}, {0xA80B, 0xA80B}, {0xA825, 0xA826}, + {0xFB1E, 0xFB1E}, {0xFE00, 0xFE0F}, {0xFE20, 0xFE23}, {0xFEFF, 0xFEFF}, + {0xFFF9, 0xFFFB}, {0x10A01, 0x10A03}, {0x10A05, 0x10A06}, {0x10A0C, 0x10A0F}, + {0x10A38, 0x10A3A}, {0x10A3F, 0x10A3F}, {0x1D167, 0x1D169}, {0x1D173, 0x1D182}, + {0x1D185, 0x1D18B}, {0x1D1AA, 0x1D1AD}, {0x1D242, 0x1D244}, {0xE0001, 0xE0001}, + {0xE0020, 0xE007F}, {0xE0100, 0xE01EF}}; + + /* test for 8-bit control characters */ + if (ucs == 0) return 0; + if (ucs < 32 || (ucs >= 0x7f && ucs < 0xa0)) return -1; + + /* binary search in table of non-spacing characters */ + if (bisearch(ucs, combining, sizeof(combining) / sizeof(struct interval) - 1)) return 0; + + /* if we arrive here, ucs is not a combining or C0/C1 control character */ + + return 1 + (ucs >= 0x1100 && + (ucs <= 0x115f || /* Hangul Jamo init. consonants */ + ucs == 0x2329 || ucs == 0x232a || + (ucs >= 0x2e80 && ucs <= 0xa4cf && ucs != 0x303f) || /* CJK ... Yi */ + (ucs >= 0xac00 && ucs <= 0xd7a3) || /* Hangul Syllables */ + (ucs >= 0xf900 && ucs <= 0xfaff) || /* CJK Compatibility Ideographs */ + (ucs >= 0xfe10 && ucs <= 0xfe19) || /* Vertical forms */ + (ucs >= 0xfe30 && ucs <= 0xfe6f) || /* CJK Compatibility Forms */ + (ucs >= 0xff00 && ucs <= 0xff60) || /* Fullwidth Forms */ + (ucs >= 0xffe0 && ucs <= 0xffe6) || (ucs >= 0x20000 && ucs <= 0x2fffd) || + (ucs >= 0x30000 && ucs <= 0x3fffd))); +} + +static inline int mk_wcswidth(const wchar_t *pwcs, size_t n) { + int w, width = 0; + + for (; *pwcs && n-- > 0; pwcs++) + if ((w = mk_wcwidth(*pwcs)) < 0) + return -1; + else + width += w; + + return width; +} + +/* + * The following functions are the same as mk_wcwidth() and + * mk_wcswidth(), except that spacing characters in the East Asian + * Ambiguous (A) category as defined in Unicode Technical Report #11 + * have a column width of 2. This variant might be useful for users of + * CJK legacy encodings who want to migrate to UCS without changing + * the traditional terminal character-width behaviour. It is not + * otherwise recommended for general use. + */ +static inline int mk_wcwidth_cjk(wchar_t ucs) { + /* sorted list of non-overlapping intervals of East Asian Ambiguous + * characters, generated by "uniset +WIDTH-A -cat=Me -cat=Mn -cat=Cf c" */ + static const struct interval ambiguous[] = { + {0x00A1, 0x00A1}, {0x00A4, 0x00A4}, {0x00A7, 0x00A8}, {0x00AA, 0x00AA}, + {0x00AE, 0x00AE}, {0x00B0, 0x00B4}, {0x00B6, 0x00BA}, {0x00BC, 0x00BF}, + {0x00C6, 0x00C6}, {0x00D0, 0x00D0}, {0x00D7, 0x00D8}, {0x00DE, 0x00E1}, + {0x00E6, 0x00E6}, {0x00E8, 0x00EA}, {0x00EC, 0x00ED}, {0x00F0, 0x00F0}, + {0x00F2, 0x00F3}, {0x00F7, 0x00FA}, {0x00FC, 0x00FC}, {0x00FE, 0x00FE}, + {0x0101, 0x0101}, {0x0111, 0x0111}, {0x0113, 0x0113}, {0x011B, 0x011B}, + {0x0126, 0x0127}, {0x012B, 0x012B}, {0x0131, 0x0133}, {0x0138, 0x0138}, + {0x013F, 0x0142}, {0x0144, 0x0144}, {0x0148, 0x014B}, {0x014D, 0x014D}, + {0x0152, 0x0153}, {0x0166, 0x0167}, {0x016B, 0x016B}, {0x01CE, 0x01CE}, + {0x01D0, 0x01D0}, {0x01D2, 0x01D2}, {0x01D4, 0x01D4}, {0x01D6, 0x01D6}, + {0x01D8, 0x01D8}, {0x01DA, 0x01DA}, {0x01DC, 0x01DC}, {0x0251, 0x0251}, + {0x0261, 0x0261}, {0x02C4, 0x02C4}, {0x02C7, 0x02C7}, {0x02C9, 0x02CB}, + {0x02CD, 0x02CD}, {0x02D0, 0x02D0}, {0x02D8, 0x02DB}, {0x02DD, 0x02DD}, + {0x02DF, 0x02DF}, {0x0391, 0x03A1}, {0x03A3, 0x03A9}, {0x03B1, 0x03C1}, + {0x03C3, 0x03C9}, {0x0401, 0x0401}, {0x0410, 0x044F}, {0x0451, 0x0451}, + {0x2010, 0x2010}, {0x2013, 0x2016}, {0x2018, 0x2019}, {0x201C, 0x201D}, + {0x2020, 0x2022}, {0x2024, 0x2027}, {0x2030, 0x2030}, {0x2032, 0x2033}, + {0x2035, 0x2035}, {0x203B, 0x203B}, {0x203E, 0x203E}, {0x2074, 0x2074}, + {0x207F, 0x207F}, {0x2081, 0x2084}, {0x20AC, 0x20AC}, {0x2103, 0x2103}, + {0x2105, 0x2105}, {0x2109, 0x2109}, {0x2113, 0x2113}, {0x2116, 0x2116}, + {0x2121, 0x2122}, {0x2126, 0x2126}, {0x212B, 0x212B}, {0x2153, 0x2154}, + {0x215B, 0x215E}, {0x2160, 0x216B}, {0x2170, 0x2179}, {0x2190, 0x2199}, + {0x21B8, 0x21B9}, {0x21D2, 0x21D2}, {0x21D4, 0x21D4}, {0x21E7, 0x21E7}, + {0x2200, 0x2200}, {0x2202, 0x2203}, {0x2207, 0x2208}, {0x220B, 0x220B}, + {0x220F, 0x220F}, {0x2211, 0x2211}, {0x2215, 0x2215}, {0x221A, 0x221A}, + {0x221D, 0x2220}, {0x2223, 0x2223}, {0x2225, 0x2225}, {0x2227, 0x222C}, + {0x222E, 0x222E}, {0x2234, 0x2237}, {0x223C, 0x223D}, {0x2248, 0x2248}, + {0x224C, 0x224C}, {0x2252, 0x2252}, {0x2260, 0x2261}, {0x2264, 0x2267}, + {0x226A, 0x226B}, {0x226E, 0x226F}, {0x2282, 0x2283}, {0x2286, 0x2287}, + {0x2295, 0x2295}, {0x2299, 0x2299}, {0x22A5, 0x22A5}, {0x22BF, 0x22BF}, + {0x2312, 0x2312}, {0x2460, 0x24E9}, {0x24EB, 0x254B}, {0x2550, 0x2573}, + {0x2580, 0x258F}, {0x2592, 0x2595}, {0x25A0, 0x25A1}, {0x25A3, 0x25A9}, + {0x25B2, 0x25B3}, {0x25B6, 0x25B7}, {0x25BC, 0x25BD}, {0x25C0, 0x25C1}, + {0x25C6, 0x25C8}, {0x25CB, 0x25CB}, {0x25CE, 0x25D1}, {0x25E2, 0x25E5}, + {0x25EF, 0x25EF}, {0x2605, 0x2606}, {0x2609, 0x2609}, {0x260E, 0x260F}, + {0x2614, 0x2615}, {0x261C, 0x261C}, {0x261E, 0x261E}, {0x2640, 0x2640}, + {0x2642, 0x2642}, {0x2660, 0x2661}, {0x2663, 0x2665}, {0x2667, 0x266A}, + {0x266C, 0x266D}, {0x266F, 0x266F}, {0x273D, 0x273D}, {0x2776, 0x277F}, + {0xE000, 0xF8FF}, {0xFFFD, 0xFFFD}, {0xF0000, 0xFFFFD}, {0x100000, 0x10FFFD}}; + + /* binary search in table of non-spacing characters */ + if (bisearch(ucs, ambiguous, sizeof(ambiguous) / sizeof(struct interval) - 1)) return 2; + + return mk_wcwidth(ucs); +} + +static inline int mk_wcswidth_cjk(const wchar_t *pwcs, size_t n) { + int w, width = 0; + + for (; *pwcs && n-- > 0; pwcs++) + if ((w = mk_wcwidth_cjk(*pwcs)) < 0) + return -1; + else + width += w; + + return width; +} + +// convert UTF-8 string to wstring +#ifdef _MSC_VER +static inline std::wstring utf8_decode(const std::string &s) { + auto r = setlocale(LC_ALL, ""); + std::string curLocale; + if (r) curLocale = r; + const char *_Source = s.c_str(); + size_t _Dsize = std::strlen(_Source) + 1; + wchar_t *_Dest = new wchar_t[_Dsize]; + size_t _Osize; + mbstowcs_s(&_Osize, _Dest, _Dsize, _Source, _Dsize); + std::wstring result = _Dest; + delete[] _Dest; + setlocale(LC_ALL, curLocale.c_str()); + return result; +} +#else +static inline std::wstring utf8_decode(const std::string &s) { + auto r = setlocale(LC_ALL, ""); + std::string curLocale; + if (r) curLocale = r; + const char *_Source = s.c_str(); + size_t _Dsize = mbstowcs(NULL, _Source, 0) + 1; + wchar_t *_Dest = new wchar_t[_Dsize]; + wmemset(_Dest, 0, _Dsize); + mbstowcs(_Dest, _Source, _Dsize); + std::wstring result = _Dest; + delete[] _Dest; + setlocale(LC_ALL, curLocale.c_str()); + return result; +} +#endif + +} // namespace details + +static inline int display_width(const std::string &input) { + using namespace unicode::details; + return mk_wcswidth(utf8_decode(input).c_str(), input.size()); +} + +static inline int display_width(const std::wstring &input) { + return details::mk_wcswidth(input.c_str(), input.size()); +} + +} // namespace unicode + +#endif +// #include +// #include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace indicators { +namespace details { + +inline void set_stream_color(std::ostream &os, Color color) { + switch (color) { + case Color::grey: + os << termcolor::grey; + break; + case Color::red: + os << termcolor::red; + break; + case Color::green: + os << termcolor::green; + break; + case Color::yellow: + os << termcolor::yellow; + break; + case Color::blue: + os << termcolor::blue; + break; + case Color::magenta: + os << termcolor::magenta; + break; + case Color::cyan: + os << termcolor::cyan; + break; + case Color::white: + os << termcolor::white; + break; + default: + assert(false); + } +} + +inline void set_font_style(std::ostream &os, FontStyle style) { + switch (style) { + case FontStyle::bold: + os << termcolor::bold; + break; + case FontStyle::dark: + os << termcolor::dark; + break; + case FontStyle::italic: + os << termcolor::italic; + break; + case FontStyle::underline: + os << termcolor::underline; + break; + case FontStyle::blink: + os << termcolor::blink; + break; + case FontStyle::reverse: + os << termcolor::reverse; + break; + case FontStyle::concealed: + os << termcolor::concealed; + break; + case FontStyle::crossed: + os << termcolor::crossed; + break; + default: + break; + } +} + +inline std::ostream &write_duration(std::ostream &os, std::chrono::nanoseconds ns) { + using namespace std; + using namespace std::chrono; + using days = duration>; + char fill = os.fill(); + os.fill('0'); + auto d = duration_cast(ns); + ns -= d; + auto h = duration_cast(ns); + ns -= h; + auto m = duration_cast(ns); + ns -= m; + auto s = duration_cast(ns); + if (d.count() > 0) os << setw(2) << d.count() << "d:"; + if (h.count() > 0) os << setw(2) << h.count() << "h:"; + os << setw(2) << m.count() << "m:" << setw(2) << s.count() << 's'; + os.fill(fill); + return os; +} + +class BlockProgressScaleWriter { +public: + BlockProgressScaleWriter(std::ostream &os, size_t bar_width) : os(os), bar_width(bar_width) {} + + std::ostream &write(float progress) { + std::string fill_text{"█"}; + std::vector lead_characters{" ", "▏", "▎", "▍", "▌", "▋", "▊", "▉"}; + auto value = (std::min)(1.0f, (std::max)(0.0f, progress / 100.0f)); + auto whole_width = std::floor(value * bar_width); + auto remainder_width = fmod((value * bar_width), 1.0f); + auto part_width = std::floor(remainder_width * lead_characters.size()); + std::string lead_text = lead_characters[size_t(part_width)]; + if ((bar_width - whole_width - 1) < 0) lead_text = ""; + for (size_t i = 0; i < whole_width; ++i) os << fill_text; + os << lead_text; + for (size_t i = 0; i < (bar_width - whole_width - 1); ++i) os << " "; + return os; + } + +private: + std::ostream &os; + size_t bar_width = 0; +}; + +class ProgressScaleWriter { +public: + ProgressScaleWriter(std::ostream &os, + size_t bar_width, + const std::string &fill, + const std::string &lead, + const std::string &remainder) + : os(os), bar_width(bar_width), fill(fill), lead(lead), remainder(remainder) {} + + std::ostream &write(float progress) { + auto pos = static_cast(progress * bar_width / 100.0); + for (size_t i = 0, current_display_width = 0; i < bar_width;) { + std::string next; + + if (i < pos) { + next = fill; + current_display_width = unicode::display_width(fill); + } else if (i == pos) { + next = lead; + current_display_width = unicode::display_width(lead); + } else { + next = remainder; + current_display_width = unicode::display_width(remainder); + } + + i += current_display_width; + + if (i > bar_width) { + // `next` is larger than the allowed bar width + // fill with empty space instead + os << std::string((bar_width - (i - current_display_width)), ' '); + break; + } + + os << next; + } + return os; + } + +private: + std::ostream &os; + size_t bar_width = 0; + std::string fill; + std::string lead; + std::string remainder; +}; + +class IndeterminateProgressScaleWriter { +public: + IndeterminateProgressScaleWriter(std::ostream &os, + size_t bar_width, + const std::string &fill, + const std::string &lead) + : os(os), bar_width(bar_width), fill(fill), lead(lead) {} + + std::ostream &write(size_t progress) { + for (size_t i = 0; i < bar_width;) { + std::string next; + size_t current_display_width = 0; + + if (i < progress) { + next = fill; + current_display_width = unicode::display_width(fill); + } else if (i == progress) { + next = lead; + current_display_width = unicode::display_width(lead); + } else { + next = fill; + current_display_width = unicode::display_width(fill); + } + + i += current_display_width; + + if (i > bar_width) { + // `next` is larger than the allowed bar width + // fill with empty space instead + os << std::string((bar_width - (i - current_display_width)), ' '); + break; + } + + os << next; + } + return os; + } + +private: + std::ostream &os; + size_t bar_width = 0; + std::string fill; + std::string lead; +}; + +} // namespace details +} // namespace indicators + +#endif + +#ifndef INDICATORS_PROGRESS_BAR +#define INDICATORS_PROGRESS_BAR + +// #include + +#include +#include +#include +#include +// #include +// #include +// #include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace indicators { + +class ProgressBar { + using Settings = std::tuple; + +public: + template < + typename... Args, + typename std::enable_if< + details::are_settings_from_tuple::type...>::value, + void *>::type = nullptr> + explicit ProgressBar(Args &&...args) + : settings_( + details::get(option::BarWidth{100}, + std::forward(args)...), + details::get(option::PrefixText{}, + std::forward(args)...), + details::get(option::PostfixText{}, + std::forward(args)...), + details::get(option::Start{"["}, + std::forward(args)...), + details::get(option::End{"]"}, + std::forward(args)...), + details::get(option::Fill{"="}, + std::forward(args)...), + details::get(option::Lead{">"}, + std::forward(args)...), + details::get(option::Remainder{" "}, + std::forward(args)...), + details::get( + option::MaxPostfixTextLen{0}, std::forward(args)...), + details::get(option::Completed{false}, + std::forward(args)...), + details::get( + option::ShowPercentage{false}, std::forward(args)...), + details::get( + option::ShowElapsedTime{false}, std::forward(args)...), + details::get( + option::ShowRemainingTime{false}, std::forward(args)...), + details::get( + option::SavedStartTime{false}, std::forward(args)...), + details::get( + option::ForegroundColor{Color::unspecified}, std::forward(args)...), + details::get( + option::FontStyles{std::vector{}}, std::forward(args)...), + details::get(option::MinProgress{0}, + std::forward(args)...), + details::get(option::MaxProgress{100}, + std::forward(args)...), + details::get( + option::ProgressType{ProgressType::incremental}, std::forward(args)...), + details::get(option::Stream{std::cout}, + std::forward(args)...)) { + // if progress is incremental, start from min_progress + // else start from max_progress + const auto type = get_value(); + if (type == ProgressType::incremental) + progress_ = get_value(); + else + progress_ = get_value(); + } + + template + void set_option(details::Setting &&setting) { + static_assert(!std::is_same( + std::declval()))>::type>::value, + "Setting has wrong type!"); + std::lock_guard lock(mutex_); + get_value() = std::move(setting).value; + } + + template + void set_option(const details::Setting &setting) { + static_assert(!std::is_same( + std::declval()))>::type>::value, + "Setting has wrong type!"); + std::lock_guard lock(mutex_); + get_value() = setting.value; + } + + void set_option( + const details::Setting &setting) { + std::lock_guard lock(mutex_); + get_value() = setting.value; + if (setting.value.length() > + get_value()) { + get_value() = setting.value.length(); + } + } + + void set_option( + details::Setting &&setting) { + std::lock_guard lock(mutex_); + get_value() = std::move(setting).value; + auto &new_value = get_value(); + if (new_value.length() > get_value()) { + get_value() = new_value.length(); + } + } + + void set_progress(size_t new_progress) { + { + std::lock_guard lock(mutex_); + progress_ = new_progress; + } + + save_start_time(); + print_progress(); + } + + void tick() { + { + std::lock_guard lock{mutex_}; + const auto type = get_value(); + if (type == ProgressType::incremental) + progress_ += 1; + else + progress_ -= 1; + } + save_start_time(); + print_progress(); + } + + size_t current() { + std::lock_guard lock{mutex_}; + return (std::min)(progress_, size_t(get_value())); + } + + bool is_completed() const { return get_value(); } + + void mark_as_completed() { + get_value() = true; + print_progress(); + } + +private: + template + auto get_value() -> decltype((details::get_value(std::declval()).value)) { + return details::get_value(settings_).value; + } + + template + auto get_value() const + -> decltype((details::get_value(std::declval()).value)) { + return details::get_value(settings_).value; + } + + size_t progress_{0}; + Settings settings_; + std::chrono::nanoseconds elapsed_; + std::chrono::time_point start_time_point_; + std::mutex mutex_; + + template + friend class MultiProgress; + template + friend class DynamicProgress; + std::atomic multi_progress_mode_{false}; + + void save_start_time() { + auto &show_elapsed_time = get_value(); + auto &saved_start_time = get_value(); + auto &show_remaining_time = get_value(); + if ((show_elapsed_time || show_remaining_time) && !saved_start_time) { + start_time_point_ = std::chrono::high_resolution_clock::now(); + saved_start_time = true; + } + } + + std::pair get_prefix_text() { + std::stringstream os; + os << get_value(); + const auto result = os.str(); + const auto result_size = unicode::display_width(result); + return {result, result_size}; + } + + std::pair get_postfix_text() { + std::stringstream os; + const auto max_progress = get_value(); + + if (get_value()) { + os << " " + << (std::min)( + static_cast(static_cast(progress_) / max_progress * 100), + size_t(100)) + << "%"; + } + + auto &saved_start_time = get_value(); + + if (get_value()) { + os << " ["; + if (saved_start_time) + details::write_duration(os, elapsed_); + else + os << "00:00s"; + } + + if (get_value()) { + if (get_value()) + os << "<"; + else + os << " ["; + + if (saved_start_time) { + auto eta = std::chrono::nanoseconds( + progress_ > 0 ? static_cast(std::ceil(float(elapsed_.count()) * + max_progress / progress_)) + : 0); + auto remaining = eta > elapsed_ ? (eta - elapsed_) : (elapsed_ - eta); + details::write_duration(os, remaining); + } else { + os << "00:00s"; + } + + os << "]"; + } else { + if (get_value()) os << "]"; + } + + os << " " << get_value(); + + const auto result = os.str(); + const auto result_size = unicode::display_width(result); + return {result, result_size}; + } + +public: + void print_progress(bool from_multi_progress = false) { + std::lock_guard lock{mutex_}; + + auto &os = get_value(); + + const auto type = get_value(); + const auto min_progress = get_value(); + const auto max_progress = get_value(); + if (multi_progress_mode_ && !from_multi_progress) { + if ((type == ProgressType::incremental && progress_ >= max_progress) || + (type == ProgressType::decremental && progress_ <= min_progress)) { + get_value() = true; + } + return; + } + auto now = std::chrono::high_resolution_clock::now(); + if (!get_value()) + elapsed_ = + std::chrono::duration_cast(now - start_time_point_); + + if (get_value() != Color::unspecified) + details::set_stream_color(os, + get_value()); + + for (auto &style : get_value()) + details::set_font_style(os, style); + + const auto prefix_pair = get_prefix_text(); + const auto prefix_text = prefix_pair.first; + const auto prefix_length = prefix_pair.second; + os << "\r" << prefix_text; + + os << get_value(); + + details::ProgressScaleWriter writer{os, get_value(), + get_value(), + get_value(), + get_value()}; + writer.write(double(progress_) / double(max_progress) * 100.0f); + + os << get_value(); + + const auto postfix_pair = get_postfix_text(); + const auto postfix_text = postfix_pair.first; + const auto postfix_length = postfix_pair.second; + os << postfix_text; + + // Get length of prefix text and postfix text + const auto start_length = get_value().size(); + const auto bar_width = get_value(); + const auto end_length = get_value().size(); + const auto terminal_width = terminal_size().second; + // prefix + bar_width + postfix should be <= terminal_width + const int remaining = terminal_width - (prefix_length + start_length + bar_width + + end_length + postfix_length); + if (prefix_length == -1 || postfix_length == -1) { + os << "\r"; + } else if (remaining > 0) { + os << std::string(remaining, ' ') << "\r"; + } else if (remaining < 0) { + // Do nothing. Maybe in the future truncate postfix with ... + } + os << "\033[F\n"; + os.flush(); + + if ((type == ProgressType::incremental && progress_ >= max_progress) || + (type == ProgressType::decremental && progress_ <= min_progress)) { + get_value() = true; + } + if (get_value() && + !from_multi_progress) // Don't std::endl if calling from MultiProgress + os << termcolor::reset << std::endl; + } +}; + +} // namespace indicators + +#endif + +#ifndef INDICATORS_BLOCK_PROGRESS_BAR +#define INDICATORS_BLOCK_PROGRESS_BAR + +// #include +// #include + +#include +#include +#include +// #include +// #include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace indicators { + +class BlockProgressBar { + using Settings = std::tuple; + +public: + template < + typename... Args, + typename std::enable_if< + details::are_settings_from_tuple::type...>::value, + void *>::type = nullptr> + explicit BlockProgressBar(Args &&...args) + : settings_(details::get( + option::ForegroundColor{Color::unspecified}, std::forward(args)...), + details::get( + option::BarWidth{100}, std::forward(args)...), + details::get(option::Start{"["}, + std::forward(args)...), + details::get(option::End{"]"}, + std::forward(args)...), + details::get( + option::PrefixText{""}, std::forward(args)...), + details::get( + option::PostfixText{""}, std::forward(args)...), + details::get( + option::ShowPercentage{true}, std::forward(args)...), + details::get( + option::ShowElapsedTime{false}, std::forward(args)...), + details::get( + option::ShowRemainingTime{false}, std::forward(args)...), + details::get( + option::Completed{false}, std::forward(args)...), + details::get( + option::SavedStartTime{false}, std::forward(args)...), + details::get( + option::MaxPostfixTextLen{0}, std::forward(args)...), + details::get( + option::FontStyles{std::vector{}}, std::forward(args)...), + details::get( + option::MaxProgress{100}, std::forward(args)...), + details::get( + option::Stream{std::cout}, std::forward(args)...)) {} + + template + void set_option(details::Setting &&setting) { + static_assert(!std::is_same( + std::declval()))>::type>::value, + "Setting has wrong type!"); + std::lock_guard lock(mutex_); + get_value() = std::move(setting).value; + } + + template + void set_option(const details::Setting &setting) { + static_assert(!std::is_same( + std::declval()))>::type>::value, + "Setting has wrong type!"); + std::lock_guard lock(mutex_); + get_value() = setting.value; + } + + void set_option( + const details::Setting &setting) { + std::lock_guard lock(mutex_); + get_value() = setting.value; + if (setting.value.length() > + get_value()) { + get_value() = setting.value.length(); + } + } + + void set_option( + details::Setting &&setting) { + std::lock_guard lock(mutex_); + get_value() = std::move(setting).value; + auto &new_value = get_value(); + if (new_value.length() > get_value()) { + get_value() = new_value.length(); + } + } + + void set_progress(float value) { + { + std::lock_guard lock{mutex_}; + progress_ = value; + } + save_start_time(); + print_progress(); + } + + void tick() { + { + std::lock_guard lock{mutex_}; + progress_ += 1; + } + save_start_time(); + print_progress(); + } + + size_t current() { + std::lock_guard lock{mutex_}; + return (std::min)(static_cast(progress_), + size_t(get_value())); + } + + bool is_completed() const { return get_value(); } + + void mark_as_completed() { + get_value() = true; + print_progress(); + } + +private: + template + auto get_value() -> decltype((details::get_value(std::declval()).value)) { + return details::get_value(settings_).value; + } + + template + auto get_value() const + -> decltype((details::get_value(std::declval()).value)) { + return details::get_value(settings_).value; + } + + Settings settings_; + float progress_{0.0}; + std::chrono::time_point start_time_point_; + std::mutex mutex_; + + template + friend class MultiProgress; + template + friend class DynamicProgress; + std::atomic multi_progress_mode_{false}; + + void save_start_time() { + auto &show_elapsed_time = get_value(); + auto &saved_start_time = get_value(); + auto &show_remaining_time = get_value(); + if ((show_elapsed_time || show_remaining_time) && !saved_start_time) { + start_time_point_ = std::chrono::high_resolution_clock::now(); + saved_start_time = true; + } + } + + std::pair get_prefix_text() { + std::stringstream os; + os << get_value(); + const auto result = os.str(); + const auto result_size = unicode::display_width(result); + return {result, result_size}; + } + + std::pair get_postfix_text() { + std::stringstream os; + const auto max_progress = get_value(); + auto now = std::chrono::high_resolution_clock::now(); + auto elapsed = + std::chrono::duration_cast(now - start_time_point_); + + if (get_value()) { + os << " " + << (std::min)(static_cast(progress_ / max_progress * 100.0), size_t(100)) + << "%"; + } + + auto &saved_start_time = get_value(); + + if (get_value()) { + os << " ["; + if (saved_start_time) + details::write_duration(os, elapsed); + else + os << "00:00s"; + } + + if (get_value()) { + if (get_value()) + os << "<"; + else + os << " ["; + + if (saved_start_time) { + auto eta = std::chrono::nanoseconds( + progress_ > 0 ? static_cast(std::ceil(float(elapsed.count()) * + max_progress / progress_)) + : 0); + auto remaining = eta > elapsed ? (eta - elapsed) : (elapsed - eta); + details::write_duration(os, remaining); + } else { + os << "00:00s"; + } + + os << "]"; + } else { + if (get_value()) os << "]"; + } + + os << " " << get_value(); + + const auto result = os.str(); + const auto result_size = unicode::display_width(result); + return {result, result_size}; + } + +public: + void print_progress(bool from_multi_progress = false) { + std::lock_guard lock{mutex_}; + + auto &os = get_value(); + + const auto max_progress = get_value(); + if (multi_progress_mode_ && !from_multi_progress) { + if (progress_ > max_progress) { + get_value() = true; + } + return; + } + + if (get_value() != Color::unspecified) + details::set_stream_color(os, + get_value()); + + for (auto &style : get_value()) + details::set_font_style(os, style); + + const auto prefix_pair = get_prefix_text(); + const auto prefix_text = prefix_pair.first; + const auto prefix_length = prefix_pair.second; + os << "\r" << prefix_text; + + os << get_value(); + + details::BlockProgressScaleWriter writer{ + os, get_value()}; + writer.write(progress_ / max_progress * 100); + + os << get_value(); + + const auto postfix_pair = get_postfix_text(); + const auto postfix_text = postfix_pair.first; + const auto postfix_length = postfix_pair.second; + os << postfix_text; + + // Get length of prefix text and postfix text + const auto start_length = get_value().size(); + const auto bar_width = get_value(); + const auto end_length = get_value().size(); + const auto terminal_width = terminal_size().second; + // prefix + bar_width + postfix should be <= terminal_width + const int remaining = terminal_width - (prefix_length + start_length + bar_width + + end_length + postfix_length); + if (prefix_length == -1 || postfix_length == -1) { + os << "\r"; + } else if (remaining > 0) { + os << std::string(remaining, ' ') << "\r"; + } else if (remaining < 0) { + // Do nothing. Maybe in the future truncate postfix with ... + } + os.flush(); + + if (progress_ > max_progress) { + get_value() = true; + } + if (get_value() && + !from_multi_progress) // Don't std::endl if calling from MultiProgress + os << termcolor::reset << std::endl; + } +}; + +} // namespace indicators + +#endif + +#ifndef INDICATORS_INDETERMINATE_PROGRESS_BAR +#define INDICATORS_INDETERMINATE_PROGRESS_BAR + +// #include + +#include +#include +#include +#include +// #include +// #include +// #include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace indicators { + +class IndeterminateProgressBar { + using Settings = std::tuple; + + enum class Direction { forward, backward }; + + Direction direction_{Direction::forward}; + +public: + template < + typename... Args, + typename std::enable_if< + details::are_settings_from_tuple::type...>::value, + void *>::type = nullptr> + explicit IndeterminateProgressBar(Args &&...args) + : settings_(details::get( + option::BarWidth{100}, std::forward(args)...), + details::get( + option::PrefixText{}, std::forward(args)...), + details::get( + option::PostfixText{}, std::forward(args)...), + details::get(option::Start{"["}, + std::forward(args)...), + details::get(option::End{"]"}, + std::forward(args)...), + details::get(option::Fill{"."}, + std::forward(args)...), + details::get(option::Lead{"<==>"}, + std::forward(args)...), + details::get( + option::MaxPostfixTextLen{0}, std::forward(args)...), + details::get( + option::Completed{false}, std::forward(args)...), + details::get( + option::ForegroundColor{Color::unspecified}, std::forward(args)...), + details::get( + option::FontStyles{std::vector{}}, std::forward(args)...), + details::get(option::Stream{std::cout}, + std::forward(args)...)) { + // starts with [<==>...........] + // progress_ = 0 + + // ends with [...........<==>] + // ^^^^^^^^^^^^^^^^^ bar_width + // ^^^^^^^^^^^^ (bar_width - len(lead)) + // progress_ = bar_width - len(lead) + progress_ = 0; + max_progress_ = get_value() - + get_value().size() + + get_value().size() + + get_value().size(); + } + + template + void set_option(details::Setting &&setting) { + static_assert(!std::is_same( + std::declval()))>::type>::value, + "Setting has wrong type!"); + std::lock_guard lock(mutex_); + get_value() = std::move(setting).value; + } + + template + void set_option(const details::Setting &setting) { + static_assert(!std::is_same( + std::declval()))>::type>::value, + "Setting has wrong type!"); + std::lock_guard lock(mutex_); + get_value() = setting.value; + } + + void set_option( + const details::Setting &setting) { + std::lock_guard lock(mutex_); + get_value() = setting.value; + if (setting.value.length() > + get_value()) { + get_value() = setting.value.length(); + } + } + + void set_option( + details::Setting &&setting) { + std::lock_guard lock(mutex_); + get_value() = std::move(setting).value; + auto &new_value = get_value(); + if (new_value.length() > get_value()) { + get_value() = new_value.length(); + } + } + + void tick() { + { + std::lock_guard lock{mutex_}; + if (get_value()) return; + + progress_ += (direction_ == Direction::forward) ? 1 : -1; + if (direction_ == Direction::forward && progress_ == max_progress_) { + // time to go back + direction_ = Direction::backward; + } else if (direction_ == Direction::backward && progress_ == 0) { + direction_ = Direction::forward; + } + } + print_progress(); + } + + bool is_completed() { return get_value(); } + + void mark_as_completed() { + get_value() = true; + print_progress(); + } + +private: + template + auto get_value() -> decltype((details::get_value(std::declval()).value)) { + return details::get_value(settings_).value; + } + + template + auto get_value() const + -> decltype((details::get_value(std::declval()).value)) { + return details::get_value(settings_).value; + } + + size_t progress_{0}; + size_t max_progress_; + Settings settings_; + std::chrono::nanoseconds elapsed_; + std::mutex mutex_; + + template + friend class MultiProgress; + template + friend class DynamicProgress; + std::atomic multi_progress_mode_{false}; + + std::pair get_prefix_text() { + std::stringstream os; + os << get_value(); + const auto result = os.str(); + const auto result_size = unicode::display_width(result); + return {result, result_size}; + } + + std::pair get_postfix_text() { + std::stringstream os; + os << " " << get_value(); + + const auto result = os.str(); + const auto result_size = unicode::display_width(result); + return {result, result_size}; + } + +public: + void print_progress(bool from_multi_progress = false) { + std::lock_guard lock{mutex_}; + + auto &os = get_value(); + + if (multi_progress_mode_ && !from_multi_progress) { + return; + } + if (get_value() != Color::unspecified) + details::set_stream_color(os, + get_value()); + + for (auto &style : get_value()) + details::set_font_style(os, style); + + const auto prefix_pair = get_prefix_text(); + const auto prefix_text = prefix_pair.first; + const auto prefix_length = prefix_pair.second; + os << "\r" << prefix_text; + + os << get_value(); + + details::IndeterminateProgressScaleWriter writer{ + os, get_value(), + get_value(), + get_value()}; + writer.write(progress_); + + os << get_value(); + + const auto postfix_pair = get_postfix_text(); + const auto postfix_text = postfix_pair.first; + const auto postfix_length = postfix_pair.second; + os << postfix_text; + + // Get length of prefix text and postfix text + const auto start_length = get_value().size(); + const auto bar_width = get_value(); + const auto end_length = get_value().size(); + const auto terminal_width = terminal_size().second; + // prefix + bar_width + postfix should be <= terminal_width + const int remaining = terminal_width - (prefix_length + start_length + bar_width + + end_length + postfix_length); + if (prefix_length == -1 || postfix_length == -1) { + os << "\r"; + } else if (remaining > 0) { + os << std::string(remaining, ' ') << "\r"; + } else if (remaining < 0) { + // Do nothing. Maybe in the future truncate postfix with ... + } + os.flush(); + + if (get_value() && + !from_multi_progress) // Don't std::endl if calling from MultiProgress + os << termcolor::reset << std::endl; + } +}; + +} // namespace indicators + +#endif + +#ifndef INDICATORS_MULTI_PROGRESS +#define INDICATORS_MULTI_PROGRESS +#include +#include +#include +#include +#include + +// #include +// #include +// #include + +namespace indicators { + +template +class MultiProgress { +public: + template ::type> + explicit MultiProgress(Indicators &...bars) { + bars_ = {bars...}; + for (auto &bar : bars_) { + bar.get().multi_progress_mode_ = true; + } + } + + template + typename std::enable_if<(index >= 0 && index < count), void>::type set_progress(size_t value) { + if (!bars_[index].get().is_completed()) bars_[index].get().set_progress(value); + print_progress(); + } + + template + typename std::enable_if<(index >= 0 && index < count), void>::type set_progress(float value) { + if (!bars_[index].get().is_completed()) bars_[index].get().set_progress(value); + print_progress(); + } + + template + typename std::enable_if<(index >= 0 && index < count), void>::type tick() { + if (!bars_[index].get().is_completed()) bars_[index].get().tick(); + print_progress(); + } + + template + typename std::enable_if<(index >= 0 && index < count), bool>::type is_completed() const { + return bars_[index].get().is_completed(); + } + +private: + std::atomic started_{false}; + std::mutex mutex_; + std::vector> bars_; + + bool _all_completed() { + bool result{true}; + for (size_t i = 0; i < count; ++i) result &= bars_[i].get().is_completed(); + return result; + } + +public: + void print_progress() { + std::lock_guard lock{mutex_}; + if (started_) move_up(count); + for (auto &bar : bars_) { + bar.get().print_progress(true); + std::cout << "\n"; + } + std::cout << termcolor::reset; + if (!started_) started_ = true; + } +}; + +} // namespace indicators + +#endif + +#ifndef INDICATORS_DYNAMIC_PROGRESS +#define INDICATORS_DYNAMIC_PROGRESS + +#include +#include +// #include +// #include +// #include +// #include +// #include +#include +#include +#include + +namespace indicators { + +template +class DynamicProgress { + using Settings = std::tuple; + +public: + template + explicit DynamicProgress(Indicators &...bars) { + bars_ = {bars...}; + for (auto &bar : bars_) { + bar.get().multi_progress_mode_ = true; + ++total_count_; + ++incomplete_count_; + } + } + + Indicator &operator[](size_t index) { + print_progress(); + std::lock_guard lock{mutex_}; + return bars_[index].get(); + } + + size_t push_back(Indicator &bar) { + std::lock_guard lock{mutex_}; + bar.multi_progress_mode_ = true; + bars_.push_back(bar); + return bars_.size() - 1; + } + + template + void set_option(details::Setting &&setting) { + static_assert(!std::is_same( + std::declval()))>::type>::value, + "Setting has wrong type!"); + std::lock_guard lock(mutex_); + get_value() = std::move(setting).value; + } + + template + void set_option(const details::Setting &setting) { + static_assert(!std::is_same( + std::declval()))>::type>::value, + "Setting has wrong type!"); + std::lock_guard lock(mutex_); + get_value() = setting.value; + } + +private: + Settings settings_; + std::atomic started_{false}; + std::mutex mutex_; + std::vector> bars_; + std::atomic total_count_{0}; + std::atomic incomplete_count_{0}; + + template + auto get_value() -> decltype((details::get_value(std::declval()).value)) { + return details::get_value(settings_).value; + } + + template + auto get_value() const + -> decltype((details::get_value(std::declval()).value)) { + return details::get_value(settings_).value; + } + +public: + void print_progress() { + std::lock_guard lock{mutex_}; + auto &hide_bar_when_complete = + get_value(); + if (hide_bar_when_complete) { + // Hide completed bars + if (started_) { + for (size_t i = 0; i < incomplete_count_; ++i) { + move_up(1); + erase_line(); + std::cout << std::flush; + } + } + incomplete_count_ = 0; + for (auto &bar : bars_) { + if (!bar.get().is_completed()) { + bar.get().print_progress(true); + std::cout << "\n"; + ++incomplete_count_; + } + } + if (!started_) started_ = true; + } else { + // Don't hide any bars + if (started_) move_up(static_cast(total_count_)); + for (auto &bar : bars_) { + bar.get().print_progress(true); + std::cout << "\n"; + } + if (!started_) started_ = true; + } + total_count_ = bars_.size(); + std::cout << termcolor::reset; + } +}; + +} // namespace indicators + +#endif + +#ifndef INDICATORS_PROGRESS_SPINNER +#define INDICATORS_PROGRESS_SPINNER + +// #include + +#include +#include +#include +#include +// #include +// #include +#include +#include +#include +#include +#include +#include +#include + +namespace indicators { + +class ProgressSpinner { + using Settings = std::tuple; + +public: + template < + typename... Args, + typename std::enable_if< + details::are_settings_from_tuple::type...>::value, + void *>::type = nullptr> + explicit ProgressSpinner(Args &&...args) + : settings_(details::get( + option::ForegroundColor{Color::unspecified}, std::forward(args)...), + details::get( + option::PrefixText{}, std::forward(args)...), + details::get( + option::PostfixText{}, std::forward(args)...), + details::get( + option::ShowPercentage{true}, std::forward(args)...), + details::get( + option::ShowElapsedTime{false}, std::forward(args)...), + details::get( + option::ShowRemainingTime{false}, std::forward(args)...), + details::get( + option::ShowSpinner{true}, std::forward(args)...), + details::get( + option::SavedStartTime{false}, std::forward(args)...), + details::get( + option::Completed{false}, std::forward(args)...), + details::get( + option::MaxPostfixTextLen{0}, std::forward(args)...), + details::get( + option::SpinnerStates{std::vector{"⠋", "⠙", "⠹", "⠸", "⠼", "⠴", + "⠦", "⠧", "⠇", "⠏"}}, + std::forward(args)...), + details::get( + option::FontStyles{std::vector{}}, std::forward(args)...), + details::get( + option::MaxProgress{100}, std::forward(args)...), + details::get( + option::Stream{std::cout}, std::forward(args)...)) {} + + template + void set_option(details::Setting &&setting) { + static_assert(!std::is_same( + std::declval()))>::type>::value, + "Setting has wrong type!"); + std::lock_guard lock(mutex_); + get_value() = std::move(setting).value; + } + + template + void set_option(const details::Setting &setting) { + static_assert(!std::is_same( + std::declval()))>::type>::value, + "Setting has wrong type!"); + std::lock_guard lock(mutex_); + get_value() = setting.value; + } + + void set_option( + const details::Setting &setting) { + std::lock_guard lock(mutex_); + get_value() = setting.value; + if (setting.value.length() > + get_value()) { + get_value() = setting.value.length(); + } + } + + void set_option( + details::Setting &&setting) { + std::lock_guard lock(mutex_); + get_value() = std::move(setting).value; + auto &new_value = get_value(); + if (new_value.length() > get_value()) { + get_value() = new_value.length(); + } + } + + void set_progress(size_t value) { + { + std::lock_guard lock{mutex_}; + progress_ = value; + } + save_start_time(); + print_progress(); + } + + void tick() { + { + std::lock_guard lock{mutex_}; + progress_ += 1; + } + save_start_time(); + print_progress(); + } + + size_t current() { + std::lock_guard lock{mutex_}; + return (std::min)(progress_, size_t(get_value())); + } + + bool is_completed() const { return get_value(); } + + void mark_as_completed() { + get_value() = true; + print_progress(); + } + +private: + Settings settings_; + size_t progress_{0}; + size_t index_{0}; + std::chrono::time_point start_time_point_; + std::mutex mutex_; + + template + auto get_value() -> decltype((details::get_value(std::declval()).value)) { + return details::get_value(settings_).value; + } + + template + auto get_value() const + -> decltype((details::get_value(std::declval()).value)) { + return details::get_value(settings_).value; + } + + void save_start_time() { + auto &show_elapsed_time = get_value(); + auto &show_remaining_time = get_value(); + auto &saved_start_time = get_value(); + if ((show_elapsed_time || show_remaining_time) && !saved_start_time) { + start_time_point_ = std::chrono::high_resolution_clock::now(); + saved_start_time = true; + } + } + +public: + void print_progress() { + std::lock_guard lock{mutex_}; + + auto &os = get_value(); + + const auto max_progress = get_value(); + auto now = std::chrono::high_resolution_clock::now(); + auto elapsed = + std::chrono::duration_cast(now - start_time_point_); + + if (get_value() != Color::unspecified) + details::set_stream_color(os, + get_value()); + + for (auto &style : get_value()) + details::set_font_style(os, style); + + os << get_value(); + if (get_value()) + os << get_value() + [index_ % get_value().size()]; + if (get_value()) { + os << " " << std::size_t(progress_ / double(max_progress) * 100) << "%"; + } + + if (get_value()) { + os << " ["; + details::write_duration(os, elapsed); + } + + if (get_value()) { + if (get_value()) + os << "<"; + else + os << " ["; + auto eta = std::chrono::nanoseconds( + progress_ > 0 ? static_cast( + std::ceil(float(elapsed.count()) * max_progress / progress_)) + : 0); + auto remaining = eta > elapsed ? (eta - elapsed) : (elapsed - eta); + details::write_duration(os, remaining); + os << "]"; + } else { + if (get_value()) os << "]"; + } + + if (get_value() == 0) + get_value() = 10; + os << " " << get_value() + << std::string(get_value(), ' ') + << "\r"; + os.flush(); + index_ += 1; + if (progress_ > max_progress) { + get_value() = true; + } + if (get_value()) os << termcolor::reset << std::endl; + } +}; + +} // namespace indicators + +#endif diff --git a/ros/launch/common_args.launch.py b/ros/launch/common_args.launch.py new file mode 100644 index 0000000..2d276dc --- /dev/null +++ b/ros/launch/common_args.launch.py @@ -0,0 +1,56 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "lidar_topic", + description="", + ), + DeclareLaunchArgument( + "lidar_odometry_topic", + default_value="lidar_odometry", + description="", + ), + DeclareLaunchArgument( + "lidar_odom_frame", + default_value="odom_lidar", + description="", + ), + DeclareLaunchArgument( + "wheel_odom_frame", + default_value="odom", + description="", + ), + DeclareLaunchArgument( + "base_frame", + default_value="base_footprint", + description="", + ), + DeclareLaunchArgument( + "publish_odom_tf", + default_value="true", + description="", + choices=["true", "false"], + ), + DeclareLaunchArgument( + "invert_odom_tf", + default_value="true", + description="", + choices=["true", "false"], + ), + DeclareLaunchArgument( + "visualize", + default_value="true", + description="", + choices=["true", "false"], + ), + DeclareLaunchArgument( + "bag_filenames", + default_value="", + description="Comma-separated list of file paths", + ), + ] + ) diff --git a/ros/launch/offline_node.launch.py b/ros/launch/offline_node.launch.py new file mode 100644 index 0000000..371d8bb --- /dev/null +++ b/ros/launch/offline_node.launch.py @@ -0,0 +1,85 @@ +import os +import sys + +import launch +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch.conditions import IfCondition +from launch.launch_description_sources import ( + get_launch_description_from_python_launch_file, +) +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument + + +def _generate_launch_description(context: launch.LaunchContext, *args, **kwargs): + use_sim_time = True + tf_timeout = 0.0 # tf buffer is always filled before processing any msg + + bag_filename = DeclareLaunchArgument(name="bag_filename", description="") + file = LaunchConfiguration("bag_filename") + output_dir_default = os.path.dirname(file.perform(context=context)) + output_dir = DeclareLaunchArgument( + name="output_dir", default_value=output_dir_default, description="" + ) + common_launch_args = get_launch_description_from_python_launch_file( + get_package_share_directory("kinematic_icp") + "/launch/common_args.launch.py", + ) + + kinematic_icp_offline_node = Node( + package="kinematic_icp", + executable="kinematic_icp_offline_node", + name="offline_node", + namespace="kinematic_icp", + output="screen", + emulate_tty=True, + remappings=[ + ("lidar_odometry", LaunchConfiguration("lidar_odometry_topic")), + ], + parameters=[ + # KISS-ICP configuration + get_package_share_directory("kinematic_icp") + + "/config/kinematic_icp_ros.yaml", + { + # Input topic, is not a remap to marry API with offline node + "input": LaunchConfiguration("lidar_topic"), + # ROS node configuration + "lidar_odom_frame": LaunchConfiguration("lidar_odom_frame"), + "wheel_odom_frame": LaunchConfiguration("wheel_odom_frame"), + "base_frame": LaunchConfiguration("base_frame"), + "publish_odom_tf": LaunchConfiguration("publish_odom_tf"), + "invert_odom_tf": LaunchConfiguration("invert_odom_tf"), + "tf_timeout": tf_timeout, + # ROS CLI arguments + "use_sim_time": use_sim_time, + # Offline node specific configuration + "bag_filename": file, + "output_dir": LaunchConfiguration("output_dir"), + "max_num_threads": 0, + }, + ], + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + output="screen", + arguments=[ + "-d", + get_package_share_directory("kinematic_icp") + "/rviz/kinematic_icp.rviz", + ], + condition=IfCondition(LaunchConfiguration("visualize")), + ) + return [ + bag_filename, + output_dir, + common_launch_args, + kinematic_icp_offline_node, + rviz_node, + ] + + +def generate_launch_description(): + return LaunchDescription([OpaqueFunction(function=_generate_launch_description)]) diff --git a/ros/launch/online_node.launch.py b/ros/launch/online_node.launch.py new file mode 100644 index 0000000..01c0a2a --- /dev/null +++ b/ros/launch/online_node.launch.py @@ -0,0 +1,56 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.launch_description_sources import ( + get_launch_description_from_python_launch_file, +) +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node +from launch_ros.substitutions.find_package import get_package_share_directory + + +def generate_launch_description(): + use_sim_time = LaunchConfiguration("use_sim_time", default=(os.getenv("SIMULATION") == "true")) + tf_timeout = LaunchConfiguration( + "tf_timeout", + default=PythonExpression(["'0.1' if ", use_sim_time, " else '0.0'"]), + ) + common_launch_args = get_launch_description_from_python_launch_file( + get_package_share_directory("kinematic_icp") + "/launch/common_args.launch.py", + ) + + kinematic_icp_online_node = Node( + package="kinematic_icp", + executable="kinematic_icp_online_node", + name="online_node", + namespace="kinematic_icp", + output="screen", + remappings=[ + ("lidar_odometry", LaunchConfiguration("lidar_odometry_topic")), + ], + parameters=[ + # KISS-ICP configuration + get_package_share_directory("kinematic_icp") + "/config/kinematic_icp_ros.yaml", + { + # Input topic, is not a remap to marry API with offline node + "input": LaunchConfiguration("lidar_topic"), + # ROS node configuration + "lidar_odom_frame": LaunchConfiguration("lidar_odom_frame"), + "wheel_odom_frame": LaunchConfiguration("wheel_odom_frame"), + "base_frame": LaunchConfiguration("base_frame"), + "publish_odom_tf": LaunchConfiguration("publish_odom_tf"), + "invert_odom_tf": LaunchConfiguration("invert_odom_tf"), + "tf_timeout": tf_timeout, + # ROS CLI arguments + "use_sim_time": use_sim_time, + }, + ], + ) + + return LaunchDescription( + [ + common_launch_args, + kinematic_icp_online_node, + ] + ) diff --git a/ros/package.xml b/ros/package.xml new file mode 100644 index 0000000..4e89867 --- /dev/null +++ b/ros/package.xml @@ -0,0 +1,60 @@ + + + + kinematic_icp + 0.0.1 + ROS 2 Wrapper + frevo137 + MIT + + ament_cmake + + geometry_msgs + nav_msgs + rclcpp + rclcpp_components + rcutils + sensor_msgs + std_msgs + std_srvs + tf2_ros + visualization_msgs + + + rosbag2_cpp + rosbag2_storage + + eigen + sophus + tbb + + ros2launch + + + + ament_cmake + + diff --git a/ros/rviz/kinematic_icp.rviz b/ros/rviz/kinematic_icp.rviz new file mode 100644 index 0000000..1a933a4 --- /dev/null +++ b/ros/rviz/kinematic_icp.rviz @@ -0,0 +1,522 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /point_clouds1 + - /point_clouds1/frame_deskew1 + - /point_clouds1/kiss_keypoints1 + - /point_clouds1/local_map1 + - /Odometry1 + - /Odometry2 + Splitter Ratio: 0.5 + Tree Height: 782 + - 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: frame_deskew +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.5 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 13.624558448791504 + Min Value: 0.5628786683082581 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 0 + Color Transformer: AxisColor + Decay Time: 10 + 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): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /kinematic_icp/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: false + 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.15000000596046448 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /kinematic_icp/keypoints + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 94.97420501708984 + Min Value: 0.734070897102356 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 100; 100; 100 + 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.05000000074505806 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /kinematic_icp/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: false + Enabled: true + Keep: 100 + Name: LiDAR 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: Reliable + Value: /husky_velocity_controller/odom + Value: true + Enabled: true + Name: Odometry + - Class: rviz_default_plugins/Marker + Enabled: false + Name: VoxelHashMap + Namespaces: + {} + Topic: + Depth: 1 + Durability Policy: System Default + Filter size: 1 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /kiss_icp/voxel_grid + Value: false + - 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: false + 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: Reliable + Value: /kinematic_icp/lidar_odometry + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + back_depth_base_link: + Value: true + back_depth_imu_frame: + Value: true + back_depth_left_camera_frame: + Value: true + back_depth_left_camera_optical_frame: + Value: true + back_depth_model_origin: + Value: true + back_depth_rgb_camera_frame: + Value: true + back_depth_rgb_camera_optical_frame: + Value: true + back_depth_right_camera_frame: + Value: true + back_depth_right_camera_optical_frame: + Value: true + base_bpearl_back_left_link: + Value: true + base_bpearl_front_right_link: + Value: true + base_cover: + Value: true + base_footprint: + Value: true + base_hokuyo_back_right_link: + Value: true + base_hokuyo_front_left_link: + Value: true + base_link: + Value: true + base_section_1: + Value: true + base_section_2: + Value: true + base_section_3: + Value: true + base_section_4: + Value: true + base_section_5: + Value: true + base_section_6: + Value: true + base_tower: + Value: true + bpearl_back_left_frame: + Value: true + bpearl_front_right_frame: + Value: true + caster_0_link: + Value: true + caster_1_link: + Value: true + caster_2_link: + Value: true + caster_3_link: + Value: true + cover_top: + Value: true + elp_back_base_camera_frame: + Value: true + elp_back_base_camera_link: + Value: true + elp_front_base_camera_frame: + Value: true + elp_front_base_camera_link: + Value: true + front_depth_base_link: + Value: true + front_depth_imu_frame: + Value: true + front_depth_left_camera_frame: + Value: true + front_depth_left_camera_optical_frame: + Value: true + front_depth_model_origin: + Value: true + front_depth_rgb_camera_frame: + Value: true + front_depth_rgb_camera_optical_frame: + Value: true + front_depth_right_camera_frame: + Value: true + front_depth_right_camera_optical_frame: + Value: true + hokuyo_back_right_frame: + Value: true + hokuyo_front_left_frame: + Value: true + left_wheel_link: + Value: true + mobile_coil: + Value: true + odom: + Value: true + odom_lidar: + Value: true + right_wheel_link: + Value: true + robot_top: + Value: true + tower_top: + Value: true + vn100_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + odom: + base_footprint: + back_depth_base_link: + back_depth_imu_frame: + {} + back_depth_left_camera_frame: + back_depth_left_camera_optical_frame: + {} + back_depth_model_origin: + {} + back_depth_rgb_camera_frame: + back_depth_rgb_camera_optical_frame: + {} + back_depth_right_camera_frame: + back_depth_right_camera_optical_frame: + {} + base_bpearl_back_left_link: + bpearl_back_left_frame: + {} + base_bpearl_front_right_link: + bpearl_front_right_frame: + {} + base_hokuyo_back_right_link: + hokuyo_back_right_frame: + {} + base_hokuyo_front_left_link: + hokuyo_front_left_frame: + {} + base_link: + caster_0_link: + {} + caster_1_link: + {} + caster_2_link: + {} + caster_3_link: + {} + left_wheel_link: + {} + right_wheel_link: + {} + base_tower: + base_cover: + cover_top: + {} + base_section_1: + base_section_2: + base_section_3: + base_section_4: + base_section_5: + base_section_6: + {} + tower_top: + {} + elp_back_base_camera_link: + elp_back_base_camera_frame: + {} + elp_front_base_camera_link: + elp_front_base_camera_frame: + {} + front_depth_base_link: + front_depth_imu_frame: + {} + front_depth_left_camera_frame: + front_depth_left_camera_optical_frame: + {} + front_depth_model_origin: + {} + front_depth_rgb_camera_frame: + front_depth_rgb_camera_optical_frame: + {} + front_depth_right_camera_frame: + front_depth_right_camera_optical_frame: + {} + mobile_coil: + {} + odom_lidar: + {} + robot_top: + {} + vn100_link: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Fixed Frame: odom_lidar + 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/ThirdPersonFollower + Distance: 76.9117660522461 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.210723876953125 + Y: -1.7889020442962646 + Z: -3.2766297408670653e-07 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5253978967666626 + Target Frame: base_footprint + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 4.713582515716553 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1403 + Hide Left Dock: true + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000048d000004cafc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000003d5000000c900fffffffb0000000a005600690065007700730000000418000000ef000000a400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014d0000069ffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000051fc0100000002fb0000000800540069006d0065010000000000000a000000027900fffffffb0000000800540069006d0065010000000000000450000000000000000000000a00000004ca00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2560 + X: 2560 + Y: 0 diff --git a/ros/src/kinematic_icp_ros/nodes/offline_node.cpp b/ros/src/kinematic_icp_ros/nodes/offline_node.cpp new file mode 100644 index 0000000..a268bc0 --- /dev/null +++ b/ros/src/kinematic_icp_ros/nodes/offline_node.cpp @@ -0,0 +1,133 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#include +#include +#include +#include +#include +#include +#include + +// ROS +#include +#include + +#include "kinematic_icp_ros/nodes/offline_node.hpp" +#include "kinematic_icp_ros/server/LidarOdometryServer.hpp" +#include "kinematic_icp_ros/utils/RosbagUtils.hpp" +#include "kinematic_icp_ros/utils/indicators.hpp" + +namespace { +std::string generateOutputFilename(const std::string &bag_filename) { + size_t last_dot = bag_filename.find_last_of("."); + std::string output_file = bag_filename.substr(0, last_dot); + output_file += "_kinematic_icp_poses_tum.txt"; + std::filesystem::path output_path(output_file); + return output_path.filename().string(); +} +} // namespace + +namespace kinematic_icp_ros { + +OfflineNode::OfflineNode(const rclcpp::NodeOptions &options) { + node_ = rclcpp::Node::make_shared("kinematic_icp_offline_node", options); + pcl_topic_ = node_->declare_parameter("input"); + odometry_server_ = std::make_shared(node_); + + auto bag_filename = node_->declare_parameter("bag_filename"); + poses_filename_ = generateOutputFilename(bag_filename); + output_dir_ = node_->declare_parameter("output_dir"); + auto tf_bridge = std::make_shared(node_); + bag_multiplexer_.AddBag(BufferableBag(bag_filename, tf_bridge, pcl_topic_)); +} + +void OfflineNode::writePosesInTumFormat() { + const std::string output_file = output_dir_ + poses_filename_; + std::ofstream file(output_file); + if (!file.is_open()) { + std::cerr << "Error opening file: " << output_file << std::endl; + return; + } + + RCLCPP_INFO_STREAM(node_->get_logger(), "Saving poses in TUM format in " << output_file); + + // Iterate over the poses and timestamps + for (const auto &[timestamp, pose] : poses_with_timestamps_) { + const Eigen::Vector3d &translation = pose.translation(); + const Eigen::Quaterniond &quaternion = pose.so3().unit_quaternion(); + // Write the timestamp, position, and quaternion to the file + file << std::fixed << std::setprecision(6) << timestamp << " " << translation.x() << " " + << translation.y() << " " << translation.z() << " " << quaternion.x() << " " + << quaternion.y() << " " << quaternion.z() << " " << quaternion.w() << "\n"; + } + + // Close the file + file.close(); +} + +void OfflineNode::Run() { + indicators::ProgressBar bar{indicators::option::BarWidth{100}, + indicators::option::Start{"["}, + indicators::option::Fill{"="}, + indicators::option::Lead{">"}, + indicators::option::Remainder{" "}, + indicators::option::End{"]"}, + indicators::option::ForegroundColor{indicators::Color::cyan}, + indicators::option::ShowPercentage{true}, + indicators::option::ShowElapsedTime{true}, + indicators::option::ShowRemainingTime{true}, + indicators::option::Stream{std::cout}, + indicators::option::MaxProgress{bag_multiplexer_.message_count()}}; + // Deserialize the next pointcloud message from the bagfiles + auto GetNextMsg = [this] { + const rclcpp::Serialization pcl2_serializer; + const auto pc_msg = std::make_shared(); + const auto &msg = bag_multiplexer_.GetNextMessage(); + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + pcl2_serializer.deserialize_message(&serialized_msg, pc_msg.get()); + return pc_msg; + }; + + // This is the main blocking loop that this simulates the subscription form the online node, + // but instead of fetching the data from a topic, it does it from the provided bagfiles + while (rclcpp::ok() && bag_multiplexer_.IsMessageAvailable()) { + const auto msg = GetNextMsg(); + odometry_server_->RegisterFrame(msg); + const auto &stamp = odometry_server_->current_stamp_; + poses_with_timestamps_.emplace_back(stamp.sec + stamp.nanosec * 1e-9, + odometry_server_->kinematic_icp_->pose()); + bar.tick(); + } + bar.mark_as_completed(); +} + +} // namespace kinematic_icp_ros + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto offline_node = kinematic_icp_ros::OfflineNode(rclcpp::NodeOptions()); + offline_node.Run(); + offline_node.writePosesInTumFormat(); + rclcpp::shutdown(); + return 0; +} diff --git a/ros/src/kinematic_icp_ros/nodes/online_node.cpp b/ros/src/kinematic_icp_ros/nodes/online_node.cpp new file mode 100644 index 0000000..a5dfeb1 --- /dev/null +++ b/ros/src/kinematic_icp_ros/nodes/online_node.cpp @@ -0,0 +1,55 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#include +#include + +// ROS +#include +#include +#include +#include + +#include "kinematic_icp_ros/nodes/online_node.hpp" +#include "kinematic_icp_ros/server/LidarOdometryServer.hpp" + +namespace kinematic_icp_ros { + +OnlineNode ::OnlineNode(const rclcpp::NodeOptions &options) { + node_ = rclcpp::Node::make_shared("kinematic_icp_online_node", options); + pcl_topic_ = node_->declare_parameter("input"); + odometry_server_ = std::make_shared(node_); + pointcloud_sub_ = node_->create_subscription( + pcl_topic_, rclcpp::SystemDefaultsQoS(), + [&](const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { + odometry_server_->RegisterFrame(msg); + }); +} + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr OnlineNode::get_node_base_interface() { + return node_->get_node_base_interface(); +} + +} // namespace kinematic_icp_ros + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(kinematic_icp_ros::OnlineNode) diff --git a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp new file mode 100644 index 0000000..66bf3de --- /dev/null +++ b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp @@ -0,0 +1,266 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#include "kinematic_icp_ros/server/LidarOdometryServer.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "kinematic_icp/pipeline/KinematicICP.hpp" +#include "kinematic_icp_ros/utils/RosUtils.hpp" + +// ROS 2 headers +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { +// Time Type aliases to simplify code +using milliseconds = std::chrono::milliseconds; +using seconds = std::chrono::duration; +using std::chrono::duration_cast; +} // namespace + +namespace kinematic_icp_ros { + +using namespace utils; + +LidarOdometryServer::LidarOdometryServer(rclcpp::Node::SharedPtr node) : node_(node) { + lidar_odom_frame_ = node->declare_parameter("lidar_odom_frame", lidar_odom_frame_); + wheel_odom_frame_ = node->declare_parameter("wheel_odom_frame", wheel_odom_frame_); + base_frame_ = node->declare_parameter("base_frame", base_frame_); + publish_odom_tf_ = node->declare_parameter("publish_odom_tf", publish_odom_tf_); + invert_odom_tf_ = node->declare_parameter("invert_odom_tf", invert_odom_tf_); + tf_timeout_ = + duration_cast(seconds(node->declare_parameter("tf_timeout", 0.0))); + + kinematic_icp::pipeline::Config config; + config.max_range = node->declare_parameter("max_range", config.max_range); + config.min_range = node->declare_parameter("min_range", config.min_range); + config.deskew = node->declare_parameter("deskew", config.deskew); + config.voxel_size = node->declare_parameter("voxel_size", config.max_range / 100.0); + config.max_points_per_voxel = + node->declare_parameter("max_points_per_voxel", config.max_points_per_voxel); + config.initial_threshold = + node->declare_parameter("initial_threshold", config.initial_threshold); + config.min_motion_th = node->declare_parameter("min_motion_th", config.min_motion_th); + config.max_num_iterations = + node->declare_parameter("max_num_iterations", config.max_num_iterations); + config.convergence_criterion = + node->declare_parameter("convergence_criterion", config.convergence_criterion); + config.max_num_threads = + node->declare_parameter("max_num_threads", config.max_num_threads); + if (config.max_range < config.min_range) { + RCLCPP_WARN(node_->get_logger(), + "[WARNING] max_range is smaller than min_range, settng min_range to 0.0"); + config.min_range = 0.0; + } + + // Construct the main KISS-ICP odometry node + kinematic_icp_ = std::make_unique(config); + + // Initialize publishers + rclcpp::QoS qos((rclcpp::SystemDefaultsQoS().keep_last(1).durability_volatile())); + odom_publisher_ = node_->create_publisher("lidar_odometry", qos); + frame_publisher_ = node_->create_publisher("frame", qos); + kpoints_publisher_ = node_->create_publisher("keypoints", qos); + map_publisher_ = node_->create_publisher("local_map", qos); + voxel_grid_pub_ = node_->create_publisher("voxel_grid", qos); + + set_pose_srv_ = node_->create_service( + "set_pose", [&](const std::shared_ptr request, + std::shared_ptr response) { + const auto pose = LookupTransform(wheel_odom_frame_, base_frame_, tf2_buffer_); + RCLCPP_WARN_STREAM(node_->get_logger(), "Resetting KISS-ICP pose:\n" + << pose.matrix() << "\n"); + kinematic_icp_->SetPose(pose); + response->success = true; + }); + + // Initialize the transform broadcaster + tf_broadcaster_ = std::make_unique(node_); + tf2_buffer_ = std::make_unique(node_->get_clock()); + tf2_listener_ = std::make_unique(*tf2_buffer_); + + // Initialize the odometry and tf msg. Since tf by design does not allow for having two frames + // in the tree with a different parent, to publish the odom_lidar frame, we need to add a child + // frame (we can't provide odom_lidar -> base_footprint, which is what we estimate internally). + // Therefore we publish the inverted transformation (base_footprint -> odom_lidar), which turns + // to be an implemantation detail as tf lookups are transparent and one can query for the + // odom_lidar -> base_footprint transform, the tf will invert it again on the fly. + if (invert_odom_tf_) { + tf_msg_.header.frame_id = base_frame_; + tf_msg_.child_frame_id = lidar_odom_frame_; + } else { + tf_msg_.header.frame_id = lidar_odom_frame_; + tf_msg_.child_frame_id = base_frame_; + } + + // fixed covariancecovariance + const auto position_covariance = node->declare_parameter("position_covariance", 0.1); + const auto orientation_covariance = + node->declare_parameter("orientation_covariance", 0.1); + odom_msg_.header.frame_id = lidar_odom_frame_; + odom_msg_.child_frame_id = base_frame_; + odom_msg_.pose.covariance.fill(0.0); + odom_msg_.pose.covariance[0] = position_covariance; + odom_msg_.pose.covariance[7] = position_covariance; + odom_msg_.pose.covariance[35] = orientation_covariance; + odom_msg_.twist.covariance.fill(0); + odom_msg_.twist.covariance[0] = position_covariance; + odom_msg_.twist.covariance[7] = position_covariance; + odom_msg_.twist.covariance[35] = orientation_covariance; +} + +void LidarOdometryServer::InitializePoseAndExtrinsic(const std::string &lidar_frame_id) { + if (!tf2_buffer_->_frameExists(wheel_odom_frame_) || !tf2_buffer_->_frameExists(base_frame_) || + !tf2_buffer_->_frameExists(lidar_frame_id)) { + return; + } + + // Independent from the service, start the ROS node from a known state + const auto pose = LookupTransform(wheel_odom_frame_, base_frame_, tf2_buffer_); + RCLCPP_INFO_STREAM(node_->get_logger(), "Resetting KISS-ICP pose:\n" << pose.matrix() << "\n"); + kinematic_icp_->SetPose(pose); + + try { + sensor_to_base_footprint_ = tf2::transformToSophus( + tf2_buffer_->lookupTransform(base_frame_, lidar_frame_id, tf2::TimePointZero)); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(node_->get_logger(), "%s", ex.what()); + return; + } + + // Initialization finished + RCLCPP_INFO(node_->get_logger(), "KISS-ICP ROS 2 odometry node initialized"); + initialize_odom_node = true; +} + +void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { + if (!initialize_odom_node) { + InitializePoseAndExtrinsic(msg->header.frame_id); + } + + // Buffer the last state This will be used for computing the veloicty + const auto last_stamp = current_stamp_; + const auto last_pose = kinematic_icp_->pose(); + // Extract timestamps + const auto timestamps = GetTimestamps(msg); + const auto &[min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); + // Convert to ros time + auto to_ros_stamp = [](const double &time) { + builtin_interfaces::msg::Time stamp; + stamp.sec = static_cast(std::floor(time)); // seconds part + stamp.nanosec = static_cast((time - stamp.sec) * 1e9); // nanoseconds part + return stamp; + }; + // Update what is the current stamp of this iteration + const auto begin_scan_stamp = min_it != timestamps.cend() ? to_ros_stamp(*min_it) : last_stamp; + const auto end_scan_stamp = + max_it != timestamps.cend() ? to_ros_stamp(*max_it) : msg->header.stamp; + current_stamp_ = end_scan_stamp; + // Get the initial guess from the wheel odometry + const auto delta = + LookupDeltaTransform(base_frame_, begin_scan_stamp, base_frame_, end_scan_stamp, + wheel_odom_frame_, tf_timeout_, tf2_buffer_); + + // Run kinematic ICP + if (delta.log().norm() > 1e-3) { + const auto &extrinsic = sensor_to_base_footprint_; + const auto points = PointCloud2ToEigen(msg, {}); + const auto normalized_timestamps = NormalizeTimestamps(timestamps); + const auto &[frame, kpoints] = + kinematic_icp_->RegisterFrame(points, normalized_timestamps, extrinsic, delta); + PublishClouds(frame, kpoints); + } + + // Compute velocities, use the elapsed time between the current msg and the last received + const double elapsed_time = *max_it - *min_it; + const Sophus::SE3d::Tangent delta_twist = (last_pose.inverse() * kinematic_icp_->pose()).log(); + const Sophus::SE3d::Tangent velocity = delta_twist / elapsed_time; + + // Spit the current estimated pose to ROS pc_out_msgs handling the desired target frame + PublishOdometryMsg(kinematic_icp_->pose(), velocity); +} + +void LidarOdometryServer::PublishOdometryMsg(const Sophus::SE3d &pose, + const Sophus::SE3d::Tangent &velocity) { + // Broadcast over the tf tree + if (publish_odom_tf_) { + tf_msg_.transform = [&]() { + if (invert_odom_tf_) return tf2::sophusToTransform(pose.inverse()); + return tf2::sophusToTransform(pose); + }(); + tf_msg_.header.stamp = current_stamp_; + tf_broadcaster_->sendTransform(tf_msg_); + } + + // publish odometry msg + odom_msg_.pose.pose = tf2::sophusToPose(pose); + odom_msg_.twist.twist.linear.x = velocity[0]; + odom_msg_.twist.twist.angular.z = velocity[5]; + odom_msg_.header.stamp = current_stamp_; + odom_publisher_->publish(odom_msg_); +} + +void LidarOdometryServer::PublishClouds(const std::vector frame, + const std::vector keypoints) { + // For re-publishing the input frame and keypoints, we do it in the LiDAR coordinate frames + std_msgs::msg::Header lidar_header; + lidar_header.frame_id = base_frame_; + lidar_header.stamp = current_stamp_; + + // The internal map representation is in the lidar_odom_frame_ + std_msgs::msg::Header map_header; + map_header.frame_id = lidar_odom_frame_; + map_header.stamp = current_stamp_; + + // Check for subscriptions before publishing to avoid unnecesary CPU usage + if (frame_publisher_->get_subscription_count() > 0) { + frame_publisher_->publish(std::move(EigenToPointCloud2(frame, lidar_header))); + } + if (kpoints_publisher_->get_subscription_count() > 0) { + kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, lidar_header))); + } + if (map_publisher_->get_subscription_count() > 0) { + map_publisher_->publish( + std::move(EigenToPointCloud2(kinematic_icp_->LocalMap(), map_header))); + } +} + +} // namespace kinematic_icp_ros diff --git a/ros/src/kinematic_icp_ros/utils/RosUtils.cpp b/ros/src/kinematic_icp_ros/utils/RosUtils.cpp new file mode 100644 index 0000000..198a1bd --- /dev/null +++ b/ros/src/kinematic_icp_ros/utils/RosUtils.cpp @@ -0,0 +1,198 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#include "kinematic_icp_ros/utils/RosUtils.hpp" + +namespace kinematic_icp_ros::utils { + +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("kinematic_icp_ros"), + "Field 't', 'timestamp', or 'time' does not exist. " + "Disabling scan deskewing"); + return {}; +} + +// Normalize timestamps from 0.0 to 1.0 +std::vector NormalizeTimestamps(const std::vector ×tamps) { + const auto [min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); + const double min_timestamp = min_it != timestamps.cend() ? *min_it : 0.0; + const double max_timestamp = max_it != timestamps.cend() ? *max_it : 1.0; + + 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; +} + +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 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"); +} + +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; +} + +std::vector PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg, + const Sophus::SE3d &T) { + std::vector points; + points.reserve(msg->height * msg->width); + sensor_msgs::PointCloud2ConstIterator iter_xyz(*msg, "x"); + for (size_t i = 0; i < msg->height * msg->width; ++i, ++iter_xyz) { + points.emplace_back(T * Eigen::Vector3d{iter_xyz[0], iter_xyz[1], iter_xyz[2]}); + } + return points; +} + +std::unique_ptr EigenToPointCloud2(const std::vector &points, + const Header &header) { + auto msg = std::make_unique(); + msg->header = header; + + auto &offset = msg->point_step; + offset = addPointField(*msg, "x", 1, PointField::FLOAT32, offset); + offset = addPointField(*msg, "y", 1, PointField::FLOAT32, offset); + offset = addPointField(*msg, "z", 1, PointField::FLOAT32, offset); + + // Resize the + sensor_msgs::PointCloud2Modifier modifier(*msg); + modifier.resize(points.size()); + + sensor_msgs::PointCloud2Iterator iter_xyz(*msg, "x"); + for (size_t i = 0; i < points.size(); i++, ++iter_xyz) { + const Eigen::Vector3d &point = points[i]; + iter_xyz[0] = point.x(); + iter_xyz[1] = point.y(); + iter_xyz[2] = point.z(); + } + return msg; +} + +visualization_msgs::msg::Marker VoxelsToMarker(const std::vector &voxels, + const double voxel_size, + const Header &header) { + visualization_msgs::msg::Marker voxel_grid; + voxel_grid.header = header; + voxel_grid.ns = "voxel_grid"; + voxel_grid.id = 0; // Always same voxel grid, this should modify the whole structure + voxel_grid.type = visualization_msgs::msg::Marker::LINE_LIST; + voxel_grid.action = visualization_msgs::msg::Marker::MODIFY; + + // TODO(God): Convert this marker into a custom vizualization type so we can modify these 5 + // parameters from rviz instead of the ROS node! + voxel_grid.scale.x = 0.01; // Line width + voxel_grid.color.a = 0.1; // alpha + voxel_grid.color.r = 1.0; // red + voxel_grid.color.g = 1.0; // green + voxel_grid.color.b = 1.0; // blue + + // Compute the 8 vertices of each voxel in grid coordinates + const auto corners = [&voxels]() { + std::vector> voxel_corners; + voxel_corners.reserve(voxels.size()); + for (const auto &voxel : voxels) { + // clang-format off + voxel_corners.emplace_back(std::array{ + voxel + Eigen::Vector3i{0, 0, 0}, + voxel + Eigen::Vector3i{1, 0, 0}, + voxel + Eigen::Vector3i{1, 1, 0}, + voxel + Eigen::Vector3i{0, 1, 0}, + voxel + Eigen::Vector3i{0, 0, 1}, + voxel + Eigen::Vector3i{1, 0, 1}, + voxel + Eigen::Vector3i{1, 1, 1}, + voxel + Eigen::Vector3i{0, 1, 1} + }); + // clang-format on + } + return voxel_corners; + }(); + + // Lambda function to add an edge between two voxel vertices + auto AddEdge = [&](const Eigen::Vector3i &vertex1, const Eigen::Vector3i &vertex2) { + geometry_msgs::msg::Point point1, point2; + point1.x = vertex1.x() * voxel_size; + point1.y = vertex1.y() * voxel_size; + point1.z = vertex1.z() * voxel_size; + voxel_grid.points.push_back(point1); + + point2.x = vertex2.x() * voxel_size; + point2.y = vertex2.y() * voxel_size; + point2.z = vertex2.z() * voxel_size; + voxel_grid.points.push_back(point2); + }; + + // Add edges for each voxel to create the line list + for (const auto &corner : corners) { + AddEdge(corner[0], corner[1]); + AddEdge(corner[1], corner[2]); + AddEdge(corner[2], corner[3]); + AddEdge(corner[3], corner[0]); + AddEdge(corner[4], corner[5]); + AddEdge(corner[5], corner[6]); + AddEdge(corner[6], corner[7]); + AddEdge(corner[7], corner[4]); + AddEdge(corner[0], corner[4]); + AddEdge(corner[1], corner[5]); + AddEdge(corner[2], corner[6]); + AddEdge(corner[3], corner[7]); + } + + return voxel_grid; +} +} // namespace kinematic_icp_ros::utils diff --git a/ros/src/kinematic_icp_ros/utils/RosbagUtils.cpp b/ros/src/kinematic_icp_ros/utils/RosbagUtils.cpp new file mode 100644 index 0000000..5c205bd --- /dev/null +++ b/ros/src/kinematic_icp_ros/utils/RosbagUtils.cpp @@ -0,0 +1,148 @@ +// MIT License + +// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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. +#include "kinematic_icp_ros/utils/RosbagUtils.hpp" + +#include +#include +#include +#include +#include +#include + +// ROS +#include + +#include +#include +#include +#include + +namespace { +auto GetTimestampsFromRosbagSerializedMsg(const rosbag2_storage::SerializedBagMessage &msg) { +#if RCLCPP_VERSION_GTE(22, 0, 0) + return std::chrono::nanoseconds(msg.send_timestamp); +#else + return std::chrono::nanoseconds(msg.time_stamp); +#endif +} +} // namespace + +/// TFBridge---------------------------------------------------------------------------------------- +BufferableBag::TFBridge::TFBridge(rclcpp::Node::SharedPtr node) { + tf_broadcaster = std::make_unique(node); + tf_static_broadcaster = std::make_unique(node); + serializer = rclcpp::Serialization(); +} + +void BufferableBag::TFBridge::ProcessTFMessage( + std::shared_ptr msg) const { + tf2_msgs::msg::TFMessage tf_message; + rclcpp::SerializedMessage serialized_msg(*msg->serialized_data); + serializer.deserialize_message(&serialized_msg, &tf_message); + // Broadcast tranforms to /tf and /tf_static topics + for (auto &transform : tf_message.transforms) { + if (msg->topic_name == "/tf_static") { + tf_static_broadcaster->sendTransform(transform); + } else { + tf_broadcaster->sendTransform(transform); + } + } +} + +/// BufferableBag----------------------------------------------------------------------------------- +BufferableBag::BufferableBag(const std::string &bag_filename, + const std::shared_ptr tf_bridge, + const std::string &topic, + const std::chrono::seconds buffer_size) + : tf_bridge_(tf_bridge), + bag_reader_(std::make_unique()), + topic_(topic), + buffer_size_(buffer_size) { + bag_reader_->open(bag_filename); + message_count_ = [&]() { + std::size_t message_count = 0; + const auto &metadata = bag_reader_->get_metadata(); + const auto topic_info = metadata.topics_with_message_count; + const auto it = std::find_if(topic_info.begin(), topic_info.end(), [&](const auto &info) { + return info.topic_metadata.name == topic_; + }); + if (it != topic_info.end()) { + message_count += it->message_count; + } + return message_count; + }(); + + BufferMessages(); +} + +bool BufferableBag::finished() const { return !bag_reader_->has_next(); }; + +std::size_t BufferableBag::message_count() const { return message_count_; } + +void BufferableBag::BufferMessages() { + auto buffer_is_filled = [&]() -> bool { + if (buffer_.empty()) return false; + const auto first_stamp = GetTimestampsFromRosbagSerializedMsg(buffer_.front()); + const auto last_stamp = GetTimestampsFromRosbagSerializedMsg(buffer_.back()); + return (last_stamp - first_stamp) > buffer_size_; + }; + + // Advance reading one message until the buffer is filled or we finish the bagfile + while (!finished() && !buffer_is_filled()) { + // Fetch next message from bagfile, could be anything + const auto msg = bag_reader_->read_next(); + // TODO(Nacho): The following logic should be customizable from the outside world + // If the msg is TFMessage, fill the tf_buffer and broadcast the transformation and don't + // populate the buffered_messages_ as we already processed it + if (msg->topic_name == "/tf" || msg->topic_name == "/tf_static") { + tf_bridge_->ProcessTFMessage(msg); + } else if (msg->topic_name == topic_) { + // If the msg is not TFMessage then push it to the interal buffer of all messages + buffer_.push(*msg); + } + } +} + +rosbag2_storage::SerializedBagMessage BufferableBag::PopNextMessage() { + const rosbag2_storage::SerializedBagMessage msg = buffer_.front(); + buffer_.pop(); + BufferMessages(); + return msg; +} + +/// Multiplexer ------------------------------------------------------------------------------------ +void BagMultiplexer::AddBag(BufferableBag &&bag) { + message_count_ += bag.message_count(); + bags_.push_back(std::move(bag)); +} + +std::size_t BagMultiplexer::message_count() const { return message_count_; }; + +bool BagMultiplexer::IsMessageAvailable() const { return !bags_[current_index_].finished(); } + +rosbag2_storage::SerializedBagMessage BagMultiplexer::GetNextMessage() { + if (bags_[current_index_].finished() && current_index_ + 1 < bags_.size()) { + current_index_++; + } + return bags_[current_index_].PopNextMessage(); +}