Fix patch application in container.

Moved to mechanum controller
Use a single URDF from the robot description
TODO: Wheel physics is not yet correct for mecanum movement.

.devcontainer/cyclonedds.xml

@@ -0,0 +1,13 @@
+<!-- ~/.ros2/dds/cyclonedds.xml -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<CycloneDDS xmlns="https://cdds.io/config"
+            xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+            xsi:schemaLocation="https://cdds.io/config
+            https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd">
+  <Domain Id="any">
+    <Discovery>
+      <ParticipantIndex>auto</ParticipantIndex>
+      <MaxAutoParticipantIndex>1000</MaxAutoParticipantIndex>
+    </Discovery>
+  </Domain>
+</CycloneDDS>

.devcontainer/jazzy/Dockerfile.jazzy

@@ -0,0 +1,75 @@
+FROM osrf/ros:jazzy-desktop-full
+ARG USERNAME=USERNAME
+ARG USER_UID=1000
+ARG USER_GID=$USER_UID
+ARG TARGETARCH
+
+# Delete user if it exists in container (e.g Ubuntu Noble: ubuntu)
+RUN if id -u $USER_UID ; then userdel `id -un $USER_UID` ; fi
+
+# Create the user
+RUN groupadd --gid $USER_GID $USERNAME \
+    && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
+    #
+    # [Optional] Add sudo support. Omit if you don't need to install software after connecting.
+    && apt-get update \
+    && apt-get install -y --no-install-recommends sudo \
+    && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
+    && chmod 0440 /etc/sudoers.d/$USERNAME \
+    && rm -rf /var/lib/apt/lists/*
+
+# Make sure the base image is completely up to date
+RUN apt-get update \
+    && apt-get upgrade -y \
+    && rm -rf /var/lib/apt/lists/*
+
+
+# C++ development tools
+RUN apt-get update \
+    && apt-get install -y --no-install-recommends \
+    build-essential \
+    cmake \
+    gdb \
+    clang \
+    clang-format \
+    clang-tidy \
+    libboost-all-dev \
+    && rm -rf /var/lib/apt/lists/*
+
+# Python development tools
+RUN apt-get update \
+    && apt-get install -y --no-install-recommends \
+    python3-pip \
+    python3-dev \
+    python3-argcomplete \
+    python3-colcon-common-extensions \
+    python3-colcon-mixin \
+    python3-rosdep \
+    python3-vcstool \
+    && rm -rf /var/lib/apt/lists/*
+
+# system deps from frozen rosdep manifest — no rosdep at build or startup
+# NOTE: There should be a check in your CI that this file is up to date.
+# Something like: 
+#   ./scripts/freeze-rosdep.sh && git diff --exit-code rosdep-packages.txt
+COPY rosdep-packages.txt /tmp/rosdep-packages.txt
+# The --mount=type=cache option caches the apt packages with buildkit
+# The id-apt-cache-${TARGETARCH} option allows for separate caches for different architectures
+# this enables parallel builds for different architectures without cache conflicts
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    rm -f /etc/apt/apt.conf.d/docker-clean \
+    && apt-get update \
+    && xargs -r -a /tmp/rosdep-packages.txt apt-get install -y --no-install-recommends \
+    && rm -rf /var/lib/apt/lists/*
+
+RUN apt-get update \
+    && apt-get install -y --no-install-recommends \
+    ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \
+    && rm -rf /var/lib/apt/lists/*
+
+
+ENV SHELL=/bin/bash
+
+# [Optional] Set the default user. Omit if you want to keep the default as root.
+USER $USERNAME
+CMD ["/bin/bash"]

.devcontainer/jazzy/devcontainer-lock.json

@@ -0,0 +1,9 @@
+{
+  "features": {
+    "ghcr.io/devcontainers/features/docker-from-docker:1": {
+      "version": "1.10.0",
+      "resolved": "ghcr.io/devcontainers/features/docker-from-docker@sha256:c2c2cf829505ead8e4892c88c31b6594ae94a2bbb209e16e1fac456c1a3a624e",
+      "integrity": "sha256:c2c2cf829505ead8e4892c88c31b6594ae94a2bbb209e16e1fac456c1a3a624e"
+    }
+  }
+}

.devcontainer/jazzy/devcontainer.json

@@ -0,0 +1,50 @@
+{
+    "name": "ROS 2 Jazzy",
+    "privileged": true,
+    "remoteUser": "${localEnv:USER}",
+    "build": {
+        "context": "../..",
+        "dockerfile": "Dockerfile.jazzy",
+        "args": {
+            "USERNAME": "${localEnv:USER}"
+        }
+    },
+    "workspaceFolder": "/workspaces/agv_pro_ros2",
+    "features": {
+        "ghcr.io/devcontainers/features/docker-from-docker:1": {
+            "version": "latest",
+            "moby": true
+        }
+    },
+    "customizations": {
+        "vscode": {
+            "extensions": [
+                "ms-python.python",
+                "anthropic.claude-code"
+            ]
+        }
+    },
+    "containerEnv": {
+        "DISPLAY": ":0",
+        "ROS_AUTOMATIC_DISCOVERY_RANGE": "SUBNET",
+        "ROS_DOMAIN_ID": "0",
+        "RMW_IMPLEMENTATION": "rmw_cyclonedds_cpp",
+        "CYCLONEDDS_URI": "/workspaces/agv_pro_ros2/.devcontainer/cyclonedds.xml"
+    },
+    "runArgs": [
+        // "--network=robot-sim",
+        "--network=host",
+        "--pid=host",
+        "--ipc=host",
+        "--group-add=dialout",
+        "-e",
+        "DISPLAY=${env:DISPLAY}"
+    ],
+    "mounts": [
+        "source=/dev,target=/dev,type=bind",
+        "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached",
+        "source=/usr/local/share/ca-certificates,target=/usr/local/share/host-certificates,type=bind,consistency=cached",
+        "source=${localEnv:HOME}/.ssh,target=/home/${localEnv:USER}/.ssh,type=bind,consistency=cached"
+    ],
+    "postCreateCommand": "bash .devcontainer/post-create.sh"
+}

.devcontainer/post-create.sh

@@ -0,0 +1,43 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
+ROSDEP_APT_LIST_FILE="${ROOT_DIR}/.devcontainer/${ROS_DISTRO}/rosdep-apt-packages.txt"
+
+
+if [[ -f /usr/local/share/host-certificates/AMD_CA.crt ]]; then
+	sudo cp /usr/local/share/host-certificates/AMD_CA.crt /usr/local/share/ca-certificates/
+	sudo update-ca-certificates
+fi
+
+if [[ ! -f "${ROSDEP_APT_LIST_FILE}" ]]; then
+	echo "Missing ${ROSDEP_APT_LIST_FILE}."
+	echo "Run .devcontainer/refresh-rosdep-cache.sh and rebuild the devcontainer."
+	exit 1
+fi
+
+tmp_packages="$(mktemp)"
+cleanup() {
+	rm -f "${tmp_packages}"
+}
+trap cleanup EXIT
+
+export DEBIAN_FRONTEND=noninteractive
+sudo apt-get update -qq
+
+rosdep keys --from-paths src --ignore-src 2>/dev/null \
+	| xargs -r rosdep resolve --rosdistro "${ROS_DISTRO}" --filter-for-installers=apt 2>/dev/null \
+	| sed -e '/^#/d' -e '/^$/d' \
+	| LC_ALL=C sort -u > "${tmp_packages}"
+
+if ! diff -u <(grep -Ev '^\s*($|#)' "${ROSDEP_APT_LIST_FILE}") "${tmp_packages}" >/dev/null; then
+	echo "rosdep-resolved apt packages differ from ${ROSDEP_APT_LIST_FILE}."
+	echo "Run .devcontainer/refresh-rosdep-cache.sh and rebuild the devcontainer."
+	exit 1
+fi
+
+echo "rosdep apt cache is current; no post-create package installation needed."
+
+if compgen -G "${ROOT_DIR}/src/robotnik/robotnik_simulation/debs/ros-jazzy-*.deb" > /dev/null; then
+	sudo apt-get install -y "${ROOT_DIR}"/src/robotnik/robotnik_simulation/debs/ros-jazzy-*.deb
+fi

.devcontainer/refresh-rosdep-cache.sh

@@ -0,0 +1,38 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
+ROS_DISTRO="${ROS_DISTRO:-jazzy}"
+CACHE_DIR="${ROOT_DIR}/.devcontainer/${ROS_DISTRO}"
+APT_LIST_FILE="${CACHE_DIR}/rosdep-apt-packages.txt"
+
+cd "${ROOT_DIR}"
+
+mkdir -p "${CACHE_DIR}"
+
+if [[ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]]; then
+    sudo rosdep init
+fi
+
+rosdep update
+export DEBIAN_FRONTEND=noninteractive
+sudo apt-get update -qq
+
+tmp_packages="$(mktemp)"
+cleanup() {
+    rm -f "${tmp_packages}"
+}
+trap cleanup EXIT
+
+rosdep keys --from-paths src --ignore-src 2>/dev/null \
+    | xargs -r rosdep resolve --rosdistro "${ROS_DISTRO}" --filter-for-installers=apt 2>/dev/null \
+    | sed -e '/^#/d' -e '/^$/d' \
+    | LC_ALL=C sort -u > "${tmp_packages}"
+
+{
+    echo "# Generated by .devcontainer/refresh-rosdep-cache.sh"
+    echo "# apt packages resolved from rosdep keys in src"
+    cat "${tmp_packages}"
+} > "${APT_LIST_FILE}"
+
+echo "Updated ${APT_LIST_FILE}"

.gitignore

@@ -5,3 +5,5 @@ install/
 log/
 /CMakeLists.txt
 __pycache__/
+# Ignore upstream as it is installedls by the upstream.jazzy.repos
+src/upstream

Dockerfile

@@ -0,0 +1,48 @@
+FROM ros:jazzy
+
+# Create the workspace and copy all source directories into it
+WORKDIR /ros2_ws
+ARG TARGETARCH
+
+COPY src ./src/
+COPY patches ./patches/
+COPY upstream.jazzy.repos ./upstream.jazzy.repos
+
+# Import all upstream repostories
+RUN mkdir -p src/upstream && vcs import ./src/upstream < ./upstream.jazzy.repos
+
+# Apply patches to upstream
+RUN ./patches/apply_patches.sh
+
+# system deps from frozen rosdep manifest — no rosdep at build or startup
+# NOTE: There should be a check in your CI that this file is up to date.
+# Something like: 
+#   ./scripts/freeze-rosdep.sh && git diff --exit-code rosdep-packages.txt
+COPY rosdep-packages.txt /tmp/rosdep-packages.txt
+# The --mount=type=cache option caches the apt packages with buildkit
+# The id-apt-cache-${TARGETARCH} option allows for separate caches for different architectures
+# this enables parallel builds for different architectures without cache conflicts
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    rm -f /etc/apt/apt.conf.d/docker-clean \
+    && apt-get update \
+    && xargs -r -a /tmp/rosdep-packages.txt apt-get install -y --no-install-recommends \
+    && rm -rf /var/lib/apt/lists/*
+
+# Install dependencies with rosdep
+# This will catch anything that wasn't installed via .deb's from the frozen rosdep manifest.
+RUN apt-get update && \
+    rosdep update && \
+    rosdep install --from-paths src --ignore-src -r -y && \
+    rm -rf /var/lib/apt/lists/*
+
+# Build the workspace with symlink install
+# livox_sdk is now an ament_cmake package built by colcon (no manual /usr/local install needed).
+RUN . /opt/ros/jazzy/setup.sh && \
+    colcon build --symlink-install \
+    --cmake-args -DROS_EDITION=ROS2 -DDISTRO_ROS=jazzy
+
+# Source the overlay on container startup
+RUN echo "source /opt/ros/jazzy/setup.bash" >> /root/.bashrc && \
+    echo "source /ros2_ws/install/setup.bash" >> /root/.bashrc
+
+CMD ["bash"]

FAST_LIO/.github/stale.yml

@@ -1,17 +0,0 @@
-# Number of days of inactivity before an issue becomes stale
-daysUntilStale: 21
-# Number of days of inactivity before a stale issue is closed
-daysUntilClose: 1
-# Issues with these labels will never be considered stale
-exemptLabels:
-  - pinned
-  - security
-# Label to use when marking an issue as stale
-staleLabel: stale
-# Comment to post when marking an issue as stale. Set to `false` to disable
-markComment: >
-  This issue has been automatically marked as stale because it has not had
-  recent activity. It will be closed if no further activity occurs. Thank you
-  for your contributions.
-# Comment to post when closing a stale issue. Set to `false` to disable
-closeComment: false

FAST_LIO/.gitignore

@@ -1,8 +0,0 @@
-build
-Log/*.png
-Log/*.txt
-Log/*.csv
-Log/*.pdf
-.vscode/c_cpp_properties.json
-.vscode/settings.json
-PCD/*.pcd

FAST_LIO/.gitmodules

@@ -1,4 +0,0 @@
-[submodule "include/ikd-Tree"]
-	path = include/ikd-Tree
-	url = https://github.com/hku-mars/ikd-Tree.git
-	branch = fast_lio

FAST_LIO/CMakeLists.txt

@@ -1,128 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(fast_lio)
-
-if(NOT CMAKE_BUILD_TYPE)
-  set(CMAKE_BUILD_TYPE Release)
-endif()
-
- ADD_COMPILE_OPTIONS(-std=c++17) 
- ADD_COMPILE_OPTIONS(-std=c++17) 
- set(CMAKE_CXX_FLAGS "-std=c++17 -O3") 
-  
- add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 
-  
- set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions") 
- set(CMAKE_CXX_STANDARD 17) 
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)
-
-message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")
-
-if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)")
-  include(ProcessorCount)
-  ProcessorCount(N)
-  message("Processer number:  ${N}")
-
-  if(N GREATER 4)
-    add_definitions(-DMP_EN)
-    add_definitions(-DMP_PROC_NUM=3)
-    message("core for MP: 3")
-  elseif(N GREATER 3)
-    add_definitions(-DMP_EN)
-    add_definitions(-DMP_PROC_NUM=2)
-    message("core for MP: 2")
-  else()
-    add_definitions(-DMP_PROC_NUM=1)
-  endif()
-else()
-  add_definitions(-DMP_PROC_NUM=1)
-endif()
-
-find_package(OpenMP QUIET)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}   ${OpenMP_C_FLAGS}")
-
-find_package(PythonLibs REQUIRED)
-find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
-
-# ROS dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_components REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(nav_msgs REQUIRED)
-find_package(sensor_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(std_srvs REQUIRED)
-find_package(visualization_msgs REQUIRED)
-find_package(pcl_ros REQUIRED)
-find_package(pcl_conversions REQUIRED)
-find_package(livox_ros_driver2 REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-
-set(dependencies
-  rclcpp
-  rclcpp_components
-  geometry_msgs
-  nav_msgs
-  sensor_msgs
-  std_msgs
-  std_srvs
-  visualization_msgs
-  pcl_ros
-  pcl_conversions
-  livox_ros_driver2
-)
-
-# Thirdparty libraries
-find_package(Eigen3 REQUIRED)
-find_package(PCL REQUIRED COMPONENTS common io)
-
-message(Eigen: ${EIGEN3_INCLUDE_DIR})
-message(STATUS "PCL: ${PCL_INCLUDE_DIRS}")
-
-set(msg_files
-  "msg/Pose6D.msg"
-)
-
-rosidl_generate_interfaces(${PROJECT_NAME}
-  ${msg_files}
-)
-ament_export_dependencies(rosidl_default_runtime)
-
-add_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp)
-target_include_directories(fastlio_mapping PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>
-  ${PCL_INCLUDE_DIRS}
-)
-target_link_libraries(fastlio_mapping ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} Eigen3::Eigen)
-target_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})
-
-list(APPEND EOL_LIST "foxy" "galactic" "eloquent" "dashing" "crystal")
-
-if($ENV{ROS_DISTRO} IN_LIST EOL_LIST)
-  # Custommsg to support foxy & galactic
-  rosidl_target_interfaces(fastlio_mapping
-    ${PROJECT_NAME} "rosidl_typesupport_cpp")
-else()
-  rosidl_get_typesupport_target(cpp_typesupport_target
-    ${PROJECT_NAME} "rosidl_typesupport_cpp")
-  target_link_libraries(fastlio_mapping ${cpp_typesupport_target})
-endif()
-
-ament_target_dependencies(fastlio_mapping ${dependencies})
-
-# ---------------- Install --------------- #
-install(TARGETS fastlio_mapping
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-install(
-  DIRECTORY config launch rviz
-  DESTINATION share/${PROJECT_NAME}
-)
-
-ament_package()

FAST_LIO/LICENSE

@@ -1,339 +0,0 @@
-                    GNU GENERAL PUBLIC LICENSE
-                       Version 2, June 1991
-
- Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
- 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
- Everyone is permitted to copy and distribute verbatim copies
- of this license document, but changing it is not allowed.
-
-                            Preamble
-
-  The licenses for most software are designed to take away your
-freedom to share and change it.  By contrast, the GNU General Public
-License is intended to guarantee your freedom to share and change free
-software--to make sure the software is free for all its users.  This
-General Public License applies to most of the Free Software
-Foundation's software and to any other program whose authors commit to
-using it.  (Some other Free Software Foundation software is covered by
-the GNU Lesser General Public License instead.)  You can apply it to
-your programs, too.
-
-  When we speak of free software, we are referring to freedom, not
-price.  Our General Public Licenses are designed to make sure that you
-have the freedom to distribute copies of free software (and charge for
-this service if you wish), that you receive source code or can get it
-if you want it, that you can change the software or use pieces of it
-in new free programs; and that you know you can do these things.
-
-  To protect your rights, we need to make restrictions that forbid
-anyone to deny you these rights or to ask you to surrender the rights.
-These restrictions translate to certain responsibilities for you if you
-distribute copies of the software, or if you modify it.
-
-  For example, if you distribute copies of such a program, whether
-gratis or for a fee, you must give the recipients all the rights that
-you have.  You must make sure that they, too, receive or can get the
-source code.  And you must show them these terms so they know their
-rights.
-
-  We protect your rights with two steps: (1) copyright the software, and
-(2) offer you this license which gives you legal permission to copy,
-distribute and/or modify the software.
-
-  Also, for each author's protection and ours, we want to make certain
-that everyone understands that there is no warranty for this free
-software.  If the software is modified by someone else and passed on, we
-want its recipients to know that what they have is not the original, so
-that any problems introduced by others will not reflect on the original
-authors' reputations.
-
-  Finally, any free program is threatened constantly by software
-patents.  We wish to avoid the danger that redistributors of a free
-program will individually obtain patent licenses, in effect making the
-program proprietary.  To prevent this, we have made it clear that any
-patent must be licensed for everyone's free use or not licensed at all.
-
-  The precise terms and conditions for copying, distribution and
-modification follow.
-
-                    GNU GENERAL PUBLIC LICENSE
-   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
-
-  0. This License applies to any program or other work which contains
-a notice placed by the copyright holder saying it may be distributed
-under the terms of this General Public License.  The "Program", below,
-refers to any such program or work, and a "work based on the Program"
-means either the Program or any derivative work under copyright law:
-that is to say, a work containing the Program or a portion of it,
-either verbatim or with modifications and/or translated into another
-language.  (Hereinafter, translation is included without limitation in
-the term "modification".)  Each licensee is addressed as "you".
-
-Activities other than copying, distribution and modification are not
-covered by this License; they are outside its scope.  The act of
-running the Program is not restricted, and the output from the Program
-is covered only if its contents constitute a work based on the
-Program (independent of having been made by running the Program).
-Whether that is true depends on what the Program does.
-
-  1. You may copy and distribute verbatim copies of the Program's
-source code as you receive it, in any medium, provided that you
-conspicuously and appropriately publish on each copy an appropriate
-copyright notice and disclaimer of warranty; keep intact all the
-notices that refer to this License and to the absence of any warranty;
-and give any other recipients of the Program a copy of this License
-along with the Program.
-
-You may charge a fee for the physical act of transferring a copy, and
-you may at your option offer warranty protection in exchange for a fee.
-
-  2. You may modify your copy or copies of the Program or any portion
-of it, thus forming a work based on the Program, and copy and
-distribute such modifications or work under the terms of Section 1
-above, provided that you also meet all of these conditions:
-
-    a) You must cause the modified files to carry prominent notices
-    stating that you changed the files and the date of any change.
-
-    b) You must cause any work that you distribute or publish, that in
-    whole or in part contains or is derived from the Program or any
-    part thereof, to be licensed as a whole at no charge to all third
-    parties under the terms of this License.
-
-    c) If the modified program normally reads commands interactively
-    when run, you must cause it, when started running for such
-    interactive use in the most ordinary way, to print or display an
-    announcement including an appropriate copyright notice and a
-    notice that there is no warranty (or else, saying that you provide
-    a warranty) and that users may redistribute the program under
-    these conditions, and telling the user how to view a copy of this
-    License.  (Exception: if the Program itself is interactive but
-    does not normally print such an announcement, your work based on
-    the Program is not required to print an announcement.)
-
-These requirements apply to the modified work as a whole.  If
-identifiable sections of that work are not derived from the Program,
-and can be reasonably considered independent and separate works in
-themselves, then this License, and its terms, do not apply to those
-sections when you distribute them as separate works.  But when you
-distribute the same sections as part of a whole which is a work based
-on the Program, the distribution of the whole must be on the terms of
-this License, whose permissions for other licensees extend to the
-entire whole, and thus to each and every part regardless of who wrote it.
-
-Thus, it is not the intent of this section to claim rights or contest
-your rights to work written entirely by you; rather, the intent is to
-exercise the right to control the distribution of derivative or
-collective works based on the Program.
-
-In addition, mere aggregation of another work not based on the Program
-with the Program (or with a work based on the Program) on a volume of
-a storage or distribution medium does not bring the other work under
-the scope of this License.
-
-  3. You may copy and distribute the Program (or a work based on it,
-under Section 2) in object code or executable form under the terms of
-Sections 1 and 2 above provided that you also do one of the following:
-
-    a) Accompany it with the complete corresponding machine-readable
-    source code, which must be distributed under the terms of Sections
-    1 and 2 above on a medium customarily used for software interchange; or,
-
-    b) Accompany it with a written offer, valid for at least three
-    years, to give any third party, for a charge no more than your
-    cost of physically performing source distribution, a complete
-    machine-readable copy of the corresponding source code, to be
-    distributed under the terms of Sections 1 and 2 above on a medium
-    customarily used for software interchange; or,
-
-    c) Accompany it with the information you received as to the offer
-    to distribute corresponding source code.  (This alternative is
-    allowed only for noncommercial distribution and only if you
-    received the program in object code or executable form with such
-    an offer, in accord with Subsection b above.)
-
-The source code for a work means the preferred form of the work for
-making modifications to it.  For an executable work, complete source
-code means all the source code for all modules it contains, plus any
-associated interface definition files, plus the scripts used to
-control compilation and installation of the executable.  However, as a
-special exception, the source code distributed need not include
-anything that is normally distributed (in either source or binary
-form) with the major components (compiler, kernel, and so on) of the
-operating system on which the executable runs, unless that component
-itself accompanies the executable.
-
-If distribution of executable or object code is made by offering
-access to copy from a designated place, then offering equivalent
-access to copy the source code from the same place counts as
-distribution of the source code, even though third parties are not
-compelled to copy the source along with the object code.
-
-  4. You may not copy, modify, sublicense, or distribute the Program
-except as expressly provided under this License.  Any attempt
-otherwise to copy, modify, sublicense or distribute the Program is
-void, and will automatically terminate your rights under this License.
-However, parties who have received copies, or rights, from you under
-this License will not have their licenses terminated so long as such
-parties remain in full compliance.
-
-  5. You are not required to accept this License, since you have not
-signed it.  However, nothing else grants you permission to modify or
-distribute the Program or its derivative works.  These actions are
-prohibited by law if you do not accept this License.  Therefore, by
-modifying or distributing the Program (or any work based on the
-Program), you indicate your acceptance of this License to do so, and
-all its terms and conditions for copying, distributing or modifying
-the Program or works based on it.
-
-  6. Each time you redistribute the Program (or any work based on the
-Program), the recipient automatically receives a license from the
-original licensor to copy, distribute or modify the Program subject to
-these terms and conditions.  You may not impose any further
-restrictions on the recipients' exercise of the rights granted herein.
-You are not responsible for enforcing compliance by third parties to
-this License.
-
-  7. If, as a consequence of a court judgment or allegation of patent
-infringement or for any other reason (not limited to patent issues),
-conditions are imposed on you (whether by court order, agreement or
-otherwise) that contradict the conditions of this License, they do not
-excuse you from the conditions of this License.  If you cannot
-distribute so as to satisfy simultaneously your obligations under this
-License and any other pertinent obligations, then as a consequence you
-may not distribute the Program at all.  For example, if a patent
-license would not permit royalty-free redistribution of the Program by
-all those who receive copies directly or indirectly through you, then
-the only way you could satisfy both it and this License would be to
-refrain entirely from distribution of the Program.
-
-If any portion of this section is held invalid or unenforceable under
-any particular circumstance, the balance of the section is intended to
-apply and the section as a whole is intended to apply in other
-circumstances.
-
-It is not the purpose of this section to induce you to infringe any
-patents or other property right claims or to contest validity of any
-such claims; this section has the sole purpose of protecting the
-integrity of the free software distribution system, which is
-implemented by public license practices.  Many people have made
-generous contributions to the wide range of software distributed
-through that system in reliance on consistent application of that
-system; it is up to the author/donor to decide if he or she is willing
-to distribute software through any other system and a licensee cannot
-impose that choice.
-
-This section is intended to make thoroughly clear what is believed to
-be a consequence of the rest of this License.
-
-  8. If the distribution and/or use of the Program is restricted in
-certain countries either by patents or by copyrighted interfaces, the
-original copyright holder who places the Program under this License
-may add an explicit geographical distribution limitation excluding
-those countries, so that distribution is permitted only in or among
-countries not thus excluded.  In such case, this License incorporates
-the limitation as if written in the body of this License.
-
-  9. The Free Software Foundation may publish revised and/or new versions
-of the General Public License from time to time.  Such new versions will
-be similar in spirit to the present version, but may differ in detail to
-address new problems or concerns.
-
-Each version is given a distinguishing version number.  If the Program
-specifies a version number of this License which applies to it and "any
-later version", you have the option of following the terms and conditions
-either of that version or of any later version published by the Free
-Software Foundation.  If the Program does not specify a version number of
-this License, you may choose any version ever published by the Free Software
-Foundation.
-
-  10. If you wish to incorporate parts of the Program into other free
-programs whose distribution conditions are different, write to the author
-to ask for permission.  For software which is copyrighted by the Free
-Software Foundation, write to the Free Software Foundation; we sometimes
-make exceptions for this.  Our decision will be guided by the two goals
-of preserving the free status of all derivatives of our free software and
-of promoting the sharing and reuse of software generally.
-
-                            NO WARRANTY
-
-  11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
-FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
-OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
-PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
-OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
-MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
-TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
-PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
-REPAIR OR CORRECTION.
-
-  12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
-WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
-REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
-INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
-OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
-TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
-YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
-PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
-POSSIBILITY OF SUCH DAMAGES.
-
-                     END OF TERMS AND CONDITIONS
-
-            How to Apply These Terms to Your New Programs
-
-  If you develop a new program, and you want it to be of the greatest
-possible use to the public, the best way to achieve this is to make it
-free software which everyone can redistribute and change under these terms.
-
-  To do so, attach the following notices to the program.  It is safest
-to attach them to the start of each source file to most effectively
-convey the exclusion of warranty; and each file should have at least
-the "copyright" line and a pointer to where the full notice is found.
-
-    <one line to give the program's name and a brief idea of what it does.>
-    Copyright (C) <year>  <name of author>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License along
-    with this program; if not, write to the Free Software Foundation, Inc.,
-    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
-
-Also add information on how to contact you by electronic and paper mail.
-
-If the program is interactive, make it output a short notice like this
-when it starts in an interactive mode:
-
-    Gnomovision version 69, Copyright (C) year name of author
-    Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
-    This is free software, and you are welcome to redistribute it
-    under certain conditions; type `show c' for details.
-
-The hypothetical commands `show w' and `show c' should show the appropriate
-parts of the General Public License.  Of course, the commands you use may
-be called something other than `show w' and `show c'; they could even be
-mouse-clicks or menu items--whatever suits your program.
-
-You should also get your employer (if you work as a programmer) or your
-school, if any, to sign a "copyright disclaimer" for the program, if
-necessary.  Here is a sample; alter the names:
-
-  Yoyodyne, Inc., hereby disclaims all copyright interest in the program
-  `Gnomovision' (which makes passes at compilers) written by James Hacker.
-
-  <signature of Ty Coon>, 1 April 1989
-  Ty Coon, President of Vice
-
-This General Public License does not permit incorporating your program into
-proprietary programs.  If your program is a subroutine library, you may
-consider it more useful to permit linking proprietary applications with the
-library.  If this is what you want to do, use the GNU Lesser General
-Public License instead of this License.

FAST_LIO/Log/fast_lio_time_log_analysis.m

@@ -1,135 +0,0 @@
-clear
-close all
-
-Color_red = [0.6350 0.0780 0.1840];
-Color_blue = [0 0.4470 0.7410];
-Color_orange = [0.8500 0.3250 0.0980];
-Color_green = [0.4660 0.6740 0.1880];
-Color_lightblue = [0.3010 0.7450 0.9330];
-Color_purple = [0.4940 0.1840 0.5560];
-Color_yellow = [0.9290 0.6940 0.1250];
-
-fast_lio_ikdtree = csvread("./fast_lio_time_log.csv",1,0);
-timestamp_ikd = fast_lio_ikdtree(:,1);
-timestamp_ikd = timestamp_ikd - min(timestamp_ikd);
-total_time_ikd = fast_lio_ikdtree(:,2)*1e3;
-scan_num = fast_lio_ikdtree(:,3);
-incremental_time_ikd = fast_lio_ikdtree(:,4)*1e3;
-search_time_ikd = fast_lio_ikdtree(:,5)*1e3;
-delete_size_ikd = fast_lio_ikdtree(:,6);
-delete_time_ikd = fast_lio_ikdtree(:,7) * 1e3;
-tree_size_ikd_st = fast_lio_ikdtree(:,8);
-tree_size_ikd = fast_lio_ikdtree(:,9);
-add_points = fast_lio_ikdtree(:,10);
-
-fast_lio_forest = csvread("fast_lio_time_log.csv",1,0);
-fov_check_time_forest = fast_lio_forest(:,5)*1e3;
-average_time_forest = fast_lio_forest(:,2)*1e3;
-total_time_forest = fast_lio_forest(:,6)*1e3;
-incremental_time_forest = fast_lio_forest(:,3)*1e3;
-search_time_forest = fast_lio_forest(:,4)*1e3;
-timestamp_forest = fast_lio_forest(:,1);
-
-% Use slide window to calculate average
-L = 1;     % Length of slide window
-for i = 1:length(timestamp_ikd)
-    if (i<L)
-        average_time_ikd(i) = mean(total_time_ikd(1:i));
-    else
-        average_time_ikd(i) = mean(total_time_ikd(i-L+1:i));
-    end
-end
-for i = 1:length(timestamp_forest)
-    if (i<L)
-        average_time_forest(i) = mean(total_time_forest(1:i));
-    else
-        average_time_forest(i) = mean(total_time_forest(i-L+1:i));
-    end
-end
-
-
-
-
-f = figure;
-set(gcf,'Position',[80 433 600 640])
-tiled_handler = tiledlayout(3,1);
-tiled_handler.TileSpacing = 'compact';
-tiled_handler.Padding = 'compact';
-nexttile;
-hold on;
-set(gca,'FontSize',12,'FontName','Times New Roman') 
-plot(timestamp_ikd, average_time_ikd,'-','Color',Color_blue,'LineWidth',1.2);
-plot(timestamp_forest, average_time_forest,'--','Color',Color_orange,'LineWidth',1.2);
-lg = legend("ikd-Tree", "ikd-Forest",'location',[0.1314 0.8559 0.2650 0.0789],'fontsize',14,'fontname','Times New Roman')
-title("Time Performance on FAST-LIO",'FontSize',16,'FontName','Times New Roman')
-xlabel("time/s",'FontSize',16,'FontName','Times New Roman')
-yl = ylabel("Run Time/ms",'FontSize',15,'Position',[285.7 5.5000 -1]);
-xlim([32,390]);
-ylim([0,23]);
-ax1 = gca;
-ax1.YAxis.FontSize = 12;
-ax1.XAxis.FontSize = 12;
-grid on
-box on
-% print('./Figures/fastlio_exp_average','-depsc','-r600')
-
-
-index_ikd = find(search_time_ikd > 0);
-search_time_ikd = search_time_ikd(index_ikd);
-index_forest = find(search_time_forest > 0);
-search_time_forest = search_time_forest(index_forest);
-
-t = nexttile;
-hold on;
-boxplot_data_ikd = [incremental_time_ikd,total_time_ikd];
-boxplot_data_forest = [incremental_time_forest,total_time_forest];
-Colors_ikd = [Color_blue;Color_blue;Color_blue];
-Colors_forest = [Color_orange;Color_orange;Color_orange];
-% xticks([3,8,13])
-h_search_ikd = boxplot(search_time_ikd,'Whisker',50,'Positions',1,'Colors',Color_blue,'Widths',0.3);
-h_search_forest = boxplot(search_time_forest,'Whisker',50,'Positions',1.5,'Colors',Color_orange,'Widths',0.3);
-h_ikd = boxplot(boxplot_data_ikd,'Whisker',50,'Positions',[3,5],'Colors',Color_blue,'Widths',0.3);
-h_forest = boxplot(boxplot_data_forest,'Whisker',50,'Positions',[3.5,5.5],'Colors',Color_orange,'Widths',0.3);
-ax2 = gca;
-ax2.YAxis.Scale = 'log';
-xlim([0.5,6.0])
-ylim([0.0008,100])
-xticks([1.25 3.25 5.25])
-xticklabels({'Nearest Search','    Incremental Updates','Total Time'});
-yticks([1e-3,1e-2,1e-1,1e0,1e1,1e2])
-ax2.YAxis.FontSize = 12;
-ax2.XAxis.FontSize = 14.5;
-% ax.XAxis.FontWeight = 'bold';
-ylabel('Run Time/ms','FontSize',14,'FontName','Times New Roman')
-box_vars = [findall(h_search_ikd,'Tag','Box');findall(h_ikd,'Tag','Box');findall(h_search_forest,'Tag','Box');findall(h_forest,'Tag','Box')];
-for j=1:length(box_vars)
-    if (j<=3)
-        Color = Color_blue;
-    else
-        Color = Color_orange;
-    end
-    patch(get(box_vars(j),'XData'),get(box_vars(j),'YData'),Color,'FaceAlpha',0.25,'EdgeColor',Color);
-end
-Lg = legend(box_vars([1,4]), {'ikd-Tree','ikd-Forest'},'Location',[0.6707 0.4305 0.265 0.07891],'fontsize',14,'fontname','Times New Roman');
-grid on
-set(gca,'YMinorGrid','off')
-nexttile;
-hold on;
-grid on;
-box on;
-set(gca,'FontSize',12,'FontName','Times New Roman') 
-plot(timestamp_ikd, alpha_bal_ikd,'-','Color',Color_blue,'LineWidth',1.2);
-plot(timestamp_ikd, alpha_del_ikd,'--','Color',Color_orange, 'LineWidth', 1.2);
-plot(timestamp_ikd, 0.6*ones(size(alpha_bal_ikd)), ':','Color','black','LineWidth',1.2);
-lg = legend("\alpha_{bal}", "\alpha_{del}",'location',[0.7871 0.1131 0.1433 0.069],'fontsize',14,'fontname','Times New Roman')
-title("Re-balancing Criterion",'FontSize',16,'FontName','Times New Roman')
-xlabel("time/s",'FontSize',16,'FontName','Times New Roman')
-yl = ylabel("\alpha",'FontSize',15, 'Position',[285.7 0.4250 -1])
-xlim([32,390]);
-ylim([0,0.85]);
-ax3 = gca;
-ax3.YAxis.FontSize = 12;
-ax3.XAxis.FontSize = 12;
-% print('./Figures/fastlio_exp_combine','-depsc','-r1200')
-% exportgraphics(f,'./Figures/fastlio_exp_combine_1.pdf','ContentType','vector')
-

FAST_LIO/Log/guide.md

@@ -1 +0,0 @@
-Here saved the debug records which can be drew by the ../Log/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h.

FAST_LIO/Log/plot.py

@@ -1,94 +0,0 @@
-# import matplotlib
-# matplotlib.use('Agg')
-import numpy as np
-import matplotlib.pyplot as plt
-
-
-#######for ikfom
-fig, axs = plt.subplots(4,2)
-lab_pre = ['', 'pre-x', 'pre-y', 'pre-z']
-lab_out = ['', 'out-x', 'out-y', 'out-z']
-plot_ind = range(7,10)
-a_pre=np.loadtxt('mat_pre.txt')
-a_out=np.loadtxt('mat_out.txt')
-time=a_pre[:,0]
-axs[0,0].set_title('Attitude')
-axs[1,0].set_title('Translation')
-axs[2,0].set_title('Extrins-R')
-axs[3,0].set_title('Extrins-T')
-axs[0,1].set_title('Velocity')
-axs[1,1].set_title('bg')
-axs[2,1].set_title('ba')
-axs[3,1].set_title('Gravity')
-for i in range(1,4):
-    for j in range(8):
-        axs[j%4, j/4].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i])
-        axs[j%4, j/4].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
-for j in range(8):
-    # axs[j].set_xlim(386,389)
-    axs[j%4, j/4].grid()
-    axs[j%4, j/4].legend()
-plt.grid()
-#######for ikfom#######
-
-
-#### Draw IMU data
-# fig, axs = plt.subplots(2)
-# imu=np.loadtxt('imu.txt')
-# time=imu[:,0]
-# axs[0].set_title('Gyroscope')
-# axs[1].set_title('Accelerameter')
-# lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
-# lab_2 = ['acc-x', 'acc-y', 'acc-z']
-# for i in range(3):
-#     # if i==1:
-#     axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
-#     axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
-# for i in range(2):
-#     # axs[i].set_xlim(386,389)
-#     axs[i].grid()
-#     axs[i].legend()
-# plt.grid()
-
-# #### Draw time calculation
-# plt.figure(3)
-# fig = plt.figure()
-# font1 = {'family' : 'Times New Roman',
-# 'weight' : 'normal',
-# 'size'   : 12,
-# }
-# c="red"
-# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt')
-# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt')
-# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt')
-# # n = a_out[:,1].size
-# # time_mean = a_out[:,1].mean()
-# # time_se   = a_out[:,1].std() / np.sqrt(n)
-# # time_err  = a_out[:,1] - time_mean
-# # feat_mean = a_out[:,2].mean()
-# # feat_err  = a_out[:,2] - feat_mean
-# # feat_se   = a_out[:,2].std() / np.sqrt(n)
-# ax1 = fig.add_subplot(111)
-# ax1.set_ylabel('Effective Feature Numbers',font1)
-# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9])
-# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9])
-# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9])
-# ax1.set_ylim([0, 3000])
-
-# ax2 = ax1.twinx()
-# ax2.spines['right'].set_color('red')
-# ax2.set_ylabel('Compute Time (ms)',font1)
-# ax2.yaxis.label.set_color('red')
-# ax2.tick_params(axis='y', colors='red')
-# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
-# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
-# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
-# ax2.set_xlim([0.5, 3.5])
-# ax2.set_ylim([0, 100])
-
-# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2'))
-# # # print(time_se)
-# # # print(a_out3[:,2])
-# plt.grid()
-# plt.savefig("time.pdf", dpi=1200)
-plt.show()

FAST_LIO/PCD/1

@@ -1 +0,0 @@
-1

FAST_LIO/README.md

@@ -1,199 +0,0 @@
-> ROS2 Fork repo maintainer: [Ericsiii](https://github.com/Ericsii)
-
-## Related Works and Extended Application
-
-**SLAM:**
-
-1. [ikd-Tree](https://github.com/hku-mars/ikd-Tree): A state-of-art dynamic KD-Tree for 3D kNN search.
-2. [R2LIVE](https://github.com/hku-mars/r2live): A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end.
-3. [LI_Init](https://github.com/hku-mars/LiDAR_IMU_Init): A robust, real-time LiDAR-IMU extrinsic initialization and synchronization package..
-4. [FAST-LIO-LOCALIZATION](https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION): The integration of FAST-LIO with **Re-localization** function module.
-
-**Control and Plan:**
-
-1. [IKFOM](https://github.com/hku-mars/IKFoM): A Toolbox for fast and high-precision on-manifold Kalman filter.
-2. [UAV Avoiding Dynamic Obstacles](https://github.com/hku-mars/dyn_small_obs_avoidance): One of the implementation of FAST-LIO in robot's planning.
-3. [UGV Demo](https://www.youtube.com/watch?v=wikgrQbE6Cs): Model Predictive Control for Trajectory Tracking on Differentiable Manifolds.
-4. [Bubble Planner](https://arxiv.org/abs/2202.12177): Planning High-speed Smooth Quadrotor Trajectories using Receding Corridors.
-
-<!-- 10. [**FAST-LIVO**](https://github.com/hku-mars/FAST-LIVO): Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry. -->
-
-## FAST-LIO
-**FAST-LIO** (Fast LiDAR-Inertial Odometry) is a computationally efficient and robust LiDAR-inertial odometry package. It fuses LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. Our package address many key issues:
-1. Fast iterated Kalman filter for odometry optimization;
-2. Automaticaly initialized at most steady environments;
-3. Parallel KD-Tree Search to decrease the computation;
-
-## FAST-LIO 2.0 (2021-07-05 Update)
-<!-- ![image](https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/real_experiment2.gif) -->
-<!-- [![Watch the video](https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/real_exp_2.png)](https://youtu.be/2OvjGnxszf8) -->
-<div align="left">
-<img src="https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/real_experiment2.gif" width=49.6% />
-<img src="https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/ulhkwh_fastlio.gif" width = 49.6% >
-</div>
-
-**Related video:**  [FAST-LIO2](https://youtu.be/2OvjGnxszf8),  [FAST-LIO1](https://youtu.be/iYCY6T79oNU)
-
-**Pipeline:**
-<div align="center">
-<img src="https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/overview_fastlio2.svg" width=99% />
-</div>
-
-**New Features:**
-1. Incremental mapping using [ikd-Tree](https://github.com/hku-mars/ikd-Tree), achieve faster speed and over 100Hz LiDAR rate.
-2. Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be disabled), achieving better accuracy.
-3. Since no requirements for feature extraction, FAST-LIO2 can support many types of LiDAR including spinning (Velodyne, Ouster) and solid-state (Livox Avia, Horizon, MID-70) LiDARs, and can be easily extended to support more LiDARs.
-4. Support external IMU.
-5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry Pi 4B(8G RAM).
-
-**Related papers**: 
-
-[FAST-LIO2: Fast Direct LiDAR-inertial Odometry](https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/Fast_LIO_2.pdf)
-
-[FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.08196)
-
-**Contributors**
-
-[Wei Xu 徐威](https://github.com/XW-HKU)，[Yixi Cai 蔡逸熙](https://github.com/Ecstasy-EC)，[Dongjiao He 贺东娇](https://github.com/Joanna-HE)，[Fangcheng Zhu 朱方程](https://github.com/zfc-zfc)，[Jiarong Lin 林家荣](https://github.com/ziv-lin)，[Zheng Liu 刘政](https://github.com/Zale-Liu), [Borong Yuan](https://github.com/borongyuan)
-
-<!-- <div align="center">
-    <img src="https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/results/HKU_HW.png" width = 49% >
-    <img src="https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/results/HKU_MB_001.png" width = 49% >
-</div> -->
-
-## 1. Prerequisites
-### 1.1 **Ubuntu** and **ROS**
-**Ubuntu >= 20.04**
-
-The **default from apt** PCL and Eigen is enough for FAST-LIO to work normally.
-
-ROS >= Foxy (Recommend to use ROS-Humble). [ROS Installation](https://docs.ros.org/en/humble/Installation.html)
-
-### 1.2. **PCL && Eigen**
-PCL    >= 1.8,   Follow [PCL Installation](https://pointclouds.org/downloads/#linux).
-
-Eigen  >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page).
-
-### <span id="1.3">1.3. **livox_ros_driver2**</span>
-Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2).
-
-You can also use the one I modified [livox_ros_driver2](https://github.com/Ericsii/livox_ros_driver2/tree/feature/use-standard-unit)
-
-*Remarks:*
-- Since the FAST-LIO must support Livox serials LiDAR firstly, so the **livox_ros_driver** must be installed and **sourced** before run any FAST-LIO launch file.
-- How to source? The easiest way is add the line ``` source $Livox_ros_driver_dir$/devel/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Livox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document).
-
-
-## 2. Build
-Clone the repository and colcon build:
-
-```bash
-    cd <ros2_ws>/src # cd into a ros2 workspace folder
-    git clone https://github.com/Ericsii/FAST_LIO_ROS2.git --recursive
-    cd ..
-    rosdep install --from-paths src --ignore-src -y
-    colcon build --symlink-install
-    . ./install/setup.bash # use setup.zsh if use zsh
-```
-- **Remember to source the livox_ros_driver before build (follow [1.3 livox_ros_driver](#1.3))**
-- If you want to use a custom build of PCL, add the following line to ~/.bashrc
-```export PCL_ROOT={CUSTOM_PCL_PATH}```
-## 3. Directly run
-Noted:
-
-A. Please make sure the IMU and LiDAR are **Synchronized**, that's important.
-
-B. The warning message "Failed to find match for field 'time'." means the timestamps of each LiDAR points are missed in the rosbag file. That is important for the forward propagation and backwark propagation.
-
-C. We recommend to set the **extrinsic_est_en** to false if the extrinsic is give. As for the extrinsic initiallization, please refer to our recent work: [**Robust Real-time LiDAR-inertial Initialization**](https://github.com/hku-mars/LiDAR_IMU_Init).
-
-### 3.1 Run use ros launch
-Connect to your PC to Livox LiDAR by following  [Livox-ros-driver2 installation](https://github.com/Livox-SDK/livox_ros_driver2), then
-```bash
-cd <ros2_ws>
-. install/setup.bash # use setup.zsh if use zsh
-ros2 launch fast_lio mapping.launch.py config_file:=avia.yaml
-```
-
-Change `config_file` parameter to other yaml file under config directory as you need.
-
-Launch livox ros driver. Use MID360 as an example.
-
-```bash
-ros2 launch livox_ros_driver2 msg_MID360_launch.py
-```
-
-- For livox serials, FAST-LIO only support the data collected by the ``` livox_lidar_msg.launch ``` since only its ``` livox_ros_driver2/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for the motion undistortion. ``` livox_lidar.launch ``` can not produce it right now.
-- If you want to change the frame rate, please modify the **publish_freq** parameter in the [livox_lidar_msg.launch](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver2/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver2) before make the livox_ros_driver pakage.
-
-### 3.2 For Livox serials with external IMU
-
-mapping_avia.launch theratically supports mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run:
-
-Edit ``` config/avia.yaml ``` to set the below parameters:
-
-1. LiDAR point cloud topic name: ``` lid_topic ```
-2. IMU topic name: ``` imu_topic ```
-3. Translational extrinsic: ``` extrinsic_T ```
-4. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)
-- The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). They can be found in the official manual.
-- FAST-LIO produces a very simple software time sync for livox LiDAR, set parameter ```time_sync_en``` to ture to turn on. But turn on **ONLY IF external time synchronization is really not possible**, since the software time sync cannot make sure accuracy.
-
-### 3.4 PCD file save
-
-1. Enable `pcd_save.pcd_save_en` in the config file and set the `map_file_path` to the path where the map will be saved.
-2. Launch the fastlio2 according to README.
-3. Open RQt and switch to `Plugins->Services->Service Caller`. Trigger the service `/map_save`, then the pcd map file will be generated
-
-```pcl_viewer scans.pcd``` can visualize the point clouds.
-
-*Tips for pcl_viewer:*
-- change what to visualize/color by pressing keyboard 1,2,3,4,5 when pcl_viewer is running. 
-```
-    1 is all random
-    2 is X values
-    3 is Y values
-    4 is Z values
-    5 is intensity
-```
-
-## 4. Rosbag Example
-### 4.1 Livox Avia Rosbag
-<div align="left">
-<img src="https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/results/HKU_LG_Indoor.png" width=47% />
-<img src="https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/results/HKU_MB_002.png" width = 51% >
-
-Files: Can be downloaded from [google drive](https://drive.google.com/drive/folders/1CGYEJ9-wWjr8INyan6q1BZz_5VtGB-fP?usp=sharing)**!!!This ros1 bag should be convert to ros2!!!**
-
-Run:
-```bash
-ros2 launch fast_lio mapping.launch.py config_path:=<path_to_your_config_file>
-ros2 bag play <your_bag_dir>
-
-```
-
-### 4.2 Velodyne HDL-32E Rosbag
-
-**NCLT Dataset**: Original bin file can be found [here](http://robots.engin.umich.edu/nclt/).
-
-We produce [Rosbag Files](https://drive.google.com/drive/folders/1VBK5idI1oyW0GC_I_Hxh63aqam3nocNK?usp=sharing) and [a python script](https://drive.google.com/file/d/1leh7DxbHx29DyS1NJkvEfeNJoccxH7XM/view) to generate Rosbag files: ```python3 sensordata_to_rosbag_fastlio.py bin_file_dir bag_name.bag```**!!!This ros1 bag should be convert to ros2!!!** To convert ros1 bag to ros2 bag, please follow the documentation [Convert rosbag versions](https://ternaris.gitlab.io/rosbags/topics/convert.html)
-    
-Run:
-```
-roslaunch fast_lio mapping_velodyne.launch
-rosbag play YOUR_DOWNLOADED.bag
-```
-
-## 5.Implementation on UAV
-In order to validate the robustness and computational efficiency of FAST-LIO in actual mobile robots, we build a small-scale quadrotor which can carry a Livox Avia LiDAR with 70 degree FoV and a DJI Manifold 2-C onboard computer with a 1.8 GHz Intel i7-8550U CPU and 8 G RAM, as shown in below.
-
-The main structure of this UAV is 3d printed (Aluminum or PLA), the .stl file will be open-sourced in the future.
-
-<div align="center">
-    <img src="https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/uav01.jpg" width=40.5% >
-    <img src="https://raw.githubusercontent.com/hku-mars/FAST_LIO/main/doc/uav_system.png" width=57% >
-</div>
-
-## 6.Acknowledgments
-
-Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), [Livox_Mapping](https://github.com/Livox-SDK/livox_mapping), [LINS](https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM) and [Loam_Livox](https://github.com/hku-mars/loam_livox).

FAST_LIO/config/avia.yaml

@@ -1,46 +0,0 @@
-/**:
-    ros__parameters:
-        feature_extract_enable: false
-        point_filter_num: 3
-        max_iteration: 3
-        filter_size_surf: 0.5
-        filter_size_map: 0.5
-        cube_side_length: 1000.0
-        runtime_pos_log_enable: false
-        map_file_path: "./test.pcd"
-
-        common:
-            lid_topic:  "/livox/lidar"
-            imu_topic:  "/livox/imu"
-            time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
-            time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
-                                        # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
-
-        preprocess:
-            lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
-            scan_line: 6
-            blind: 4.0
-
-        mapping:
-            acc_cov: 0.1
-            gyr_cov: 0.1
-            b_acc_cov: 0.0001
-            b_gyr_cov: 0.0001
-            fov_degree:    90.0
-            det_range:     450.0
-            extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
-            extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
-            extrinsic_R: [ 1., 0., 0.,
-                        0., 1., 0.,
-                        0., 0., 1.]
-
-        publish:
-            path_en:  false
-            scan_publish_en:  true       # false: close all the point cloud output
-            dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
-            scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
-
-        pcd_save:
-            pcd_save_en: true
-            interval: -1                 # how many LiDAR frames saved in each pcd file; 
-                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

FAST_LIO/config/horizon.yaml

@@ -1,46 +0,0 @@
-/**:
-    ros__parameters:
-        feature_extract_enable: false
-        point_filter_num: 3
-        max_iteration: 3
-        filter_size_surf: 0.5
-        filter_size_map: 0.5
-        cube_side_length: 1000.0
-        runtime_pos_log_enable: false
-        map_file_path: "./test.pcd"
-
-        common:
-            lid_topic:  "/livox/lidar"
-            imu_topic:  "/livox/imu"
-            time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
-            time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
-                                        # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
-            
-        preprocess:
-            lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
-            scan_line:  6
-            blind: 4.0
-
-        mapping:
-            acc_cov: 0.1
-            gyr_cov: 0.1
-            b_acc_cov: 0.0001
-            b_gyr_cov: 0.0001
-            fov_degree:    100.0
-            det_range:     260.0
-            extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic
-            extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]
-            extrinsic_R: [ 1., 0., 0.,
-                        0., 1., 0.,
-                        0., 0., 1.]
-
-        publish:
-            path_en:  false
-            scan_publish_en:  true       # false: close all the point cloud output
-            dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
-            scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
-
-        pcd_save:
-            pcd_save_en: true
-            interval: -1                 # how many LiDAR frames saved in each pcd file; 
-                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

FAST_LIO/config/mid360.yaml

@@ -1,50 +0,0 @@
-/**:
-    ros__parameters:
-        feature_extract_enable: false
-        point_filter_num: 3
-        max_iteration: 3
-        filter_size_surf: 0.5
-        filter_size_map: 0.5
-        cube_side_length: 400.0
-        runtime_pos_log_enable: false
-        map_file_path: "./test.pcd"
-
-        common:
-            lid_topic:  "/livox/lidar"
-            imu_topic:  "/livox/imu"
-            time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
-            time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
-                                        # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
-
-        preprocess:
-            lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for any other pointcloud input
-            scan_line:  4
-            blind: 0.5
-            timestamp_unit: 3
-            scan_rate: 10
-
-        mapping:
-            acc_cov: 0.1
-            gyr_cov: 0.1
-            b_acc_cov: 0.0001
-            b_gyr_cov: 0.0001
-            fov_degree:    360.0
-            det_range:     60.0
-            extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic
-            extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
-            extrinsic_R: [ 1., 0., 0.,
-                            0., 1., 0.,
-                            0., 0., 1.]
-
-        publish:
-            path_en: true                # true: publish Path
-            effect_map_en: true         # true: publish Effects
-            map_en: true                 # true: publish Map cloud
-            scan_publish_en:  true       # false: close all the point cloud output
-            dense_publish_en: false      # false: low down the points number in a global-frame point clouds scan.
-            scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
-
-        pcd_save:
-            pcd_save_en: true
-            interval: -1                 # how many LiDAR frames saved in each pcd file; 
-                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

FAST_LIO/config/ouster64.yaml

@@ -1,47 +0,0 @@
-/**:
-    ros__parameters:
-        feature_extract_enable: false
-        point_filter_num: 3
-        max_iteration: 3
-        filter_size_surf: 0.5
-        filter_size_map: 0.5
-        cube_side_length: 1000.0
-        runtime_pos_log_enable: false
-        map_file_path: "./test.pcd"
-
-        common:
-            lid_topic:  "/os_cloud_node/points"
-            imu_topic:  "/os_cloud_node/imu"
-            time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
-            time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
-                                        # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
-            
-        preprocess:
-            lidar_type: 3                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
-            scan_line: 64
-            timestamp_unit: 3                 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
-            blind: 4.0
-
-        mapping:
-            acc_cov: 0.1
-            gyr_cov: 0.1
-            b_acc_cov: 0.0001
-            b_gyr_cov: 0.0001
-            fov_degree:    360.0
-            det_range:     150.0
-            extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
-            extrinsic_T: [ 0.0, 0.0, 0.0 ]
-            extrinsic_R: [1., 0., 0.,
-                        0., 1., 0.,
-                        0., 0., 1.]
-
-        publish:
-            path_en:  false
-            scan_publish_en:  true       # false: close all the point cloud output
-            dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
-            scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
-
-        pcd_save:
-            pcd_save_en: true
-            interval: -1                 # how many LiDAR frames saved in each pcd file; 
-                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

FAST_LIO/config/unilidar_l2.yaml

@@ -1,61 +0,0 @@
-/**:
-    ros__parameters:
-        # ================== Global Settings ==================
-        feature_extract_enable: false
-        max_iteration: 3
-        filter_size_surf: 0.5
-        filter_size_map: 0.5
-        cube_side_length: 1000.0
-        runtime_pos_log_enable: false
-        map_file_path: "./test.pcd"
-
-        # ================== Sensor Topics ==================
-        common:
-            lid_topic: "/unilidar/cloud"          # LiDAR点云话题
-            imu_topic: "/unilidar/imu"            # IMU话题
-            time_sync_en: false                   # 关闭内部时间同步（若需要同步请设为true）
-            time_offset_lidar_to_imu: 0.0          # IMU到LiDAR时间偏移（与您配置的time_lag_imu_to_lidar取反）
-
-        # ================== LiDAR预处理 ==================
-        preprocess:
-            lidar_type: 5                        # 雷达类型（需确认类型编号对应关系）
-            scan_line: 18                         # 扫描线数
-            point_filter_num: 1                   # 点云降采样率（原con_frame_num）
-            blind: 0.5                            # 盲区过滤半径（米）
-            timestamp_unit: 0                     # 时间戳单位：0=秒，1=毫秒，2=微秒，3=纳秒
-
-        # ================== SLAM核心参数 ==================
-        mapping:
-            # IMU参数
-            imu_en: true                          # 启用IMU
-            imu_time_inte: 0.004                  # IMU采样间隔（1/frequency）
-            acc_cov: 0.1                          # 加速度计噪声协方差
-            gyr_cov: 0.1                          # 陀螺仪噪声协方差
-            b_acc_cov: 0.0001                     # 加速度计零偏噪声
-            b_gyr_cov: 0.0001                     # 陀螺仪零偏噪声
-            
-            # 外参标定
-            extrinsic_est_en: false               # 关闭在线外参标定
-            extrinsic_T: [0.007698, 0.014655, -0.00667]  # IMU到LiDAR平移
-            extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]     # IMU到LiDAR旋转矩阵
-            
-            # 环境参数
-            fov_degree: 180.0                       # 有效FOV角度
-            det_range: 100.0                      # 最大探测距离（米）
-            plane_thr: 0.1                        # 平面拟合阈值
-            
-            # 重力对齐
-            gravity_align: true                   # 启用重力对齐
-            gravity: [0.0, 0.0, -9.810]           # 重力向量（与您配置一致）
-
-        # ================== 输出设置 ==================
-        publish:
-            path_en: true                         # 发布轨迹路径
-            scan_publish_en: true                 # 发布原始点云
-            scan_bodyframe_pub_en: true           # 发布IMU坐标系点云
-            dense_publish_en: true                # 发布稠密地图
-
-        # ================== 地图保存 ==================
-        pcd_save:
-            pcd_save_en: true                     # 启用PCD保存
-            interval: -1                          # 全帧保存（注意内存风险）

FAST_LIO/config/velodyne.yaml

@@ -1,48 +0,0 @@
-/**:
-    ros__parameters:
-        feature_extract_enable: false
-        point_filter_num: 4
-        max_iteration: 3
-        filter_size_surf: 0.5
-        filter_size_map: 0.5
-        cube_side_length: 1000.0
-        runtime_pos_log_enable: false
-        map_file_path: "./test.pcd"
-
-        common:
-            lid_topic:  "/velodyne_points"
-            imu_topic:  "/imu/data"
-            time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
-            time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
-                                        # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
-
-        preprocess:
-            lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
-            scan_line: 32
-            scan_rate: 10                # only need to be set for velodyne, unit: Hz,
-            timestamp_unit: 2            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
-            blind: 2.0
-
-        mapping:
-            acc_cov: 0.1
-            gyr_cov: 0.1
-            b_acc_cov: 0.0001
-            b_gyr_cov: 0.0001
-            fov_degree:    360.0
-            det_range:     100.0
-            extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
-            extrinsic_T: [ 0., 0., 0.28]
-            extrinsic_R: [ 1., 0., 0., 
-                        0., 1., 0., 
-                        0., 0., 1.]
-
-        publish:
-            path_en:  false
-            scan_publish_en:  true       # false: close all the point cloud output
-            dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
-            scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
-
-        pcd_save:
-            pcd_save_en: true
-            interval: -1                 # how many LiDAR frames saved in each pcd file; 
-                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

FAST_LIO/include/Exp_mat.h

@@ -1,103 +0,0 @@
-#ifndef EXP_MAT_H
-#define EXP_MAT_H
-
-#include <math.h>
-#include <Eigen/Core>
-#include <opencv2/core.hpp>
-// #include <common_lib.h>
-
-#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0
-
-template<typename T>
-Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &&ang)
-{
-    T ang_norm = ang.norm();
-    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
-    if (ang_norm > 0.0000001)
-    {
-        Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;
-        Eigen::Matrix<T, 3, 3> K;
-        K << SKEW_SYM_MATRX(r_axis);
-        /// Roderigous Tranformation
-        return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;
-    }
-    else
-    {
-        return Eye3;
-    }
-}
-
-template<typename T, typename Ts>
-Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
-{
-    T ang_vel_norm = ang_vel.norm();
-    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
-
-    if (ang_vel_norm > 0.0000001)
-    {
-        Eigen::Matrix<T, 3, 1> r_axis = ang_vel / ang_vel_norm;
-        Eigen::Matrix<T, 3, 3> K;
-
-        K << SKEW_SYM_MATRX(r_axis);
-
-        T r_ang = ang_vel_norm * dt;
-
-        /// Roderigous Tranformation
-        return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;
-    }
-    else
-    {
-        return Eye3;
-    }
-}
-
-template<typename T>
-Eigen::Matrix<T, 3, 3> Exp(const T &v1, const T &v2, const T &v3)
-{
-    T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
-    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
-    if (norm > 0.00001)
-    {
-        T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
-        Eigen::Matrix<T, 3, 3> K;
-        K << SKEW_SYM_MATRX(r_ang);
-
-        /// Roderigous Tranformation
-        return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;
-    }
-    else
-    {
-        return Eye3;
-    }
-}
-
-/* Logrithm of a Rotation Matrix */
-template<typename T>
-Eigen::Matrix<T,3,1> Log(const Eigen::Matrix<T, 3, 3> &R)
-{
-    T &&theta = std::acos(0.5 * (R.trace() - 1));
-    Eigen::Matrix<T,3,1> K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));
-    return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
-}
-
-// template<typename T>
-// cv::Mat Exp(const T &v1, const T &v2, const T &v3)
-// {
-    
-//     T norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
-//     cv::Mat Eye3 = cv::Mat::eye(3, 3, CV_32F);
-//     if (norm > 0.0000001)
-//     {
-//         T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
-//         cv::Mat K = (cv::Mat_<T>(3,3) << SKEW_SYM_MATRX(r_ang));
-
-//         /// Roderigous Tranformation
-//         return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;
-//     }
-//     else
-//     {
-//         return Eye3;
-//     }
-// }
-
-#endif

FAST_LIO/include/IKFoM_toolkit/esekfom/util.hpp

@@ -1,82 +0,0 @@
-/*
- *  Copyright (c) 2019--2023, The University of Hong Kong
- *  All rights reserved.
- *
- *  Author: Dongjiao HE <hdj65822@connect.hku.hk>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __MEKFOM_UTIL_HPP__
-#define __MEKFOM_UTIL_HPP__
-
-#include <Eigen/Core>
-#include "../mtk/src/mtkmath.hpp"
-namespace esekfom {
-
-template <typename T1, typename T2>
-class is_same {
-public:
-    operator bool() {
-        return false;
-    }
-};
-template<typename T1>
-class is_same<T1, T1> {
-public:
-    operator bool() {
-        return true;
-    }
-};
-
-template <typename T>
-class is_double {
-public:
-    operator bool() {
-        return false;
-    }
-};
-
-template<>
-class is_double<double> {
-public:
-    operator bool() {
-        return true;
-    }
-};
-
-template<typename T>
-static T
-id(const T &x)
-{
-	return x;
-}
-
-} // namespace esekfom
-	
-#endif // __MEKFOM_UTIL_HPP__

FAST_LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp

@@ -1,229 +0,0 @@
-// This is an advanced implementation of the algorithm described in the
-// following paper:
-//    C. Hertzberg,  R.  Wagner,  U.  Frese,  and  L.  Schroder.  Integratinggeneric   sensor   fusion   algorithms   with   sound   state   representationsthrough  encapsulation  of  manifolds.
-//    CoRR,  vol.  abs/1107.1119,  2011.[Online]. Available: http://arxiv.org/abs/1107.1119
-
-/*
- *  Copyright (c) 2019--2023, The University of Hong Kong
- *  All rights reserved.
- *
- *  Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
- 
-/*
- *  Copyright (c) 2008--2011, Universitaet Bremen
- *  All rights reserved.
- *
- *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-/**
- * @file mtk/build_manifold.hpp
- * @brief Macro to automatically construct compound manifolds.
- * 
- */
-#ifndef MTK_AUTOCONSTRUCT_HPP_
-#define MTK_AUTOCONSTRUCT_HPP_
-
-#include <vector>
-
-#include <boost/preprocessor/seq.hpp>
-#include <boost/preprocessor/cat.hpp>
-#include <Eigen/Core>
-
-#include "src/SubManifold.hpp"
-#include "startIdx.hpp"
-
-#ifndef PARSED_BY_DOXYGEN
-//////// internals //////
-
-#define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple
-
-#define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries))
-
-#define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)
-
-#define MTK_CONSTRUCTOR_ARG(  type, id) const type& id = type()
-#define MTK_CONSTRUCTOR_COPY( type, id) id(id)
-#define MTK_BOXPLUS(          type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale);
-#define MTK_OPLUS(               type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale);
-#define MTK_BOXMINUS(         type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id);
-#define MTK_S2_hat(          type, id) if(id.IDX == idx){id.S2_hat(res);}
-#define MTK_S2_Nx_yy(            type, id) if(id.IDX == idx){id.S2_Nx_yy(res);}
-#define MTK_S2_Mx(          type, id) if(id.IDX == idx){id.S2_Mx(res, dx);}
-#define MTK_OSTREAM(          type, id) << __var.id << " "
-#define MTK_ISTREAM(          type, id) >> __var.id
-#define MTK_S2_state(         type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));}
-#define MTK_SO3_state(        type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));}
-#define MTK_vect_state(        type, id) if(id.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));}
-
-#define MTK_SUBVARLIST(seq, S2state, SO3state) \
-BOOST_PP_FOR_1( \
-		( \
-				BOOST_PP_SEQ_SIZE(seq), \
-				BOOST_PP_SEQ_HEAD(seq), \
-				BOOST_PP_SEQ_TAIL(seq) (~), \
-				0,\
-				0,\
-				S2state,\
-				SO3state ),\
-		MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT)
-
-#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \
-	MTK::SubManifold<type, dof, dim> id; 
-#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state) \
-	MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \
-	enum {DOF = type::DOF + dof}; \
-	enum {DIM = type::DIM+dim}; \
-	typedef type::scalar scalar; 
-
-#define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state
-#define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state) \
-	MTK_APPLY_MACRO_ON_TUPLE(~, \
-		BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \
-		( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state)) 
-
-#define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state
-
-//! this used to be BOOST_PP_TUPLE_ELEM_4_0:
-#define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g) a
-
-#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state
-#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state) ( \
-		BOOST_PP_DEC(len), \
-		BOOST_PP_SEQ_HEAD(seq), \
-		BOOST_PP_SEQ_TAIL(seq), \
-		dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\
-		dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\
-		S2state,\
-		SO3state)
-
-#endif /* not PARSED_BY_DOXYGEN */
-
-
-/**
- * Construct a manifold.
- * @param name is the class-name of the manifold, 
- * @param entries is the list of sub manifolds 
- * 
- * Entries must be given in a list like this:
- * @code
- * typedef MTK::trafo<MTK::SO3<double> > Pose;
- * typedef MTK::vect<double, 3> Vec3;
- * MTK_BUILD_MANIFOLD(imu_state,
- *    ((Pose, pose))
- *    ((Vec3, vel))
- *    ((Vec3, acc_bias))
- * )
- * @endcode
- * Whitespace is optional, but the double parentheses are necessary.
- * Construction is done entirely in preprocessor.
- * After construction @a name is also a manifold. Its members can be 
- * accessed by names given in @a entries.
- * 
- * @note Variable types are not allowed to have commas, thus types like
- *       @c vect<double, 3> need to be typedef'ed ahead.
- */
-#define MTK_BUILD_MANIFOLD(name, entries) \
-struct name { \
-	typedef name self; \
-	std::vector<std::pair<int, int> > S2_state;\
-	std::vector<std::pair<int, int> > SO3_state;\
-	std::vector<std::pair<std::pair<int, int>, int> > vect_state;\
-	MTK_SUBVARLIST(entries, S2_state, SO3_state) \
-	name ( \
-		MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \
-		) : \
-		MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\
-	int getDOF() const { return DOF; } \
-	void boxplus(const MTK::vectview<const scalar, DOF> & __vec, scalar __scale = 1 ) { \
-		MTK_TRANSFORM(MTK_BOXPLUS, entries)\
-	} \
-	void oplus(const MTK::vectview<const scalar, DIM> & __vec, scalar __scale = 1 ) { \
-		MTK_TRANSFORM(MTK_OPLUS, entries)\
-	} \
-	void boxminus(MTK::vectview<scalar,DOF> __res, const name& __oth) const { \
-		MTK_TRANSFORM(MTK_BOXMINUS, entries)\
-	} \
-	friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \
-		return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \
-	} \
-	void build_S2_state(){\
-		MTK_TRANSFORM(MTK_S2_state, entries)\
-	}\
-	void build_vect_state(){\
-		MTK_TRANSFORM(MTK_vect_state, entries)\
-	}\
-	void build_SO3_state(){\
-		MTK_TRANSFORM(MTK_SO3_state, entries)\
-	}\
-	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res, int idx) {\
-		MTK_TRANSFORM(MTK_S2_hat, entries)\
-	}\
-	void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res, int idx) {\
-		MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\
-	}\
-	void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, Eigen::Matrix<scalar, 2, 1> dx, int idx) {\
-		MTK_TRANSFORM(MTK_S2_Mx, entries)\
-	}\
-	friend std::istream& operator>>(std::istream& __is, name& __var){ \
-		return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \
-	} \
-};
-
-
-
-#endif /*MTK_AUTOCONSTRUCT_HPP_*/

FAST_LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp

@@ -1,123 +0,0 @@
-// This is an advanced implementation of the algorithm described in the
-// following paper:
-//    C. Hertzberg,  R.  Wagner,  U.  Frese,  and  L.  Schroder.  Integratinggeneric   sensor   fusion   algorithms   with   sound   state   representationsthrough  encapsulation  of  manifolds.
-//    CoRR,  vol.  abs/1107.1119,  2011.[Online]. Available: http://arxiv.org/abs/1107.1119
-
-/*
- *  Copyright (c) 2019--2023, The University of Hong Kong
- *  All rights reserved.
- *
- *  Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
- 
-/*
- *  Copyright (c) 2008--2011, Universitaet Bremen
- *  All rights reserved.
- *
- *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-/**
- * @file mtk/src/SubManifold.hpp
- * @brief Defines the SubManifold class
- */
-
-
-#ifndef SUBMANIFOLD_HPP_
-#define SUBMANIFOLD_HPP_
-
-
-#include "vectview.hpp"
-
-
-namespace MTK {
-
-/**
- * @ingroup SubManifolds
- * Helper class for compound manifolds. 
- * This class wraps a manifold T and provides an enum IDX refering to the 
- * index of the SubManifold within the compound manifold. 
- *  
- * Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds".
- * 
- * @tparam T   The manifold type of the sub-type
- * @tparam idx The index of the sub-type within the compound manifold
- */
-template<class T, int idx, int dim>
-struct SubManifold : public T 
-{
-	enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ };
-	//! manifold type
-	typedef T type;
-	
-	//! Construct from derived type
-	template<class X>
-	explicit
-	SubManifold(const X& t) : T(t) {};
-	
-	//! Construct from internal type
-	//explicit
-	SubManifold(const T& t) : T(t) {};
-	
-	//! inherit assignment operator
-	using T::operator=;
-	
-};
-
-}  // namespace MTK
-
-
-#endif /* SUBMANIFOLD_HPP_ */

FAST_LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp

@@ -1,294 +0,0 @@
-// This is an advanced implementation of the algorithm described in the
-// following paper:
-//    C. Hertzberg,  R.  Wagner,  U.  Frese,  and  L.  Schroder.  Integratinggeneric   sensor   fusion   algorithms   with   sound   state   representationsthrough  encapsulation  of  manifolds.
-//    CoRR,  vol.  abs/1107.1119,  2011.[Online]. Available: http://arxiv.org/abs/1107.1119
-
-/*
- *  Copyright (c) 2019--2023, The University of Hong Kong
- *  All rights reserved.
- *
- *  Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
- 
-/*
- *  Copyright (c) 2008--2011, Universitaet Bremen
- *  All rights reserved.
- *
- *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-/**
- * @file mtk/src/mtkmath.hpp
- * @brief several math utility functions.
- */
-
-#ifndef MTKMATH_H_
-#define MTKMATH_H_
-
-#include <cmath>
-
-#include <boost/math/tools/precision.hpp>
-
-#include "../types/vect.hpp"
-
-#ifndef M_PI
-#define M_PI  3.1415926535897932384626433832795
-#endif
-
-
-namespace MTK {
-
-namespace internal {
-
-template<class Manifold>
-struct traits {
-	typedef typename Manifold::scalar scalar;
-	enum {DOF = Manifold::DOF};
-	typedef vect<DOF, scalar> vectorized_type;
-	typedef Eigen::Matrix<scalar, DOF, DOF> matrix_type;
-};
-
-template<>
-struct traits<float> : traits<Scalar<float> > {};
-template<>
-struct traits<double> : traits<Scalar<double> > {};
-
-}  // namespace internal
-
-/**
- * \defgroup MTKMath Mathematical helper functions
- */
-//@{
-
-//! constant @f$ \pi @f$
-const double pi = M_PI;
-
-template<class scalar> inline scalar tolerance();
-
-template<> inline float  tolerance<float >() { return 1e-5f; }
-template<> inline double tolerance<double>() { return 1e-11; }
-
-
-/**
- * normalize @a x to @f$[-bound, bound] @f$.
- * 
- * result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$.
- */
-template<class scalar>
-inline scalar normalize(scalar x, scalar bound){ //not used
-	if(std::fabs(x) <= bound) return x;
-	int r = (int)(x *(scalar(1.0)/ bound));
-	return x - ((r + (r>>31) + 1) & ~1)*bound; 
-}
-
-/**
- * Calculate cosine and sinc of sqrt(x2).
- * @param x2 the squared angle must be non-negative
- * @return a pair containing cos and sinc of sqrt(x2)
- */
-template<class scalar>
-std::pair<scalar, scalar> cos_sinc_sqrt(const scalar &x2){
-	using std::sqrt;
-	using std::cos;
-	using std::sin;
-	static scalar const taylor_0_bound = boost::math::tools::epsilon<scalar>();
-	static scalar const taylor_2_bound = sqrt(taylor_0_bound);
-	static scalar const taylor_n_bound = sqrt(taylor_2_bound);
-	
-	assert(x2>=0 && "argument must be non-negative");
-	
-	// FIXME check if bigger bounds are possible
-	if(x2>=taylor_n_bound) {
-		// slow fall-back solution
-		scalar x = sqrt(x2);
-		return std::make_pair(cos(x), sin(x)/x); // x is greater than 0.
-	}
-	
-	// FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd)
-	// TODO Find optimal coefficients using Remez algorithm
-	static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.};
-	scalar cosi = 1., sinc=1;
-	scalar term = -1/2. * x2;
-	for(int i=0; i<3; ++i) {
-		cosi += term;
-		term *= inv[2*i];
-		sinc += term;
-		term *= -inv[2*i+1] * x2;
-	}
-	
-	return std::make_pair(cosi, sinc);
-	
-}
-
-template<typename Base>
-Eigen::Matrix<typename Base::scalar, 3, 3> hat(const Base& v) {
-    Eigen::Matrix<typename Base::scalar, 3, 3> res;
-	res << 0, -v[2], v[1],
-		v[2], 0, -v[0],
-		-v[1], v[0], 0;
-	return res;
-}
-
-template<typename Base>
-Eigen::Matrix<typename Base::scalar, 3, 3> A_inv_trans(const Base& v){
-    Eigen::Matrix<typename Base::scalar, 3, 3> res;
-    if(v.norm() > MTK::tolerance<typename Base::scalar>())
-    {
-        res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
-    
-    }
-    else
-    {
-        res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
-    }
-    
-    return res;
-}
-
-template<typename Base>
-Eigen::Matrix<typename Base::scalar, 3, 3> A_inv(const Base& v){
-    Eigen::Matrix<typename Base::scalar, 3, 3> res;
-    if(v.norm() > MTK::tolerance<typename Base::scalar>())
-    {
-        res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() - 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
-    
-    }
-    else
-    {
-        res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
-    }
-    
-    return res;
-}
-
-template<typename scalar>
-Eigen::Matrix<scalar, 2, 3> S2_w_expw_( Eigen::Matrix<scalar, 2, 1> v, scalar length)
-	{
-    	Eigen::Matrix<scalar, 2, 3> res;
-    	scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]);
-		if(norm < MTK::tolerance<scalar>()){
-	    	res = Eigen::Matrix<scalar, 2, 3>::Zero();
-	    	res(0, 1) = 1;
-	    	res(1, 2) = 1;
-        	res /= length;
-		}
-		else{
-			res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0,
-            	   -v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm);
-        	res /= length;
-    	}	
-	}
-
-template<typename Base>
-Eigen::Matrix<typename Base::scalar, 3, 3> A_matrix(const Base & v){
-    Eigen::Matrix<typename Base::scalar, 3, 3> res;
-    double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
-	double norm = std::sqrt(squaredNorm);
-	if(norm < MTK::tolerance<typename Base::scalar>()){
-		res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
-	}
-	else{
-		res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v);
-	}
-    return res;
-}
-
-template<class scalar, int n>
-scalar exp(vectview<scalar, n> result, vectview<const scalar, n> vec, const scalar& scale = 1) {
-	scalar norm2 = vec.squaredNorm();
-	std::pair<scalar, scalar> cos_sinc = cos_sinc_sqrt(scale*scale * norm2);
-	scalar mult = cos_sinc.second * scale; 
-	result = mult * vec;
-	return cos_sinc.first;
-}
-
-
-/**
- * Inverse function to @c exp.
- * 
- * @param result @c vectview to the result
- * @param w      scalar part of input
- * @param vec    vector part of input
- * @param scale  scale result by this value
- * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result 
- */
-template<class scalar, int n>
-void log(vectview<scalar, n> result,
-		const scalar &w, const vectview<const scalar, n> vec,
-		const scalar &scale, bool plus_minus_periodicity)
-{
-	// FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division
-	scalar nv = vec.norm();
-	if(nv < tolerance<scalar>()) {
-		if(!plus_minus_periodicity && w < 0) {
-			// find the maximal entry:
-			int i;
-			nv = vec.cwiseAbs().maxCoeff(&i);
-			result = scale * std::atan2(nv, w) * vect<n, scalar>::Unit(i);
-			return;
-		}
-		nv = tolerance<scalar>();
-	}
-	scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) );
-	
-	result = s * vec;
-}
-
-
-} // namespace MTK
-
-
-#endif /* MTKMATH_H_ */

FAST_LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp

@@ -1,168 +0,0 @@
-
-/*
- *  Copyright (c) 2008--2011, Universitaet Bremen
- *  All rights reserved.
- *
- *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-/**
- * @file mtk/src/vectview.hpp
- * @brief Wrapper class around a pointer used as interface for plain vectors.
- */
-
-#ifndef VECTVIEW_HPP_
-#define VECTVIEW_HPP_
-
-#include <Eigen/Core>
-
-namespace MTK {
-
-/**
- * A view to a vector.
- * Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions.
- * The dimension of the vector is given as template parameter and type-checked when used in expressions.
- * Data has to be modifiable.
- * 
- * @tparam scalar Scalar type of the vector.
- * @tparam dim    Dimension of the vector.
- * 
- * @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct
- */
-namespace internal {
-	template<class Base, class T1, class T2>
-	struct CovBlock {
-		typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> Type;
-		typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> ConstType;
-	};
-
-	template<class Base, class T1, class T2>
-	struct CovBlock_ {
-		typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> Type;
-		typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> ConstType;
-	};
-
-	template<typename Base1, typename Base2, typename T1, typename T2>
-	struct CrossCovBlock {
-		typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> Type;
-		typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> ConstType;
-	};
-
-	template<typename Base1, typename Base2, typename T1, typename T2>
-	struct CrossCovBlock_ {
-		typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> Type;
-		typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> ConstType;
-	};
-
-	template<class scalar, int dim>
-	struct VectviewBase {
-		typedef Eigen::Matrix<scalar, dim, 1> matrix_type;
-		typedef typename matrix_type::MapType Type;
-		typedef typename matrix_type::ConstMapType ConstType;
-	};
-
-	template<class T>
-	struct UnalignedType {
-		typedef T type;
-	};
-}
-
-template<class scalar, int dim>
-class vectview : public internal::VectviewBase<scalar, dim>::Type {
-	typedef internal::VectviewBase<scalar, dim> VectviewBase;
-public:
-	//! plain matrix type
-	typedef typename VectviewBase::matrix_type matrix_type;
-	//! base type
-	typedef typename VectviewBase::Type base;
-	//! construct from pointer
-	explicit
-	vectview(scalar* data, int dim_=dim) : base(data, dim_) {}
-	//! construct from plain matrix
-	vectview(matrix_type& m) : base(m.data(), m.size()) {}
-	//! construct from another @c vectview
-	vectview(const vectview &v) : base(v) {}
-	//! construct from Eigen::Block:
-	template<class Base>
-	vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0), block.size()) {}
-	template<class Base, bool PacketAccess>
-	vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0), block.size()) {}
-
-	//! inherit assignment operator
-	using base::operator=;
-	//! data pointer
-	scalar* data() {return const_cast<scalar*>(base::data());}
-};
-
-/**
- * @c const version of @c vectview.
- * Compared to @c Eigen::Map this implementation is const correct, i.e.,
- * data will not be modifiable using this view.
- * 
- * @tparam scalar Scalar type of the vector.
- * @tparam dim    Dimension of the vector.
- * 
- * @sa vectview
- */
-template<class scalar, int dim>
-class vectview<const scalar, dim> : public internal::VectviewBase<scalar, dim>::ConstType {
-	typedef internal::VectviewBase<scalar, dim> VectviewBase;
-public:
-	//! plain matrix type
-	typedef typename VectviewBase::matrix_type matrix_type;
-	//! base type
-	typedef typename VectviewBase::ConstType base;
-	//! construct from const pointer
-	explicit
-	vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {}
-	//! construct from column vector
-	template<int options>
-	vectview(const Eigen::Matrix<scalar, dim, 1, options>& m) : base(m.data()) {}
-	//! construct from row vector
-	template<int options, int phony>
-	vectview(const Eigen::Matrix<scalar, 1, dim, options, phony>& m) : base(m.data()) {}
-	//! construct from another @c vectview
-	vectview(vectview<scalar, dim> x) : base(x.data()) {}
-	//! construct from base
-	vectview(const base &x) : base(x) {}
-	/**
-	 * Construct from Block
-	 * @todo adapt this, when Block gets const-correct
-	 */
-	template<class Base>
-	vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0)) {}
-	template<class Base, bool PacketAccess>
-	vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0)) {}
-
-};
-
-
-} // namespace MTK
-
-#endif /* VECTVIEW_HPP_ */

FAST_LIO/include/IKFoM_toolkit/mtk/startIdx.hpp

@@ -1,328 +0,0 @@
-// This is an advanced implementation of the algorithm described in the
-// following paper:
-//    C. Hertzberg,  R.  Wagner,  U.  Frese,  and  L.  Schroder.  Integratinggeneric   sensor   fusion   algorithms   with   sound   state   representationsthrough  encapsulation  of  manifolds.
-//    CoRR,  vol.  abs/1107.1119,  2011.[Online]. Available: http://arxiv.org/abs/1107.1119
-
-/*
- *  Copyright (c) 2019--2023, The University of Hong Kong
- *  All rights reserved.
- *
- *  Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
- 
-/*
- *  Copyright (c) 2008--2011, Universitaet Bremen
- *  All rights reserved.
- *
- *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-/**
- * @file mtk/startIdx.hpp 
- * @brief Tools to access sub-elements of compound manifolds.
- */
-#ifndef GET_START_INDEX_H_
-#define GET_START_INDEX_H_
-
-#include <Eigen/Core>
-
-#include "src/SubManifold.hpp"
-#include "src/vectview.hpp"
-
-namespace MTK {
-
-
-/** 
- * \defgroup SubManifolds Accessing Submanifolds
- * For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers
- * can be used to get sub-vectors or matrix-blocks of a corresponding big matrix.
- * E.g. for a type @a pose consisting of @a orient and @a trans the member pointers
- * @c &pose::orient and @c &pose::trans give all required information and are still
- * valid if the base type gets extended or the actual types of @a orient and @a trans
- * change (e.g. from 2D to 3D).
- * 
- * @todo Maybe require manifolds to typedef MatrixType and VectorType, etc.
- */
-//@{
-
-/**
- * Determine the index of a sub-variable within a compound variable.
- */
-template<class Base, class T, int idx, int dim> 
-int getStartIdx( MTK::SubManifold<T, idx, dim> Base::*)
-{
-	return idx;
-}
-
-template<class Base, class T, int idx, int dim> 
-int getStartIdx_( MTK::SubManifold<T, idx, dim> Base::*)
-{
-	return dim;
-}
-
-/**
- * Determine the degrees of freedom of a sub-variable within a compound variable.
- */
-template<class Base, class T, int idx, int dim> 
-int getDof( MTK::SubManifold<T, idx, dim> Base::*)
-{
-	return T::DOF;
-}
-template<class Base, class T, int idx, int dim> 
-int getDim( MTK::SubManifold<T, idx, dim> Base::*)
-{
-	return T::DIM;
-}
-
-/**
- * set the diagonal elements of a covariance matrix corresponding to a sub-variable
- */
-template<class Base, class T, int idx, int dim> 
-void setDiagonal(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov, 
-		MTK::SubManifold<T, idx, dim> Base::*, const typename Base::scalar &val)
-{
-	cov.diagonal().template segment<T::DOF>(idx).setConstant(val);
-}
-
-template<class Base, class T, int idx, int dim> 
-void setDiagonal_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov, 
-		MTK::SubManifold<T, idx, dim> Base::*, const typename Base::scalar &val)
-{
-	cov.diagonal().template segment<T::DIM>(dim).setConstant(val);
-}
-
-/**
- * Get the subblock of corresponding to two members, i.e.
- * \code
- *  Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
- *  MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression;
- *  MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans();
- * \endcode
- * lets you modify mixed covariance entries in a bigger covariance matrix.
- */
-template<class Base, class T1, int idx1, int dim1, class T2, int idx2, int dim2>
-typename MTK::internal::CovBlock<Base, T1, T2>::Type
-subblock(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov, 
-		MTK::SubManifold<T1, idx1, dim1> Base::*, MTK::SubManifold<T2, idx2, dim2> Base::*)
-{
-	return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
-}
-
-template<class Base, class T1, int idx1,  int dim1, class T2, int idx2, int dim2>
-typename MTK::internal::CovBlock_<Base, T1, T2>::Type
-subblock_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov, 
-		MTK::SubManifold<T1, idx1, dim1> Base::*, MTK::SubManifold<T2, idx2, dim2> Base::*)
-{
-	return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
-}
-
-template<typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
-typename MTK::internal::CrossCovBlock<Base1, Base2, T1, T2>::Type
-subblock(Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF> &cov, MTK::SubManifold<T1, idx1, dim1> Base1::*, MTK::SubManifold<T2, idx2, dim2> Base2::*)
-{
-	return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
-}
-
-template<typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
-typename MTK::internal::CrossCovBlock_<Base1, Base2, T1, T2>::Type
-subblock_(Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM> &cov, MTK::SubManifold<T1, idx1, dim1> Base1::*, MTK::SubManifold<T2, idx2, dim2> Base2::*)
-{
-	return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
-}
-/**
- * Get the subblock of corresponding to a member, i.e.
- * \code
- *  Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
- *  MTK::subblock(m, &Pose::orient) = some_expression;
- * \endcode
- * lets you modify covariance entries in a bigger covariance matrix.
- */
-template<class Base, class T, int idx, int dim>
-typename MTK::internal::CovBlock_<Base, T, T>::Type
-subblock_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov, 
-		MTK::SubManifold<T, idx, dim> Base::*)
-{
-	return cov.template block<T::DIM, T::DIM>(dim, dim);
-}
-
-template<class Base, class T, int idx, int dim>
-typename MTK::internal::CovBlock<Base, T, T>::Type
-subblock(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov, 
-		MTK::SubManifold<T, idx, dim> Base::*)
-{
-	return cov.template block<T::DOF, T::DOF>(idx, idx);
-}
-
-template<typename Base>
-class get_cov { 
-public:
-    typedef Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> type;
-    typedef const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> const_type;
-};
-
-template<typename Base>
-class get_cov_ { 
-public:
-    typedef Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> type;
-    typedef const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> const_type;
-};
-
-template<typename Base1, typename Base2>
-class get_cross_cov {
-public:
-    typedef Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF> type;
-    typedef const type const_type;
-};
-
-template<typename Base1, typename Base2>
-class get_cross_cov_ {
-public:
-    typedef Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM> type;
-    typedef const type const_type;
-};
-
-
-template<class Base, class T, int idx, int dim>
-vectview<typename Base::scalar, T::DIM>
-subvector_impl_(vectview<typename Base::scalar, Base::DIM> vec, SubManifold<T, idx, dim> Base::*)
-{
-	return vec.template segment<T::DIM>(dim);
-}
-
-template<class Base, class T, int idx, int dim>
-vectview<typename Base::scalar, T::DOF>
-subvector_impl(vectview<typename Base::scalar, Base::DOF> vec, SubManifold<T, idx, dim> Base::*)
-{
-	return vec.template segment<T::DOF>(idx);
-}
-
-/**
- * Get the subvector corresponding to a sub-manifold from a bigger vector.
- */
- template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
-vectview<Scalar, T::DIM>
-subvector_(vectview<Scalar, BaseDIM> vec, SubManifold<T, idx, dim> Base::* ptr)
-{
-	return subvector_impl_(vec, ptr);
-}
-
-template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
-vectview<Scalar, T::DOF>
-subvector(vectview<Scalar, BaseDOF> vec, SubManifold<T, idx, dim> Base::* ptr)
-{
-	return subvector_impl(vec, ptr);
-}
-
-/**
- * @todo This should be covered already by subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx> Base::*)
- */
-template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
-vectview<Scalar, T::DOF>
-subvector(Eigen::Matrix<Scalar, BaseDOF, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
-{
-	return subvector_impl(vectview<Scalar, BaseDOF>(vec), ptr);
-}
- 
-template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
-vectview<Scalar, T::DIM>
-subvector_(Eigen::Matrix<Scalar, BaseDIM, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
-{
-	return subvector_impl_(vectview<Scalar, BaseDIM>(vec), ptr);
-}
-
-template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
-vectview<const Scalar, T::DIM>
-subvector_(const Eigen::Matrix<Scalar, BaseDIM, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
-{
-	return subvector_impl_(vectview<const Scalar, BaseDIM>(vec), ptr);
-}
-
-template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
-vectview<const Scalar, T::DOF>
-subvector(const Eigen::Matrix<Scalar, BaseDOF, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
-{
-	return subvector_impl(vectview<const Scalar, BaseDOF>(vec), ptr);
-}
-
-
-/**
- * const version of subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx> Base::*)
- */
-template<class Base, class T, int idx, int dim>
-vectview<const typename Base::scalar, T::DOF>
-subvector_impl(const vectview<const typename Base::scalar, Base::DOF> cvec, SubManifold<T, idx, dim> Base::*)
-{
-	return cvec.template segment<T::DOF>(idx);
-}
-
-template<class Base, class T, int idx, int dim>
-vectview<const typename Base::scalar, T::DIM>
-subvector_impl_(const vectview<const typename Base::scalar, Base::DIM> cvec, SubManifold<T, idx, dim> Base::*)
-{
-	return cvec.template segment<T::DIM>(dim);
-}
-
-template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
-vectview<const Scalar, T::DOF>
-subvector(const vectview<const Scalar, BaseDOF> cvec, SubManifold<T, idx, dim> Base::* ptr)
-{
-	return subvector_impl(cvec, ptr);
-}
-
-
-} // namespace MTK
-
-#endif // GET_START_INDEX_H_

FAST_LIO/include/IKFoM_toolkit/mtk/types/S2.hpp

@@ -1,316 +0,0 @@
-// This is a NEW implementation of the algorithm described in the
-// following paper:
-//    C. Hertzberg,  R.  Wagner,  U.  Frese,  and  L.  Schroder.  Integratinggeneric   sensor   fusion   algorithms   with   sound   state   representationsthrough  encapsulation  of  manifolds.
-//    CoRR,  vol.  abs/1107.1119,  2011.[Online]. Available: http://arxiv.org/abs/1107.1119
-
-/*
- *  Copyright (c) 2019--2023, The University of Hong Kong
- *  All rights reserved.
- *
- *  Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-
-/*
- *  Copyright (c) 2008--2011, Universitaet Bremen
- *  All rights reserved.
- *
- *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-/**
- * @file mtk/types/S2.hpp
- * @brief Unit vectors on the sphere, or directions in 3D.
- */
-#ifndef S2_H_
-#define S2_H_
-
-
-#include "vect.hpp"
-
-#include "SOn.hpp"
-#include "../src/mtkmath.hpp"
-
-
-
-
-namespace MTK {
-
-/**
- * Manifold representation of @f$ S^2 @f$. 
- * Used for unit vectors on the sphere or directions in 3D.
- * 
- * @todo add conversions from/to polar angles?
- */
-template<class _scalar = double, int den = 1, int num = 1, int S2_typ = 3>
-struct S2 {
-	
-	typedef _scalar scalar;
-	typedef vect<3, scalar> vect_type; 
-	typedef SO3<scalar> SO3_type;
-	typedef typename vect_type::base vec3; 
-	scalar length = scalar(den)/scalar(num);
-	enum {DOF=2, TYP = 1, DIM = 3};
-	
-//private:
-	/**
-	 * Unit vector on the sphere, or vector pointing in a direction
-	 */
-	vect_type vec; 
-	
-public:
-	S2() {
-		if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1));
-		if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0);
-		if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0);
-	} 
-	S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) { 
-		vec.normalize();
-		vec = vec * length;
-	}
-	
-	S2(const vect_type &_vec) : vec(_vec) {
-		vec.normalize();
-		vec = vec * length;
-	}
-
-	void oplus(MTK::vectview<const scalar, 3> delta, scalar scale = 1)
-	{
-		SO3_type res;
-		res.w() = MTK::exp<scalar, 3>(res.vec(), delta, scalar(scale/2));
-		vec = res.toRotationMatrix() * vec;
-	}
-	
-	void boxplus(MTK::vectview<const scalar, 2> delta, scalar scale=1) {
-		Eigen::Matrix<scalar, 3, 2> Bx;
-		S2_Bx(Bx);
-		vect_type Bu = Bx*delta;SO3_type res;
-		res.w() = MTK::exp<scalar, 3>(res.vec(), Bu, scalar(scale/2));
-		vec = res.toRotationMatrix() * vec;
-	} 
-	
-	void boxminus(MTK::vectview<scalar, 2> res, const S2<scalar, den, num, S2_typ>& other) const {
-		scalar v_sin = (MTK::hat(vec)*other.vec).norm();
-		scalar v_cos = vec.transpose() * other.vec;
-		scalar theta = std::atan2(v_sin, v_cos);
-		if(v_sin < MTK::tolerance<scalar>())
-		{
-			if(std::fabs(theta) > MTK::tolerance<scalar>() )
-			{
-				res[0] = 3.1415926;
-				res[1] = 0;
-			}
-			else{
-				res[0] = 0;
-				res[1] = 0;
-			}
-		}
-		else
-		{
-			S2<scalar, den, num, S2_typ> other_copy = other;
-			Eigen::Matrix<scalar, 3, 2>Bx;
-			other_copy.S2_Bx(Bx);
-			res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec;
-		}
-	}
-	
-	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
-	{
-		Eigen::Matrix<scalar, 3, 3> skew_vec;
-		skew_vec << scalar(0), -vec[2], vec[1],
-								vec[2], scalar(0), -vec[0],
-								-vec[1], vec[0], scalar(0);
-		res = skew_vec;
-	}
-
-
-	void S2_Bx(Eigen::Matrix<scalar, 3, 2> &res)
-	{
-		if(S2_typ == 3)
-		{
-		if(vec[2] + length > tolerance<scalar>())
-		{
-			
-			res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]),
-					 -vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]),
-					 -vec[0], -vec[1];
-			res /= length;
-		}
-		else
-		{
-			res = Eigen::Matrix<scalar, 3, 2>::Zero();
-			res(1, 1) = -1;
-			res(2, 0) = 1;
-		}
-		}
-		else if(S2_typ == 2)
-		{
-		if(vec[1] + length > tolerance<scalar>())
-		{
-			
-			res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]),
-					 -vec[0], -vec[2],
-					 -vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]);
-			res /= length;
-		}
-		else
-		{
-			res = Eigen::Matrix<scalar, 3, 2>::Zero();
-			res(1, 1) = -1;
-			res(2, 0) = 1;
-		}
-		}
-		else
-		{
-		if(vec[0] + length > tolerance<scalar>())
-		{
-			
-			res << -vec[1], -vec[2],
-					 length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]),
-					 -vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]);
-			res /= length;
-		}
-		else
-		{
-			res = Eigen::Matrix<scalar, 3, 2>::Zero();
-			res(1, 1) = -1;
-			res(2, 0) = 1;
-		}
-		}
-	}
-
-	void S2_Nx(Eigen::Matrix<scalar, 2, 3> &res, S2<scalar, den, num, S2_typ>& subtrahend)
-	{
-		if((vec+subtrahend.vec).norm() > tolerance<scalar>())
-		{
-			Eigen::Matrix<scalar, 3, 2> Bx;
-			S2_Bx(Bx);
-			if((vec-subtrahend.vec).norm() > tolerance<scalar>())
-			{
-				scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm();
-				scalar v_cos = vec.transpose() * subtrahend.vec;
-				
-				res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length));
-			}
-			else
-			{
-				res = 1/length/length*Bx.transpose()*MTK::hat(vec);
-			}
-		}
-		else
-		{
-			std::cerr << "No N(x, y) for x=-y" << std::endl;
-			std::exit(100);
-		}
-	}
-
-	void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
-	{
-		Eigen::Matrix<scalar, 3, 2> Bx;
-		S2_Bx(Bx);
-		res = 1/length/length*Bx.transpose()*MTK::hat(vec);
-	}
-
-	void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
-	{
-		Eigen::Matrix<scalar, 3, 2> Bx;
-		S2_Bx(Bx);
-		if(delta.norm() < tolerance<scalar>())
-		{
-			res = -MTK::hat(vec)*Bx;
-		}
-		else{
-			vect_type Bu = Bx*delta;
-			SO3_type exp_delta;
-			exp_delta.w() = MTK::exp<scalar, 3>(exp_delta.vec(), Bu, scalar(1/2));
-			res = -exp_delta.toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx;
-		}
-	}
-
-	operator const vect_type&() const{
-		return vec;
-	}
-	
-	const vect_type& get_vect() const {
-		return vec;
-	}
-	
-	friend S2<scalar, den, num, S2_typ> operator*(const SO3<scalar>& rot, const S2<scalar, den, num, S2_typ>& dir)
-	{
-		S2<scalar, den, num, S2_typ> ret;
-		ret.vec = rot * dir.vec;
-		return ret;
-	}
-	
-	scalar operator[](int idx) const {return vec[idx]; }
-	
-	friend std::ostream& operator<<(std::ostream &os, const S2<scalar, den, num, S2_typ>& vec){
-		return os << vec.vec.transpose() << " ";
-	}
-	friend std::istream& operator>>(std::istream &is, S2<scalar, den, num, S2_typ>& vec){
-		for(int i=0; i<3; ++i)
-			is >> vec.vec[i];
-		vec.vec.normalize();
-		vec.vec = vec.vec * vec.length;
-		return is;
-	
-	}
-};
-
-
-}  // namespace MTK
-
-
-#endif /*S2_H_*/

FAST_LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp

@@ -1,317 +0,0 @@
-// This is an advanced implementation of the algorithm described in the
-// following paper:
-//    C. Hertzberg,  R.  Wagner,  U.  Frese,  and  L.  Schroder.  Integratinggeneric   sensor   fusion   algorithms   with   sound   state   representationsthrough  encapsulation  of  manifolds.
-//    CoRR,  vol.  abs/1107.1119,  2011.[Online]. Available: http://arxiv.org/abs/1107.1119
-
-/*
- *  Copyright (c) 2019--2023, The University of Hong Kong
- *  All rights reserved.
- *
- *  Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
- 
-/*
- *  Copyright (c) 2008--2011, Universitaet Bremen
- *  All rights reserved.
- *
- *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-/**
- * @file mtk/types/SOn.hpp
- * @brief Standard Orthogonal Groups i.e.\ rotatation groups.
- */
-#ifndef SON_H_
-#define SON_H_
-
-#include <Eigen/Geometry>
-
-#include "vect.hpp"
-#include "../src/mtkmath.hpp"
-
-
-namespace MTK {
-
-
-/**
- * Two-dimensional orientations represented as scalar.
- * There is no guarantee that the representing scalar is within any interval,
- * but the result of boxminus will always have magnitude @f$\le\pi @f$.
- */
-template<class _scalar = double, int Options = Eigen::AutoAlign>
-struct SO2 : public Eigen::Rotation2D<_scalar> {
-	enum {DOF = 1, DIM = 2, TYP = 3};
-	
-	typedef _scalar scalar;
-	typedef Eigen::Rotation2D<scalar> base;
-	typedef vect<DIM, scalar, Options> vect_type;
-	
-	//! Construct from angle
-	SO2(const scalar& angle = 0) : base(angle) {	}
-	
-	//! Construct from Eigen::Rotation2D
-	SO2(const base& src) : base(src) {}
-	
-	/**
-	 * Construct from 2D vector.
-	 * Resulting orientation will rotate the first unit vector to point to vec.
-	 */
-	SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {};
-	
-	
-	//! Calculate @c this->inverse() * @c r
-	SO2 operator%(const base &r) const {
-		return base::inverse() * r;
-	}
-
-	//! Calculate @c this->inverse() * @c r
-	template<class Derived>
-	vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
-		return base::inverse() * vec;
-	}
-	
-	//! Calculate @c *this * @c r.inverse()
-	SO2 operator/(const SO2 &r) const {
-		return *this * r.inverse();
-	}
-	
-	//! Gets the angle as scalar.
-	operator scalar() const {
-		return base::angle(); 
-	}
-	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
-	{
-		res = Eigen::Matrix<scalar, 3, 3>::Zero();
-	}
-	//! @name Manifold requirements
-	void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 2, 3>::Zero();
-	}
-
-	void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 3, 2>::Zero();
-	}
-
-	void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
-		base::angle() += scale * vec[0];
-	}
-	
-	void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
-		base::angle() += scale * vec[0];
-	}
-	void boxminus(MTK::vectview<scalar, DOF> res, const SO2<scalar>& other) const {
-		res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi));
-	}
-	
-	friend std::istream& operator>>(std::istream &is, SO2<scalar>& ang){
-		return is >> ang.angle();
-	}
-
-};
-
-
-/**
- * Three-dimensional orientations represented as Quaternion.
- * It is assumed that the internal Quaternion always stays normalized,
- * should this not be the case, call inherited member function @c normalize().
- */
-template<class _scalar = double, int Options = Eigen::AutoAlign>
-struct SO3 : public Eigen::Quaternion<_scalar, Options> {
-	enum {DOF = 3, DIM = 3, TYP = 2}; 
-	typedef _scalar scalar;
-	typedef Eigen::Quaternion<scalar, Options> base;
-	typedef Eigen::Quaternion<scalar> Quaternion;
-	typedef vect<DIM, scalar, Options> vect_type;
-	
-	//! Calculate @c this->inverse() * @c r
-	template<class OtherDerived> EIGEN_STRONG_INLINE 
-	Quaternion operator%(const Eigen::QuaternionBase<OtherDerived> &r) const {
-		return base::conjugate() * r;
-	}
-	
-	//! Calculate @c this->inverse() * @c r
-	template<class Derived>
-	vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
-		return base::conjugate() * vec;
-	}
-	
-	//! Calculate @c this * @c r.conjugate()
-	template<class OtherDerived> EIGEN_STRONG_INLINE 
-	Quaternion operator/(const Eigen::QuaternionBase<OtherDerived> &r) const {
-		return *this * r.conjugate();
-	}
-	
-	/**
-	 * Construct from real part and three imaginary parts.
-	 * Quaternion is normalized after construction.
-	 */
-	SO3(const scalar& w, const scalar& x, const scalar& y, const scalar& z) : base(w, x, y, z) {
-		base::normalize();
-	}
-	
-	/**
-	 * Construct from Eigen::Quaternion.
-	 * @note Non-normalized input may result result in spurious behavior.
-	 */
-	SO3(const base& src = base::Identity()) : base(src) {}
-	
-	/**
-	 * Construct from rotation matrix.
-	 * @note Invalid rotation matrices may lead to spurious behavior.
-	 */
-	template<class Derived>
-	SO3(const Eigen::MatrixBase<Derived>& matrix) : base(matrix) {}
-	
-	/**
-	 * Construct from arbitrary rotation type.
-	 * @note Invalid rotation matrices may lead to spurious behavior.
-	 */
-	template<class Derived>
-	SO3(const Eigen::RotationBase<Derived, 3>& rotation) : base(rotation.derived()) {}
-	
-	//! @name Manifold requirements
-	
-	void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
-		SO3 delta = exp(vec, scale);
-		*this = *this * delta;
-	}
-	void boxminus(MTK::vectview<scalar, DOF> res, const SO3<scalar>& other) const {
-		res = SO3::log(other.conjugate() * *this);
-	}
-	//}
-
-	void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
-		SO3 delta = exp(vec, scale);
-		*this = *this * delta;
-	}
-
-	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
-	{
-		res = Eigen::Matrix<scalar, 3, 3>::Zero();
-	}
-	void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 2, 3>::Zero();
-	}
-
-	void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 3, 2>::Zero();
-	}
-
-	friend std::ostream& operator<<(std::ostream &os, const SO3<scalar, Options>& q){
-		return os << q.coeffs().transpose() << " ";
-	}
-
-	friend std::istream& operator>>(std::istream &is, SO3<scalar, Options>& q){
-		vect<4,scalar> coeffs;
-		is >> coeffs;
-		q.coeffs() = coeffs.normalized();
-		return is;
-	}
-	
-	//! @name Helper functions
-	//{
-	/**
-	 * Calculate the exponential map. In matrix terms this would correspond 
-	 * to the Rodrigues formula.
-	 */
-	// FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround
-//	static SO3 exp(MTK::vectview<const scalar, 3> dvec, scalar scale = 1){
-	static SO3 exp(const Eigen::Matrix<scalar, 3, 1>& dvec, scalar scale = 1){
-		SO3 res;
-		res.w() = MTK::exp<scalar, 3>(res.vec(), dvec, scalar(scale/2));
-		return res;
-	}
-	/**
-	 * Calculate the inverse of @c exp.
-	 * Only guarantees that <code>exp(log(x)) == x </code>
-	 */
-	static typename base::Vector3 log(const SO3 &orient){
-		typename base::Vector3 res;
-		MTK::log<scalar, 3>(res, orient.w(), orient.vec(), scalar(2), true);
-		return res;
-	}
-};
-
-namespace internal {
-template<class Scalar, int Options>
-struct UnalignedType<SO2<Scalar, Options > >{
-	typedef SO2<Scalar, Options | Eigen::DontAlign> type;
-};
-
-template<class Scalar, int Options>
-struct UnalignedType<SO3<Scalar, Options > >{
-	typedef SO3<Scalar, Options | Eigen::DontAlign> type;
-};
-
-}  // namespace internal
-
-
-}  // namespace MTK
-
-#endif /*SON_H_*/
-

FAST_LIO/include/IKFoM_toolkit/mtk/types/vect.hpp

@@ -1,461 +0,0 @@
-// This is an advanced implementation of the algorithm described in the
-// following paper:
-//    C. Hertzberg,  R.  Wagner,  U.  Frese,  and  L.  Schroder.  Integratinggeneric   sensor   fusion   algorithms   with   sound   state   representationsthrough  encapsulation  of  manifolds.
-//    CoRR,  vol.  abs/1107.1119,  2011.[Online]. Available: http://arxiv.org/abs/1107.1119
-
-/*
- *  Copyright (c) 2019--2023, The University of Hong Kong
- *  All rights reserved.
- *
- *  Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-
-/*
- *  Copyright (c) 2008--2011, Universitaet Bremen
- *  All rights reserved.
- *
- *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-/**
- * @file mtk/types/vect.hpp
- * @brief Basic vectors interpreted as manifolds.
- * 
- * This file also implements a simple wrapper for matrices, for arbitrary scalars
- * and for positive scalars.
- */
-#ifndef VECT_H_
-#define VECT_H_
-
-#include <iosfwd>
-#include <iostream>
-#include <vector>
-
-#include "../src/vectview.hpp"
-
-namespace MTK {
-
-static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]"); 
-
-
-/**
- * A simple vector class.
- * Implementation is basically a wrapper around Eigen::Matrix with manifold 
- * requirements added.
- */
-template<int D = 3, class _scalar = double, int _Options=Eigen::AutoAlign>
-struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> {
-	typedef Eigen::Matrix<_scalar, D, 1, _Options> base;
-	enum {DOF = D, DIM = D, TYP = 0};
-	typedef _scalar scalar;
-	
-	//using base::operator=;
-	
-	/** Standard constructor. Sets all values to zero. */
-	vect(const base &src = base::Zero()) : base(src) {}
-
-	/** Constructor copying the value of the expression \a other */
-	template<typename OtherDerived>
-	EIGEN_STRONG_INLINE vect(const Eigen::DenseBase<OtherDerived>& other) : base(other) {}
-	
-	/** Construct from memory. */
-	vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { }
-
-	void boxplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
-		*this += scale * vec;
-	}
-	void boxminus(MTK::vectview<scalar, D> res, const vect<D, scalar>& other) const {
-		res = *this - other;
-	}
-
-	void oplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
-		*this += scale * vec;
-	}
-
-	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
-	{
-		res = Eigen::Matrix<scalar, 3, 3>::Zero();
-	}
-
-	void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 2, 3>::Zero();
-	}
-
-	void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 3, 2>::Zero();
-	}
-
-	friend std::ostream& operator<<(std::ostream &os, const vect<D, scalar, _Options>& v){
-		// Eigen sometimes messes with the streams flags, so output manually:
-		for(int i=0; i<DOF; ++i)
-			os << v(i) << " ";
-		return os;
-	}
-	friend std::istream& operator>>(std::istream &is, vect<D, scalar, _Options>& v){
-		char term=0;
-		is >> std::ws; // skip whitespace
-		switch(is.peek()) {
-		case '(': term=')'; is.ignore(1); break;
-		case '[': term=']'; is.ignore(1); break;
-		case '{': term='}'; is.ignore(1); break;
-		default: break;
-		}
-		if(D==Eigen::Dynamic) { 
-			assert(term !=0 && "Dynamic vectors must be embraced");
-			std::vector<scalar> temp;
-			while(is.good() && is.peek() != term) {
-				scalar x;
-				is >> x;
-				temp.push_back(x);
-				if(is.peek()==',') is.ignore(1);
-			}
-			v = vect::Map(temp.data(), temp.size());
-		} else
-			for(int i=0; i<v.size(); ++i){
-				is >> v[i];
-				if(is.peek()==',') { // ignore commas between values
-					is.ignore(1);
-				}
-			}
-		if(term!=0) {
-			char x;
-			is >> x;
-			if(x!=term) {
-				is.setstate(is.badbit);
-//				assert(x==term && "start and end bracket do not match!");
-			}
-		}
-		return is;
-	}
-	
-	template<int dim>
-	vectview<scalar, dim> tail(){
-		BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
-		return base::template tail<dim>();
-	}
-	template<int dim>
-	vectview<const scalar, dim> tail() const{
-		BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
-		return base::template tail<dim>();
-	}
-	template<int dim>
-	vectview<scalar, dim> head(){
-		BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
-		return base::template head<dim>();
-	}
-	template<int dim>
-	vectview<const scalar, dim> head() const{
-		BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
-		return base::template head<dim>();
-	}
-};
-
-
-/**
- * A simple matrix class.
- * Implementation is basically a wrapper around Eigen::Matrix with manifold 
- * requirements added, i.e., matrix is viewed as a plain vector for that.
- */
-template<int M, int N, class _scalar = double, int _Options = Eigen::Matrix<_scalar, M, N>::Options>
-struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> {
-	typedef Eigen::Matrix<_scalar, M, N, _Options> base; 
-	enum {DOF = M * N, TYP = 4, DIM=0};
-	typedef _scalar scalar;
-	
-	using base::operator=; 
-	
-	/** Standard constructor. Sets all values to zero. */
-	matrix() {
-		base::setZero();
-	}
-	
-	/** Constructor copying the value of the expression \a other */
-	template<typename OtherDerived>
-	EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase<OtherDerived>& other) : base(other) {}
-	
-	/** Construct from memory. */
-	matrix(const scalar* src) : base(src) { } 
-	
-	void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
-		*this += scale * base::Map(vec.data());
-	}
-	void boxminus(MTK::vectview<scalar, DOF> res, const matrix& other) const {
-		base::Map(res.data()) = *this - other;
-	}
-
-	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
-	{
-		res = Eigen::Matrix<scalar, 3, 3>::Zero();
-	}
-
-	void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
-		*this += scale * base::Map(vec.data());
-	}
-
-	void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 2, 3>::Zero();
-	}
-
-	void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 3, 2>::Zero();
-	}
-
-	friend std::ostream& operator<<(std::ostream &os, const matrix<M, N, scalar, _Options>& mat){
-		for(int i=0; i<DOF; ++i){
-			os << mat.data()[i] << " ";
-		}
-		return os;
-	}
-	friend std::istream& operator>>(std::istream &is, matrix<M, N, scalar, _Options>& mat){
-		for(int i=0; i<DOF; ++i){
-			is >> mat.data()[i];
-		}
-		return is;
-	}
-};// @todo What if M / N = Eigen::Dynamic?
-
-
-
-/**
- * A simple scalar type.
- */
-template<class _scalar = double>
-struct Scalar {
-	enum {DOF = 1, TYP = 5, DIM=0};
-	typedef _scalar scalar;
-	
-	scalar value;
-	
-	Scalar(const scalar& value = scalar(0)) : value(value) {}
-	operator const scalar&() const { return value; }
-	operator scalar&() { return value; }
-	Scalar& operator=(const scalar& val) { value = val; return *this; }
-
-	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
-	{
-		res = Eigen::Matrix<scalar, 3, 3>::Zero();
-	}
-
-	void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 2, 3>::Zero();
-	}
-
-	void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 3, 2>::Zero();
-	}
-
-	void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
-		value += scale * vec[0];
-	}
-
-	void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
-		value += scale * vec[0];
-	}
-	void boxminus(MTK::vectview<scalar, DOF> res, const Scalar& other) const {
-		res[0] = *this - other;
-	}
-};
-
-/**
- * Positive scalars.
- * Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$.
- */
-template<class _scalar = double>
-struct PositiveScalar {
-	enum {DOF = 1, TYP = 6, DIM=0};
-	typedef _scalar scalar;
-	
-	scalar value;
-	
-	PositiveScalar(const scalar& value = scalar(1)) : value(value) {
-		assert(value > scalar(0));
-	}
-	operator const scalar&() const { return value; }
-	PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; }
-
-	void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
-		value *= std::exp(scale * vec[0]);
-	}
-	void boxminus(MTK::vectview<scalar, DOF> res, const PositiveScalar& other) const {
-		res[0] = std::log(*this / other);
-	}
-
-	void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
-		value *= std::exp(scale * vec[0]);
-	}
-
-	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
-	{
-		res = Eigen::Matrix<scalar, 3, 3>::Zero();
-	}
-
-	void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 2, 3>::Zero();
-	}
-
-	void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 3, 2>::Zero();
-	}
-
-
-	friend std::istream& operator>>(std::istream &is, PositiveScalar<scalar>& s){
-		is >> s.value;
-		assert(s.value > 0);
-		return is;
-	}
-};
-
-template<class _scalar = double>
-struct Complex : public std::complex<_scalar>{
-	enum {DOF = 2, TYP = 7, DIM=0};
-	typedef _scalar scalar;
-	
-	typedef std::complex<scalar> Base;
-	
-	Complex(const Base& value) : Base(value) {}
-	Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {}
-	Complex(const MTK::vectview<const scalar, 2> &in) : Base(in[0], in[1]) {}
-	template<class Derived>
-	Complex(const Eigen::DenseBase<Derived> &in) : Base(in[0], in[1]) {}
-	
-	void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
-		Base::real() += scale * vec[0];
-		Base::imag() += scale * vec[1];
-	};
-	void boxminus(MTK::vectview<scalar, DOF> res, const Complex& other) const {
-		Complex diff = *this - other;
-		res << diff.real(), diff.imag();
-	}
-
-	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
-	{
-		res = Eigen::Matrix<scalar, 3, 3>::Zero();
-	}
-
-	void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
-		Base::real() += scale * vec[0];
-		Base::imag() += scale * vec[1];
-	};
-
-	void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 2, 3>::Zero();
-	}
-
-	void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
-	{
-		std::cerr << "wrong idx for S2" << std::endl;
-		std::exit(100);	
-    	res = Eigen::Matrix<scalar, 3, 2>::Zero();
-	}
-	
-	scalar squaredNorm() const {
-		return std::pow(Base::real(),2) + std::pow(Base::imag(),2);
-	}
-	
-	const scalar& operator()(int i) const {
-		assert(0<=i && i<2 && "Index out of range");
-		return i==0 ? Base::real() : Base::imag();
-	}
-	scalar& operator()(int i){
-		assert(0<=i && i<2 && "Index out of range");
-		return i==0 ? Base::real() : Base::imag();
-	}
-};
-
-
-namespace internal {
-
-template<int dim, class Scalar, int Options>
-struct UnalignedType<vect<dim, Scalar, Options > >{
-	typedef vect<dim, Scalar, Options | Eigen::DontAlign> type;
-};
-
-}  // namespace internal
-
-
-}  // namespace MTK
-
-
-
-
-#endif /*VECT_H_*/

FAST_LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp

@@ -1,113 +0,0 @@
-/*
- *  Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH
- *  All rights reserved.
- *
- *  Author: Rene Wagner <rene.wagner@dfki.de>
- *          Christoph Hertzberg <chtz@informatik.uni-bremen.de>
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Universitaet Bremen nor the DFKI GmbH 
- *     nor the names of its contributors may be used to endorse or 
- *     promote products derived from this software without specific 
- *     prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef WRAPPED_CV_MAT_HPP_
-#define WRAPPED_CV_MAT_HPP_
-
-#include <Eigen/Core>
-#include <opencv/cv.h>
-
-namespace MTK {
-
-template<class f_type>
-struct cv_f_type;
-
-template<>
-struct cv_f_type<double>
-{
-	enum {value = CV_64F};
-};
-
-template<>
-struct cv_f_type<float>
-{
-	enum {value = CV_32F};
-};
-
-/**
- * cv_mat wraps a CvMat around an Eigen Matrix
- */
-template<int rows, int cols, class f_type = double>
-class cv_mat : public matrix<rows, cols, f_type, cols==1 ? Eigen::ColMajor : Eigen::RowMajor>
-{
-	typedef matrix<rows, cols, f_type, cols==1 ? Eigen::ColMajor : Eigen::RowMajor> base_type;
-	enum {type_ = cv_f_type<f_type>::value};
-	CvMat cv_mat_;
-
-public:
-	cv_mat()
-	{
-		cv_mat_ = cvMat(rows, cols, type_, base_type::data());
-	}
-
-	cv_mat(const cv_mat& oth) : base_type(oth)
-	{
-		cv_mat_ = cvMat(rows, cols, type_, base_type::data());
-	}
-
-	template<class Derived>
-	cv_mat(const Eigen::MatrixBase<Derived> &value) : base_type(value)
-	{
-		cv_mat_ = cvMat(rows, cols, type_, base_type::data());
-	}
-
-	template<class Derived>
-	cv_mat& operator=(const Eigen::MatrixBase<Derived> &value)
-	{
-		base_type::operator=(value);
-		return *this;
-	}
-
-	cv_mat& operator=(const cv_mat& value)
-	{
-		base_type::operator=(value);
-		return *this;
-	}
-	
-	// FIXME: Maybe overloading operator& is not a good idea ...
-	CvMat* operator&()
-	{
-		return &cv_mat_;
-	}
-	const CvMat* operator&() const
-	{
-		return &cv_mat_;
-	}
-};
-
-} // namespace MTK
-
-#endif /* WRAPPED_CV_MAT_HPP_ */

FAST_LIO/include/common_lib.h

@@ -1,270 +0,0 @@
-#ifndef COMMON_LIB_H
-#define COMMON_LIB_H
-
-#include <so3_math.h>
-#include <Eigen/Eigen>
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
-#include <fast_lio/msg/pose6_d.hpp>
-#include <sensor_msgs/msg/imu.hpp>
-#include <nav_msgs/msg/odometry.hpp>
-
-using namespace std;
-using namespace Eigen;
-
-#define USE_IKFOM
-
-#define PI_M (3.14159265358)
-#define G_m_s2 (9.81)         // Gravaty const in GuangDong/China
-#define DIM_STATE (18)        // Dimension of states (Let Dim(SO(3)) = 3)
-#define DIM_PROC_N (12)       // Dimension of process noise (Let Dim(SO(3)) = 3)
-#define CUBE_LEN  (6.0)
-#define LIDAR_SP_LEN    (2)
-#define INIT_COV   (1)
-#define NUM_MATCH_POINTS    (5)
-#define MAX_MEAS_DIM        (10000)
-
-#define VEC_FROM_ARRAY(v)        v[0],v[1],v[2]
-#define MAT_FROM_ARRAY(v)        v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8]
-#define CONSTRAIN(v,min,max)     ((v>min)?((v<max)?v:max):min)
-#define ARRAY_FROM_EIGEN(mat)    mat.data(), mat.data() + mat.rows() * mat.cols()
-#define STD_VEC_FROM_EIGEN(mat)  vector<decltype(mat)::Scalar> (mat.data(), mat.data() + mat.rows() * mat.cols())
-#define DEBUG_FILE_DIR(name)     (string(string(ROOT_DIR) + "Log/"+ name))
-
-typedef fast_lio::msg::Pose6D Pose6D;
-typedef pcl::PointXYZINormal PointType;
-typedef pcl::PointCloud<PointType> PointCloudXYZI;
-typedef vector<PointType, Eigen::aligned_allocator<PointType>>  PointVector;
-typedef Vector3d V3D;
-typedef Matrix3d M3D;
-typedef Vector3f V3F;
-typedef Matrix3f M3F;
-
-#define MD(a,b)  Matrix<double, (a), (b)>
-#define VD(a)    Matrix<double, (a), 1>
-#define MF(a,b)  Matrix<float, (a), (b)>
-#define VF(a)    Matrix<float, (a), 1>
-
-M3D Eye3d(M3D::Identity());
-M3F Eye3f(M3F::Identity());
-V3D Zero3d(0, 0, 0);
-V3F Zero3f(0, 0, 0);
-
-struct MeasureGroup     // Lidar data and imu dates for the curent process
-{
-    MeasureGroup()
-    {
-        lidar_beg_time = 0.0;
-        this->lidar.reset(new PointCloudXYZI());
-    };
-    double lidar_beg_time;
-    double lidar_end_time;
-    PointCloudXYZI::Ptr lidar;
-    deque<sensor_msgs::msg::Imu::ConstSharedPtr> imu;
-};
-
-struct StatesGroup
-{
-    StatesGroup() {
-		this->rot_end = M3D::Identity();
-		this->pos_end = Zero3d;
-        this->vel_end = Zero3d;
-        this->bias_g  = Zero3d;
-        this->bias_a  = Zero3d;
-        this->gravity = Zero3d;
-        this->cov     = MD(DIM_STATE,DIM_STATE)::Identity() * INIT_COV;
-        this->cov.block<9,9>(9,9) = MD(9,9)::Identity() * 0.00001;
-	};
-
-    StatesGroup(const StatesGroup& b) {
-		this->rot_end = b.rot_end;
-		this->pos_end = b.pos_end;
-        this->vel_end = b.vel_end;
-        this->bias_g  = b.bias_g;
-        this->bias_a  = b.bias_a;
-        this->gravity = b.gravity;
-        this->cov     = b.cov;
-	};
-
-    StatesGroup& operator=(const StatesGroup& b)
-	{
-        this->rot_end = b.rot_end;
-		this->pos_end = b.pos_end;
-        this->vel_end = b.vel_end;
-        this->bias_g  = b.bias_g;
-        this->bias_a  = b.bias_a;
-        this->gravity = b.gravity;
-        this->cov     = b.cov;
-        return *this;
-	};
-
-    StatesGroup operator+(const Matrix<double, DIM_STATE, 1> &state_add)
-	{
-        StatesGroup a;
-		a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
-		a.pos_end = this->pos_end + state_add.block<3,1>(3,0);
-        a.vel_end = this->vel_end + state_add.block<3,1>(6,0);
-        a.bias_g  = this->bias_g  + state_add.block<3,1>(9,0);
-        a.bias_a  = this->bias_a  + state_add.block<3,1>(12,0);
-        a.gravity = this->gravity + state_add.block<3,1>(15,0);
-        a.cov     = this->cov;
-		return a;
-	};
-
-    StatesGroup& operator+=(const Matrix<double, DIM_STATE, 1> &state_add)
-	{
-        this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
-		this->pos_end += state_add.block<3,1>(3,0);
-        this->vel_end += state_add.block<3,1>(6,0);
-        this->bias_g  += state_add.block<3,1>(9,0);
-        this->bias_a  += state_add.block<3,1>(12,0);
-        this->gravity += state_add.block<3,1>(15,0);
-		return *this;
-	};
-
-    Matrix<double, DIM_STATE, 1> operator-(const StatesGroup& b)
-	{
-        Matrix<double, DIM_STATE, 1> a;
-        M3D rotd(b.rot_end.transpose() * this->rot_end);
-        a.block<3,1>(0,0)  = Log(rotd);
-        a.block<3,1>(3,0)  = this->pos_end - b.pos_end;
-        a.block<3,1>(6,0)  = this->vel_end - b.vel_end;
-        a.block<3,1>(9,0)  = this->bias_g  - b.bias_g;
-        a.block<3,1>(12,0) = this->bias_a  - b.bias_a;
-        a.block<3,1>(15,0) = this->gravity - b.gravity;
-		return a;
-	};
-
-    void resetpose()
-    {
-        this->rot_end = M3D::Identity();
-		this->pos_end = Zero3d;
-        this->vel_end = Zero3d;
-    }
-
-	M3D rot_end;      // the estimated attitude (rotation matrix) at the end lidar point
-    V3D pos_end;      // the estimated position at the end lidar point (world frame)
-    V3D vel_end;      // the estimated velocity at the end lidar point (world frame)
-    V3D bias_g;       // gyroscope bias
-    V3D bias_a;       // accelerator bias
-    V3D gravity;      // the estimated gravity acceleration
-    Matrix<double, DIM_STATE, DIM_STATE>  cov;     // states covariance
-};
-
-template<typename T>
-T rad2deg(T radians)
-{
-  return radians * 180.0 / PI_M;
-}
-
-template<typename T>
-T deg2rad(T degrees)
-{
-  return degrees * PI_M / 180.0;
-}
-
-template<typename T>
-auto set_pose6d(const double t, const Matrix<T, 3, 1> &a, const Matrix<T, 3, 1> &g, \
-                const Matrix<T, 3, 1> &v, const Matrix<T, 3, 1> &p, const Matrix<T, 3, 3> &R)
-{
-    Pose6D rot_kp;
-    rot_kp.offset_time = t;
-    for (int i = 0; i < 3; i++)
-    {
-        rot_kp.acc[i] = a(i);
-        rot_kp.gyr[i] = g(i);
-        rot_kp.vel[i] = v(i);
-        rot_kp.pos[i] = p(i);
-        for (int j = 0; j < 3; j++)  rot_kp.rot[i*3+j] = R(i,j);
-    }
-    return move(rot_kp);
-}
-
-/* comment
-plane equation: Ax + By + Cz + D = 0
-convert to: A/D*x + B/D*y + C/D*z = -1
-solve: A0*x0 = b0
-where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T
-normvec:  normalized x0
-*/
-template<typename T>
-bool esti_normvector(Matrix<T, 3, 1> &normvec, const PointVector &point, const T &threshold, const int &point_num)
-{
-    MatrixXf A(point_num, 3);
-    MatrixXf b(point_num, 1);
-    b.setOnes();
-    b *= -1.0f;
-
-    for (int j = 0; j < point_num; j++)
-    {
-        A(j,0) = point[j].x;
-        A(j,1) = point[j].y;
-        A(j,2) = point[j].z;
-    }
-    normvec = A.colPivHouseholderQr().solve(b);
-    
-    for (int j = 0; j < point_num; j++)
-    {
-        if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold)
-        {
-            return false;
-        }
-    }
-
-    normvec.normalize();
-    return true;
-}
-
-float calc_dist(PointType p1, PointType p2){
-    float d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
-    return d;
-}
-
-template<typename T>
-bool esti_plane(Matrix<T, 4, 1> &pca_result, const PointVector &point, const T &threshold)
-{
-    Matrix<T, NUM_MATCH_POINTS, 3> A;
-    Matrix<T, NUM_MATCH_POINTS, 1> b;
-    A.setZero();
-    b.setOnes();
-    b *= -1.0f;
-
-    for (int j = 0; j < NUM_MATCH_POINTS; j++)
-    {
-        A(j,0) = point[j].x;
-        A(j,1) = point[j].y;
-        A(j,2) = point[j].z;
-    }
-
-    Matrix<T, 3, 1> normvec = A.colPivHouseholderQr().solve(b);
-
-    T n = normvec.norm();
-    pca_result(0) = normvec(0) / n;
-    pca_result(1) = normvec(1) / n;
-    pca_result(2) = normvec(2) / n;
-    pca_result(3) = 1.0 / n;
-
-    for (int j = 0; j < NUM_MATCH_POINTS; j++)
-    {
-        if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold)
-        {
-            return false;
-        }
-    }
-    return true;
-}
-
-double get_time_sec(const builtin_interfaces::msg::Time &time)
-{
-    return rclcpp::Time(time).seconds();
-}
-
-rclcpp::Time get_ros_time(double timestamp)
-{
-    int32_t sec = std::floor(timestamp);
-    auto nanosec_d = (timestamp - std::floor(timestamp)) * 1e9;
-    uint32_t nanosec = nanosec_d;
-    return rclcpp::Time(sec, nanosec);
-}
-
-#endif

FAST_LIO/include/ikd-Tree/README.md

@@ -1,2 +0,0 @@
-# ikd-Tree
-ikd-Tree is an incremental k-d tree for robotic applications.

FAST_LIO/include/ikd-Tree/ikd_Tree.h

@@ -1,344 +0,0 @@
-#pragma once
-#include <stdio.h>
-#include <queue>
-#include <pthread.h>
-#include <chrono>
-#include <time.h>
-#include <unistd.h>
-#include <math.h>
-#include <algorithm>
-#include <memory.h>
-#include <pcl/point_types.h>
-
-#define EPSS 1e-6
-#define Minimal_Unbalanced_Tree_Size 10
-#define Multi_Thread_Rebuild_Point_Num 1500
-#define DOWNSAMPLE_SWITCH true
-#define ForceRebuildPercentage 0.2
-#define Q_LEN 1000000
-
-using namespace std;
-
-// typedef pcl::PointXYZINormal PointType;
-// typedef vector<PointType, Eigen::aligned_allocator<PointType>>  PointVector;
-
-struct BoxPointType
-{
-    float vertex_min[3];
-    float vertex_max[3];
-};
-
-enum operation_set
-{
-    ADD_POINT,
-    DELETE_POINT,
-    DELETE_BOX,
-    ADD_BOX,
-    DOWNSAMPLE_DELETE,
-    PUSH_DOWN
-};
-
-enum delete_point_storage_set
-{
-    NOT_RECORD,
-    DELETE_POINTS_REC,
-    MULTI_THREAD_REC
-};
-
-template <typename PointType>
-class KD_TREE
-{
-    // using MANUAL_Q_ = MANUAL_Q<typename PointType>;
-    // using PointVector = std::vector<PointType>;
-    
-    // using MANUAL_Q_ = MANUAL_Q<typename PointType>;
-public:
-    using PointVector = std::vector<PointType, Eigen::aligned_allocator<PointType>>;
-    using Ptr = std::shared_ptr<KD_TREE<PointType>>;
-    
-    struct KD_TREE_NODE
-    {
-        PointType point;
-        int division_axis;
-        int TreeSize = 1;
-        int invalid_point_num = 0;
-        int down_del_num = 0;
-        bool point_deleted = false;
-        bool tree_deleted = false;
-        bool point_downsample_deleted = false;
-        bool tree_downsample_deleted = false;
-        bool need_push_down_to_left = false;
-        bool need_push_down_to_right = false;
-        bool working_flag = false;
-        pthread_mutex_t push_down_mutex_lock;
-        float node_range_x[2], node_range_y[2], node_range_z[2];
-        float radius_sq;
-        KD_TREE_NODE *left_son_ptr = nullptr;
-        KD_TREE_NODE *right_son_ptr = nullptr;
-        KD_TREE_NODE *father_ptr = nullptr;
-        // For paper data record
-        float alpha_del;
-        float alpha_bal;
-    };
-
-    struct Operation_Logger_Type
-    {
-        PointType point;
-        BoxPointType boxpoint;
-        bool tree_deleted, tree_downsample_deleted;
-        operation_set op;
-    };
-    // static const PointType zeroP;
-
-    struct PointType_CMP
-    {
-        PointType point;
-        float dist = 0.0;
-        PointType_CMP(PointType p = PointType(), float d = INFINITY)
-        {
-            this->point = p;
-            this->dist = d;
-        };
-        bool operator<(const PointType_CMP &a) const
-        {
-            if (fabs(dist - a.dist) < 1e-10)
-                return point.x < a.point.x;
-            else
-                return dist < a.dist;
-        }
-    };
-
-    class MANUAL_HEAP
-    {
-
-    public:
-        MANUAL_HEAP(int max_capacity = 100)
-
-        {
-            cap = max_capacity;
-            heap = new PointType_CMP[max_capacity];
-            heap_size = 0;
-        }
-
-        ~MANUAL_HEAP()
-        {
-            delete[] heap;
-        }
-        void pop()
-        {
-            if (heap_size == 0)
-                return;
-            heap[0] = heap[heap_size - 1];
-            heap_size--;
-            MoveDown(0);
-            return;
-        }
-        PointType_CMP top()
-        {
-            return heap[0];
-        }
-        void push(PointType_CMP point)
-        {
-            if (heap_size >= cap)
-                return;
-            heap[heap_size] = point;
-            FloatUp(heap_size);
-            heap_size++;
-            return;
-        }
-        int size()
-        {
-            return heap_size;
-        }
-        void clear()
-        {
-            heap_size = 0;
-            return;
-        }
-
-    private:
-        PointType_CMP *heap;
-        void MoveDown(int heap_index)
-        {
-            int l = heap_index * 2 + 1;
-            PointType_CMP tmp = heap[heap_index];
-            while (l < heap_size)
-            {
-                if (l + 1 < heap_size && heap[l] < heap[l + 1])
-                    l++;
-                if (tmp < heap[l])
-                {
-                    heap[heap_index] = heap[l];
-                    heap_index = l;
-                    l = heap_index * 2 + 1;
-                }
-                else
-                    break;
-            }
-            heap[heap_index] = tmp;
-            return;
-        }
-        void FloatUp(int heap_index)
-        {
-            int ancestor = (heap_index - 1) / 2;
-            PointType_CMP tmp = heap[heap_index];
-            while (heap_index > 0)
-            {
-                if (heap[ancestor] < tmp)
-                {
-                    heap[heap_index] = heap[ancestor];
-                    heap_index = ancestor;
-                    ancestor = (heap_index - 1) / 2;
-                }
-                else
-                    break;
-            }
-            heap[heap_index] = tmp;
-            return;
-        }
-        int heap_size = 0;
-        int cap = 0;
-    };
-
-    class MANUAL_Q
-    {
-    private:
-        int head = 0, tail = 0, counter = 0;
-        Operation_Logger_Type q[Q_LEN];
-        bool is_empty;
-
-    public:
-        void pop()
-        {
-            if (counter == 0)
-                return;
-            head++;
-            head %= Q_LEN;
-            counter--;
-            if (counter == 0)
-                is_empty = true;
-            return;
-        }
-        Operation_Logger_Type front()
-        {
-            return q[head];
-        }
-        Operation_Logger_Type back()
-        {
-            return q[tail];
-        }
-        void clear()
-        {
-            head = 0;
-            tail = 0;
-            counter = 0;
-            is_empty = true;
-            return;
-        }
-        void push(Operation_Logger_Type op)
-        {
-            q[tail] = op;
-            counter++;
-            if (is_empty)
-                is_empty = false;
-            tail++;
-            tail %= Q_LEN;
-        }
-        bool empty()
-        {
-            return is_empty;
-        }
-        int size()
-        {
-            return counter;
-        }
-    };
-
-private:
-    // Multi-thread Tree Rebuild
-    bool termination_flag = false;
-    bool rebuild_flag = false;
-    pthread_t rebuild_thread;
-    pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex;
-    pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock;
-    // queue<Operation_Logger_Type> Rebuild_Logger;
-    MANUAL_Q Rebuild_Logger;
-    PointVector Rebuild_PCL_Storage;
-    KD_TREE_NODE **Rebuild_Ptr = nullptr;
-    int search_mutex_counter = 0;
-    static void *multi_thread_ptr(void *arg);
-    void multi_thread_rebuild();
-    void start_thread();
-    void stop_thread();
-    void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation);
-    // KD Tree Functions and augmented variables
-    int Treesize_tmp = 0, Validnum_tmp = 0;
-    float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0;
-    float delete_criterion_param = 0.5f;
-    float balance_criterion_param = 0.7f;
-    float downsample_size = 0.2f;
-    bool Delete_Storage_Disabled = false;
-    KD_TREE_NODE *STATIC_ROOT_NODE = nullptr;
-    PointVector Points_deleted;
-    PointVector Downsample_Storage;
-    PointVector Multithread_Points_deleted;
-    void InitTreeNode(KD_TREE_NODE *root);
-    void Test_Lock_States(KD_TREE_NODE *root);
-    void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage);
-    void Rebuild(KD_TREE_NODE **root);
-    int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample);
-    void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild);
-    void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis);
-    void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild);
-    void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist); //priority_queue<PointType_CMP>
-    void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage);
-    void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage);
-    bool Criterion_Check(KD_TREE_NODE *root);
-    void Push_Down(KD_TREE_NODE *root);
-    void Update(KD_TREE_NODE *root);
-    void delete_tree_nodes(KD_TREE_NODE **root);
-    void downsample(KD_TREE_NODE **root);
-    bool same_point(PointType a, PointType b);
-    float calc_dist(PointType a, PointType b);
-    float calc_box_dist(KD_TREE_NODE *node, PointType point);
-    static bool point_cmp_x(PointType a, PointType b);
-    static bool point_cmp_y(PointType a, PointType b);
-    static bool point_cmp_z(PointType a, PointType b);
-
-public:
-    KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2);
-    ~KD_TREE();
-    void Set_delete_criterion_param(float delete_param)
-    {
-        delete_criterion_param = delete_param;
-    }
-    void Set_balance_criterion_param(float balance_param)
-    {
-        balance_criterion_param = balance_param;
-    }
-    void set_downsample_param(float downsample_param)
-    {
-        downsample_size = downsample_param;
-    }
-    void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2);
-    int size();
-    int validnum();
-    void root_alpha(float &alpha_bal, float &alpha_del);
-    void Build(PointVector point_cloud);
-    void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector<float> &Point_Distance, float max_dist = INFINITY);
-    void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage);
-    void Radius_Search(PointType point, const float radius, PointVector &Storage);
-    int Add_Points(PointVector &PointToAdd, bool downsample_on);
-    void Add_Point_Boxes(vector<BoxPointType> &BoxPoints);
-    void Delete_Points(PointVector &PointToDel);
-    int Delete_Point_Boxes(vector<BoxPointType> &BoxPoints);
-    void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type);
-    void acquire_removed_points(PointVector &removed_points);
-    BoxPointType tree_range();
-    PointVector PCL_Storage;
-    KD_TREE_NODE *Root_Node = nullptr;
-    int max_queue_size = 0;
-};
-
-// template <typename PointType>
-// PointType KD_TREE<PointType>::zeroP = PointType(0,0,0);

FAST_LIO/include/so3_math.h

@@ -1,111 +0,0 @@
-#ifndef SO3_MATH_H
-#define SO3_MATH_H
-
-#include <math.h>
-#include <Eigen/Core>
-
-#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0
-
-template<typename T>
-Eigen::Matrix<T, 3, 3> skew_sym_mat(const Eigen::Matrix<T, 3, 1> &v)
-{
-    Eigen::Matrix<T, 3, 3> skew_sym_mat;
-    skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0;
-    return skew_sym_mat;
-}
-
-template<typename T>
-Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &&ang)
-{
-    T ang_norm = ang.norm();
-    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
-    if (ang_norm > 0.0000001)
-    {
-        Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;
-        Eigen::Matrix<T, 3, 3> K;
-        K << SKEW_SYM_MATRX(r_axis);
-        /// Roderigous Tranformation
-        return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;
-    }
-    else
-    {
-        return Eye3;
-    }
-}
-
-template<typename T, typename Ts>
-Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
-{
-    T ang_vel_norm = ang_vel.norm();
-    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
-
-    if (ang_vel_norm > 0.0000001)
-    {
-        Eigen::Matrix<T, 3, 1> r_axis = ang_vel / ang_vel_norm;
-        Eigen::Matrix<T, 3, 3> K;
-
-        K << SKEW_SYM_MATRX(r_axis);
-
-        T r_ang = ang_vel_norm * dt;
-
-        /// Roderigous Tranformation
-        return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;
-    }
-    else
-    {
-        return Eye3;
-    }
-}
-
-template<typename T>
-Eigen::Matrix<T, 3, 3> Exp(const T &v1, const T &v2, const T &v3)
-{
-    T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
-    Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
-    if (norm > 0.00001)
-    {
-        T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
-        Eigen::Matrix<T, 3, 3> K;
-        K << SKEW_SYM_MATRX(r_ang);
-
-        /// Roderigous Tranformation
-        return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;
-    }
-    else
-    {
-        return Eye3;
-    }
-}
-
-/* Logrithm of a Rotation Matrix */
-template<typename T>
-Eigen::Matrix<T,3,1> Log(const Eigen::Matrix<T, 3, 3> &R)
-{
-    T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1));
-    Eigen::Matrix<T,3,1> K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));
-    return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
-}
-
-template<typename T>
-Eigen::Matrix<T, 3, 1> RotMtoEuler(const Eigen::Matrix<T, 3, 3> &rot)
-{
-    T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));
-    bool singular = sy < 1e-6;
-    T x, y, z;
-    if(!singular)
-    {
-        x = atan2(rot(2, 1), rot(2, 2));
-        y = atan2(-rot(2, 0), sy);   
-        z = atan2(rot(1, 0), rot(0, 0));  
-    }
-    else
-    {    
-        x = atan2(-rot(1, 2), rot(1, 1));    
-        y = atan2(-rot(2, 0), sy);    
-        z = 0;
-    }
-    Eigen::Matrix<T, 3, 1> ang(x, y, z);
-    return ang;
-}
-
-#endif

FAST_LIO/include/use-ikfom.hpp

@@ -1,126 +0,0 @@
-#ifndef USE_IKFOM_H
-#define USE_IKFOM_H
-
-#include <IKFoM_toolkit/esekfom/esekfom.hpp>
-
-typedef MTK::vect<3, double> vect3;
-typedef MTK::SO3<double> SO3;
-typedef MTK::S2<double, 98090, 10000, 1> S2; 
-typedef MTK::vect<1, double> vect1;
-typedef MTK::vect<2, double> vect2;
-
-MTK_BUILD_MANIFOLD(state_ikfom,
-((vect3, pos))
-((SO3, rot))
-((SO3, offset_R_L_I))
-((vect3, offset_T_L_I))
-((vect3, vel))
-((vect3, bg))
-((vect3, ba))
-((S2, grav))
-);
-
-MTK_BUILD_MANIFOLD(input_ikfom,
-((vect3, acc))
-((vect3, gyro))
-);
-
-MTK_BUILD_MANIFOLD(process_noise_ikfom,
-((vect3, ng))
-((vect3, na))
-((vect3, nbg))
-((vect3, nba))
-);
-
-MTK::get_cov<process_noise_ikfom>::type process_noise_cov()
-{
-	MTK::get_cov<process_noise_ikfom>::type cov = MTK::get_cov<process_noise_ikfom>::type::Zero();
-	MTK::setDiagonal<process_noise_ikfom, vect3, 0>(cov, &process_noise_ikfom::ng, 0.0001);// 0.03
-	MTK::setDiagonal<process_noise_ikfom, vect3, 3>(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05
-	MTK::setDiagonal<process_noise_ikfom, vect3, 6>(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01
-	MTK::setDiagonal<process_noise_ikfom, vect3, 9>(cov, &process_noise_ikfom::nba, 0.00001);   //0.001 0.05 0.0001/out 0.01
-	return cov;
-}
-
-//double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia 
-//vect3 Lidar_offset_to_IMU(L_offset_to_I, 3);
-Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in)
-{
-	Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
-	vect3 omega;
-	in.gyro.boxminus(omega, s.bg);
-	vect3 a_inertial = s.rot * (in.acc-s.ba); 
-	for(int i = 0; i < 3; i++ ){
-		res(i) = s.vel[i];
-		res(i + 3) =  omega[i]; 
-		res(i + 12) = a_inertial[i] + s.grav[i]; 
-	}
-	return res;
-}
-
-Eigen::Matrix<double, 24, 23> df_dx(state_ikfom &s, const input_ikfom &in)
-{
-	Eigen::Matrix<double, 24, 23> cov = Eigen::Matrix<double, 24, 23>::Zero();
-	cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
-	vect3 acc_;
-	in.acc.boxminus(acc_, s.ba);
-	vect3 omega;
-	in.gyro.boxminus(omega, s.bg);
-	cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix()*MTK::hat(acc_);
-	cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix();
-	Eigen::Matrix<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero();
-	Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix;
-	s.S2_Mx(grav_matrix, vec, 21);
-	cov.template block<3, 2>(12, 21) =  grav_matrix; 
-	cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); 
-	return cov;
-}
-
-
-Eigen::Matrix<double, 24, 12> df_dw(state_ikfom &s, const input_ikfom &in)
-{
-	Eigen::Matrix<double, 24, 12> cov = Eigen::Matrix<double, 24, 12>::Zero();
-	cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
-	cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity();
-	cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity();
-	cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity();
-	return cov;
-}
-
-vect3 SO3ToEuler(const SO3 &orient) 
-{
-	Eigen::Matrix<double, 3, 1> _ang;
-	Eigen::Vector4d q_data = orient.coeffs().transpose();
-	//scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2];
-	double sqw = q_data[3]*q_data[3];
-	double sqx = q_data[0]*q_data[0];
-	double sqy = q_data[1]*q_data[1];
-	double sqz = q_data[2]*q_data[2];
-	double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor
-	double test = q_data[3]*q_data[1] - q_data[2]*q_data[0];
-
-	if (test > 0.49999*unit) { // singularity at north pole
-	
-		_ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0;
-		double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
-		vect3 euler_ang(temp, 3);
-		return euler_ang;
-	}
-	if (test < -0.49999*unit) { // singularity at south pole
-		_ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0;
-		double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
-		vect3 euler_ang(temp, 3);
-		return euler_ang;
-	}
-		
-	_ang <<
-			std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw),
-			std::asin (2*test/unit),
-			std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw);
-	double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
-	vect3 euler_ang(temp, 3);
-		// euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw
-	return euler_ang;
-}
-
-#endif

FAST_LIO/launch/gdb_debug_example.launch

@@ -1,22 +0,0 @@
-<launch>
-
-  <arg name="rviz" default="true" />
-
-  <node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" required="true" launch-prefix="gdb -ex run --args">
-	<param name="imu_topic" type="string" value="/livox/imu" />
-	<param name="map_file_path" type="string" value=" " />
-	<param name="max_iteration" type="int" value="4" />
-	<param name="dense_map_enable" type="bool" value="1" />
-	<param name="fov_degree" type="double" value="75" />
-	<param name="filter_size_corner" type="double" value="0.2" />
-	<param name="filter_size_surf" type="double" value="0.2" />
-	<param name="filter_size_map" type="double" value="0.5" />
-	<param name="runtime_pos_log_enable" type="bool" value="1" />
-	<param name="cube_side_length" type="double" value="2000" />
-  </node>
-
-  <!-- <group if="$(arg rviz)">
-    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
-  </group> -->
-
-</launch>

FAST_LIO/launch/mapping.launch.py

@@ -1,70 +0,0 @@
-import os.path
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch.conditions import IfCondition
-
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-    package_path = get_package_share_directory('fast_lio')
-    default_config_path = os.path.join(package_path, 'config')
-    default_rviz_config_path = os.path.join(
-        package_path, 'rviz', 'fastlio.rviz')
-
-    use_sim_time = LaunchConfiguration('use_sim_time')
-    config_path = LaunchConfiguration('config_path')
-    config_file = LaunchConfiguration('config_file')
-    rviz_use = LaunchConfiguration('rviz')
-    rviz_cfg = LaunchConfiguration('rviz_cfg')
-
-    declare_use_sim_time_cmd = DeclareLaunchArgument(
-        'use_sim_time', default_value='false',
-        description='Use simulation (Gazebo) clock if true'
-    )
-    declare_config_path_cmd = DeclareLaunchArgument(
-        'config_path', default_value=default_config_path,
-        description='Yaml config file path'
-    )
-    decalre_config_file_cmd = DeclareLaunchArgument(
-        'config_file', default_value='mid360.yaml',
-        description='Config file'
-    )
-    declare_rviz_cmd = DeclareLaunchArgument(
-        'rviz', default_value='true',
-        description='Use RViz to monitor results'
-    )
-    declare_rviz_config_path_cmd = DeclareLaunchArgument(
-        'rviz_cfg', default_value=default_rviz_config_path,
-        description='RViz config file path'
-    )
-
-    fast_lio_node = Node(
-        package='fast_lio',
-        executable='fastlio_mapping',
-        parameters=[PathJoinSubstitution([config_path, config_file]),
-                    {'use_sim_time': use_sim_time}],
-        output='screen'
-    )
-    rviz_node = Node(
-        package='rviz2',
-        executable='rviz2',
-        arguments=['-d', rviz_cfg],
-        condition=IfCondition(rviz_use)
-    )
-
-    ld = LaunchDescription()
-    ld.add_action(declare_use_sim_time_cmd)
-    ld.add_action(declare_config_path_cmd)
-    ld.add_action(decalre_config_file_cmd)
-    ld.add_action(declare_rviz_cmd)
-    ld.add_action(declare_rviz_config_path_cmd)
-
-    ld.add_action(fast_lio_node)
-    ld.add_action(rviz_node)
-
-    return ld

FAST_LIO/launch/unilidar_l2.launch.py

@@ -1,70 +0,0 @@
-import os.path
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch.conditions import IfCondition
-
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-    package_path = get_package_share_directory('fast_lio')
-    default_config_path = os.path.join(package_path, 'config')
-    default_rviz_config_path = os.path.join(
-        package_path, 'rviz', 'fastlio.rviz')
-
-    use_sim_time = LaunchConfiguration('use_sim_time')
-    config_path = LaunchConfiguration('config_path')
-    config_file = LaunchConfiguration('config_file')
-    rviz_use = LaunchConfiguration('rviz')
-    rviz_cfg = LaunchConfiguration('rviz_cfg')
-
-    declare_use_sim_time_cmd = DeclareLaunchArgument(
-        'use_sim_time', default_value='false',
-        description='Use simulation (Gazebo) clock if true'
-    )
-    declare_config_path_cmd = DeclareLaunchArgument(
-        'config_path', default_value=default_config_path,
-        description='Yaml config file path'
-    )
-    decalre_config_file_cmd = DeclareLaunchArgument(
-        'config_file', default_value='unilidar_l2.yaml',
-        description='Config file'
-    )
-    declare_rviz_cmd = DeclareLaunchArgument(
-        'rviz', default_value='true',
-        description='Use RViz to monitor results'
-    )
-    declare_rviz_config_path_cmd = DeclareLaunchArgument(
-        'rviz_cfg', default_value=default_rviz_config_path,
-        description='RViz config file path'
-    )
-
-    fast_lio_node = Node(
-        package='fast_lio',
-        executable='fastlio_mapping',
-        parameters=[PathJoinSubstitution([config_path, config_file]),
-                    {'use_sim_time': use_sim_time}],
-        output='screen'
-    )
-    rviz_node = Node(
-        package='rviz2',
-        executable='rviz2',
-        arguments=['-d', rviz_cfg],
-        condition=IfCondition(rviz_use)
-    )
-
-    ld = LaunchDescription()
-    ld.add_action(declare_use_sim_time_cmd)
-    ld.add_action(declare_config_path_cmd)
-    ld.add_action(decalre_config_file_cmd)
-    ld.add_action(declare_rviz_cmd)
-    ld.add_action(declare_rviz_config_path_cmd)
-
-    ld.add_action(fast_lio_node)
-    ld.add_action(rviz_node)
-
-    return ld

FAST_LIO/msg/Pose6D.msg

@@ -1,7 +0,0 @@
-# the preintegrated Lidar states at the time of IMU measurements in a frame
-float64  offset_time # the offset time of IMU measurement w.r.t the first lidar point
-float64[3] acc       # the preintegrated total acceleration (global frame) at the Lidar origin
-float64[3] gyr       # the unbiased angular velocity (body frame) at the Lidar origin
-float64[3] vel       # the preintegrated velocity (global frame) at the Lidar origin
-float64[3] pos       # the preintegrated position (global frame) at the Lidar origin
-float64[9] rot       # the preintegrated rotation (global frame) at the Lidar origin

FAST_LIO/package.xml

@@ -1,38 +0,0 @@
-<?xml version="1.0"?>
-<package format="3">
-  <name>fast_lio</name>
-  <version>0.0.0</version>
-
-  <description>
-    This is a modified version of LOAM which is original algorithm
-    is described in the following paper:
-    J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
-    Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
-  </description>
-
-  <maintainer email="dev@livoxtech.com">claydergc</maintainer>
-
-  <license>BSD</license>
-
-  <author email="zhangji@cmu.edu">Ji Zhang</author>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <buildtool_depend>rosidl_default_generators</buildtool_depend>
-  <depend>geometry_msgs</depend>
-  <depend>nav_msgs</depend>
-  <depend>rclcpp</depend>
-  <depend>std_msgs</depend>
-  <depend>sensor_msgs</depend>
-  <depend>common_interfaces</depend>
-  <depend>tf2</depend>
-  <depend>pcl_ros</depend>
-  <depend>pcl_conversions</depend>
-  <depend>livox_ros_driver2</depend>
-
-  <exec_depend>rosidl_default_runtime</exec_depend>
-  <member_of_group>rosidl_interface_packages</member_of_group>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

FAST_LIO/rviz/fastlio.rviz

@@ -1,304 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-      Splitter Ratio: 0.5
-    Tree Height: 549
-  - 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: CloudRegistered
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Class: rviz_default_plugins/TF
-      Enabled: true
-      Frame Timeout: 15
-      Frames:
-        All Enabled: true
-        body:
-          Value: true
-        camera_init:
-          Value: true
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: false
-      Tree:
-        camera_init:
-          body:
-            {}
-      Update Interval: 0
-      Value: true
-    - Angle Tolerance: 0.10000000149011612
-      Class: rviz_default_plugins/Odometry
-      Covariance:
-        Orientation:
-          Alpha: 0.5
-          Color: 255; 255; 127
-          Color Style: Unique
-          Frame: Local
-          Offset: 1
-          Scale: 1
-          Value: true
-        Position:
-          Alpha: 0.30000001192092896
-          Color: 204; 51; 204
-          Scale: 1
-          Value: true
-        Value: true
-      Enabled: true
-      Keep: 100
-      Name: Odometry
-      Position Tolerance: 0.10000000149011612
-      Shape:
-        Alpha: 1
-        Axes Length: 1
-        Axes Radius: 0.10000000149011612
-        Color: 255; 25; 0
-        Head Length: 0.30000001192092896
-        Head Radius: 0.10000000149011612
-        Shaft Length: 1
-        Shaft Radius: 0.05000000074505806
-        Value: Arrow
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /Odometry
-      Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz_default_plugins/Path
-      Color: 25; 255; 0
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Lines
-      Line Width: 0.029999999329447746
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /path
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 11.062739372253418
-        Min Value: -13.864188194274902
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: AxisColor
-      Decay Time: 30
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 186
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: CloudRegistered
-      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: Reliable
-        Value: /cloud_registered
-      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: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 184
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: CloudEffected
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.10000000149011612
-      Style: Flat Squares
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /cloud_effected
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: -9999
-        Min Value: 9999
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: AxisColor
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 255
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: CloudMap
-      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: Reliable
-        Value: /Laser_map
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 0; 0; 0
-    Fixed Frame: camera_init
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/Interact
-      Hide Inactive Objects: true
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Covariance x: 0.25
-      Covariance y: 0.25
-      Covariance yaw: 0.06853891909122467
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /initialpose
-    - Class: rviz_default_plugins/SetGoal
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /goal_pose
-    - Class: rviz_default_plugins/PublishPoint
-      Single click: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /clicked_point
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 216.99887084960938
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: -0.008504047989845276
-        Y: -0.0005770106799900532
-        Z: 0.034441977739334106
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 1.5697963237762451
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz_default_plugins)
-      Yaw: 4.88355827331543
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 846
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ad0000003efc0100000002fb0000000800540069006d00650100000000000005ad000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000451000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1453
-  X: 368
-  Y: 104

FAST_LIO/rviz_cfg/loam_livox.rviz

@@ -1,358 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 0
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /mapping1
-        - /mapping1/surround1
-        - /mapping1/currPoints1
-        - /mapping1/currPoints1/Autocompute Value Bounds1
-        - /Odometry1/Odometry1
-        - /Odometry1/Odometry1/Shape1
-        - /Odometry1/Odometry1/Covariance1
-        - /Odometry1/Odometry1/Covariance1/Position1
-        - /Odometry1/Odometry1/Covariance1/Orientation1
-        - /MarkerArray1/Namespaces1
-      Splitter Ratio: 0.6432291865348816
-    Tree Height: 811
-  - Class: rviz/Selection
-    Name: Selection
-  - Class: rviz/Tool Properties
-    Expanded:
-      - /2D Pose Estimate1
-      - /2D Nav Goal1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz/Time
-    Experimental: false
-    Name: Time
-    SyncMode: 0
-    SyncSource: surround
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 1
-      Cell Size: 1000
-      Class: rviz/Grid
-      Color: 160; 160; 164
-      Enabled: false
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 40
-      Reference Frame: <Fixed Frame>
-      Value: false
-    - Class: rviz/Axes
-      Enabled: false
-      Length: 0.699999988079071
-      Name: Axes
-      Radius: 0.05999999865889549
-      Reference Frame: <Fixed Frame>
-      Value: false
-    - Class: rviz/Group
-      Displays:
-        - Alpha: 1
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 10
-            Min Value: -10
-            Value: true
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz/PointCloud2
-          Color: 238; 238; 236
-          Color Transformer: Intensity
-          Decay Time: 0
-          Enabled: true
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Min Color: 238; 238; 236
-          Name: surround
-          Position Transformer: XYZ
-          Queue Size: 1
-          Selectable: false
-          Size (Pixels): 3
-          Size (m): 0.05000000074505806
-          Style: Points
-          Topic: /cloud_registered
-          Unreliable: false
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: true
-        - Alpha: 0.10000000149011612
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 15
-            Min Value: -5
-            Value: false
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz/PointCloud2
-          Color: 255; 255; 255
-          Color Transformer: Intensity
-          Decay Time: 1000
-          Enabled: true
-          Invert Rainbow: true
-          Max Color: 255; 255; 255
-          Min Color: 0; 0; 0
-          Name: currPoints
-          Position Transformer: XYZ
-          Queue Size: 100000
-          Selectable: true
-          Size (Pixels): 1
-          Size (m): 0.009999999776482582
-          Style: Points
-          Topic: /cloud_registered
-          Unreliable: false
-          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/PointCloud2
-          Color: 255; 0; 0
-          Color Transformer: FlatColor
-          Decay Time: 0
-          Enabled: false
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Min Color: 0; 0; 0
-          Name: PointCloud2
-          Position Transformer: XYZ
-          Queue Size: 10
-          Selectable: true
-          Size (Pixels): 3
-          Size (m): 0.10000000149011612
-          Style: Flat Squares
-          Topic: /Laser_map
-          Unreliable: false
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: false
-      Enabled: true
-      Name: mapping
-    - Class: rviz/Group
-      Displays:
-        - Angle Tolerance: 0.009999999776482582
-          Class: rviz/Odometry
-          Covariance:
-            Orientation:
-              Alpha: 0.5
-              Color: 255; 255; 127
-              Color Style: Unique
-              Frame: Local
-              Offset: 1
-              Scale: 1
-              Value: true
-            Position:
-              Alpha: 0.30000001192092896
-              Color: 204; 51; 204
-              Scale: 1
-              Value: true
-            Value: true
-          Enabled: true
-          Keep: 1
-          Name: Odometry
-          Position Tolerance: 0.0010000000474974513
-          Shape:
-            Alpha: 1
-            Axes Length: 1
-            Axes Radius: 0.20000000298023224
-            Color: 255; 85; 0
-            Head Length: 0
-            Head Radius: 0
-            Shaft Length: 0.05000000074505806
-            Shaft Radius: 0.05000000074505806
-            Value: Axes
-          Topic: /Odometry
-          Unreliable: false
-          Value: true
-      Enabled: true
-      Name: Odometry
-    - Class: rviz/Axes
-      Enabled: true
-      Length: 0.699999988079071
-      Name: Axes
-      Radius: 0.10000000149011612
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 0
-      Buffer Length: 2
-      Class: rviz/Path
-      Color: 25; 255; 255
-      Enabled: true
-      Head Diameter: 0
-      Head Length: 0
-      Length: 0.30000001192092896
-      Line Style: Billboards
-      Line Width: 0.20000000298023224
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 25; 255; 255
-      Pose Style: None
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.4000000059604645
-      Shaft Length: 0.4000000059604645
-      Topic: /path
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: false
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 239; 41; 41
-      Max Intensity: 0
-      Min Color: 239; 41; 41
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 4
-      Size (m): 0.30000001192092896
-      Style: Spheres
-      Topic: /cloud_effected
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 13.139549255371094
-        Min Value: -32.08251953125
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 138; 226; 52
-      Color Transformer: FlatColor
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 138; 226; 52
-      Min Color: 138; 226; 52
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.10000000149011612
-      Style: Flat Squares
-      Topic: /Laser_map
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Class: rviz/MarkerArray
-      Enabled: false
-      Marker Topic: /MarkerArray
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: false
-  Enabled: true
-  Global Options:
-    Background Color: 0; 0; 0
-    Default Light: true
-    Fixed Frame: camera_init
-    Frame Rate: 10
-  Name: root
-  Tools:
-    - Class: rviz/Interact
-      Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
-      Theta std deviation: 0.2617993950843811
-      Topic: /initialpose
-      X std deviation: 0.5
-      Y std deviation: 0.5
-    - Class: rviz/SetGoal
-      Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
-      Single click: true
-      Topic: /clicked_point
-  Value: true
-  Views:
-    Current:
-      Class: rviz/Orbit
-      Distance: 46.0853271484375
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: -4.982542037963867
-        Y: -15.83572006225586
-        Z: -3.063523054122925
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.399796724319458
-      Target Frame: global
-      Value: Orbit (rviz)
-      Yaw: 1.277182698249817
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 1028
-  Hide Left Dock: false
-  Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001c800000368fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002700000368000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000061f00000052fc0100000002fb0000000800540069006d006501000000000000061f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004510000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: true
-  Width: 1567
-  X: 67
-  Y: 24

FAST_LIO/src/IMU_Processing.hpp

@@ -1,379 +0,0 @@
-#include <cmath>
-#include <math.h>
-#include <deque>
-#include <mutex>
-#include <thread>
-#include <fstream>
-#include <csignal>
-#include <so3_math.h>
-#include <Eigen/Eigen>
-#include <common_lib.h>
-#include <pcl/common/io.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <condition_variable>
-#include <nav_msgs/msg/odometry.hpp>
-#include <pcl/common/transforms.h>
-#include <pcl/kdtree/kdtree_flann.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <sensor_msgs/msg/imu.hpp>
-#include <sensor_msgs/msg/point_cloud2.hpp>
-#include <geometry_msgs/msg/vector3.hpp>
-#include "use-ikfom.hpp"
-
-/// *************Preconfiguration
-
-#define MAX_INI_COUNT (10)
-
-const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
-
-/// *************IMU Process and undistortion
-class ImuProcess
-{
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  ImuProcess();
-  ~ImuProcess();
-  
-  void Reset();
-  // void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu);
-  void Reset(double start_timestamp, const sensor_msgs::msg::Imu::ConstSharedPtr &lastimu);
-  void set_extrinsic(const V3D &transl, const M3D &rot);
-  void set_extrinsic(const V3D &transl);
-  void set_extrinsic(const MD(4,4) &T);
-  void set_gyr_cov(const V3D &scaler);
-  void set_acc_cov(const V3D &scaler);
-  void set_gyr_bias_cov(const V3D &b_g);
-  void set_acc_bias_cov(const V3D &b_a);
-  Eigen::Matrix<double, 12, 12> Q;
-  void Process(const MeasureGroup &meas,  esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr pcl_un_);
-
-  ofstream fout_imu;
-  V3D cov_acc;
-  V3D cov_gyr;
-  V3D cov_acc_scale;
-  V3D cov_gyr_scale;
-  V3D cov_bias_gyr;
-  V3D cov_bias_acc;
-  double first_lidar_time;
-
- private:
-  void IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N);
-  void UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_in_out);
-
-  PointCloudXYZI::Ptr cur_pcl_un_;
-  // sensor_msgs::ImuConstPtr last_imu_;
-  sensor_msgs::msg::Imu::ConstSharedPtr last_imu_;
-  deque<sensor_msgs::msg::Imu::ConstSharedPtr> v_imu_;
-  vector<Pose6D> IMUpose;
-  vector<M3D>    v_rot_pcl_;
-  M3D Lidar_R_wrt_IMU;
-  V3D Lidar_T_wrt_IMU;
-  V3D mean_acc;
-  V3D mean_gyr;
-  V3D angvel_last;
-  V3D acc_s_last;
-  double start_timestamp_;
-  double last_lidar_end_time_;
-  int    init_iter_num = 1;
-  bool   b_first_frame_ = true;
-  bool   imu_need_init_ = true;
-};
-
-ImuProcess::ImuProcess()
-    : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1)
-{
-  init_iter_num = 1;
-  Q = process_noise_cov();
-  cov_acc       = V3D(0.1, 0.1, 0.1);
-  cov_gyr       = V3D(0.1, 0.1, 0.1);
-  cov_bias_gyr  = V3D(0.0001, 0.0001, 0.0001);
-  cov_bias_acc  = V3D(0.0001, 0.0001, 0.0001);
-  mean_acc      = V3D(0, 0, -1.0);
-  mean_gyr      = V3D(0, 0, 0);
-  angvel_last     = Zero3d;
-  Lidar_T_wrt_IMU = Zero3d;
-  Lidar_R_wrt_IMU = Eye3d;
-  last_imu_.reset(new sensor_msgs::msg::Imu());
-}
-
-ImuProcess::~ImuProcess() {}
-
-void ImuProcess::Reset() 
-{
-  // ROS_WARN("Reset ImuProcess");
-  mean_acc      = V3D(0, 0, -1.0);
-  mean_gyr      = V3D(0, 0, 0);
-  angvel_last       = Zero3d;
-  imu_need_init_    = true;
-  start_timestamp_  = -1;
-  init_iter_num     = 1;
-  v_imu_.clear();
-  IMUpose.clear();
-  last_imu_.reset(new sensor_msgs::msg::Imu());
-  cur_pcl_un_.reset(new PointCloudXYZI());
-}
-
-void ImuProcess::set_extrinsic(const MD(4,4) &T)
-{
-  Lidar_T_wrt_IMU = T.block<3,1>(0,3);
-  Lidar_R_wrt_IMU = T.block<3,3>(0,0);
-}
-
-void ImuProcess::set_extrinsic(const V3D &transl)
-{
-  Lidar_T_wrt_IMU = transl;
-  Lidar_R_wrt_IMU.setIdentity();
-}
-
-void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot)
-{
-  Lidar_T_wrt_IMU = transl;
-  Lidar_R_wrt_IMU = rot;
-}
-
-void ImuProcess::set_gyr_cov(const V3D &scaler)
-{
-  cov_gyr_scale = scaler;
-}
-
-void ImuProcess::set_acc_cov(const V3D &scaler)
-{
-  cov_acc_scale = scaler;
-}
-
-void ImuProcess::set_gyr_bias_cov(const V3D &b_g)
-{
-  cov_bias_gyr = b_g;
-}
-
-void ImuProcess::set_acc_bias_cov(const V3D &b_a)
-{
-  cov_bias_acc = b_a;
-}
-
-void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N)
-{
-  /** 1. initializing the gravity, gyro bias, acc and gyro covariance
-   ** 2. normalize the acceleration measurenments to unit gravity **/
-  
-  V3D cur_acc, cur_gyr;
-  
-  if (b_first_frame_)
-  {
-    Reset();
-    N = 1;
-    b_first_frame_ = false;
-    const auto &imu_acc = meas.imu.front()->linear_acceleration;
-    const auto &gyr_acc = meas.imu.front()->angular_velocity;
-    mean_acc << imu_acc.x, imu_acc.y, imu_acc.z;
-    mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
-    first_lidar_time = meas.lidar_beg_time;
-  }
-
-  for (const auto &imu : meas.imu)
-  {
-    const auto &imu_acc = imu->linear_acceleration;
-    const auto &gyr_acc = imu->angular_velocity;
-    cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
-    cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
-
-    mean_acc      += (cur_acc - mean_acc) / N;
-    mean_gyr      += (cur_gyr - mean_gyr) / N;
-
-    cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N);
-    cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N);
-
-    // cout<<"acc norm: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl;
-
-    N ++;
-  }
-  state_ikfom init_state = kf_state.get_x();
-  init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2);
-  
-  //state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
-  init_state.bg  = mean_gyr;
-  init_state.offset_T_L_I = Lidar_T_wrt_IMU;
-  init_state.offset_R_L_I = Lidar_R_wrt_IMU;
-  kf_state.change_x(init_state);
-
-  esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_P = kf_state.get_P();
-  init_P.setIdentity();
-  init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001;
-  init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001;
-  init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001;
-  init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001;
-  init_P(21,21) = init_P(22,22) = 0.00001; 
-  kf_state.change_P(init_P);
-  last_imu_ = meas.imu.back();
-
-}
-
-void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_out)
-{
-  /*** add the imu of the last frame-tail to the of current frame-head ***/
-  auto v_imu = meas.imu;
-  v_imu.push_front(last_imu_);
-  const double &imu_beg_time = rclcpp::Time(v_imu.front()->header.stamp).seconds();
-  const double &imu_end_time = rclcpp::Time(v_imu.back()->header.stamp).seconds();
-  const double &pcl_beg_time = meas.lidar_beg_time;
-  const double &pcl_end_time = meas.lidar_end_time;
-  
-  /*** sort point clouds by offset time ***/
-  pcl_out = *(meas.lidar);
-  sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
-  // cout<<"[ IMU Process ]: Process lidar from "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
-  //          <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<endl;
-
-  /*** Initialize IMU pose ***/
-  state_ikfom imu_state = kf_state.get_x();
-  IMUpose.clear();
-  IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
-
-  /*** forward propagation at each imu point ***/
-  V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu;
-  M3D R_imu;
-
-  double dt = 0;
-
-  input_ikfom in;
-  for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)
-  {
-    auto &&head = *(it_imu);
-    auto &&tail = *(it_imu + 1);
-
-    double tail_stamp = rclcpp::Time(tail->header.stamp).seconds();
-    double head_stamp = rclcpp::Time(head->header.stamp).seconds();
-
-    if (tail_stamp < last_lidar_end_time_)    continue;
-    
-    angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
-                0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
-                0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
-    acc_avr   <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
-                0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
-                0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);
-
-    // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
-
-    acc_avr     = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;
-
-    if(head_stamp < last_lidar_end_time_)
-    {
-      dt = tail_stamp - last_lidar_end_time_;
-      // dt = tail->header.stamp.toSec() - pcl_beg_time;
-    }
-    else
-    {
-      dt = tail_stamp - head_stamp;
-    }
-    
-    in.acc = acc_avr;
-    in.gyro = angvel_avr;
-    Q.block<3, 3>(0, 0).diagonal() = cov_gyr;
-    Q.block<3, 3>(3, 3).diagonal() = cov_acc;
-    Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
-    Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;
-    kf_state.predict(dt, Q, in);
-
-    /* save the poses at each IMU measurements */
-    imu_state = kf_state.get_x();
-    angvel_last = angvel_avr - imu_state.bg;
-    acc_s_last  = imu_state.rot * (acc_avr - imu_state.ba);
-    for(int i=0; i<3; i++)
-    {
-      acc_s_last[i] += imu_state.grav[i];
-    }
-    double &&offs_t = tail_stamp - pcl_beg_time;
-    IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
-  }
-
-  /*** calculated the pos and attitude prediction at the frame-end ***/
-  double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;
-  dt = note * (pcl_end_time - imu_end_time);
-  kf_state.predict(dt, Q, in);
-  
-  imu_state = kf_state.get_x();
-  last_imu_ = meas.imu.back();
-  last_lidar_end_time_ = pcl_end_time;
-
-  /*** undistort each lidar point (backward propagation) ***/
-  if (pcl_out.points.begin() == pcl_out.points.end()) return;
-  auto it_pcl = pcl_out.points.end() - 1;
-  for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
-  {
-    auto head = it_kp - 1;
-    auto tail = it_kp;
-    R_imu<<MAT_FROM_ARRAY(head->rot);
-    // cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
-    vel_imu<<VEC_FROM_ARRAY(head->vel);
-    pos_imu<<VEC_FROM_ARRAY(head->pos);
-    acc_imu<<VEC_FROM_ARRAY(tail->acc);
-    angvel_avr<<VEC_FROM_ARRAY(tail->gyr);
-
-    for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
-    {
-      dt = it_pcl->curvature / double(1000) - head->offset_time;
-
-      /* Transform to the 'end' frame, using only the rotation
-       * Note: Compensation direction is INVERSE of Frame's moving direction
-       * So if we want to compensate a point at timestamp-i to the frame-e
-       * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */
-      M3D R_i(R_imu * Exp(angvel_avr, dt));
-      
-      V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);
-      V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
-      V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!
-      
-      // save Undistorted points and their rotation
-      it_pcl->x = P_compensate(0);
-      it_pcl->y = P_compensate(1);
-      it_pcl->z = P_compensate(2);
-
-      if (it_pcl == pcl_out.points.begin()) break;
-    }
-  }
-}
-
-void ImuProcess::Process(const MeasureGroup &meas,  esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr cur_pcl_un_)
-{
-  double t1,t2,t3;
-  t1 = omp_get_wtime();
-
-  if(meas.imu.empty()) {return;};
-  assert(meas.lidar != nullptr);
-
-  if (imu_need_init_)
-  {
-    /// The very first lidar frame
-    IMU_init(meas, kf_state, init_iter_num);
-
-    imu_need_init_ = true;
-    
-    last_imu_   = meas.imu.back();
-
-    state_ikfom imu_state = kf_state.get_x();
-    if (init_iter_num > MAX_INI_COUNT)
-    {
-      cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
-      imu_need_init_ = false;
-
-      cov_acc = cov_acc_scale;
-      cov_gyr = cov_gyr_scale;
-      std::cout << "IMU Initial Done" << std::endl;
-      // ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
-      //          imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
-      fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out);
-    }
-
-    return;
-  }
-
-  UndistortPcl(meas, kf_state, *cur_pcl_un_);
-
-  t2 = omp_get_wtime();
-  t3 = omp_get_wtime();
-  
-  // cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl;
-}

FAST_LIO/src/preprocess.h

@@ -1,196 +0,0 @@
-// #include <ros/ros.h>
-#include <rclcpp/rclcpp.hpp>
-#include <pcl_conversions/pcl_conversions.h>
-#include <sensor_msgs/msg/point_cloud2.hpp>
-#include <livox_ros_driver2/msg/custom_msg.hpp>
-
-using namespace std;
-
-#define IS_VALID(a) ((abs(a) > 1e8) ? true : false)
-
-typedef pcl::PointXYZINormal PointType;
-typedef pcl::PointCloud<PointType> PointCloudXYZI;
-
-enum LID_TYPE
-{
-  AVIA = 1,
-  VELO16,
-  OUST64,
-  MID360
-};  //{1, 2, 3}
-enum TIME_UNIT
-{
-  SEC = 0,
-  MS = 1,
-  US = 2,
-  NS = 3
-};
-enum Feature
-{
-  Nor,
-  Poss_Plane,
-  Real_Plane,
-  Edge_Jump,
-  Edge_Plane,
-  Wire,
-  ZeroPoint
-};
-enum Surround
-{
-  Prev,
-  Next
-};
-enum E_jump
-{
-  Nr_nor,
-  Nr_zero,
-  Nr_180,
-  Nr_inf,
-  Nr_blind
-};
-
-struct orgtype
-{
-  double range;
-  double dista;
-  double angle[2];
-  double intersect;
-  E_jump edj[2];
-  Feature ftype;
-  orgtype()
-  {
-    range = 0;
-    edj[Prev] = Nr_nor;
-    edj[Next] = Nr_nor;
-    ftype = Nor;
-    intersect = 2;
-  }
-};
-
-namespace velodyne_ros
-{
-struct EIGEN_ALIGN16 Point
-{
-  PCL_ADD_POINT4D;
-  float intensity;
-  float time;
-  uint16_t ring;
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-}  // namespace velodyne_ros
-POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
-                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity,
-                                                                          intensity)(float, time, time)(uint16_t, ring,
-                                                                                                        ring))
-
-namespace ouster_ros
-{
-struct EIGEN_ALIGN16 Point
-{
-  PCL_ADD_POINT4D;
-  float intensity;
-  uint32_t t;
-  uint16_t reflectivity;
-  uint8_t ring;
-  uint16_t ambient;
-  uint32_t range;
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-}  // namespace ouster_ros
-
-// clang-format off
-POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
-    (float, x, x)
-    (float, y, y)
-    (float, z, z)
-    (float, intensity, intensity)
-    // use std::uint32_t to avoid conflicting with pcl::uint32_t
-    (std::uint32_t, t, t)
-    (std::uint16_t, reflectivity, reflectivity)
-    (std::uint8_t, ring, ring)
-    (std::uint16_t, ambient, ambient)
-    (std::uint32_t, range, range)
-)
-
-namespace livox_ros
-{
-typedef struct {
-  float x;            /**< X axis, Unit:m */
-  float y;            /**< Y axis, Unit:m */
-  float z;            /**< Z axis, Unit:m */
-  float reflectivity; /**< Reflectivity   */
-  uint8_t tag;        /**< Livox point tag   */
-  uint8_t line;       /**< Laser line id     */
-} LivoxPointXyzrtl;
-
-typedef struct {
-  float x;            /**< X axis, Unit:m */
-  float y;            /**< Y axis, Unit:m */
-  float z;            /**< Z axis, Unit:m */
-  float intensity;    /**< Intensity   */
-  uint8_t tag;        /**< Livox point tag   */
-  uint8_t line;       /**< Laser line id     */
-} LivoxPointXyzitl;
-}
-POINT_CLOUD_REGISTER_POINT_STRUCT(livox_ros::LivoxPointXyzrtl,
-    (float, x, x)
-    (float, y, y)
-    (float, z, z)
-    (float, reflectivity, reflectivity)
-    (uint8_t, tag, tag)
-    (uint8_t, line, line)
-)
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(livox_ros::LivoxPointXyzitl,
-    (float, x, x)
-    (float, y, y)
-    (float, z, z)
-    (float, intensity, intensity)
-    (uint8_t, tag, tag)
-    (uint8_t, line, line)
-)
-
-class Preprocess
-{
-  public:
-//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  Preprocess();
-  ~Preprocess();
-  
-  void process(const livox_ros_driver2::msg::CustomMsg::UniquePtr &msg, PointCloudXYZI::Ptr &pcl_out);
-  void process(const sensor_msgs::msg::PointCloud2::UniquePtr &msg, PointCloudXYZI::Ptr &pcl_out);
-  void set(bool feat_en, int lid_type, double bld, int pfilt_num);
-
-  // sensor_msgs::PointCloud2::ConstPtr pointcloud;
-  PointCloudXYZI pl_full, pl_corn, pl_surf;
-  PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
-  vector<orgtype> typess[128]; //maximum 128 line lidar
-  float time_unit_scale;
-  int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit;
-  double blind;
-  bool feature_enabled, given_offset_time;
-  // ros::Publisher pub_full, pub_surf, pub_corn;
-
-private:
-  void avia_handler(const livox_ros_driver2::msg::CustomMsg::UniquePtr &msg);
-  void oust64_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg);
-  void velodyne_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg);
-  void mid360_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg);
-  void default_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg);
-  void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
-  void pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct);
-  int  plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
-  bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
-  bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);
-  
-  int group_size;
-  double disA, disB, inf_bound;
-  double limit_maxmid, limit_midmin, limit_maxmin;
-  double p2l_ratio;
-  double jump_up_limit, jump_down_limit;
-  double cos160;
-  double edgea, edgeb;
-  double smallp_intersect, smallp_ratio;
-  double vx, vy, vz;
-};

Lslidar_ROS2_driver/README.md

@@ -1,44 +0,0 @@
-# lslidar
-
-## Description
-The `lslidar package is a linux ROS2 driver for lslidar M10 ,M10_GPS,M10_P,M10_PLUS and N10.
-The package is tested on Ubuntu 20.04 with ROS2 FOXY.
-
-## Compling
-This is a Catkin package. Make sure the package is on `ROS_PACKAGE_PATH` after cloning the package to your workspace. And the normal procedure for compling a catkin package will work.
-
-```
-cd your_work_space
-colcon build
-source install/setup.bash
-ros2 launch lslidar_driver lslidar_launch.py
-```
-open new terminal
-ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 1\ 		(open radar)
-ros2 topic pub -1 /lslidar_order std_msgs/msg/Int8 data:\ 0\ 		(close radar)
-
-
-
-
-
-ros2 launch lslidar_driver lslidar_launch.py
-
-```
-
-Note that this launch file launches both the driver, which is the only launch file needed to be used.
-
-
-## FAQ
-
-
-## Bug Report
-
-Prefer to open an issue. You can also send an E-mail to honghangli@lslidar.com
-
-
-
-
-RERTION 
-
-
-

Lslidar_ROS2_driver/lslidar_driver/CMakeLists.txt

@@ -1,56 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(lslidar_driver)
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-set(libpcap_LIBRARIES -lpcap)
-
-#set(FastRTPS_INCLUDE_DIR /opt/ros/foxy/include)
-#set(FastRTPS_LIBRARY_RELEASE /opt/ros/foxy/lib/libfastrtps.so)
-
-#find_package(Boost REQUIRED COMPONENTS )
-find_package(Boost REQUIRED thread)
-find_package(rclcpp REQUIRED)
-find_package(PCL REQUIRED)
-find_package(diagnostic_updater REQUIRED)
-find_package(lslidar_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(ament_cmake REQUIRED)
-find_package(pluginlib REQUIRED)
-find_package(rclpy REQUIRED)
-find_package(pcl_conversions REQUIRED)
-find_package(sensor_msgs REQUIRED)
-#find_package(PCL REQUIRED COMPONENTS common io)											   
-
-include_directories(
-		include
-    ${PCL_INCLUDE_DIRS}
-    ${PCL_COMMON_INCLUDE_DIRS}
-    ${Boost_INCLUDE_DIRS}
-)
-
-# Node
-add_executable(lslidar_driver_node src/lslidar_driver_node.cc src/lslidar_driver.cc src/input.cc src/lsiosr.cpp)
-target_link_libraries(lslidar_driver_node ${rclcpp_LIBRARIES} ${libpcap_LIBRARIES} ${Boost_LIBRARIES} Boost::thread)
-ament_target_dependencies(lslidar_driver_node rclcpp std_msgs lslidar_msgs sensor_msgs diagnostic_updater pcl_conversions)
-
-
-install(DIRECTORY launch params rviz
-		DESTINATION share/${PROJECT_NAME})
-
-install(TARGETS
-  lslidar_driver_node
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-ament_export_dependencies(rclcpp pluginlib lslidar_msgs sensor_msgs pcl_conversions)
-ament_export_include_directories(include ${PCL_COMMON_INCLUDE_DIRS})
-
-ament_package()

Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/input.h

@@ -1,134 +0,0 @@
-/*
- * This file is part of lslidar_ch driver.
- *
- * The driver is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * The driver is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
- *
- *     Input -- base class used to access the data independently of
- *              its source
- *
- *     InputSocket -- derived class reads live data from the device
- *              via a UDP socket
- *
- *     InputPCAP -- derived class provides a similar interface from a
- *              PCAP dump
- */
-
-#ifndef __LSLIDAR_INPUT_H_
-#define __LSLIDAR_INPUT_H_
-
-#include <unistd.h>
-#include <stdio.h>
-#include <pcap.h>
-#include <netinet/in.h>
-#include "rclcpp/rclcpp.hpp"
-#include <lslidar_msgs/msg/lslidar_packet.hpp>
-#include <string>
-#include <sstream>
-#include <sys/socket.h>
-#include <arpa/inet.h>
-#include <poll.h>
-#include <errno.h>
-#include <fcntl.h>
-#include <sys/file.h>
-#include <signal.h>
-#include <sensor_msgs/msg/time_reference.hpp>
-#include <std_msgs/msg/int8.hpp>
-#include <cmath>
-
-namespace lslidar_driver
-{
-static uint16_t MSOP_DATA_PORT_NUMBER = 2368;   // lslidar default data port on PC
-/**
- *  从在线的网络数据或离线的网络抓包数据（pcap文件）中提取出lidar的原始数据，即packet数据包
- * @brief The Input class,
-     *
-     * @param private_nh  一个NodeHandled,用于通过节点传递参数
-     * @param port
-     * @returns 0 if successful,
-     *          -1 if end of file
-     *          >0 if incomplete packet (is this possible?)
- */
-class Input
-{
-public:
-  Input(rclcpp::Node* private_nh, uint16_t port);
-
-  virtual ~Input()
-  {
-  }
-
-  virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet) = 0;
-
-  int getRpm(void);
-  int getReturnMode(void);
-  bool getUpdateFlag(void);
-  void clearUpdateFlag(void);
-  void UDP_order(const std_msgs::msg::Int8 msg);
-  void UDP_difop();
-protected:
-  rclcpp::Node*  private_nh_;
-  uint16_t port_;
-  std::string devip_str_;
-  std::string lidar_name;
-  int cur_rpm_;
-  int return_mode_;
-  bool npkt_update_flag_;
-  bool add_multicast;
-  std::string group_ip;
-  int UDP_PORT_NUMBER_DIFOP;
-	int socket_id_difop;
-  int sockfd_;
-  std::string devip_str_difop;
-};
-
-/** @brief Live lslidar input from socket. */
-class InputSocket : public Input
-{
-public:
-  InputSocket(rclcpp::Node* private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER);
-
-  virtual ~InputSocket();
-
-  virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet);
-
-private:
-private:
-  
-  in_addr devip_;
-  in_addr devip_difop;
-  //struct ip_mreq group;
-
-};
-class InputPCAP : public Input
-{
-public:
-    InputPCAP(rclcpp::Node* private_nh,uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0,
-            std::string filename="");
-    virtual ~InputPCAP();
-    virtual int getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt);
-private:
-
-    rclcpp::Rate packet_rate_;
-    std::string filename_;
-    pcap_t *pcap_;
-    bpf_program pcap_packet_filter_;
-    char errbuf_[PCAP_ERRBUF_SIZE];
-    bool empty_;
-    bool read_once_;
-    bool read_fast_;
-    double repeat_delay_;
-  };
-}
-
-#endif  // __LSLIDAR_INPUT_H

Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/lsiosr.h

@@ -1,88 +0,0 @@
-/*******************************************************
-@company: Copyright (C) 2021, Leishen Intelligent System
-@product: LSM10_N10
-@filename: lsiosr.cpp
-@brief:
-@version:       date:       author:     comments:
-@v1.0           22-10-24      li          new
-*******************************************************/
-#ifndef LSIOSR_H
-#define LSIOSR_H
-
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <termios.h>
-#include <errno.h>
-#include <unistd.h>
-#include <string.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <stdint.h>
-#include <fstream>
-#include <iostream>
-
-//波特率
-#define     BAUD_230400     230400
-#define     BAUD_460800     460800
-#define     BAUD_500000     500000
-#define     BAUD_921600     921600
-
-//奇偶校验位
-#define     PARITY_ODD    	'O' //奇数
-#define     PARITY_EVEN   	'E' //偶数
-#define     PARITY_NONE   	'N' //无奇偶校验位
-
-//停止位
-#define     STOP_BIT_1     	1
-#define     STOP_BIT_2     	2
-
-//数据位
-#define     DATA_BIT_7     	7
-#define     DATA_BIT_8     	8
-
-namespace lslidar_driver
-{
-class LSIOSR{
-public:
-  static LSIOSR* instance(std::string name, int speed, int fd = 0);
-
-  ~LSIOSR();
-
-  /* 从串口中读取数据 */
-  int read(unsigned char *buffer, int length, int timeout = 30);
-
-  /* 向串口传数据 */
-  int send(const char* buffer, int length, int timeout = 30);
-
-  /* Empty serial port input buffer */
-  void flushinput();
-
-  /* 串口初始化 */
-  int init();
-
-  int close();
-
-  /* 获取串口号 */
-  std::string getPort();
-
-  /* 设置串口号 */
-  int setPortName(std::string name);
-
-private:
-  LSIOSR(std::string name, int speed, int fd);
-
-  int waitWritable(int millis);
-  int waitReadable(int millis);
-
-  /* 串口配置的函数 */
-  int setOpt(int nBits, uint8_t nEvent, int nStop);
-
-  std::string port_;
-  int baud_rate_;
-
-  int fd_;
-};
-}
-#endif
-

Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/lslidar_driver.h

@@ -1,159 +0,0 @@
-/*
- * This file is part of lslidar driver.
- *
- * The driver is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * The driver is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef LSLIDAR_DRIVER_H
-#define LSLIDAR_DRIVER_H
-
-#include <unistd.h>
-#include <stdio.h>
-#include <netinet/in.h>
-#include <string>
-
-#include <boost/shared_ptr.hpp>
-#include <boost/date_time/posix_time/posix_time.hpp>
-#include <boost/thread.hpp>
-#include "rclcpp/rclcpp.hpp"
-#include <thread>
-#include "diagnostic_updater/diagnostic_updater.hpp"
-#include "diagnostic_updater/publisher.hpp"
-#include "lslidar_msgs/msg/lslidar_packet.hpp"
-#include "std_msgs/msg/byte.hpp"
-
-#include "sensor_msgs/msg/point_cloud2.hpp"
-#include "pcl_conversions/pcl_conversions.h"
-#include "pcl/point_types.h"
-
-#include "time.h"
-#include "input.h"
-#include "lsiosr.h"
-#include "sensor_msgs/msg/laser_scan.hpp"
-namespace lslidar_driver {
-
-struct PointXYZIT {
-    PCL_ADD_POINT4D;
-    uint8_t intensity;
-    double timestamp;
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
-} EIGEN_ALIGN16;
- 
-typedef struct {
-    double degree;
-    double range;
-    double intensity;
-} ScanPoint;
-
-class LslidarDriver: public rclcpp::Node {
-public:
-	LslidarDriver();
-	LslidarDriver(const rclcpp::NodeOptions& options);
-	~LslidarDriver();
-
-    bool initialize();
-    bool polling();
-
-    typedef std::shared_ptr<LslidarDriver> LslidarDriverPtr;
-    typedef std::shared_ptr<const LslidarDriver> LslidarDriverConstPtr;
-
-private:    
-    uint64_t get_gps_stamp(struct tm t);
-    uint8_t N10_CalCRC8(unsigned char * p, int len);
-    bool loadParameters();
-    bool createRosIO();
-    void open_serial();
-    void lidar_difop();
-    void lidar_order(const std_msgs::msg::Int8::SharedPtr msg);
-    void data_processing(unsigned char *packet_bytes,int len);
-    void data_processing_2(unsigned char *packet_bytes,int len);
-    void difop_processing(unsigned char *packet_bytes);
-    void pubScanThread();
-    void recvThread_crc(int &count,int &link_time);
-    int receive_data(unsigned char *packet_bytes);
-    int getScan(std::vector<ScanPoint> &points, rclcpp::Time &scan_time, float &scan_duration);
-
-    boost::thread *pubscan_thread_ ;
-    boost::shared_ptr<Input> msop_input_;
-    boost::mutex mutex_; 
-    boost::mutex pubscan_mutex_;
-    boost::condition_variable pubscan_cond_;
-    
-    int UDP_PORT_NUMBER; 
-    int count_num;
-    int package_points;
-    int data_bits_start;
-    int degree_bits_start;
-    int end_degree_bits_start;
-    int rpm_bits_start;
-	int baud_rate_;
-    int points_size_;
-    int idx = 0;
-    int link_time = 0;
-
-    bool use_gps_ts;   
-    bool is_start;
-    bool high_reflection;
-    bool compensation;
-    bool first_compensation = true;
-    bool pubScan;
-    bool pubPointCloud2;
-
-    double min_range;
-    double max_range;
-    double angle_disable_min;
-    double angle_disable_max;
-    double angle_able_min;
-    double angle_able_max;
-    double last_degree = 0.0;	
-    double degree_compensation = 0.0;
-
-    uint16_t PACKET_SIZE ;
-    uint64_t sweep_end_time_gps;
-    uint64_t sweep_end_time_hardware;
-    uint64_t sub_second;
-
-    std::string frame_id;
-    std::string interface_selection;
-    std::string scan_topic;
-    std::string lidar_name;
-    std::string serial_port_;
-    std::string dump_file;
-    std::string pointcloud_topic;
-    std::string in_file_name;
-    
-    tm pTime;    
-    rclcpp::Time pre_time_;
-    rclcpp::Time time_;
-    std::vector<ScanPoint> scan_points_;
-    std::vector<ScanPoint> scan_points_bak_;
-    // Diagnostics updater
-    diagnostic_updater::Updater diagnostics;
-    std::shared_ptr<diagnostic_updater::TopicDiagnostic> diag_topic;
-    double diag_min_freq;
-    double diag_max_freq;
-    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub;
-    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_pub;
-	rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr difop_switch;
-    LSIOSR * serial_;
-};
-typedef PointXYZIT VPoint;
-typedef pcl::PointCloud<VPoint> VPointCloud;
-
-} // namespace lslidar_driver
-POINT_CLOUD_REGISTER_POINT_STRUCT(lslidar_driver::PointXYZIT,
-                                  (float, x, x)(float, y, y)(float, z, z)(
-                                          std::uint8_t, intensity,
-                                          intensity)(double, timestamp, timestamp))
-#endif // _LSLIDAR_DRIVER_H_

Lslidar_ROS2_driver/lslidar_driver/launch/lsn10p_launch.py

@@ -1,28 +0,0 @@
-#!/usr/bin/python3
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch_ros.actions import LifecycleNode
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-from launch.actions import DeclareLaunchArgument
-
-import lifecycle_msgs.msg
-import os
-
-def generate_launch_description():
-
-    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params','lidar_uart_ros2', 'lsn10p.yaml')
-                     
-    driver_node = LifecycleNode(package='lslidar_driver',
-                                executable='lslidar_driver_node',
-                                name='lslidar_driver_node',		#设置激光数据topic名称
-                                output='screen',
-                                emulate_tty=True,
-                                namespace='',
-                                parameters=[driver_dir],
-                                )
-
-    return LaunchDescription([
-        driver_node,
-    ])
-

Lslidar_ROS2_driver/lslidar_driver/launch/viewer_scan_launch.py

@@ -1,26 +0,0 @@
-#!/usr/bin/python3
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch_ros.actions import LifecycleNode
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-from launch.actions import DeclareLaunchArgument
-
-import lifecycle_msgs.msg
-import os
-
-def generate_launch_description():
-
-    rviz2_config = os.path.join(get_package_share_directory('lslidar_driver'),'rviz','lslidar.rviz')
-      
-    rviz2_node = Node(
-        package='rviz2',
-        executable='rviz2',
-        name='rviz2',
-        arguments=['-d',rviz2_config],
-        output='screen')                                               
-
-    return LaunchDescription([
-        rviz2_node,
-    ])
-    

Lslidar_ROS2_driver/lslidar_driver/package.xml

@@ -1,36 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>lslidar_driver</name>
-  <version>1.2.0</version>
-  <description>ROS device driver for Leishen lidar.</description>
-  <maintainer email="shaohuashu@lslidar.com">Nick Shu</maintainer>
-  <author>Nick Shu</author>
-  <license>GNU General Public License V3.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <build_depend>rclcpp</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>lslidar_msgs</build_depend>
-  <build_depend>pcl_conversions</build_depend>
-  <build_depend>rclpy</build_depend>
-  <build_depend>libpcap</build_depend>
-  <build_depend>libpcl-all-dev</build_depend>
-  <build_depend>pluginlib</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-
-  <exec_depend>rclcpp</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>lslidar_msgs</exec_depend>
-  <exec_depend>pcl_conversions</exec_depend>
-  <exec_depend>rclpy</exec_depend>
-  <exec_depend>libpcap</exec_depend>
-  <exec_depend>libpcl-all</exec_depend>
-  <exec_depend>pluginlib</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
-
-  <depend>diagnostic_updater</depend>
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

Lslidar_ROS2_driver/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml

@@ -1,25 +0,0 @@
-/lslidar_driver_node:
-  ros__parameters: 
-    frame_id: laser_link                                    #激光坐标
-    group_ip: 224.1.1.2
-    add_multicast: false 
-    device_ip: 192.168.1.200                   	            #雷达源IP
-    device_ip_difop: 192.168.1.102                          #雷达目的ip
-    msop_port: 2368                                         #雷达目的端口号
-    difop_port: 2369                                        #雷达源端口号
-    lidar_name: N10_P                                       #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 L10 N10_P
-    angle_disable_min: 0.0                                  #角度裁剪开始值
-    angle_disable_max: 0.0                                  #角度裁剪结束值
-    min_range: 0.2                                          #雷达接收距离最小值
-    max_range: 200.0                                        #雷达接收距离最大值
-    use_gps_ts: false                                       #雷达是否使用GPS授时
-    scan_topic: /scan                                       #设置激光数据topic名称
-    interface_selection: serial                             #接口选择:net 为网口,serial 为串口。
-    serial_port_: /dev/agvpro_lidar                         #串口连接时的串口号
-    high_reflection: false                                  #M10_P雷达需填写该值,若不确定，请联系技术支持。
-    compensation: false				            #M10系列是否使用角度补偿功能
-    pubScan: true                                           #是否发布scan话题
-    pubPointCloud2: false                                   #是否发布pointcloud2话题
-    pointcloud_topic: /lslidar_point_cloud                  #设置激光数据topic名称
-#    pcap: /home/ls/1.pcap                                  #雷达是否使用pcap包读取功能
-#    in_file_name: /home/ls/1.txt                           #雷达是否使用txt文件读取功能

Lslidar_ROS2_driver/lslidar_driver/rviz/lslidar.rviz

@@ -1,161 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /LaserScan1
-      Splitter Ratio: 0.3441176414489746
-    Tree Height: 617
-  - 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
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/LaserScan
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 0
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: LaserScan
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: scan
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: laser_link
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/Interact
-      Hide Inactive Objects: true
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Covariance x: 0.25
-      Covariance y: 0.25
-      Covariance yaw: 0.06853891909122467
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /initialpose
-    - Class: rviz_default_plugins/SetGoal
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /goal_pose
-    - Class: rviz_default_plugins/PublishPoint
-      Single click: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /clicked_point
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 3.635173797607422
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.8653978705406189
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 3.095397710800171
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 846
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000292000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1283
-  X: 406
-  Y: 152

Lslidar_ROS2_driver/lslidar_driver/src/input.cc

@@ -1,398 +0,0 @@
-#include "lslidar_driver/input.h"
-
-extern volatile sig_atomic_t flag;
-namespace lslidar_driver
-{
-    static const size_t packet_size_input = 400;
-    ////////////////////////////////////////////////////////////////////////
-    // Input base class implementation
-    ////////////////////////////////////////////////////////////////////////
-
-    /** @brief constructor
-     *
-     *  @param private_nh ROS private handle for calling node.
-     *  @param port UDP port number.
-     */
-    Input::Input(rclcpp::Node *private_nh, uint16_t port) : private_nh_(private_nh), port_(port) {
-        npkt_update_flag_ = false;
-        cur_rpm_ = 0;
-        return_mode_ = 1;
-        devip_str_difop = std::string("192.168.1.200");
-        devip_str_ = std::string("192.168.1.102");
-        lidar_name = std::string("M10");
-        add_multicast = false;
-        group_ip = std::string("224.1.1.2");
-        UDP_PORT_NUMBER_DIFOP = 2369;
-
-
-	    private_nh->declare_parameter<std::string>("device_ip","192.168.1.102");
-        private_nh->declare_parameter<std::string>("device_ip_difop","192.168.1.200");
-        private_nh->declare_parameter<bool>("add_multicast",false);
-        private_nh->declare_parameter<std::string>("group_ip","224.1.1.2");
-        private_nh->declare_parameter<int>("difop_port",2369);
-
-
-        private_nh->get_parameter("lidar_name", lidar_name);
-        private_nh->get_parameter("device_ip", devip_str_);
-        private_nh->get_parameter("add_multicast", add_multicast);
-        private_nh->get_parameter("group_ip", group_ip);
-        private_nh->get_parameter("difop_port", UDP_PORT_NUMBER_DIFOP);
-        private_nh->get_parameter("device_ip_difop", devip_str_difop);
-
-        if (!devip_str_.empty())
-            RCLCPP_INFO(private_nh->get_logger(), "[driver][input] accepting packets from IP address: %s  port: %d",
-                        devip_str_.c_str(),port);
-    }
-
-    /** @brief constructor
-     *
-     *  @param private_nh ROS private handle for calling node.
-     *  @param port UDP port number
-     */
-    InputSocket::InputSocket(rclcpp::Node *private_nh, uint16_t port) : Input(private_nh, port) {
-        sockfd_ = -1;
-
-        if (!devip_str_.empty()) {
-            inet_aton(devip_str_.c_str(), &devip_);
-            inet_aton(devip_str_difop.c_str(), &devip_difop);
-        }
-
-        RCLCPP_INFO(private_nh_->get_logger(), "[driver][socket] Opening UDP socket: port %d", port);
-        sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
-        if (sockfd_ == -1) {
-            perror("socket");  // TODO: ROS_ERROR errno
-            return;
-        }
-
-        int opt = 1;
-        if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void *) &opt, sizeof(opt))) {
-            perror("setsockopt error!\n");
-            return;
-        }
-
-        sockaddr_in my_addr;                   // my address information
-        memset(&my_addr, 0, sizeof(my_addr));  // initialize to zeros
-        my_addr.sin_family = AF_INET;          // host byte order
-        my_addr.sin_port = htons(port);        // port in network byte order
-        my_addr.sin_addr.s_addr = INADDR_ANY;  // automatically fill in my IP
-
-        if (bind(sockfd_, (sockaddr * ) & my_addr, sizeof(sockaddr)) == -1) {
-            perror("bind");  // TODO: ROS_ERROR errno
-            return;
-        }
-
-        if (add_multicast) {
-            struct ip_mreq group;
-            group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str());
-            group.imr_interface.s_addr = htonl(INADDR_ANY);
-
-            if (setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &group, sizeof(group)) < 0) {
-                perror("Adding multicast group error ");
-                close(sockfd_);
-                exit(1);
-            } else
-                printf("Adding multicast group...OK.\n");
-        }
-        if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
-            perror("non-block");
-            return;
-        }
-    }
-
-    /** @brief destructor */
-    InputSocket::~InputSocket(void) {
-        (void) close(sockfd_);
-    }
-
-    void Input::UDP_difop()
-    {
-        sockaddr_in server_sai;
-        server_sai.sin_family = AF_INET; // IPV4 协议族
-        server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP);
-        server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str());
-        for (int k = 0; k < 10; k++)
-        {
-            unsigned char data[188]= {0x00};
-            data[0] = 0xA5;
-            data[1] = 0x5A;
-            data[2] = 0x55;
-            data[184] = 0x08;
-            data[185] = 0x01;
-            data[186] = 0xFA;
-            data[187] = 0xFB;
-            int rtn = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr));
-            if (rtn < 0)    printf("start scan error !\n");
-            else return; 
-        }
-        return; 
-    }
-    
-    void Input::UDP_order(const std_msgs::msg::Int8 msg)
-    {
-        int i = msg.data;
-        sockaddr_in server_sai;
-        server_sai.sin_family = AF_INET; // IPV4 协议族
-        server_sai.sin_port = htons(UDP_PORT_NUMBER_DIFOP);
-        server_sai.sin_addr.s_addr = inet_addr(devip_str_.c_str());
-        int rtn = 0;
-        for (int k = 0; k < 10; k++)
-        {
-            unsigned char data[188]= {0x00};
-            data[0] = 0xA5;
-            data[1] = 0x5A;
-            data[2] = 0x55;
-            data[186] = 0xFA;
-            data[187] = 0xFB;  
-            if(lidar_name == "M10" || lidar_name == "M10_GPS" || lidar_name == "M10_P"){
-                if (i <= 1){				    //雷达启停
-                    data[184] = 0x01;
-                    data[185] = char(i);
-                }
-                else if (i == 2){			    //雷达点云不滤波
-                    data[181] = 0x0A;
-                    data[184] = 0x06;
-                    data[185] = 0x01;
-                }
-                else if (i == 3){				//雷达点云正常滤波
-                    data[181] = 0x0B;
-                    data[184] = 0x06;
-                    data[185] = 0x01;
-                }
-                else if (i == 4){				//雷达近距离滤波
-                    data[181] = 0x0C;
-                    data[184] = 0x06;
-                    data[185] = 0x01;
-                }    
-                else if (i == 100){				//接收设备包
-                    data[184] = 0x08;
-                    data[185] = 0x01;
-                }      
-                else return;    
-            }
-            else if (lidar_name == "M10_PLUS"){   
-                data[184] = 0x0A;
-                data[185] = 0x01;           
-                if(i == 5)       {
-                data[141] = 0x01;
-                data[142] = 0x2c;
-                }
-                else if(i == 6)  {
-                data[141] = 0x01;
-                data[142] = 0x68;
-                }
-                else if(i == 8)  {
-                data[141] = 0x01;
-                data[142] = 0xe0;
-                }
-                else if(i == 10) {
-                data[141] = 0x02;
-                data[142] = 0x58;
-                }
-                else if(i == 12) {
-                data[141] = 0x02;
-                data[142] = 0xd0;
-                }
-                else if(i == 15) {
-                data[141] = 0x03;
-                data[142] = 0x84;
-                }
-                else if(i == 20) {
-                data[141] = 0x04;
-                data[142] = 0xb0;
-                }
-                else if(i <= 1)  {
-                data[184] = 0x01;
-                data[185] = char(i);
-                }
-                else if(i == 100) {				//接收设备包
-                    data[184] = 0x08;
-                    data[185] = 0x01;
-                }      
-                else return;
-            }
-            else if(lidar_name == "N10"){
-                if(i <= 1){
-                data[185] = char(i);
-                data[184] = 0x01;
-                }
-                else if(i>=6 && i<=12){
-                data[172] = char(i);
-                data[184] = 0x0a;
-                data[185] = 0X01;
-                }
-                else return; 
-            }
-            rtn = sendto(sockfd_, data, 188, 0, (struct sockaddr *)&server_sai, sizeof(struct sockaddr));
-            if (rtn < 0)
-            {
-                printf("start scan error !\n");
-            }
-            else
-            {
-                if (i == 1)
-                    usleep(3000000);
-                return;
-            }
-        }
-        return;
-    }
-
-
-
-    int InputSocket::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &packet)
-    {
-        int q = 0;
-        struct pollfd fds[1];
-        fds[0].fd = sockfd_;
-        fds[0].events = POLLIN;
-        static const int POLL_TIMEOUT = 2000; // one second (in msec)
-
-        sockaddr_in sender_address{};
-        socklen_t sender_address_len = sizeof(sender_address);
-        while (flag == 1)
-        {
-            // poll() until input available
-            do {
-                int retval = poll(fds, 1, POLL_TIMEOUT);
-                if (retval < 0)  // poll() error?
-                {
-                    if (errno != EINTR)
-                        RCLCPP_ERROR(private_nh_->get_logger(), "[driver][socket] poll() error: %s", strerror(errno));
-                    return 0;
-                }
-                if (retval == 0) // poll() timeout?
-                {
-                    RCLCPP_WARN(private_nh_->get_logger(), "lslidar poll() timeout, port: %d",port_);
-                    return 0;
-                }
-                if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error?
-                {
-                    RCLCPP_ERROR(private_nh_->get_logger(),"poll() reports lslidar error");
-                    return 0;
-                }
-            } while ((fds[0].revents & POLLIN) == 0);
-
-            // Receive packets that should now be available from the
-            // socket using a blocking read.
-            ssize_t nbytes = recvfrom(sockfd_, &packet->data[0], packet_size_input, 0,
-                                      (sockaddr *)&sender_address, &sender_address_len);
-            //        ROS_DEBUG_STREAM("incomplete lslidar packet read: "
-            //                         << nbytes << " bytes");
-            q = (int)nbytes;
-            if (nbytes < 0)
-            {
-                if (errno != EWOULDBLOCK)
-                {
-                    perror("recvfail");
-                    RCLCPP_ERROR(private_nh_->get_logger(),"recvfail");
-                    return 1;
-                }
-            }
-            else if ((size_t)nbytes <= packet_size_input || (size_t)nbytes >= 50)
-            {
-
-                // read successful,
-                // if packet is not from the lidar scanner we selected by IP,
-                // continue otherwise we are done
-                if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr)
-                    continue;
-                else
-                    break; // done
-            }
-            
-        }
-        if (flag == 0)
-        {
-            abort();
-        }
-
-        return q;
-    }
-    InputPCAP::InputPCAP(rclcpp::Node *private_nh, uint16_t port, double packet_rate, std::string filename) : Input(private_nh, port),
-                                                                                packet_rate_(packet_rate),
-                                                                                filename_(filename)
-    {
-        pcap_ = NULL;
-        empty_ = true;
-        read_once_ = false;
-        read_fast_ = false;
-        repeat_delay_ = 0.0;
-        private_nh->get_parameter("read_once", read_once_);
-        private_nh->get_parameter("read_fast", read_fast_);
-        private_nh->get_parameter("repeat_delay", repeat_delay_);
-
-        if (read_once_)
-            RCLCPP_WARN(private_nh_->get_logger(),"Read input file only once.");
-        if (read_fast_)
-            RCLCPP_WARN(private_nh_->get_logger(),"Read input file as quickly as possible.");
-        if (repeat_delay_ > 0.0)
-            RCLCPP_WARN(private_nh_->get_logger(),"Delay %.3f seconds before repeating input file.", repeat_delay_);
-
-        RCLCPP_INFO(private_nh_->get_logger(),"Opening PCAP file %s",filename_.c_str());
-        if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL)
-        {
-            RCLCPP_WARN(private_nh_->get_logger(),"Error opening lslidar socket dump file.");
-            return;
-        }
-        std::stringstream filter;
-        if (devip_str_ != "")
-        {
-            filter << "src host " << devip_str_ << "&&";
-        }
-        filter << "udp dst port " << port;
-        pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
-    }
-
-    InputPCAP::~InputPCAP(void)
-    {
-        pcap_close(pcap_);
-    }
-
-    int InputPCAP::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt)
-    {
-        struct pcap_pkthdr *header;
-        const u_char *pkt_data;
-        while (flag == 1)
-            {
-            int res;
-            if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
-            {
-                // skip packets not for the correct port and from the selected IP address
-                if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data)))
-                    continue;
-
-                if (read_fast_ == false)
-                    packet_rate_.sleep();
-                mempcpy(&pkt->data[0], pkt_data + 42, packet_size_input);
-                empty_ = false;
-                return 0;
-            }
-            if (empty_)
-            {
-                RCLCPP_WARN(private_nh_->get_logger(),"Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_));
-                return -1;
-            }
-            if (read_once_)
-            {
-                RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- done reading.");
-                return -1;
-            }
-            if (repeat_delay_ > 0.0)
-            {
-                RCLCPP_WARN(private_nh_->get_logger(),"end of file reached -- delaying %.3f seconds.", repeat_delay_);
-                usleep(rint(repeat_delay_ * 1000000.0));
-            }
-            RCLCPP_WARN(private_nh_->get_logger(),"replayding lsliar dump file");
-
-            pcap_close(pcap_);
-            pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
-            empty_ = true;
-        }
-        if (flag == 0)
-        {
-            abort();
-        }
-        return 0;
-    }
-
-} // namespace

Lslidar_ROS2_driver/lslidar_driver/src/lsiosr.cpp

@@ -1,400 +0,0 @@
-/*******************************************************
-@company: Copyright (C) 2022, Leishen Intelligent System
-@product: LSM10 and N10
-@filename: lsiosr.cpp
-@brief:
-@version:       date:       author:     comments:
-@v1.0           21-2-4      yao          new
-*******************************************************/
-#include "lslidar_driver/lsiosr.h"
-
-namespace lslidar_driver {
-
-LSIOSR * LSIOSR::instance(std::string name, int speed, int fd)
-{
-  static LSIOSR obj(name, speed, fd);
-  return &obj;
-}
-
-LSIOSR::LSIOSR(std::string port, int baud_rate, int fd):port_(port), baud_rate_(baud_rate), fd_(fd)
-{
-  printf("port = %s, baud_rate = %d\n", port.c_str(), baud_rate);
-}
-
-LSIOSR::~LSIOSR()
-{
-  close();
-}
-/* 串口配置的函数 */
-int LSIOSR::setOpt(int nBits, uint8_t nEvent, int nStop)
-{
-  struct termios newtio, oldtio;
-  /*保存测试现有串口参数设置，在这里如果串口号等出错，会有相关的出错信息*/
-  if (tcgetattr(fd_, &oldtio) != 0)
-  {
-    perror("SetupSerial 1");
-    return -1;
-  }
-  bzero(&newtio, sizeof(newtio));
-  /*步骤一，设置字符大小*/
-  newtio.c_cflag |= CLOCAL;   //如果设置，modem 的控制线将会被忽略。如果没有设置，则 open()函数会阻塞直到载波检测线宣告 modem 处于摘机状态为止。
-  newtio.c_cflag |= CREAD;    //使端口能读取输入的数据
-  /*设置每个数据的位数*/
-  switch (nBits)
-  {
-  case 7:
-    newtio.c_cflag |= CS7;
-    break;
-  case 8:
-    newtio.c_cflag |= CS8;
-    break;
-  }
-  /*设置奇偶校验位*/
-  switch (nEvent)
-  {
-  case 'O': //奇数
-    newtio.c_iflag |= (INPCK | ISTRIP);
-    newtio.c_cflag |= PARENB;   //使能校验，如果不设PARODD则是偶校验
-    newtio.c_cflag |= PARODD;   //奇校验
-    break;
-  case 'E': //偶数
-    newtio.c_iflag |= (INPCK | ISTRIP);
-    newtio.c_cflag |= PARENB;
-    newtio.c_cflag &= ~PARODD;
-    break;
-  case 'N':  //无奇偶校验位
-    newtio.c_cflag &= ~PARENB;
-    break;
-  }
-  /*设置波特率*/
-  switch (baud_rate_)
-  {
-  case 230400:
-    cfsetispeed(&newtio, B230400);
-    cfsetospeed(&newtio, B230400);
-    break;
-  case 460800:
-    cfsetispeed(&newtio, B460800);
-    cfsetospeed(&newtio, B460800);
-    break;
-  case 500000:
-    cfsetispeed(&newtio, B500000);
-    cfsetospeed(&newtio, B500000);
-    break;
-  case 921600:
-    cfsetispeed(&newtio, B921600);
-    cfsetospeed(&newtio, B921600);
-    break;
-  default:
-    cfsetispeed(&newtio, B460800);
-    cfsetospeed(&newtio, B460800);
-    break;
-  }
-
-  /*
-   * 设置停止位
-   * 设置停止位的位数， 如果设置，则会在每帧后产生两个停止位， 如果没有设置，则产生一个
-   * 停止位。一般都是使用一位停止位。需要两位停止位的设备已过时了。
-   * */
-  if (nStop == 1)
-    newtio.c_cflag &= ~CSTOPB;
-  else if (nStop == 2)
-    newtio.c_cflag |= CSTOPB;
-  /*设置等待时间和最小接收字符*/
-  newtio.c_cc[VTIME] = 0;
-  newtio.c_cc[VMIN] = 0;
-  /*处理未接收字符*/
-  tcflush(fd_, TCIFLUSH);
-  /*激活新配置*/
-  if ((tcsetattr(fd_, TCSANOW, &newtio)) != 0)
-  {
-    perror("serial set error");
-    return -1;
-  }
-
-  return 0;
-}
-
-void LSIOSR::flushinput() {
-  tcflush(fd_, TCIFLUSH);
-}
-
-/* 从串口中读取数据 */
-int LSIOSR::read(unsigned char *buffer, int length, int timeout)
-{
-  memset(buffer, 0, length);
-
-  int totalBytesRead = 0;
-  int rc;
-  int unlink = 0;
-  unsigned char* pb = buffer;
-
-  if (timeout > 0)
-  {
-    rc = waitReadable(timeout);
-    if (rc <= 0)
-    {
-      return (rc == 0) ? 0 : -1;
-    }
-
-    int	retry = 3;
-    while (length > 0)
-    {
-      rc = ::read(fd_, pb, (size_t)length);
-
-      if (rc > 0)
-      {		
-        length -= rc;
-        pb += rc;
-        totalBytesRead += rc;
-
-        if (length == 0)
-        {
-          break;
-        }
-      }
-      else if (rc < 0)
-      {
-        printf("error \n");
-        retry--;
-        if (retry <= 0)
-        {
-          break;
-        }
-      }
-	  unlink++;
-      rc = waitReadable(20);
-	  if(unlink > 10)
-		  return -1;
-	  
-      if (rc <= 0)
-      {
-        break;
-      }
-    }
-  }
-  else
-  {
-    rc = ::read(fd_, pb, (size_t)length);
-
-    if (rc > 0)
-    {
-      totalBytesRead += rc;
-    }
-    else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN))
-    {
-      printf("read error\n");
-      return -1;
-    }
-  }
-
-  return totalBytesRead;
-}
-
-int LSIOSR::waitReadable(int millis)
-{
-  if (fd_ < 0)
-  {
-    return -1;
-  }
-  int serial = fd_;
-  
-  fd_set fdset;
-  struct timeval tv;
-  int rc = 0;
-  
-  while (millis > 0)
-  {
-    if (millis < 5000)
-    {
-      tv.tv_usec = millis % 1000 * 1000;
-      tv.tv_sec  = millis / 1000;
-
-      millis = 0;
-    }
-    else
-    {
-      tv.tv_usec = 0;
-      tv.tv_sec  = 5;
-
-      millis -= 5000;
-    }
-
-    FD_ZERO(&fdset);
-    FD_SET(serial, &fdset);
-    
-    rc = select(serial + 1, &fdset, NULL, NULL, &tv);
-    if (rc > 0)
-    {
-      rc = (FD_ISSET(serial, &fdset)) ? 1 : -1;
-      break;
-    }
-    else if (rc < 0)
-    {
-      rc = -1;
-      break;
-    }
-  }
-
-  return rc;
-}
-
-
-int LSIOSR::waitWritable(int millis)
-{
-  if (fd_ < 0)
-  {
-    return -1;
-  }
-  int serial = fd_;
-
-  fd_set fdset;
-  struct timeval tv;
-  int rc = 0;
-
-  while (millis > 0)
-  {
-    if (millis < 5000)
-    {
-      tv.tv_usec = millis % 1000 * 1000;
-      tv.tv_sec  = millis / 1000;
-
-      millis = 0;
-    }
-    else
-    {
-      tv.tv_usec = 0;
-      tv.tv_sec  = 5;
-
-      millis -= 5000;
-    }
-
-    FD_ZERO(&fdset);
-    FD_SET(serial, &fdset);
-
-    rc = select(serial + 1, NULL, &fdset, NULL, &tv);
-    if (rc > 0)
-    {
-      rc = (FD_ISSET(serial, &fdset)) ? 1 : -1;
-      break;
-    }
-    else if (rc < 0)
-    {
-      rc = -1;
-      break;
-    }
-  }
-
-  return rc;
-}
-
-/* 向串口中发送数据 */
-int LSIOSR::send(const char* buffer, int length, int timeout)
-{
-  if (fd_ < 0)
-  {
-    return -1;
-  }
-
-  if ((buffer == 0) || (length <= 0))
-  {
-    return -1;
-  }
-
-  int	totalBytesWrite = 0;
-  int rc;
-  char* pb = (char*)buffer;
-
-
-  if (timeout > 0)
-  {
-    rc = waitWritable(timeout);
-    if (rc <= 0)
-    {
-      return (rc == 0) ? 0 : -1;
-    }
-
-    int	retry = 3;
-    while (length > 0)
-    {
-      rc = write(fd_, pb, (size_t)length);
-      if (rc > 0)
-      {
-        length -= rc;
-        pb += rc;
-        totalBytesWrite += rc;
-
-        if (length == 0)
-        {
-          break;
-        }
-      }
-      else
-      {
-        retry--;
-        if (retry <= 0)
-        {
-          break;
-        }
-      }
-
-      rc = waitWritable(50);
-      if (rc <= 0)
-      {
-        break;
-      }
-    }
-  }
-  else
-  {
-    rc = write(fd_, pb, (size_t)length);
-    if (rc > 0)
-    {
-      totalBytesWrite += rc;
-    }
-    else if ((rc < 0) && (errno != EINTR) && (errno != EAGAIN))
-    {
-      return -1;
-    }
-  }
-
-  return totalBytesWrite;
-}
-
-int LSIOSR::init()
-{
-	int error_code = 0;
-		
-	fd_ = open(port_.c_str(), O_RDWR|O_NOCTTY|O_NDELAY);
-	if (0 < fd_)
-	{
-		error_code = 0;
-		setOpt(DATA_BIT_8, PARITY_NONE, STOP_BIT_1);//设置串口参数
-		//printf("open_port %s  OK !\n", port_.c_str());
-	}
-	else
-	{
-		error_code = -1;
-	}
-
-	return error_code;
-}
-
-int LSIOSR::close()
-{
-  ::close(fd_);
-  return 0;
-}
-
-std::string LSIOSR::getPort()
-{
-  return port_;
-}
-
-int LSIOSR::setPortName(std::string name)
-{
-  port_ = name;
-  return 0;
-}
-
-}

Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver_node.cc

@@ -1,35 +0,0 @@
-/*
- * This file is part of lslidar driver.
- *
- * The driver is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * The driver is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with the driver.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include "rclcpp/rclcpp.hpp"
-#include "lslidar_driver/lslidar_driver.h"
-
-using namespace lslidar_driver;
-volatile sig_atomic_t flag = 1;
-
-int main(int argc, char* argv[])
-{
-  rclcpp::init(argc, argv);
-  auto node = std::make_shared<lslidar_driver::LslidarDriver>();
-  
-  while (rclcpp::ok() && node->polling()) {
-        rclcpp::spin_some(node);
-  }
-  //rclcpp::spin(node);
-  rclcpp::shutdown();
-  return 0;
-}

Lslidar_ROS2_driver/lslidar_msgs/CMakeLists.txt

@@ -1,39 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(lslidar_msgs)
-
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(sensor_msgs REQUIRED)
-find_package(builtin_interfaces REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-rosidl_generate_interfaces(lslidar_msgs
-  "msg/LslidarDifop.msg"
-  "msg/LslidarPacket.msg"
-  "msg/LslidarPoint.msg"
-  "msg/LslidarScan.msg"
-  "msg/LslidarSweep.msg"
-  DEPENDENCIES builtin_interfaces std_msgs
-  )
-
-ament_package()

Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarDifop.msg

@@ -1,2 +0,0 @@
-int64 temperature
-int64 rpm

Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPacket.msg

@@ -1,5 +0,0 @@
-# Raw Leishen LIDAR packet.
-
-builtin_interfaces/Time stamp              # packet timestamp
-uint8[2000] data        # packet contents
-

Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPoint.msg

@@ -1,12 +0,0 @@
-# Time when the point is captured
-float32 time
-
-# Converted distance in the sensor frame
-float64 x
-float64 y
-float64 z
-
-# Raw measurement from Leishen M10
-float64 azimuth
-float64 distance
-float64 intensity

Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarScan.msg

@@ -1,6 +0,0 @@
-# Altitude of all the points within this scan
-float64 altitude
-
-# The valid points in this scan sorted by azimuth
-# from 0 to 359.99
-LslidarPoint[] points

Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarSweep.msg

@@ -1,4 +0,0 @@
-std_msgs/Header header
-
-# The 0th scan is at the bottom
-LslidarScan[16] scans

Lslidar_ROS2_driver/lslidar_msgs/package.xml

@@ -1,25 +0,0 @@
-<?xml version="1.0"?>
-<package format="3">
-  <name>lslidar_msgs</name>
-  <version>1.2.0</version>
-  <description>ROS message definitions for Leishen LIDARs.</description>
-  <maintainer email="shaohuashu@lslidar.com">Nick Shu</maintainer>
-  <author>Nick Shu</author>
-  <license>GNU General Public License V3.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <buildtool_depend>rosidl_default_generators</buildtool_depend>
-
-  <exec_depend>rosidl_default_runtime</exec_depend>
-  <exec_depend>builtin_interfaces</exec_depend>
-
-  <depend>std_msgs</depend>
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-  
-  <member_of_group>rosidl_interface_packages</member_of_group>
-  
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

Lslidar_ROS2_driver/version.txt

@@ -1,40 +0,0 @@
-版本变更
-/***************************************************************
-初始版本: LSLIDAR_M10_N10_V2.5.0_221104_ROS2
-变更内容: 
-	1.实现M10/M10_P/M10_PLUS/N10/M10_GPS网口和串口传输数据生成点云功能
-    	2.实现点云角度裁剪和距离过滤功能
-    	3.可以通过lslidar_order话题控制雷达启停
-    	4.支持读取pcap包
-	
-更改日期: 2022-11-04
-***************************************************************/
-
-/***************************************************************
-初始版本: LSLIDAR_M10_N10_V2.5.0_221104_ROS2
-变更版本：LSLIDAR_M10_N10_V2.5.1_221130_ROS2
-变更内容: 
-	1.新增L10雷达。
-	2.针对M10_P雷达出现的点云问题进行紧急修复。
-	3.针对M10和M10_GPS雷达在最近出货雷达出现的点云问题进行紧急修复。
-	4.修复循环索取新内存导致机器卡死bug。
-	5.更新M10_P和M10_PLUS雷达协议。
-	6.修复角度值分配错误问题。
-	
-更改日期: 2022-11-30
-***************************************************************/
-
-/***************************************************************
-初始版本: LSLIDAR_M10_N10_V2.5.1_221130_ROS2
-变更版本：LSLIDAR_M10_N10_V2.5.2_230110_ROS2
-变更内容: 
-	1.修复驱动串口读取时包头检测漏洞。
-	2.更新M10系列的角度补偿方法。
-	3.修复发现的点角度不连续bug。
-	4.添加可选择是否发布pointcloud2话题。
-	5.添加可选择是否发布scan话题。
-	6.兼容ROS2的humble版本
-	7.兼容双回波雷达
-	8.添加txt文件读取方法
-更改日期: 2023-01-10
-***************************************************************/

OrbbecSDK_ROS2/.clang-format

@@ -1,167 +0,0 @@
----
-Language:        Cpp
-BasedOnStyle:  Google
-AccessModifierOffset: -1
-AlignAfterOpenBracket: Align
-AlignConsecutiveMacros: false
-AlignConsecutiveAssignments: false
-AlignConsecutiveDeclarations: false
-AlignEscapedNewlines: Left
-AlignOperands:   true
-AlignTrailingComments: true
-AllowAllArgumentsOnNextLine: true
-AllowAllConstructorInitializersOnNextLine: true
-AllowAllParametersOfDeclarationOnNextLine: true
-AllowShortBlocksOnASingleLine: Never
-AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: All
-AllowShortLambdasOnASingleLine: All
-AllowShortIfStatementsOnASingleLine: WithoutElse
-AllowShortLoopsOnASingleLine: true
-AlwaysBreakAfterDefinitionReturnType: None
-AlwaysBreakAfterReturnType: None
-AlwaysBreakBeforeMultilineStrings: true
-AlwaysBreakTemplateDeclarations: Yes
-BinPackArguments: true
-BinPackParameters: true
-BraceWrapping:
-  AfterCaseLabel:  false
-  AfterClass:      false
-  AfterControlStatement: false
-  AfterEnum:       false
-  AfterFunction:   false
-  AfterNamespace:  false
-  AfterObjCDeclaration: false
-  AfterStruct:     false
-  AfterUnion:      false
-  AfterExternBlock: false
-  BeforeCatch:     false
-  BeforeElse:      false
-  IndentBraces:    false
-  SplitEmptyFunction: true
-  SplitEmptyRecord: true
-  SplitEmptyNamespace: true
-BreakBeforeBinaryOperators: None
-BreakBeforeBraces: Attach
-BreakBeforeInheritanceComma: false
-BreakInheritanceList: BeforeColon
-BreakBeforeTernaryOperators: true
-BreakConstructorInitializersBeforeComma: false
-BreakConstructorInitializers: BeforeColon
-BreakAfterJavaFieldAnnotations: false
-BreakStringLiterals: true
-ColumnLimit:     100
-CommentPragmas:  '^ IWYU pragma:'
-CompactNamespaces: false
-ConstructorInitializerAllOnOneLineOrOnePerLine: true
-ConstructorInitializerIndentWidth: 4
-ContinuationIndentWidth: 4
-Cpp11BracedListStyle: true
-DeriveLineEnding: true
-DerivePointerAlignment: true
-DisableFormat:   false
-ExperimentalAutoDetectBinPacking: false
-FixNamespaceComments: true
-ForEachMacros:
-  - foreach
-  - Q_FOREACH
-  - BOOST_FOREACH
-IncludeBlocks:   Regroup
-IncludeCategories:
-  - Regex:           '^<ext/.*\.h>'
-    Priority:        2
-    SortPriority:    0
-  - Regex:           '^<.*\.h>'
-    Priority:        1
-    SortPriority:    0
-  - Regex:           '^<.*'
-    Priority:        2
-    SortPriority:    0
-  - Regex:           '.*'
-    Priority:        3
-    SortPriority:    0
-IncludeIsMainRegex: '([-_](test|unittest))?$'
-IncludeIsMainSourceRegex: ''
-IndentCaseLabels: true
-IndentGotoLabels: true
-IndentPPDirectives: None
-IndentWidth:     2
-IndentWrappedFunctionNames: false
-JavaScriptQuotes: Leave
-JavaScriptWrapImports: true
-KeepEmptyLinesAtTheStartOfBlocks: false
-MacroBlockBegin: ''
-MacroBlockEnd:   ''
-MaxEmptyLinesToKeep: 1
-NamespaceIndentation: None
-ObjCBinPackProtocolList: Never
-ObjCBlockIndentWidth: 2
-ObjCSpaceAfterProperty: false
-ObjCSpaceBeforeProtocolList: true
-PenaltyBreakAssignment: 2
-PenaltyBreakBeforeFirstCallParameter: 1
-PenaltyBreakComment: 300
-PenaltyBreakFirstLessLess: 120
-PenaltyBreakString: 1000
-PenaltyBreakTemplateDeclaration: 10
-PenaltyExcessCharacter: 1000000
-PenaltyReturnTypeOnItsOwnLine: 200
-PointerAlignment: Left
-RawStringFormats:
-  - Language:        Cpp
-    Delimiters:
-      - cc
-      - CC
-      - cpp
-      - Cpp
-      - CPP
-      - 'c++'
-      - 'C++'
-    CanonicalDelimiter: ''
-    BasedOnStyle:    google
-  - Language:        TextProto
-    Delimiters:
-      - pb
-      - PB
-      - proto
-      - PROTO
-    EnclosingFunctions:
-      - EqualsProto
-      - EquivToProto
-      - PARSE_PARTIAL_TEXT_PROTO
-      - PARSE_TEST_PROTO
-      - PARSE_TEXT_PROTO
-      - ParseTextOrDie
-      - ParseTextProtoOrDie
-    CanonicalDelimiter: ''
-    BasedOnStyle:    google
-ReflowComments:  true
-SortIncludes:    false
-SortUsingDeclarations: true
-SpaceAfterCStyleCast: false
-SpaceAfterLogicalNot: false
-SpaceAfterTemplateKeyword: true
-SpaceBeforeAssignmentOperators: true
-SpaceBeforeCpp11BracedList: false
-SpaceBeforeCtorInitializerColon: true
-SpaceBeforeInheritanceColon: true
-SpaceBeforeParens: ControlStatements
-SpaceBeforeRangeBasedForLoopColon: true
-SpaceInEmptyBlock: false
-SpaceInEmptyParentheses: false
-SpacesBeforeTrailingComments: 2
-SpacesInAngles:  false
-SpacesInConditionalStatement: false
-SpacesInContainerLiterals: true
-SpacesInCStyleCastParentheses: false
-SpacesInParentheses: false
-SpacesInSquareBrackets: false
-SpaceBeforeSquareBrackets: false
-Standard:        Auto
-StatementMacros:
-  - Q_UNUSED
-  - QT_REQUIRE_VERSION
-TabWidth:        8
-UseCRLF:         false
-UseTab:          Never
-...

OrbbecSDK_ROS2/.make_deb.sh

@@ -1,13 +0,0 @@
-#!/usr/bin/bash
-set -e
-CURR_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd -P)"
-sub_dir=(
-  orbbec_camera_msgs
-  orbbec_camera
-)
-
-for pkg in "${sub_dir[@]}"; do
-  cd "${pkg}"
-  bash .make_deb.sh
-  cd "${CURR_DIR}"
-done

OrbbecSDK_ROS2/LICENSE

@@ -1,201 +0,0 @@
-                                 Apache License
-                           Version 2.0, January 2004
-                        http://www.apache.org/licenses/
-
-   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
-
-   1. Definitions.
-
-      "License" shall mean the terms and conditions for use, reproduction,
-      and distribution as defined by Sections 1 through 9 of this document.
-
-      "Licensor" shall mean the copyright owner or entity authorized by
-      the copyright owner that is granting the License.
-
-      "Legal Entity" shall mean the union of the acting entity and all
-      other entities that control, are controlled by, or are under common
-      control with that entity. For the purposes of this definition,
-      "control" means (i) the power, direct or indirect, to cause the
-      direction or management of such entity, whether by contract or
-      otherwise, or (ii) ownership of fifty percent (50%) or more of the
-      outstanding shares, or (iii) beneficial ownership of such entity.
-
-      "You" (or "Your") shall mean an individual or Legal Entity
-      exercising permissions granted by this License.
-
-      "Source" form shall mean the preferred form for making modifications,
-      including but not limited to software source code, documentation
-      source, and configuration files.
-
-      "Object" form shall mean any form resulting from mechanical
-      transformation or translation of a Source form, including but
-      not limited to compiled object code, generated documentation,
-      and conversions to other media types.
-
-      "Work" shall mean the work of authorship, whether in Source or
-      Object form, made available under the License, as indicated by a
-      copyright notice that is included in or attached to the work
-      (an example is provided in the Appendix below).
-
-      "Derivative Works" shall mean any work, whether in Source or Object
-      form, that is based on (or derived from) the Work and for which the
-      editorial revisions, annotations, elaborations, or other modifications
-      represent, as a whole, an original work of authorship. For the purposes
-      of this License, Derivative Works shall not include works that remain
-      separable from, or merely link (or bind by name) to the interfaces of,
-      the Work and Derivative Works thereof.
-
-      "Contribution" shall mean any work of authorship, including
-      the original version of the Work and any modifications or additions
-      to that Work or Derivative Works thereof, that is intentionally
-      submitted to Licensor for inclusion in the Work by the copyright owner
-      or by an individual or Legal Entity authorized to submit on behalf of
-      the copyright owner. For the purposes of this definition, "submitted"
-      means any form of electronic, verbal, or written communication sent
-      to the Licensor or its representatives, including but not limited to
-      communication on electronic mailing lists, source code control systems,
-      and issue tracking systems that are managed by, or on behalf of, the
-      Licensor for the purpose of discussing and improving the Work, but
-      excluding communication that is conspicuously marked or otherwise
-      designated in writing by the copyright owner as "Not a Contribution."
-
-      "Contributor" shall mean Licensor and any individual or Legal Entity
-      on behalf of whom a Contribution has been received by Licensor and
-      subsequently incorporated within the Work.
-
-   2. Grant of Copyright License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      copyright license to reproduce, prepare Derivative Works of,
-      publicly display, publicly perform, sublicense, and distribute the
-      Work and such Derivative Works in Source or Object form.
-
-   3. Grant of Patent License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      (except as stated in this section) patent license to make, have made,
-      use, offer to sell, sell, import, and otherwise transfer the Work,
-      where such license applies only to those patent claims licensable
-      by such Contributor that are necessarily infringed by their
-      Contribution(s) alone or by combination of their Contribution(s)
-      with the Work to which such Contribution(s) was submitted. If You
-      institute patent litigation against any entity (including a
-      cross-claim or counterclaim in a lawsuit) alleging that the Work
-      or a Contribution incorporated within the Work constitutes direct
-      or contributory patent infringement, then any patent licenses
-      granted to You under this License for that Work shall terminate
-      as of the date such litigation is filed.
-
-   4. Redistribution. You may reproduce and distribute copies of the
-      Work or Derivative Works thereof in any medium, with or without
-      modifications, and in Source or Object form, provided that You
-      meet the following conditions:
-
-      (a) You must give any other recipients of the Work or
-          Derivative Works a copy of this License; and
-
-      (b) You must cause any modified files to carry prominent notices
-          stating that You changed the files; and
-
-      (c) You must retain, in the Source form of any Derivative Works
-          that You distribute, all copyright, patent, trademark, and
-          attribution notices from the Source form of the Work,
-          excluding those notices that do not pertain to any part of
-          the Derivative Works; and
-
-      (d) If the Work includes a "NOTICE" text file as part of its
-          distribution, then any Derivative Works that You distribute must
-          include a readable copy of the attribution notices contained
-          within such NOTICE file, excluding those notices that do not
-          pertain to any part of the Derivative Works, in at least one
-          of the following places: within a NOTICE text file distributed
-          as part of the Derivative Works; within the Source form or
-          documentation, if provided along with the Derivative Works; or,
-          within a display generated by the Derivative Works, if and
-          wherever such third-party notices normally appear. The contents
-          of the NOTICE file are for informational purposes only and
-          do not modify the License. You may add Your own attribution
-          notices within Derivative Works that You distribute, alongside
-          or as an addendum to the NOTICE text from the Work, provided
-          that such additional attribution notices cannot be construed
-          as modifying the License.
-
-      You may add Your own copyright statement to Your modifications and
-      may provide additional or different license terms and conditions
-      for use, reproduction, or distribution of Your modifications, or
-      for any such Derivative Works as a whole, provided Your use,
-      reproduction, and distribution of the Work otherwise complies with
-      the conditions stated in this License.
-
-   5. Submission of Contributions. Unless You explicitly state otherwise,
-      any Contribution intentionally submitted for inclusion in the Work
-      by You to the Licensor shall be under the terms and conditions of
-      this License, without any additional terms or conditions.
-      Notwithstanding the above, nothing herein shall supersede or modify
-      the terms of any separate license agreement you may have executed
-      with Licensor regarding such Contributions.
-
-   6. Trademarks. This License does not grant permission to use the trade
-      names, trademarks, service marks, or product names of the Licensor,
-      except as required for reasonable and customary use in describing the
-      origin of the Work and reproducing the content of the NOTICE file.
-
-   7. Disclaimer of Warranty. Unless required by applicable law or
-      agreed to in writing, Licensor provides the Work (and each
-      Contributor provides its Contributions) on an "AS IS" BASIS,
-      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-      implied, including, without limitation, any warranties or conditions
-      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-      PARTICULAR PURPOSE. You are solely responsible for determining the
-      appropriateness of using or redistributing the Work and assume any
-      risks associated with Your exercise of permissions under this License.
-
-   8. Limitation of Liability. In no event and under no legal theory,
-      whether in tort (including negligence), contract, or otherwise,
-      unless required by applicable law (such as deliberate and grossly
-      negligent acts) or agreed to in writing, shall any Contributor be
-      liable to You for damages, including any direct, indirect, special,
-      incidental, or consequential damages of any character arising as a
-      result of this License or out of the use or inability to use the
-      Work (including but not limited to damages for loss of goodwill,
-      work stoppage, computer failure or malfunction, or any and all
-      other commercial damages or losses), even if such Contributor
-      has been advised of the possibility of such damages.
-
-   9. Accepting Warranty or Additional Liability. While redistributing
-      the Work or Derivative Works thereof, You may choose to offer,
-      and charge a fee for, acceptance of support, warranty, indemnity,
-      or other liability obligations and/or rights consistent with this
-      License. However, in accepting such obligations, You may act only
-      on Your own behalf and on Your sole responsibility, not on behalf
-      of any other Contributor, and only if You agree to indemnify,
-      defend, and hold each Contributor harmless for any liability
-      incurred by, or claims asserted against, such Contributor by reason
-      of your accepting any such warranty or additional liability.
-
-   END OF TERMS AND CONDITIONS
-
-   APPENDIX: How to apply the Apache License to your work.
-
-      To apply the Apache License to your work, attach the following
-      boilerplate notice, with the fields enclosed by brackets "[]"
-      replaced with your own identifying information. (Don't include
-      the brackets!)  The text should be enclosed in the appropriate
-      comment syntax for the file format. We also recommend that a
-      file or class name and description of purpose be included on the
-      same "printed page" as the copyright notice for easier
-      identification within third-party archives.
-
-   Copyright [2023] [Orbbec 3D Technology, Inc]
-
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.

OrbbecSDK_ROS2/NOTICE

@@ -1,50 +0,0 @@
-This project uses code from the following third-party projects, listed here
-with the full text of their respective licenses.
-
-JSON for Modern C++ (BSD 2 Clause) - https://github.com/nlohmann/json
-===============================================================================
-MIT License 
-
-Copyright (c) 2013-2022 Niels Lohmann
-
-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.
-
-Magic Enum C++ (MIT License) - https://github.com/Neargye/magic_enum
-===============================================================================
-MIT License
-
-Copyright (c) 2019 - 2022 Daniil Goncharov
-
-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.

OrbbecSDK_ROS2/README.MD

@@ -1,903 +0,0 @@
-# OrbbecSDK ROS2 Wrapper
-
-> [!IMPORTANT]
->
-> Welcome to the OrbbecSDK ROS2 Wrapper. Before you begin using this version of ROS2 wrapper, it's crucial to check the following [device support list ](#supported-devices)to verify the compatibility.
-
-OrbbecSDK ROS2 Wrapper provides seamless integration of Orbbec cameras with ROS 2 environment. It supports ROS2 Foxy, Humble, and Jazzy distributions.
-
-With a major update in October 2024, we release the [OrbbecSDK ROS2 Wrapper v2](https://github.com/orbbec/OrbbecSDK_ROS2/tree/v2-main) connected to the open source [OrbbecSDK v2](https://github.com/orbbec/OrbbecSDK_v2/releases) with enhanced flexibility and extensibility. This update ensures compatibility with all Orbbec USB products adhering to UVC standard. However, it no longer supports Orbbec's traditional OpenNI protocol devices. We strongly encourage you to use the v2-main branch if your device is supported.
-
-Here is the device support list of main branch (v1.x) and v2-main branch (v2.x):
-
-<table border="1" style="border-collapse: collapse; text-align: left; width: 100%;">
-  <thead>
-    <tr style="background-color: #1f4e78; color: white; text-align: center;">
-      <th>Product Series</th>
-      <th>Product</th>
-      <th><a href="https://github.com/orbbec/OrbbecSDK_ROS2/tree/main" style="color: black; text-decoration: none;">Branch main</a></th>
-      <th><a href="https://github.com/orbbec/OrbbecSDK_ROS2/tree/v2-main" style="color: black; text-decoration: none;">Branch v2-main</a></th>
-    </tr>
-  </thead>
-  <tbody>
-    <tr>
-      <td rowspan="8" style="text-align: center; font-weight: bold;">Gemini 330</td>
-      <td>Gemini 335</td>
-      <td>full maintenance</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Gemini 336</td>
-      <td>full maintenance</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Gemini 330</td>
-      <td>full maintenance</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Gemini 335L</td>
-      <td>full maintenance</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Gemini 336L</td>
-      <td>full maintenance</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Gemini 330L</td>
-      <td>full maintenance</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Gemini 335Lg</td>
-      <td>not supported</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Gemini 335Le</td>
-      <td>not supported</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td rowspan="3" style="text-align: center; font-weight: bold;">Gemini 2</td>
-      <td>Gemini 2</td>
-      <td>full maintenance</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Gemini 2 L</td>
-      <td>full maintenance</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Gemini 2 XL</td>
-      <td>recommended for new designs</td>
-      <td>to be supported</td>
-    </tr>
-    <tr>
-      <td rowspan="3" style="text-align: center; font-weight: bold;">Femto</td>
-      <td>Femto Bolt</td>
-      <td>full maintenance</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Femto Mega</td>
-      <td>full maintenance</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Femto Mega I</td>
-      <td>full maintenance</td>
-      <td>to be supported</td>
-    </tr>
-    <tr>
-      <td rowspan="3" style="text-align: center; font-weight: bold;">Astra</td>
-      <td>Astra 2</td>
-      <td>full maintenance</td>
-      <td>recommended for new designs</td>
-    </tr>
-    <tr>
-      <td>Astra+</td>
-      <td>limited maintenance</td>
-      <td>not supported</td>
-    </tr>
-    <tr>
-      <td>Astra Pro Plus</td>
-      <td>limited maintenance</td>
-      <td>not supported</td>
-    </tr>
-    <tr>
-      <td style="text-align: center; font-weight: bold;">Astra Mini</td>
-      <td>Astra Mini Pro</td>
-      <td>full maintenance</td>
-      <td>not supported</td>
-    </tr>
-  </tbody>
-</table>
-
-**Note**: If you do not find your device, please contact our FAE or sales representative for help.
-
-**Definition**:
-
-1. recommended for new designs: we will provide full supports with new features,  bug fix and performance optimization;
-2. full maintenance: we will provide bug fix support;
-3. limited maintenance: we will provide critical bug fix support;
-4. not supported: we will not support specific device in this version;
-5. to be supported: we will add support in the near future.
-
-## Table of Contents
-
-- [OrbbecSDK ROS2 Wrapper](#orbbecsdk-ros2-wrapper)
-  - [Table of Contents](#table-of-contents)
-  - [Installation Instructions](#installation-instructions)
-  - [Getting start](#getting-start)
-  - [Efficient intra-process communication:](#efficient-intra-process-communication)
-    - [Example](#example)
-      - [Manually loading multiple components into the same process](#manually-loading-multiple-components-into-the-same-process)
-      - [Using a launch file](#using-a-launch-file)
-    - [Limitations](#limitations)
-  - [Use V4L2 backend](#use-v4l2-backend)
-  - [Launch parameters](#launch-parameters)
-  - [Predefined presets](#predefined-presets)
-  - [Depth work mode switch](#depth-work-mode-switch)
-  - [Configuration of depth NFOV and WFOV modes](#configuration-of-depth-nfov-and-wfov-modes)
-  - [All available service for camera control](#all-available-service-for-camera-control)
-  - [All available topics](#all-available-topics)
-  - [Network device enumeration](#network-device-enumeration)
-  - [Multi-Camera](#multi-camera)
-  - [Compressed Image](#compressed-image)
-  - [Use hardware decoder to decode JPEG](#use-hardware-decoder-to-decode-jpeg)
-    - [rockchip and Amlogic](#rockchip-and-amlogic)
-    - [Nvidia Jetson](#nvidia-jetson)
-  - [Check which profiles the camera supports](#check-which-profiles-the-camera-supports)
-  - [Building a Debian Package](#building-a-debian-package)
-    - [Preparing the Environment](#preparing-the-environment)
-    - [Configuring ROS Dependencies](#configuring-ros-dependencies)
-    - [Building the Package](#building-the-package)
-  - [Supported Devices](#supported-devices)
-  - [DDS Tuning](#dds-tuning)
-  - [Frequently Asked Questions](#frequently-asked-questions)
-    - [Unexpected Crash](#unexpected-crash)
-    - [No Data Stream from Multiple Cameras](#no-data-stream-from-multiple-cameras)
-    - [Additional Troubleshooting](#additional-troubleshooting)
-    - [Why Are There So Many Launch Files?](#why-are-there-so-many-launch-files)
-  - [Other useful links](#other-useful-links)
-  - [License](#license)
-
-## Installation Instructions
-
-Install ROS 2
-
-- Please refer to the
-  official [ROS 2 installation guide](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html)
-  guidance
-
-> If your ROS 2 command does not auto-complete, put the following two lines into your `.bashrc`
-> or `.zshrc`
-
-```bash
-eval "$(register-python-argcomplete3 ros2)"
-eval "$(register-python-argcomplete3 colcon)"
-```
-
-Create `colcon` workspace
-
-```bash
-mkdir -p ~/ros2_ws/src
-```
-
-Get source code
-
-```bash
-cd ~/ros2_ws/src
-git clone https://github.com/orbbec/OrbbecSDK_ROS2.git
-```
-
-Install deb dependencies
-
-```bash
-# assume you have sourced ROS environment, same blow
-sudo apt install libgflags-dev nlohmann-json3-dev  \
-ros-$ROS_DISTRO-image-transport  ros-${ROS_DISTRO}-image-transport-plugins ros-${ROS_DISTRO}-compressed-image-transport \
-ros-$ROS_DISTRO-image-publisher ros-$ROS_DISTRO-camera-info-manager \
-ros-$ROS_DISTRO-diagnostic-updater ros-$ROS_DISTRO-diagnostic-msgs ros-$ROS_DISTRO-statistics-msgs \
-ros-$ROS_DISTRO-backward-ros libdw-dev
-```
-
-Install udev rules.
-
-```bash
-cd  ~/ros2_ws/src/OrbbecSDK_ROS2/orbbec_camera/scripts
-sudo bash install_udev_rules.sh
-sudo udevadm control --reload-rules && sudo udevadm trigger
-```
-
-## Getting start
-
-```bash
-cd ~/ros2_ws/
-# build release, Default is Debug
-colcon build --event-handlers  console_direct+  --cmake-args  -DCMAKE_BUILD_TYPE=Release
-```
-
-Launch camera node
-
-- On terminal 1
-
-```bash
-. ./install/setup.bash
-ros2 launch orbbec_camera astra.launch.py # or other launch file, see below table
-```
-
-- On terminal 2
-
-```bash
-. ./install/setup.bash
-rviz2
-```
-
-Select the topic you want to display
-
-- List topics / services/ parameters ( on terminal 3)
-
-```bash
-ros2 topic list
-ros2 service list
-ros2 param list
-```
-
-- Get device info
-
-```bash
-ros2 service call /camera/get_device_info orbbec_camera_msgs/srv/GetDeviceInfo '{}'
-```
-
-- Get SDK version
-
-```bash
-ros2 service call /camera/get_sdk_version orbbec_camera_msgs/srv/GetString '{}'
-
-```
-
-- Get exposure
-
-```bash
-ros2 service call /camera/get_color_exposure orbbec_camera_msgs/srv/GetInt32 '{}'
-```
-
-> If your check `ir` or `depth`, please change `/camera/get_color_exposure`
-> to `/camera/get_ir_exposure` or `/camera/get_depth_exposure`, Same below.
-
-- Get gain
-
-```bash
-ros2 service call /camera/get_color_gain orbbec_camera_msgs/srv/GetInt32 '{}'
-```
-
-- Get white balance
-
-```bash
-ros2 service call /camera/get_white_balance orbbec_camera_msgs/srv/GetInt32 '{}'
-```
-
-- Set auto exposure
-
-```bash
-ros2 service call /camera/set_color_auto_exposure std_srvs/srv/SetBool '{data: false}'
-```
-
-- Set white balance
-
-```bash
-ros2 service call /camera/set_white_balance orbbec_camera_msgs/srv/SetInt32 '{data: 4600}'
-```
-
-- Set laser enable
-
-```bash
-ros2 service call  /camera/set_laser_enable std_srvs/srv/SetBool "{data: true}"
-```
-
-- toggle sensor
-
-```bash
-ros2 service call /camera/toggle_ir std_srvs/srv/SetBool "{data : true}"
-
-```
-
-- save point cloud
-
-```bash
-ros2 service call /camera/save_point_cloud std_srvs/srv/Empty "{}"
-```
-
-## Efficient intra-process communication:
-
-Our ROS2 Wrapper node supports zero-copy communications if loaded in the same process as a subscriber node. This can
-reduce copy times on image/pointcloud topics, especially with big frame resolutions and high FPS.
-
-You will need to launch a component container and launch our node as a component together with other component nodes.
-Further details on "Composing multiple nodes in a single process" can be
-found [here](https://docs.ros.org/en/rolling/Tutorials/Composition.html).
-
-Further details on efficient intra-process communication can be
-found [here](https://docs.ros.org/en/humble/Tutorials/Intra-Process-Communication.html#efficient-intra-process-communication).
-
-### Example
-
-#### Manually loading multiple components into the same process
-
-* Start the component:
-
-  ```bash
-  ros2 run rclcpp_components component_container
-  ```
-* Add the wrapper:
-
-  ```bash
-  ros2 component load /ComponentManager orbbec_camera orbbec_camera::OBCameraNodeDriver -e use_intra_process_comms:=true
-  ```
-
-  Load other component nodes (consumers of the wrapper topics) in the same way.
-
-#### Using a launch file
-
-```bash
-ros2 launch orbbec_camera gemini_intra_process_demo_launch.py
-```
-
-### Limitations
-
-* Node components are currently not supported on RCLPY
-* Compressed images using `image_transport` will be disabled as this isn't supported with intra-process communication
-
-## Use V4L2 backend
-
-To enable the V4L2 backend for the Gemini2 series cameras, follow these steps:
-
-1. The Gemini2 series cameras support the V4L2 backend.
-2. Open the `config/OrbbecSDKConfig_v1.0.xml` file.
-3. Set the navigation option to `LinuxUVCBackend`.
-4. Change the backend setting to `V4L2`.
-
-Note: The V4L2 backend is not enabled by default.
-
-## Launch parameters
-
-The following are the launch parameters available:
-
-- `connection_delay`: The delay time in milliseconds for reopening the device. Some devices, such as Astra mini, require
-  a longer time to initialize and reopening the device immediately can cause firmware crashes when hot plugging.
-- `enable_point_cloud`: Enables the point cloud.
-- `enable_colored_point_cloud`: Enables the RGB point cloud.
-- `point_cloud_qos`, `[color|depth|ir]_qos`, `[color|depth|ir]_camera_info_qos`: ROS 2 Message Quality of Service (QoS)
-  settings. The possible values
-  are `SYSTEM_DEFAULT`, `DEFAULT`, `PARAMETER_EVENTS`, `SERVICES_DEFAULT`, `PARAMETERS`, `SENSOR_DATA` and are
-  case-insensitive. These correspond
-  to `rmw_qos_profile_system_default`, `rmw_qos_profile_default`, `rmw_qos_profile_parameter_events`,
-  `rmw_qos_profile_services_default`, `rmw_qos_profile_parameters`,
-  and `SENSOR_DATA`, respectively.
-- `color_ae_roi_[left|right|top|bottom],depth_ae_roi_[left|right|top|bottom]`:Set Color and Depth auto exposure ROI.
-- `enable_d2c_viewer`: Publishes the D2C overlay image (for testing only).
-- `device_num`: The number of devices. This must be filled in if multiple cameras are required.
-- `color_width`, `color_height`, `color_fps`: The resolution and frame rate of the color stream.
-- `ir_width`, `ir_height`, `ir_fps`: The resolution and frame rate of the IR stream.
-- `depth_width`, `depth_height`, `depth_fps`: The resolution and frame rate of the depth stream.
-- `enable_color`: Enables the RGB camera.
-- `enable_depth`: Enables the depth camera.
-- `enable_ir`: Enables the IR camera.
-- `depth_registration`: Enables alignment of the depth frame to the color frame. This field is required when
-  the `enable_colored_point_cloud` is set to `true`.
-- `usb_port`: The USB port of the camera. This is required when multiple cameras are used.
-- `enable_accel`: Enables the accelerometer.
-- `accel_rate`: The frequency of the accelerometer, the optional values
-  are `1.5625hz`, `3.125hz`, `6.25hz`, `12.5hz`, `25hz`, `50hz`, `100hz`, `200hz`, `500hz`, `1khz`, `2khz`, `4khz`,
-  `8khz`, `16khz`, `32khz`.
-  The specific value depends on the current camera.
-- `accel_range`: The range of the accelerometer, the optional values are `2g`, `4g`, `8g`, `16g`. The specific value
-  depends on the current camera.
-- `enable_gyro`: Enables the gyroscope.
-- `gyro_rate`: The frequency of the gyroscope, the optional values
-  are `1.5625hz`, `3.125hz`, `6.25hz`, `12.5hz`, `25hz`, `50hz`, `100hz`, `200hz`, `500hz`, `1khz`, `2khz`, `4khz`,
-  `8khz`, `16khz`, `32khz`.
-  The specific value depends on the current camera.
-- `gyro_range`: The range of the gyroscope, the optional values
-  are `16dps`, `31dps`, `62dps`, `125dps`, `250dps`, `500dps`, `1000dps`, `2000dps`. The specific value depends on the
-  current camera.
-- `enumerate_net_device`: Enables the function of enumerating network devices. True means enabled, false means disabled.
-  This feature is only supported by Femto Mega and Gemini 2 XL devices. When accessing these devices through the
-  network, the IP address of the device needs to be configured in advance. The enable switch needs to be set to true.
-- `depth_filter_config`: Configures the loading path for the depth filtering configuration file. By default, the depth
-  filtering configuration file is located in the /config/depthfilter directory. Supported only on Gemini2.
-- `depth_precision`: The depth precision should be in the format `1mm`. The default value is `1mm`.
-- `enable_laser`: Enables the laser. The default value is `true`.
-- `laser_on_off_mode`: Laser on/off alternate mode, 0: off, 1: on-off alternate, 2: off-on alternate. The default value
-  is `0`.
-- `laser_energy_level`: Laser energy level.
-- `device_preset`: The default value is `Default`. Only the G330 series is supported. For more information, refer to
-  the [G330 documentation](https://www.orbbec.com/docs/g330-use-depth-presets/). Please refer to the table below to set
-  the `device_preset` value based on your use case. The value should be one of the preset names
-  listed [in the table](#predefined-presets).
-- `enable_decimation_filter`: This filter effectively reduces the depth scene complexity. The filter runs on kernel
-  sizes [2x2] to [8x8] pixels. The image size is scaled down proportionally in both dimensions to preserve the aspect
-  ratio.
-- `enable_hdr_merge`: This filter is used jointly with the depth HDR function. By merging consecutive depth images of
-  alternating exposure values, we can overcome challenges in acquiring depth values for under-illuminated and
-  over-illuminated objects simultaneously.
-- `enable_sequence_id_filter`: This filter is used jointly with the depth HDR function and outputs only the sequence
-  with the specified sequence ID.
-- `enable_threshold_filter`: This filter preserves depth values of interest and omits depth values out of scope.
-- `enable_noise_removal_filter`: This filter removes speckle noise in clusters and gives rise to a less-filled depth
-  map.
-- `enable_spatial_filter`: This filter performs multiple iterations of processing as specified by the magnitude
-  parameter to enhance the smoothness of depth data. It is also capable of filling small holes in depth maps.
-- `enable_temporal_filter`: This filter is intended to improve the depth data persistency by manipulating per-pixel
-  values based on previous frames. The filter performs a single pass on the data, adjusting the depth values while also
-  updating the tracking history.
-- `enable_hole_filling_filter`: This filter fills all holes in the depth map using the specified mode.
-- `retry_on_usb3_detection_failure`: If the camera is connected to a USB 2.0 port and is not detected, the system will
-  attempt to reset the camera up to three times. This setting aims to prevent USB 3.0 devices from being incorrectly
-  recognized as USB 2.0. It is recommended to set this parameter to `false` when using a USB 2.0 connection to avoid
-  unnecessary resets.
-- `tf_publish_rate`: The rate at which the camera publishes dynamic transforms. The default value is `0.0`, which means
-  static transforms are published.
-- `time_domain`: The frame time domain, string type, can be `device`, `global`, or `system`. `device` means using the
-  hardware timestamp from the camera,
-  `system` means using the timestamp when the PC received the first packet of data or frame, and `global` is used for
-  synchronized time across multiple
-  devices, aligning data from different sources to a common time base.
-- `enable_sync_host_time`: Enables synchronization of the host time with the camera time. The default value is `true`,
-  if
-  use global time, set to `false`. Some old devices may not support this feature.
-- `config_file_path`: The path to the YAML configuration file. The default value is `""`. If the configuration file is
-  not specified,
-  the default parameters from the launch file will be used. If you want to use a custom configuration file, please refer
-  to `gemini_330_series.launch.py`.
-  `enable_heartbeat` enables the heartbeat function, which is set to `false` by default. If set to `true`, the camera
-  node will send heartbeat signals to
-  the firmware, and if hardware logging is desired, it should also be set to `true`.
-- `log_level` : SDK log level, the default value is `info`, the optional values are `debug`, `info`, `warn`, `error`,
-  `fatal`.
-- `enable_color_undistortion`: Enables color undistortion, the default value is `false`. Note that our color cameras
-  exhibit minimal distortion, and typically, undistortion is not necessary.
-- `color_brightness`: Color brightness.
-- `ir_brightness`: IR brightness.
-- `color_ae_max_exposure`: Color auto exposure maximum exposure.
-- `ir_ae_max_exposure`: IR auto exposure maximum exposure.
-- `enable_hardware_reset`: This option enables the hardware reset function. By default, it is set to `false`.
-  If set to `true`, the camera will reboot upon its first connection.
-
-**IMPORTANT**: *Please carefully read the instructions regarding software filtering settings
-at [this link](https://www.orbbec.com/docs/g330-use-depth-post-processing-blocks/). If you are uncertain, do not modify
-these settings.*
-
-## Predefined presets
-
-| Preset         | Features                                                                                                                                                                                                           | Recommended use cases                                                                                                                                              |
-| -------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| Default        | - Best visual perception``- Overall good performance in accuracy, fill rate, tiny objects, etc.                                                                                                                    | - Generic `<br>`- Robotics                                                                                                                                       |
-| Hand           | - Clear hand and finger edges                                                                                                                                                                                      | - Gesture recognition                                                                                                                                              |
-| High Accuracy  | - Depth of high confidence `<br>`- Barely noise depth values `<br>`- Lower fill rate                                                                                                                           | - Collision avoidance `<br>`- Object scanning                                                                                                                    |
-| High Density   | - Higher fill rate `<br>`- More tiny objects `<br>`- May suffer from noise depth values                                                                                                                        | - Object recognition `<br>`- Pick & place `<br>`- Foreground & background animation                                                                            |
-| Medium Density | - Balanced performance in fill rate and accuracy `<br>`- In comparison to Default: lower fill rate, better edge quality                                                                                          | - Generic and alternative to Default                                                                                                                               |
-| Custom         | - User defined Preset `<br>`- Derived from Presets above, with customized modifications, e.g. a new configuration for the post-processing pipeline, modified mean intensity set point of depth AE function, etc. | - Better depth performance achieved using customized configurations in comparison to using predefined presets `<br>`- For well-established custom configurations |
-
-Choose the appropriate preset name based on your specific use case and set it as the value for the `device_preset`
-parameter.
-
-## Depth work mode switch
-
-Orbbec SDK ROS 2 supports the depth work mode switch. The depth work mode switch is supported by Gemini 2, Gemini 2 L,
-and Femto and Femto Bolt cameras.
-
-- Before starting the camera, depth work mode (depth_work_mode) can be configured for the corresponding xxx.launch.py
-  file's support.
-- The depth work mode switch is supported by Gemini 2, Gemini 2 L, and Gemini 2 XL cameras.
-- The default depth work mode configuration of xxx.launch.py is the camera's default configuration. If you need to
-  modify it, you can switch to the corresponding mode as needed.
-- The specific camera depth work mode support types can be found in the comments of the depth mode.
-
-```python
-# Depth work mode support is as follows:
-# Unbinned Dense Default
-# Unbinned Sparse Default
-# Binned Sparse Default
-# Obstacle Avoidance
-DeclareLaunchArgument('depth_work_mode', default_value='')
-```
-
-- View depth work modes:
-
-```bash
-ros2 run orbbec_camera list_depth_work_mode_node
-```
-
-## Configuration of depth NFOV and WFOV modes
-
-For the Femto Mega and Femto Bolt devices, the NFOV and WFOV modes are implemented by configuring the resolution of
-Depth and IR in the launch file.
-In launch file, depth_width、depth_height、ir_width、ir_height represents the resolution of the depth and the resolution of
-the IR.
-The frame fps and resolution of IR must be consistent with the depth. The correspondence between different modes and
-resolutions is as follows:
-
-- NFOV unbinned: 640 x 576.
-- NFOV binned: 320 x 288.
-- WFOV unbinned: 1024 x 1024.
-- WFOV binned: 512 x 512.
-
-## All available service for camera control
-
-The name of the following service already expresses its function.
-However, it should be noted that the corresponding `set_[ir|depth|color]*`
-and `get[ir|depth|color]*` **services are only available if you set** `enable[ir|depth|color]`
-to `true` in the stream that corresponds to the argument of the launch file.
-
-- `/camera/get_auto_white_balance`
-- `/camera/get_color_exposure`
-- `/camera/get_color_gain`
-- `/camera/get_depth_exposure`
-- `/camera/get_depth_gain`
-- `/camera/get_device_info`
-- `/camera/get_ir_exposure`
-- `/camera/get_ir_gain`
-- `/camera/get_ldp_status`
-- `/camera/get_sdk_version`
-- `/camera/get_white_balance`
-- `/camera/set_auto_white_balance`
-- `/camera/set_color_auto_exposure`
-- `/camera/set_color_exposure`
-- `/camera/set_color_gain`
-- `/camera/set_depth_auto_exposure`
-- `/camera/set_depth_exposure`
-- `/camera/set_depth_gain`
-- `/camera/set_fan_work_mode`
-- `/camera/set_floor_enable`
-- `/camera/set_ir_auto_exposure`
-- `/camera/set_ir_exposure`
-- `/camera/set_ir_gain`
-- `/camera/set_laser_enable`
-- `/camera/set_ldp_enable`
-- `/camera/set_white_balance`
-- `/camera/toggle_color`
-- `/camera/toggle_depth`
-- `/camera/toggle_ir`
-
-## All available topics
-
-- `/camera/color/camera_info` : The color camera info.
-- `/camera/color/image_raw`: The color stream image.
-- `/camera/depth/camera_info`: The depth stream image.
-- `/camera/depth/image_raw`: The depth stream image
-- `/camera/depth/points` : The point cloud, only available when `enable_point_cloud` is `true`.
-- `/camera/depth_registered/points`: The colored point cloud, only available when `enable_colored_point_cloud`
-  is `true`.
-- `/camera/ir/camera_info`: The IR camera info.
-- `/camera/ir/image_raw`: The IR stream image
-- `/camera/accel/sample`: Acceleration data stream `enable_sync_output_accel_gyro`turned off，`enable_accel`turned on
-- `/camera/gyro/sample`: Gyroscope data stream，enable_sync_output_accel_gyro `turned off，`enable_gyro`turned on
-- `camera/gyro_accel/sample`: Synchronized data stream of acceleration and gyroscope，`enable_sync_output_accel_gyro`
-  turned on
-- `/diagnostics`: The diagnostic information of the camera, Currently, the diagnostic information only includes the
-  temperature of the camera.
-
-## Network device enumeration
-
-Currently, the network device enumeration function is supported only by the Femto Mega device. When accessing this
-device over the network, if `enumerate_net_device` is set to `true`, the device will be automatically enumerated,
-eliminating the need to configure the IP address in advance or set the enable switch to true. The specific configuration
-methods are as follows:
-
-- `enumerate_net_device`: enumeration network device automatically, only supported by Femto Mega.
-  if `enumerate_net_device` set to `true`, the device will be enumerated automatically,No need to set
-  the `net_device_ip`
-  and `net_device_port` parameters.
-- `net_device_ip`: The IP address of the device.
-- `net_device_port`: The port number of the device.
-
-## Multi-Camera
-
-- To get the `usb_port` of the camera, plug in the camera and run the following command in the terminal:
-
-```bash
-ros2 run orbbec_camera list_devices_node
-```
-
-- Set the `device_num` parameter to the number of cameras you have.
-- Go to the `OrbbecSDK_ROS2/launch/multi_xxx.launch.py` file and change the `usb_port`.
-- Don't forget to put the `include` tag inside the `group` tag.
-  Otherwise, the parameter values of different cameras may become contaminated.
-
-```python
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction, ExecuteProcess
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch_ros.actions import Node
-from ament_index_python.packages import get_package_share_directory
-import os
-
-
-def generate_launch_description():
-    # Include launch files
-    package_dir = get_package_share_directory('orbbec_camera')
-    launch_file_dir = os.path.join(package_dir, 'launch')
-    launch1_include = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'gemini2L.launch.py')
-        ),
-        launch_arguments={
-            'camera_name': 'camera_01',
-            'usb_port': '6-2.4.4.2',  # replace your usb port here
-            'device_num': '2'
-        }.items()
-    )
-
-    launch2_include = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'gemini2L.launch.py')
-        ),
-        launch_arguments={
-            'camera_name': 'camera_02',
-            'usb_port': '6-2.4.1',  # replace your usb port here
-            'device_num': '2'
-        }.items()
-    )
-
-    # If you need more cameras, just add more launch_include here, and change the usb_port and device_num
-
-    # Launch description
-    ld = LaunchDescription([
-        GroupAction([launch1_include]),
-        GroupAction([launch2_include]),
-    ])
-
-    return ld
-
-```
-
-- To launch the cameras, run the following command:
-
-```bash
-ros2 launch orbbec_camera multi_camera.launch.py
-```
-
-## Compressed Image
-
-You can use `image_transport` to compress the image using `jpeg`. Below is an example of how to use it:
-
-To access the compressed color image, you can use the following command:
-
-```bash
-ros2 topic echo /camera/color/image_raw/compressed --no-arr
-```
-
-This command will allow you to receive the compressed color image from the specified topic.
-
-## Use hardware decoder to decode JPEG
-
-### rockchip and Amlogic
-
-Depends on `rockchip-mpp-dev` and `rockchip-rga-dev`, not all systems have these two packages, the names may be
-different, please search by yourself.
-Open `CMakeLists.txt` and set `USE_RK_HW_DECODER` to `ON`.
-
-### Nvidia Jetson
-
-Depends on: `jetson_multimedia_api`,`libyuv`.
-Open `CMakeLists.txt` and set `USE_NV_HW_DECODER` to `ON`.
-
-## Check which profiles the camera supports
-
-```bash
-ros2 run orbbec_camera list_camera_profile_mode_node
-```
-
-## Building a Debian Package
-
-### Preparing the Environment
-
-Before starting, install the required tools:
-
-```bash
-sudo apt install debhelper fakeroot python3-bloom
-```
-
-### Configuring ROS Dependencies
-
-Add the following YAML file to your system at `/etc/ros/rosdep/sources.list.d/00-orbbec.yaml`. Make sure to
-replace `focal` with the codename of your Ubuntu version and `humble` with your ROS2 distribution name:
-
-```yaml
-orbbec_camera_msgs:
-  ubuntu:
-    focal: [ ros-humble-orbbec-camera-msgs ]
-```
-
-Next, create a new file `/etc/ros/rosdep/sources.list.d/50-orbbec.list` and add this line to specify the path to the
-YAML file:
-
-```bash
-yaml file:///etc/ros/rosdep/sources.list.d/00-orbbec.yaml
-```
-
-Update the rosdep database to reflect these changes:
-
-```bash
-rosdep update
-```
-
-### Building the Package
-
-Navigate to your workspace and build the project:
-
-```bash
-cd ~/ros2_ws/
-colcon build --event-handlers console_direct+ --cmake-args -DCMAKE_BUILD_TYPE=Release
-. install/setup.bash
-cd src/OrbbecSDK_ROS2/
-bash .make_deb.sh
-```
-
-## Supported Devices
-
-The following devices are supported by the OrbbecSDK ROS2 Wrapper.
-
-| Product          | **Minimal Firmware Version** | launch file                 |
-| ---------------- | ---------------------------------- | --------------------------- |
-| Gemini 330       | 1.2.20                             | gemini_330_series.launch.py |
-| Gemini 330L      | 1.2.20                             | gemini_330_series.launch.py |
-| Gemini 335       | 1.2.20                             | gemini_330_series.launch.py |
-| Gemini 335L      | 1.2.20                             | gemini_330_series.launch.py |
-| Gemini 336L      | 1.2.20                             | gemini_330_series.launch.py |
-| Femto Bolt       | 1.0.6                              | femto_bolt.launch.py        |
-| Femto Mega       | 1.1.7                              | femto_mega.launch.py        |
-| Gemini 2 XL      | Obox: V1.2.5 VL:1.4.54             | gemini2XL.launch.py         |
-| Astra 2          | 2.8.20                             | astra2.launch.py            |
-| Gemini 2 L       | 1.4.32                             | gemini2L.launch.py          |
-| Gemini 2         | 1.4.60                             | gemini2.launch.py           |
-| Astra+           | 1.0.19                             | astra_adv.launch.py         |
-| Femto            | 1.6.7                              | femto.launch.py             |
-| DaBai            | 2436                               | dabai.launch.py             |
-| DaBai DCW        | 2460                               | dabai_dcw.launch.py         |
-| DaBai DW         | 2606                               | dabai_dw.launch.py          |
-| Astra Mini Pro   | 1007                               | astra.launch.py             |
-| Gemini E         | 3460                               | gemini_e.launch.py          |
-| Gemini E Lite    | 3606                               | gemini_e_lite.launch.py     |
-| Gemini           | 3018                               | gemini.launch.py            |
-| Astra Mini S Pro | 1005                               | astra.launch.py             |
-
-> [!IMPORTANT]
->
-> We recommend using the [OrbbecSDK ROS2 Wrapper v2](https://github.com/orbbec/OrbbecSDK_ROS2/tree/v2-main) to get better flexibility and extensibility for the following devices: Astra 2, Femto Mega, Femto Bolt, Gemini 2, Gemini 2 L, Gemini 335, Gemini 335L, Gemini 336, Gemini 336L, Gemini 335Lg(only supported in v2-main branch).
-
-All launch files are essentially similar, with the primary difference being the default values of the parameters set
-for different models within the same series. Differences in USB standards, such as USB 2.0 versus USB 3.0, may require adjustments to these parameters. If you encounter a startup failure, please carefully review the specification manual. Pay special attention to the resolution settings in the launch file, as well as other parameters, to ensure compatibility and optimal performance.
-
-## DDS Tuning
-
-The default DDS settings (Galactic) may not be optimal for data transmission. Different DDS settings can have varying
-performance. In this example, we use CycloneDDS. For more detailed information, please refer to the
-[ROS DDS Tuning](https://docs.ros.org/en/humble/How-To-Guides/DDS-tuning.html)。
-
-● Edit cyclonedds configuration file
-
-```bash
-sudo gedit /etc/cyclonedds/config.xml
-```
-
-Add
-
-```xml
-<?xml version="1.0" encoding="UTF-8"?>
-<CycloneDDS xmlns="https://cdds.io/config" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
-            xsi:schemaLocation="https://cdds.io/confighttps://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd">
-    <Domain id="any">
-        <General>
-            <NetworkInterfaceAddress>lo</NetworkInterfaceAddress>
-            <AllowMulticast>false</AllowMulticast>
-        </General>
-        <Internal>
-            <MinimumSocketReceiveBufferSize>16MB</MinimumSocketReceiveBufferSize>
-        </Internal>
-        <Discovery>
-            <ParticipantIndex>auto</ParticipantIndex>
-            <MaxAutoParticipantIndex>30</MaxAutoParticipantIndex>
-            <Peers>
-                <Peer address="localhost"/>
-            </Peers>
-        </Discovery>
-    </Domain>
-</CycloneDDS>
-```
-
-● Set the environment variables, add to `.zshrc` or `.bashrc`
-
-```bash
-export ROS_DOMAIN_ID=42 # Numbers from 0 to 232
-export ROS_LOCALHOST_ONLY=1
-export CYCLONEDDS_URI=file:///etc/cyclonedds/config.xml
-```
-
-Tip：to understand why the maximum ROS_DOMAIN_ID is 232, please
-visit [The ROS DOMAIN ID](https://docs.ros.org/en/humble/Concepts/About-Domain-ID.html)
-● Increase UDP receive buffer size
-Edit
-
-```bash
-/etc/sysctl.d/10-cyclone-max.conf
-```
-
-Add
-
-```bash
-net.core.rmem_max=2147483647
-net.core.rmem_default=2147483647
-```
-
-If you use Fast DDS, you can refer to the [Fast DDS Configuration](./docs/fastdds_tuning.md) file.
-
-## Frequently Asked Questions
-
-### Unexpected Crash
-
-If the camera node crashes unexpectedly, it will generate a crash log in the current running directory:
-`Log/camera_crash_stack_trace_xx.log`.
-Please send this log to the support team or submit it to a GitHub issue for further assistance.
-
-### No Data Stream from Multiple Cameras
-
-**Insufficient Power Supply**:
-
-- Ensure that each camera is connected to a separate hub.
-- Use a powered hub to provide sufficient power to each camera.
-
-**High Resolution**:
-
-- Try lowering the resolution to resolve data stream issues.
-
-**Increase usbfs_memory_mb Value**:
-
-- Increase the `usbfs_memory_mb` value to 128MB (this is a reference value and can be adjusted based on your system’s
-  needs)
-  by running the following command:
-
-```bash
-    echo 128 | sudo tee /sys/module/usbcore/parameters/usbfs_memory_mb
-```
-
-- To make this change permanent, check [this link](https://github.com/OpenKinect/libfreenect2/issues/807).
-
-### Additional Troubleshooting
-
-- If you encounter other issues, set the `log_level` parameter to `debug`. This will generate an SDK log file in the
-  running directory: `Log/OrbbecSDK.log.txt`.
-  Please provide this file to the support team for further assistance.
-- If firmware logs are required, set `enable_heartbeat` to `true` to activate this feature.
-
-### Why Are There So Many Launch Files?
-
-- Different cameras have varying default resolutions and image formats.
-- To simplify usage, each camera has its own launch file.
-
-## Other useful links
-
-- [Orbbec SDK](https://orbbec.github.io/OrbbecSDK/)
-- [Gemini 330 depth presets](https://www.orbbec.com/docs/g330-use-depth-presets/)
-
-## License
-
-Copyright 2024 Orbbec Ltd.
-
-Licensed under the Apache License, Version 2.0 (the "License"); you may not use this project except in compliance with
-the License. You may obtain a copy of the License at
-
-[http://www.apache.org/licenses/LICENSE-2.0](http://www.apache.org/licenses/LICENSE-2.0)
-
-Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "
-AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific
-language governing permissions and limitations under the License.
-
-**Other names and brands may be claimed as the property of others**

OrbbecSDK_ROS2/README_CN.MD

@@ -1,824 +0,0 @@
-# OrbbecSDK ROS2 Wrapper
-
-> [!重要]
->
-> 欢迎使用 OrbbecSDK ROS2 Wrapper。在开始使用本版本的 ROS2 封装前，请务必查看以下的 [设备支持列表](#支持的设备)，以确认您的设备是否兼容。
-
-OrbbecSDK ROS2 封装包为 Orbbec 相机与 ROS 2 环境之间提供了无缝集成，支持 ROS2 的 Foxy、Humble 和 Jazzy 发行版。
-
-2024 年 10 月，我们发布了重大更新版本 —— [OrbbecSDK ROS2 Wrapper v2](https://github.com/orbbec/OrbbecSDK_ROS2/tree/v2-main)，该版本基于开源的[OrbbecSDK v2](https://github.com/orbbec/OrbbecSDK_v2/releases)，具有更强的灵活性与可扩展性。
-
-此更新确保了对所有符合 UVC 标准的 Orbbec USB 产品的兼容性，但不再支持使用 Orbbec 传统 OpenNI 协议的设备。如果您的设备在支持列表中，我们强烈建议您使用 v2-main 分支。
-
-以下是 main 分支（v1.x）与 v2-main 分支（v2.x）对应的设备支持列表：
-
-<table border="1" style="border-collapse: collapse; text-align: left; width: 100%;">
-  <thead>
-    <tr style="background-color: #1f4e78; color: white; text-align: center;">
-      <th>产品系列</th>
-      <th>产品</th>
-      <th><a href="https://github.com/orbbec/OrbbecSDK_ROS2/tree/main" style="color: black; text-decoration: none;">main分支</a></th>
-      <th><a href="https://github.com/orbbec/OrbbecSDK_ROS2/tree/v2-main" style="color: black; text-decoration: none;">v2-main分支</a></th>
-    </tr>
-  </thead>
-  <tbody>
-    <tr>
-      <td rowspan="8" style="text-align: center; font-weight: bold;">Gemini 330</td>
-      <td>Gemini 335</td>
-      <td>全面维护</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Gemini 336</td>
-      <td>全面维护</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Gemini 330</td>
-      <td>全面维护</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Gemini 335L</td>
-      <td>全面维护</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Gemini 336L</td>
-      <td>全面维护</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Gemini 330L</td>
-      <td>全面维护</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Gemini 335Lg</td>
-      <td>不支持</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Gemini 335Le</td>
-      <td>不支持</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td rowspan="3" style="text-align: center; font-weight: bold;">Gemini 2</td>
-      <td>Gemini 2</td>
-      <td>全面维护</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Gemini 2 L</td>
-      <td>全面维护</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Gemini 2 XL</td>
-      <td>推荐用于新设计</td>
-      <td>即将支持</td>
-    </tr>
-    <tr>
-      <td rowspan="3" style="text-align: center; font-weight: bold;">Femto</td>
-      <td>Femto Bolt</td>
-      <td>全面维护</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Femto Mega</td>
-      <td>全面维护</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Femto Mega I</td>
-      <td>全面维护</td>
-      <td>即将支持</td>
-    </tr>
-    <tr>
-      <td rowspan="3" style="text-align: center; font-weight: bold;">Astra</td>
-      <td>Astra 2</td>
-      <td>全面维护</td>
-      <td>推荐用于新设计</td>
-    </tr>
-    <tr>
-      <td>Astra+</td>
-      <td>有限的维护</td>
-      <td>不支持</td>
-    </tr>
-    <tr>
-      <td>Astra Pro Plus</td>
-      <td>有限的维护</td>
-      <td>不支持</td>
-    </tr>
-    <tr>
-      <td style="text-align: center; font-weight: bold;">Astra Mini</td>
-      <td>Astra Mini Pro</td>
-      <td>全面维护</td>
-      <td>不支持</td>
-    </tr>
-  </tbody>
-</table>
-
-注意：如果您未在列表中找到您的设备，请联系我们的 FAE 或销售代表获取支持。
-
-**定义说明**:
-
-1. 推荐用于新设计: 我们将提供全面支持，包括新功能、漏洞修复以及性能优化。
-2. 全面维护: 我们将提供漏洞修复支持。
-3. 有限的维护: 我们仅提供关键性问题的修复支持。
-4. 不支持: 本版本中不支持该设备。
-5. 即将支持: 我们将在近期添加对该设备的支持。
-
-## 目录
-
-- [OrbbecSDK ROS2 Wrapper v2](#orbbecsdk-ros2-wrapper-v2)
-  - [目录](#目录)
-  - [安装说明](#安装说明)
-  - [入门](#入门)
-  - [高效的进程内通信](#高效的进程内通信)
-    - [例子](#例子)
-      - [手动将多个组件加载到同一进程中](#手动将多个组件加载到同一进程中)
-      - [使用launch文件启动](#使用launch文件启动)
-    - [限制](#限制)
-  - [使用V4L2后端](#使用v4l2后端)
-  - [Launch参数](#launch参数)
-  - [预设presets](#预设presets)
-  - [深度工作模式开关](#深度工作模式开关)
-  - [深度 NFOV 和 WFOV 模式的配置](#深度-nfov-和-wfov-模式的配置)
-  - [所有可用的摄像机控制服务](#所有可用的摄像机控制服务)
-  - [所有可用主题](#所有可用主题)
-  - [网络相机设备枚举](#网络相机设备枚举)
-  - [多相机](#多相机)
-  - [压缩图像](#压缩图像)
-  - [使用硬件解码器解码JPEG](#使用硬件解码器解码jpeg)
-    - [rockchip and Amlogic](#rockchip-and-amlogic)
-    - [Nvidia Jetson](#nvidia-jetson)
-  - [检查相机支持哪些配置文件](#检查相机支持哪些配置文件)
-  - [构建Debian软件包](#构建debian软件包)
-    - [准备环境](#准备环境)
-    - [配置ROS依赖项](#配置ros依赖项)
-    - [构建包](#构建包)
-  - [支持的设备](#支持的设备)
-  - [DDS调优](#dds调优)
-  - [常见问题](#常见问题)
-    - [意外崩溃](#意外崩溃)
-    - [没有来自多个摄像机的数据流](#没有来自多个摄像机的数据流)
-    - [其他故障排除](#其他故障排除)
-    - [为什么有这么多启动文件？](#为什么有这么多启动文件)
-  - [其他有用链接](#其他有用链接)
-  - [License](#license)
-
-## 安装说明
-
-下载ROS2
-
-- 请根据您所使用的 ROS 2 版本，参考官方安装指南进行安装：[ROS 2 安装指南](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html)
-
-> 如果你的 ROS 2 命令无法自动补全，请将以下两行添加到你的 `.bashrc `或 `.zshrc` 文件中：
-
-```bash
-eval "$(register-python-argcomplete3 ros2)"
-eval "$(register-python-argcomplete3 colcon)"
-```
-
-创建 `colcon` 工作空间
-
-```bash
-mkdir -p ~/ros2_ws/src
-```
-
-获取OrbbecSDK_ROS2源码
-
-```bash
-cd ~/ros2_ws/src
-git clone https://github.com/orbbec/OrbbecSDK_ROS2.git
-```
-
-安装 deb 依赖项
-
-```bash
-# assume you have sourced ROS environment, same blow
-sudo apt install libgflags-dev nlohmann-json3-dev  \
-ros-$ROS_DISTRO-image-transport ros-${ROS_DISTRO}-image-transport-plugins ros-${ROS_DISTRO}-compressed-image-transport \
-ros-$ROS_DISTRO-image-publisher ros-$ROS_DISTRO-camera-info-manager \
-ros-$ROS_DISTRO-diagnostic-updater ros-$ROS_DISTRO-diagnostic-msgs ros-$ROS_DISTRO-statistics-msgs \
-ros-$ROS_DISTRO-backward-ros libdw-dev
-```
-
-安装 udev 规则
-
-```bash
-cd  ~/ros2_ws/src/OrbbecSDK_ROS2/orbbec_camera/scripts
-sudo bash install_udev_rules.sh
-sudo udevadm control --reload-rules && sudo udevadm trigger
-```
-
-## 入门
-
-```bash
-cd ~/ros2_ws/
-# build release, Default is Debug
-colcon build --event-handlers  console_direct+  --cmake-args  -DCMAKE_BUILD_TYPE=Release
-```
-
-启动相机节点
-
-- 第一个终端
-
-```bash
-. ./install/setup.bash
-ros2 launch orbbec_camera gemini_330_series.launch.py # or other launch file, see below table
-```
-
-- 第二个终端
-
-```bash
-. ./install/setup.bash
-rviz2
-```
-
-选择您想要显示的topic
-
-- 获取topics / services/ parameters列表 (第3个终端)
-
-```bash
-ros2 topic list
-ros2 service list
-ros2 param list
-```
-
-- 获取设备信息
-
-```bash
-ros2 service call /camera/get_device_info orbbec_camera_msgs/srv/GetDeviceInfo '{}'
-```
-
-- 获取SDK版本号
-
-```bash
-ros2 service call /camera/get_sdk_version orbbec_camera_msgs/srv/GetString '{}'
-
-```
-
-- 获取曝光
-
-```bash
-ros2 service call /camera/get_color_exposure orbbec_camera_msgs/srv/GetInt32 '{}'
-```
-
-> 如果你选择的是 `ir` 或 `depth` 模式，请将 `/camera/get_color_exposure` 修改为 `/camera/get_ir_exposure` 或 `/camera/get_depth_exposure`，下文同理。
-
-- 获取增益
-
-```bash
-ros2 service call /camera/get_color_gain orbbec_camera_msgs/srv/GetInt32 '{}'
-```
-
-- 获取白平衡
-
-```bash
-ros2 service call /camera/get_white_balance orbbec_camera_msgs/srv/GetInt32 '{}'
-```
-
-- 设置自动曝光
-
-```bash
-ros2 service call /camera/set_color_auto_exposure std_srvs/srv/SetBool '{data: false}'
-```
-
-* 设置自动曝光ROI区域
-
-> 在 data_param 中，第一个值表示 左侧 `Left` 设置，第二个值表示 右侧 `Right` 设置，第三个值表示 上方 `Top` 设置，第四个值表示 下方 `Bottom` 设置。
-
-```bash
-ros2 service call /camera/set_color_ae_roi orbbec_camera_msgs/srv/SetArrays '{data_param: [0,1279,0,719]}'
-```
-
-* 设置白平衡
-
-```bash
-ros2 service call /camera/set_white_balance orbbec_camera_msgs/srv/SetInt32 '{data: 4600}'
-```
-
-- 设置激光开关
-
-```bash
-ros2 service call  /camera/set_laser_enable std_srvs/srv/SetBool "{data: true}"
-```
-
-- 切换传感器状态
-
-```bash
-ros2 service call /camera/toggle_ir std_srvs/srv/SetBool "{data : true}"
-
-```
-
-- 保存点云
-
-```bash
-ros2 service call /camera/save_point_cloud std_srvs/srv/Empty "{}"
-```
-
-## 高效的进程内通信:
-
-我们的 ROS2 Wrapper 节点在与订阅节点加载到同一进程中时，支持 零拷贝通信。这可以显著减少图像或点云等大数据主题的复制次数，尤其是在高分辨率和高帧率的场景下。
-
-您需要启动一个组件容器（Component Container），并将本节点与其他组件节点一同作为组件方式加载。有关“在同一进程中组合多个节点”的更多详细信息，请参考：[官方文档说明](https://docs.ros.org/en/rolling/Tutorials/Composition.html)
-
-关于高效的进程内通信（Intra-process communication），请参考此文档：[高效的 Intra-Process通信](https://docs.ros.org/en/humble/Tutorials/Intra-Process-Communication.html#efficient-intra-process-communication)。
-
-### 例子
-
-#### 手动将多个组件加载到同一进程中
-
-* 启动 容器（component）:
-
-  ```bash
-  ros2 run rclcpp_components component_container
-  ```
-* 添加组件:
-
-  ```bash
-  ros2 component load /ComponentManager orbbec_camera orbbec_camera::OBCameraNodeDriver -e use_intra_process_comms:=true
-  ```
-
-  以相同的方式加载其他组件节点 。
-
-#### 使用launch文件启动
-
-```bash
-ros2 launch orbbec_camera gemini_intra_process_demo_launch.py
-```
-
-### 限制
-
-* Node组件目前不支持 on RCLPY
-* 使用 `image_transport`压缩图像话题将被禁止，因为这不支持进程内通信。
-
-## 使用V4L2后端
-
-要为 Gemini2 系列相机启用 V4L2 后端，请按以下步骤操作：
-
-1. Gemini2 系列相机支持 V4L2 后端。
-2. 打开 `config/OrbbecSDKConfig_v1.0.xml` 文件。
-3. 将导航选项设置为 `LinuxUVCBackend`。
-4. 将后端设置更改为 `V4L2`。
-
-注意：默认情况下未启用 V4L2 后端。
-
-## Launch参数
-
-以下是可用的launch启动参数：
-
-- `connection_delay`: 重新打开设备的延迟时间（以毫秒为单位）。
-- `enable_point_cloud`: 深度点云开关。
-- `enable_colored_point_cloud`: RGB点云开关。
-- `cloud_frame_id`:修改ros消息中frame_id名称。
-- `ordered_pc`:启动无效点云过滤开关。
-- `point_cloud_qos`, `[color|depth|ir]_qos`, `[color|depth|ir]_camera_info_qos`: ROS2消息服务质量(QoS)设置
-  可能的值包括： `SYSTEM_DEFAULT`, `DEFAULT`, `PARAMETER_EVENTS`, `SERVICES_DEFAULT`, `PARAMETERS`, `SENSOR_DATA` 并且不区分大小写。这些值分别对应： `rmw_qos_profile_system_default`, `rmw_qos_profile_default`, `rmw_qos_profile_parameter_events`, `rmw_qos_profile_services_default`, `rmw_qos_profile_parameters`,和 `SENSOR_DATA`。
-- `color_ae_roi_[left|right|top|bottom]`,`depth_ae_roi_[left|right|top|bottom]`:设置Color或Depth的自动曝光ROI区域。
-- `enable_d2c_viewer`: 发布depth图像叠加到彩色图像的D2C话题（仅提供测试）。
-- `device_num`: 设备数量。
-- `color_width`, `color_height`, `color_fps`: 彩色流的分辨率和帧率。
-- `ir_width`, `ir_height`, `ir_fps`: 红外流的分辨率和帧速率。
-- `depth_width`, `depth_height`, `depth_fps`: 深度流的分辨率和帧速率。
-- `enable_color`: 启用彩色流。
-- `enable_depth`: 启用深度流。
-- `enable_ir`: 启动红外流。
-- `depth_registration`: 启用深度帧与颜色帧的对齐。当 `enable_colored_point_cloud` 设置为 `true` 时，此字段为必填项。
-- `usb_port`: 相机的 USB 端口。使用多个相机时需要此端口。
-- `enable_accel`: 启用加速度计。
-- `accel_rate`: 加速度计的频率，可选值 `1.5625hz`, `3.125hz`, `6.25hz`, `12.5hz`, `25hz`, `50hz`, `100hz`, `200hz`, `500hz`, `1khz`, `2khz`, `4khz`, `8khz`, `16khz`, `32khz`。
-  具体值取决于当前的相机。
-- `accel_range`: 加速度计的范围，可选值为 `2g`, `4g`, `8g`, `16g`. 具体值取决于当前相机。
-- `enable_gyro`: 启用陀螺仪。
-- `gyro_rate`: 陀螺仪的频率，可选值 `1.5625hz`, `3.125hz`, `6.25hz`, `12.5hz`, `25hz`, `50hz`, `100hz`, `200hz`, `500hz`, `1khz`, `2khz`, `4khz`, `8khz`, `16khz`, `32khz`。
-  具体值取决于当前的相机。
-- `gyro_range`: 陀螺仪的范围，可选值为 `16dps`, `31dps`, `62dps`, `125dps`, `250dps`, `500dps`, `1000dps`, `2000dps`. 具体值取决于当前相机。
-- `enumerate_net_device`: 启用枚举网络设备的功能。true 表示启用，false 表示禁用。
-  此功能仅支持 Femto Mega 和 Gemini 2 XL 设备。通过网络访问这些设备时，需要提前配置设备的 IP 地址。启用开关需要设置为 true。
-- `depth_filter_config`: 配置深度过滤配置文件的加载路径。默认情况下，深度过滤配置文件位于 /config/depthfilter 目录。仅 Gemini2 支持。
-- `depth_precision`: 深度精度应采用 `1mm`格式。默认值为 `1mm`。
-- `enable_laser`: 启用激光。默认值为 `true`。
-- `device_preset`: 默认值为 `Default`。仅支持 G330 系列。更多信息，请参阅 [G330 文档](https://www.orbbec.com/docs/g330-use-depth-presets/)。请参考下表，根据您的使用情况设置 `device_preset` 值。该值应为[表格](#预设presets) 中列出的预设名称之一。
-- `enable_decimation_filter`: 此滤波器有效降低了深度场景的复杂度。滤波器在核尺寸 [2x2] 至 [8x8] 像素上运行。图像尺寸在两个维度上均按比例缩小，以保持纵横比。
-- `enable_hdr_merge`: 此滤镜与深度 HDR 功能配合使用。通过合并交替曝光值的连续深度图像，我们可以克服同时获取欠照和过照物体深度值的挑战。
-- `enable_sequence_id_filter`: 此过滤器与深度 HDR 函数联合使用，仅输出具有指定序列 ID 的序列。
-- `enable_threshold_filter`: 该过滤器保留感兴趣的深度值并省略范围之外的深度值。
-- `enable_noise_removal_filter`: 该滤波器可消除簇状斑点噪声，并生成填充程度较低的深度图。
-- `enable_spatial_filter`: 此滤波器根据幅值参数的指定执行多次迭代处理，以增强深度数据的平滑度。它还能够填充深度图中的小孔。
-- `enable_temporal_filter`: 此滤镜旨在通过基于先前帧操纵每个像素的值来提高深度数据的持久性。该滤镜对数据执行单次传递，在调整深度值的同时更新追踪历史记录。
-- `enable_hole_filling_filter`: 此过滤器使用指定的模式填充深度图中的所有孔洞。
-- `retry_on_usb3_detection_failure`:如果摄像头连接到 USB 2.0 端口但未被检测到，系统将尝试重置摄像头最多三次。此设置旨在防止 USB 3.0 设备被错误地识别为 USB 2.0。建议在使用 USB 2.0 连接时将此参数设置为 `false`，以避免不必要的重置。
-- `tf_publish_rate`: 相机发布动态变换的速率。默认值为 0.0，表示发布静态变换。
-- `time_domain`: 帧时间域，字符串类型，可以是 `device`、`global`或 `system`。`device`表示使用相机的硬件时间戳；`system`表示使用PC接收到第一个数据包或帧的时间戳；`global`用于跨多个设备同步时间，将来自不同来源的数据对齐到公共时间基准。
-- `enable_sync_host_time`: 启用主机时间与相机时间的同步。默认值为 `true`，如果使用全局时间，请设置为 `false`。某些旧设备可能不支持此功能。
-- `config_file_path`: YAML 配置文件的路径。如果未指定配置文件，则将使用启动文件中的默认参数。如果您想使用自定义配置文件，请参阅 `gemini_330_series.launch.py`。
-- `enable_heartbeat`启用心跳功能，默认设置为 `false`。如果设置为 `true`，摄像头节点将向固件发送心跳信号；如果需要硬件日志记录，也应设置为 `true`。
-- `log_level` : SDK 日志级别，默认值为 `info`，可选值为 `debug`, `info`, `warn`, `error`, `fatal`。
-- `enable_color_undistortion`: 启用颜色去畸变功能，默认值为 `false`。请注意，我们的彩色相机畸变程度极小，通常无需进行去畸变设置。
-- `color_brightness`: 设置彩色流亮度。
-- `ir_brightness`: 设置红外流亮度。
-- `color_ae_max_exposure`: 设置彩色流自动曝光最大曝光值。
-- `ir_ae_max_exposure`: 设置红外流自动曝光最大曝光值。
-
-**重要提示**：*请仔细阅读[此链接](https://www.orbbec.com/docs/g330-use-depth-post-processing-blocks/)中关于软件过滤设置的说明。如果您不确定，请勿修改这些设置。*
-
-## 预设presets
-
-| Preset         | 特征                                                                                                            | 推荐用例                                                                          |
-| -------------- | --------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------- |
-| Default        | - 最佳视觉感知 - 准确度、填充率、微小物体等方面的整体表现良好。                                                 | - 通用 - 机器人                                                                  |
-| Hand           | - 清除手和手指边缘                                                                                              | - 手势识别                                                                       |
-| High Accuracy  | - 高置信度的深度 - 几乎没有噪声的深度值  - 较低的填充率                                                        | - 防撞 - 物体扫描                                                                |
-| High Density   | - 更高的填充率 - 更多微小物体  - 可能受到噪声深度值的影响                                                      | - 物体识别 - 拾取和放置 - 前景和背景动画                                         |
-| Medium Density | - 填充率和准确度的平衡性能 - 与默认设置相比：填充率较低，边缘质量更好                                           | - 通用且可替代默认                                                               |
-| Custom         | - 用户定义预设 - 从上述预设衍生，并经过自定义修改，例如后处理管道的新配置、深度 AE 函数的修改平均强度设定点等。 | - 与使用预定义预设相比，使用自定义配置可实现更好的深度性能 - 对于完善的自定义配置 |
-
-根据您的具体用例选择合适的预设名称，并将其设置为 `device_preset` 参数的值。
-
-## 深度工作模式开关
-
-Orbbec SDK ROS2 支持深度工作模式切换。Gemini 2、Gemini 2 L、Femto 和 Femto Bolt 相机均支持深度工作模式切换。
-
-- 在启动相机之前，可以配置深度工作模式（depth_work_mode），以获得相应 xxx.launch.py 文件的支持。
-- 深度工作模式切换支持 Gemini 2、Gemini 2 L 和 Gemini 2 XL 相机。
-- xxx.launch.py 默认的深度工作模式配置是相机的默认配置，如果需要修改，可以根据需要切换到相应的模式。
-- 具体的相机深度工作模式支持类型可以参见深度模式的注释。
-
-```python
-# 深度工作模式支持如下：
-# 未分箱密集默认值
-# Unbinned Sparse Default
-# Binned Sparse Default
-# Obstacle Avoidance
-DeclareLaunchArgument('depth_work_mode', default_value='')
-```
-
-- 查看深度工作模式：
-
-```bash
-ros2 run orbbec_camera list_depth_work_mode_node
-```
-
-## 深度 NFOV 和 WFOV 模式的配置
-
-对于 Femto Mega 和 Femto Bolt 设备，NFOV 和 WFOV 模式是通过在启动文件中配置深度和红外分辨率来实现的。
-在启动文件中，depth_width、depth_height、ir_width、ir_height 分别表示深度分辨率和红外分辨率。
-红外的帧率和分辨率必须与深度分辨率一致。不同模式与分辨率的对应关系如下：
-
-- NFOV unbinned: 640 x 576.
-- NFOV binned: 320 x 288.
-- WFOV unbinned: 1024 x 1024.
-- WFOV binned: 512 x 512.
-
-## 所有可用的摄像机控制服务
-
-以下服务的名称已经表达了其功能。
-但需要注意的是，相应的 `set_[ir|depth|color]`
-和 `get[ir|depth|color]*`服务只有在启动文件参数对应的流中将 `enable[ir|depth|color]`
-设置为 `true` 时才可用。
-
-- `/camera/get_auto_white_balance`
-- `/camera/get_color_exposure`
-- `/camera/get_color_gain`
-- `/camera/get_depth_exposure`
-- `/camera/get_depth_gain`
-- `/camera/get_device_info`
-- `/camera/get_ir_exposure`
-- `/camera/get_ir_gain`
-- `/camera/get_ldp_status`
-- `/camera/get_sdk_version`
-- `/camera/get_white_balance`
-- `/camera/set_auto_white_balance`
-- `/camera/set_color_auto_exposure`
-- `/camera/set_color_exposure`
-- `/camera/set_color_gain`
-- `/camera/set_depth_auto_exposure`
-- `/camera/set_depth_exposure`
-- `/camera/set_depth_gain`
-- `/camera/set_fan_work_mode`
-- `/camera/set_floor_enable`
-- `/camera/set_ir_auto_exposure`
-- `/camera/set_ir_exposure`
-- `/camera/set_ir_gain`
-- `/camera/set_laser_enable`
-- `/camera/set_ldp_enable`
-- `/camera/set_white_balance`
-- `/camera/toggle_color`
-- `/camera/toggle_depth`
-- `/camera/toggle_ir`
-
-## 所有可用主题
-
-- `/camera/color/camera_info` : 彩色相机信息。
-- `/camera/color/image_raw`: 彩色流图像。
-- `/camera/depth/camera_info`: 深度相机信息。
-- `/camera/depth/image_raw`: 深度流图像。
-- `/camera/depth/points` :点云，仅当 `enable_point_cloud`为 `true`时可用。
-- `/camera/depth_registered/points`: 彩色点云，仅当 `enable_colored_point_cloud` 为 `true` 时可用。
-- `/camera/ir/camera_info`: 红外相机信息。
-- `/camera/ir/image_raw`: 红外流图像
-- `/camera/accel/sample`: 加速度数据流 `enable_sync_output_accel_gyro`关闭，`enable_accel`开启
-- `/camera/gyro/sample`: 陀螺仪数据流，`enable_sync_output_accel_gyro` 关闭，`enable_gyro`开启
-- `camera/gyro_accel/sample`: 加速度计和陀螺仪数据流同步，`enable_sync_output_accel_gyro` 已开启
-- `/diagnostics`: 摄像机的诊断信息。目前，诊断信息仅包括摄像机的温度。
-
-## 网络相机设备枚举
-
-目前，仅 Femto Mega 设备支持网络设备枚举功能。通过网络访问此设备时，如果将 `enumerate_net_device` 设置为 `true`，则会自动枚举该设备，无需提前配置 IP 地址或将启用开关设置为 true。具体配置方法如下：
-
-- `enumerate_net_device`: 自动枚举网络设备，仅 Femto Mega 支持。
-  如果 `enumerate_net_device` 设置为 `true`，则将自动枚举设备，无需设置 `net_device_ip`
-  和 `net_device_port` 参数。
-- `net_device_ip`: 设备的 IP 地址。
-- `net_device_port`: 设备的端口号。
-
-## 多相机
-
-- 要获取相机的 `usb_port`，请插入相机并在终端中运行以下命令：
-
-```bash
-ros2 run orbbec_camera list_devices_node
-```
-
-- 将“device_num”参数设置为您拥有的摄像机数量。
-- 进入 `OrbbecSDK_ROS2/launch/multi_xxx.launch.py`文件，修改 `usb_port`。
-- 不要忘记将 `include` 标签放在 `group` 标签内。
-  否则，不同摄像机的参数值可能会受到污染。
-
-```python
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction, ExecuteProcess
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch_ros.actions import Node
-from ament_index_python.packages import get_package_share_directory
-import os
-
-
-def generate_launch_description():
-    # Include launch files
-    package_dir = get_package_share_directory('orbbec_camera')
-    launch_file_dir = os.path.join(package_dir, 'launch')
-    launch1_include = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'gemini2L.launch.py')
-        ),
-        launch_arguments={
-            'camera_name': 'camera_01',
-            'usb_port': '6-2.4.4.2',  # replace your usb port here
-            'device_num': '2'
-        }.items()
-    )
-
-    launch2_include = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'gemini2L.launch.py')
-        ),
-        launch_arguments={
-            'camera_name': 'camera_02',
-            'usb_port': '6-2.4.1',  # replace your usb port here
-            'device_num': '2'
-        }.items()
-    )
-
-    # If you need more cameras, just add more launch_include here, and change the usb_port and device_num
-
-    # Launch description
-    ld = LaunchDescription([
-        GroupAction([launch1_include]),
-        GroupAction([launch2_include]),
-    ])
-
-    return ld
-
-```
-
-- 要启动摄像头，请运行以下命令：
-
-```bash
-ros2 launch orbbec_camera multi_camera.launch.py
-```
-
-## 压缩图像
-
-您可以使用 `image_transport` 来使用 `jpeg` 格式压缩图像。以下是使用示例：
-
-要访问压缩后的彩色图像，您可以使用以下命令：
-
-```bash
-ros2 topic echo /camera/color/image_raw/compressed --no-arr
-```
-
-此命令将允许您从指定主题接收压缩的彩色图像。
-
-## 使用硬件解码器解码JPEG
-
-### rockchip and Amlogic
-
-依赖于 `rockchip-mpp-dev` 和 `rockchip-rga-dev`，并非所有系统都包含这两个软件包，名称可能不同，请自行搜索。
-打开 `CMakeLists.txt`，并将 `USE_RK_HW_DECODER` 设置为 `ON`。
-
-### Nvidia Jetson
-
-依赖于：`jetson_multimedia_api`、`libyuv`。
-打开 `CMakeLists.txt` 并将 `USE_NV_HW_DECODER` 设置为 `ON`。
-
-## 检查相机支持哪些配置文件
-
-```bash
-ros2 run orbbec_camera list_camera_profile_mode_node
-```
-
-## 构建Debian软件包
-
-### 准备环境
-
-开始之前，请安装所需的工具：
-
-```bash
-sudo apt install debhelper fakeroot python3-bloom
-```
-
-### 配置ROS依赖项
-
-将以下 YAML 文件添加到您的系统 `/etc/ros/rosdep/sources.list.d/00-orbbec.yaml`。请确保将 `focal` 替换为您的 Ubuntu 版本的代号，将 `humble` 替换为您的 ROS2 发行版名称：
-
-```yaml
-orbbec_camera_msgs:
-  ubuntu:
-    focal: [ ros-humble-orbbec-camera-msgs ]
-```
-
-接下来，创建一个新文件 `/etc/ros/rosdep/sources.list.d/50-orbbec.list` 并添加此行以指定
-YAML file:
-
-```bash
-yaml file:///etc/ros/rosdep/sources.list.d/00-orbbec.yaml
-```
-
-更新 rosdep 数据库以反映这些更改：
-
-```bash
-rosdep update
-```
-
-### 构建包
-
-导航到您的工作区并构建项目：
-
-```bash
-cd ~/ros2_ws/
-colcon build --event-handlers console_direct+ --cmake-args -DCMAKE_BUILD_TYPE=Release
-. install/setup.bash
-cd src/OrbbecSDK_ROS2/
-bash .make_deb.sh
-```
-
-## 支持的设备
-
-OrbbecSDK ROS2 Wrapper 支持以下设备。
-
-| 产品列表                              | 最低固件版本 | **Launch File**       |
-| :------------------------------------ | :----------- | :-------------------------- |
-| Astra+                                | 1.0.19       | astra_adv.launch.py         |
-| Astra Mini /Astra Mini Pro /Astra Pro | 1007         | astra.launch.py             |
-| Astra Mini Pro S                      | 1005         | astra.launch.py             |
-| Astra 2                               | 2.8.20       | astra2.launch.py            |
-| Dabai                                 | 2436         | dabai.launch.py             |
-| Dabai DW                              | 2606         | dabai_dw.launch.py          |
-| Dabai DCW                             | 2460         | dabai_dcw.launch.py         |
-| Femto/Femto W                         | 1.1.8        | femto.launch.py             |
-| Femto Mega                            | 1.1.7        | femto_mega.launch.py        |
-| Femto Bolt                            | 1.0.6        | femto_bolt.launch.py        |
-| Femto Mega I                          | 2.0.2        | femto_net_camera.launch.py  |
-| Gemini                                | 3018         | gemini.launch.py            |
-| Gemini 2 / Dabai DCL                  | 1.4.60       | gemini2.launch.py           |
-| Gemini 2 L                            | 1.4.32       | gemini2L.launch.py          |
-| Gemini E                              | 3460         | gemini_e.launch.py          |
-| Gemini E Lite                         | 3606         | gemini_e_lite.launch.py     |
-| Gemini 335/335L/336/336L              | 1.2.20       | gemini_330_series.launch.py |
-
-> [!重要]
->
-> 我们建议使用 [OrbbecSDK ROS2 Wrapper v2](https://github.com/orbbec/OrbbecSDK_ROS2/tree/v2-main)，以便为以下设备提供更好的灵活性和可扩展性：Astra 2、Femto Mega、Femto Bolt、Gemini 2、Gemini 2 L、Gemini 335、Gemini 335L、Gemini 336、Gemini 336L、Gemini 335Lg（仅支持 v2-main 分支）。
-
-所有启动文件本质上都类似，主要区别在于同一系列不同型号的参数默认值。USB 标准的差异（例如 USB 2.0 和 USB 3.0）可能需要调整这些参数。如果您遇到启动失败，请仔细阅读规格手册。请特别注意启动文件中的分辨率设置以及其他参数，以确保兼容性和最佳性能。
-
-## DDS调优
-
-默认的 DDS 设置 (Galactic) 可能并非数据传输的最佳选择。不同的 DDS 设置可能会带来不同的性能。本例中使用 CycloneDDS。更多详细信息，请参阅
-[ROS DDS 调优](https://docs.ros.org/en/humble/How-To-Guides/DDS-tuning.html)。
-
-● 编辑cyclonedds配置文件
-
-```bash
-sudo gedit /etc/cyclonedds/config.xml
-```
-
-增加
-
-```xml
-<?xml version="1.0" encoding="UTF-8"?>
-<CycloneDDS xmlns="https://cdds.io/config" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
-            xsi:schemaLocation="https://cdds.io/confighttps://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd">
-    <Domain id="any">
-        <General>
-            <NetworkInterfaceAddress>lo</NetworkInterfaceAddress>
-            <AllowMulticast>false</AllowMulticast>
-        </General>
-        <Internal>
-            <MinimumSocketReceiveBufferSize>16MB</MinimumSocketReceiveBufferSize>
-        </Internal>
-        <Discovery>
-            <ParticipantIndex>auto</ParticipantIndex>
-            <MaxAutoParticipantIndex>30</MaxAutoParticipantIndex>
-            <Peers>
-                <Peer address="localhost"/>
-            </Peers>
-        </Discovery>
-    </Domain>
-</CycloneDDS>
-```
-
-● 设置环境变量，添加到 `.zshrc`或 `.bashrc`
-
-```bash
-export ROS_DOMAIN_ID=42 # Numbers from 0 to 232
-export ROS_LOCALHOST_ONLY=1
-export CYCLONEDDS_URI=file:///etc/cyclonedds/config.xml
-```
-
-提示：要了解为什么 ROS_DOMAIN_ID 的最大值为 232，请访问 [ROS DOMAIN ID](https://docs.ros.org/en/humble/Concepts/About-Domain-ID.html)
-● 增加 UDP 接收缓冲区大小
-编辑
-
-```bash
-/etc/sysctl.d/10-cyclone-max.conf
-```
-
-增加
-
-```bash
-net.core.rmem_max=2147483647
-net.core.rmem_default=2147483647
-```
-
-如果您使用 Fast DDS，可以参考 [Fast DDS 配置](./docs/fastdds_tuning.md) 文件。
-
-## 常见问题
-
-### 意外崩溃
-
-如果摄像头节点意外崩溃，它将在当前运行目录中生成崩溃日志：`Log/camera_crash_stack_trace_xx.log`。请将此日志发送给支持团队或将其提交至 GitHub 问题以获取进一步帮助。
-
-### 没有来自多个摄像机的数据流
-
-**电源不足**:
-
-- 确保每个摄像头都连接到单独的集线器。
-- 使用有源集线器为每个摄像头提供足够的电力。
-
-**高分辨率**:
-
-- 尝试降低分辨率来解决数据流问题。
-
-**增加 usbfs_memory_mb 值**:
-
-- 运行以下命令，将 `usbfs_memory_mb` 值增加到 128MB（这是一个参考值，可以根据系统需求进行调整）：
-
-```bash
-    echo 128 | sudo tee /sys/module/usbcore/parameters/usbfs_memory_mb
-```
-
-- 要使此更改永久生效，请检查[此链接](https://github.com/OpenKinect/libfreenect2/issues/807)。
-
-### 其他故障排除
-
-- 如果您遇到其他问题，请将 `log_level` 参数设置为 `debug`。这将在运行目录中生成一个 SDK 日志文件：`Log/OrbbecSDK.log.txt`。
-  请将此文件提供给支持团队以获取进一步帮助。
-- 如果需要固件日志，请将 `enable_heartbeat`设置为 `true`以激活此功能。
-
-### 为什么有这么多启动文件？
-
-- 不同的相机有不同的默认分辨率和图像格式。
-- 为了简化使用，每个相机都有自己的启动文件。
-
-## 其他有用链接
-
-- [OrbbecSDK_v2](https://github.com/orbbec/OrbbecSDK_v2/releases)
-- [Gemini 330 depth presets](https://www.orbbec.com/docs/g330-use-depth-presets/)
-
-## License
-
-版权所有 2024 Orbbec Ltd。
-
-根据 Apache 许可证 2.0 版（"许可证"）获得许可；除非遵守许可证，否则您不得使用此项目。您可以在以下网址获取许可证副本：
-
-[http://www.apache.org/licenses/LICENSE-2.0](http://www.apache.org/licenses/LICENSE-2.0)
-
-除非适用法律要求或书面同意，否则根据本许可证分发的软件均按“原样”分发，不附带任何明示或暗示的保证或条件。请参阅许可证，了解本许可证下特定语言的权限和限制规定。
-
-**Other names and brands may be claimed as the property of others**

OrbbecSDK_ROS2/docs/align_depth_color.MD

@@ -1,40 +0,0 @@
-## Aligning Depth to Color in ROS 2
-
-This section explains how to align depth images with color images to create an overlay image using ROS 2. This is particularly useful for applications requiring synchronized visual information from different sensor modalities.
-
-### Commands to Align and View Depth and Color Images
-
-1. **Basic Depth to Color Alignment:**
-
-   To simply align the depth image to the color image, use the following command:
-
-   ```bash
-   ros2 launch orbbec_camera gemini_330_series.launch.py depth_registration:=true
-   ```
-
-   This command activates the depth registration feature without opening a viewer.
-
-2. **Viewing Depth to Color Overlay:**
-
-   If you wish to view the depth to color overlay, you need to enable the viewer by using the command below:
-
-   ```bash
-   ros2 launch orbbec_camera gemini_330_series.launch.py depth_registration:=true enable_d2c_viewer:=true
-   ```
-
-   This launches the camera node with depth to color registration and opens a viewer to display the overlay image.
-
-### Selecting Topics in RViz2
-
-To visualize the aligned images in RViz2:
-
-1. Launch RViz2 after running one of the above commands.
-2. Select the topic for the depth to color overlay image. An example topic selection is shown here:
-   
-   ![Topic Selection for Depth to Color Overlay](./images/image3.png)
-
-### Example of Depth to Color Overlay
-
-After selecting the appropriate topic in RViz2, you will be able to see the depth to color overlay image. Here's what it might look like:
-
-![Depth to Color Overlay Image](./images/image4.jpg)

OrbbecSDK_ROS2/docs/align_depth_color_CN.MD

@@ -1,40 +0,0 @@
-## 在ROS 2中将深度图像与彩色图像对齐
-
-本节介绍如何使用ROS 2将深度图像与彩色图像对齐，以创建叠加图像。这对于需要来自不同传感器模态的同步视觉信息的应用程序特别有用。
-
-### 对齐并查看深度和彩色图像的命令
-
-1. **基本的深度到彩色对齐：**
-
-   要简单地将深度图像对齐到彩色图像，使用以下命令：
-
-   ```bash
-   ros2 launch orbbec_camera gemini_330_series.launch.py depth_registration:=true
-   ```
-
-   此命令激活深度注册功能，但不打开查看器。
-
-2. **查看深度到彩色叠加：**
-
-   如果您希望查看深度到彩色叠加，您需要通过使用以下命令来启用查看器：
-
-   ```bash
-   ros2 launch orbbec_camera gemini_330_series.launch.py depth_registration:=true enable_d2c_viewer:=true
-   ```
-
-   这将启动具有深度到彩色注册的相机节点，并打开一个查看器以显示叠加图像。
-
-### 在RViz2中选择主题
-
-要在RViz2中可视化对齐的图像：
-
-1. 在运行上述命令之一后启动RViz2。
-2. 选择深度到彩色叠加图像的主题。这里显示了一个示例主题选择：
-
-   ![深度到彩色叠加主题选择](./images/image3.png)
-
-### 深度到彩色叠加示例
-
-在RViz2中选择合适的主题后，您将能够看到深度到彩色叠加图像。这是它可能看起来的样子：
-
-![深度到彩色叠加图像](./images/image4.jpg)

OrbbecSDK_ROS2/docs/fastdds_tuning.md

@@ -1,154 +0,0 @@
-# Fast DDS Optimization for Orbbec Camera with ROS2
-
-When operating with the default configuration, Fast DDS exhibits suboptimal transmission efficiency, resulting in
-significant image transmission delays when used with the Orbbec camera in ROS2. This document provides guidance on
-optimizing Fast DDS to enhance image transfer efficiency.
-
-## 1. Adjusting System Parameters
-
-### IP Fragmentation Time
-
-- **Path**: `/proc/sys/net/ipv4/ipfrag_time` (default: 30 seconds)
-- **Purpose**: Defines the duration that IP fragments are kept in memory.
-- **Adjustment**: Decrease this value to reduce the time window where no fragments are received, which can help reduce
-  delays. Consider the specific needs of your environment as this setting affects all incoming fragments.
-
-  **Example**: Set to 3 seconds.
-
-  ```bash
-  sudo sysctl net.ipv4.ipfrag_time=3
-  ```
-
-### IP Fragmentation Memory Threshold
-
-- **Path**: `/proc/sys/net/ipv4/ipfrag_high_thresh` (default: 262144 bytes)
-- **Purpose**: Sets the maximum memory used to reassemble IP fragments.
-- **Adjustment**: Increase this value to allow more memory for fragment reassembly, which can improve handling of larger
-  data packets.
-
-  **Example**: Increase to 128 MB.
-
-  ```bash
-  sudo sysctl net.ipv4.ipfrag_high_thresh=134217728
-  ```
-
-### Maximum Buffer Sizes
-
-- **Purpose**: Configures the maximum buffer sizes for receiving and sending data, which is critical for high-throughput
-  data transmission.
-- **Adjustment**: Set the maximum buffer sizes for both receiving and sending operations.
-
-  **Commands**:
-
-  ```bash
-  sudo sysctl -w net.core.rmem_max=2147483647
-  sudo sysctl -w net.core.rmem_default=2147483647
-  sudo sysctl -w net.core.wmem_max=2147483647
-  sudo sysctl -w net.core.wmem_default=2147483647
-  ```
-
-Alternatively, make these settings permanent by adding them to the `/etc/sysctl.d/10-fastrtps-max.conf` file.
-
-```bash
-sudo gedit /etc/sysctl.d/10-fastrtps-max.conf
-```
-
-add blow lines to the file:
-
-```bash
-net.core.rmem_max=2147483647
-net.core.rmem_default=2147483647
-net.core.wmem_max=2147483647
-net.core.wmem_default=2147483647
-```
-
-then save and exit the file. run `sudo sysctl -p` to apply the changes.
-
-For detailed guidance, refer
-to [ROS 2 DDS Tuning Documentation](https://docs.ros.org/en/foxy/How-To-Guides/DDS-tuning.html).
-
-## 2. Fast DDS Configuration
-
-Below is an example of a Fast DDS configuration file optimized for ROS2 usage with the Orbbec camera. This configuration
-enhances the overall data transmission by adjusting buffer sizes and transport settings.
-
-### Configuration File: `shm_fastdds.xml`
-
-Place this file in the `$HOME` directory.
-
-```xml
-<?xml version="1.0" encoding="UTF-8"?>
-<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
-    <transport_descriptors>
-        <transport_descriptor>
-            <transport_id>UDP_transport</transport_id>
-            <type>UDPv4</type>
-            <maxInitialPeersRange>10</maxInitialPeersRange>
-            <maxMessageSize>65000</maxMessageSize>
-            <sendBufferSize>1048576</sendBufferSize>
-            <receiveBufferSize>1048576</receiveBufferSize>
-        </transport_descriptor>
-    </transport_descriptors>
-    <participant profile_name="participant_profile_ros2" is_default_profile="true">
-        <rtps>
-            <name>profile_for_ros2_context</name>
-            <userTransports>
-                <transport_id>UDP_transport</transport_id>
-            </userTransports>
-            <useBuiltinTransports>false</useBuiltinTransports>
-            <sendSocketBufferSize>1048576</sendSocketBufferSize>
-            <listenSocketBufferSize>1048576</listenSocketBufferSize>
-            <builtin>
-                <initialPeersList>
-                    <locator>
-                        <udpv4>
-                            <address>127.0.0.1</address>
-                        </udpv4>
-                    </locator>
-                </initialPeersList>
-            </builtin>
-        </rtps>
-    </participant>
-    <data_writer profile_name="default publisher profile" is_default_profile="true">
-        <qos>
-            <publishMode>
-                <kind>ASYNCHRONOUS</kind>
-            </publishMode>
-            <latencyBudget>
-                <duration>
-                    <sec>0</sec>
-                    <nanosec>1000000</nanosec>
-                </duration>
-            </latencyBudget>
-        </qos>
-        <historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
-    </data_writer>
-    <data_reader profile_name="default subscription profile" is_default_profile="true">
-        <qos>
-            <data_sharing>
-                <kind>AUTOMATIC</kind>
-            </data_sharing>
-            <latencyBudget>
-                <duration>
-                    <sec>0</sec>
-                    <nanosec>1000000</nanosec>
-                </duration>
-            </latencyBudget>
-        </qos>
-        <historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
-    </data_reader>
-</profiles>
-```
-
-### Environment Variables
-
-Set the following environment variables to use the custom Fast DDS profile:
-
-```bash
-export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
-export FASTRTPS_DEFAULT_PROFILES_FILE=$HOME/shm_fastdds.xml
-export RMW_FASTRTPS_USE_QOS_FROM_XML=1
-```
-
-This configuration aims to optimize the data flow and reduce transmission delays, improving the responsiveness and
-reliability of the Orbbec camera system in a ROS2 environment.

OrbbecSDK_ROS2/docs/multi_camera.MD

@@ -1,108 +0,0 @@
-## Using Multiple Cameras with the Orbbec ROS 2 Package
-
-This section describes how to configure and use multiple Orbbec cameras simultaneously in a ROS 2 environment.
-
-### Identifying Camera USB Ports
-
-#### Script to List Connected Cameras
-
-To determine which USB ports the cameras are connected to, you can use the following bash script. This script lists all Orbbec devices attached to the system along with their USB port and serial number.
-
-```bash
-#!/bin/bash
-
-VID="2bc5"
-
-for dev in /sys/bus/usb/devices/*; do
-  if [ -e "$dev/idVendor" ]; then
-    vid=$(cat "$dev/idVendor")
-    if [ "$vid" == "${VID}" ]; then
-      port=$(basename $dev)
-      product=$(cat "$dev/product" 2>/dev/null) # product name
-      serial=$(cat "$dev/serial" 2>/dev/null) # serial number
-      echo "Found Orbbec device $product, usb port $port, serial number $serial"
-    fi
-  fi
-done
-```
-
-Save this script to a file and execute it in your terminal to output a list of connected cameras.
-
-### Launching Multiple Cameras
-
-#### Setup for Multiple Camera Launch
-
-You can launch multiple cameras by specifying different USB ports for each camera. Below is an example Python script that uses the ROS 2 launch system to start two cameras with individual configurations.
-
-```python
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction, ExecuteProcess
-from launch.launch_description_sources from ament_index_python.packages import get_package_share_directory
-import os
-
-def generate_launch_description():
-    package_dir = get_package_share_directory('orbbec_camera')
-    launch_file_dir = os.path.join(package_dir, 'launch')
-
-    launch1_include = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(launch_file_dir, 'gemini_330_series.launch.py')),
-        launch_arguments={'camera_name': 'camera_01', 'usb_port': '2-3.4.4.4.1', 'device_num': '2', 'sync_mode': 'free_run'}.items()
-    )
-
-    launch2_include = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(launch_file_dir, 'gemini_330_series.launch.py')),
-        launch_arguments={'camera_name': 'camera_02', 'usb_port': '2-3.4.4.4.3', 'device_num': '2', 'sync_mode': 'free_run'}.items()
-    )
-
-    ld = LaunchDescription([
-        GroupAction([launch1_include]),
-        GroupAction([launch2_include])
-    ])
-
-    return ld
-```
-
-#### Running the Launch File
-
-To execute the launch configuration for multiple cameras, use the command:
-
-```bash
-ros2 launch orbbec_camera multi_camera.launch.py
-```
-
-### Configuring the TF Tree for Multiple Cameras
-
-#### Example TF Configuration for Two Cameras
-
-When using multiple cameras, it's essential to calibrate them and publish a static TF tree for each camera. The following Python script configures the TF tree based on your calibration results:
-
-```python
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-def generate_launch_description():
-    ld = LaunchDescription([
-        Node(
-            package='tf2_ros',
-            executable='static_transform_publisher',
-            name='camera_01_tf',
-            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'camera_01_link'],
-            output='screen'
-        ),
-        Node(
-            package='tf2_ros',
-            executable='static_transform_publisher',
-            name='camera_02_tf',
-            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'camera_02_link'],
-            output='screen'
-        )
-    ])
-
-    return ld
-```
-
-Save this configuration as `multi_camera_tf.launch.py` in the launch directory of the Orbbec camera package. To run it, use:
-
-```bash
-ros2 launch orbbec_camera multi_camera_tf.launch.py
-```

OrbbecSDK_ROS2/docs/multi_camera_CN.MD

@@ -1,109 +0,0 @@
-## 使用Orbbec ROS 2包配置多个摄像头
-
-本节介绍如何在ROS 2环境中同时配置和使用多个Orbbec摄像头。
-
-### 识别摄像头USB端口
-
-#### 列出连接的摄像头的脚本
-
-要确定摄像头连接到哪些USB端口，您可以使用以下bash脚本。该脚本列出了连接到系统的所有Orbbec设备及其USB端口和序列号。
-
-```bash
-#!/bin/bash
-
-VID="2bc5"
-
-for dev in /sys/bus/usb/devices/*; do
-  if [ -e "$dev/idVendor" ]; then
-    vid=$(cat "$dev/idVendor")
-    if [ "$vid" == "${VID}" ]; then
-      port=$(basename $dev)
-      product=$(cat "$dev/product" 2>/dev/null) # 产品名称
-      serial=$(cat "$dev/serial" 2>/dev/null) # 序列号
-      echo "发现Orbbec设备 $product，usb端口 $port，序列号 $serial"
-    fi
-  fi
-done
-```
-
-将此脚本保存为一个文件，并在您的终端中执行，以输出连接的摄像头列表。
-
-### 启动多个摄像头
-
-#### 多摄像头启动配置
-
-您可以通过为每个摄像头指定不同的USB端口来启动多个摄像头。下面是一个使用ROS 2启动系统启动两个摄像头的Python脚本示例。
-
-```python
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction, ExecuteProcess
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from ament_index_python.packages import get_package_share_directory
-import os
-
-def generate_launch_description():
-    package_dir = get_package_share_directory('orbbec_camera')
-    launch_file_dir = os.path.join(package_dir, 'launch')
-
-    launch1_include = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(launch_file_dir, 'gemini_330_series.launch.py')),
-        launch_arguments={'camera_name': 'camera_01', 'usb_port': '2-3.4.4.4.1', 'device_num': '2', 'sync_mode': 'free_run'}.items()
-    )
-
-    launch2_include = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(launch_file_dir, 'gemini_330_series.launch.py')),
-        launch_arguments={'camera_name': 'camera_02', 'usb_port': '2-3.4.4.4.3', 'device_num': '2', 'sync_mode': 'free_run'}.items()
-    )
-
-    ld = LaunchDescription([
-        GroupAction([launch1_include]),
-        GroupAction([launch2_include])
-    ])
-
-    return ld
-```
-
-#### 运行启动文件
-
-要执行多摄像头的启动配置，请使用命令：
-
-```bash
-ros2 launch orbbec_camera multi_camera.launch.py
-```
-
-### 配置多摄像头的TF树
-
-#### 两个摄像头的TF配置示例
-
-使用多个摄像头时，校准它们并为每个摄像头发布静态TF树是必不可少的。以下Python脚本基于您的校准结果配置TF树：
-
-```python
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-def generate_launch_description():
-    ld = LaunchDescription([
-        Node(
-            package='tf2_ros',
-            executable='static_transform_publisher',
-            name='camera_01_tf',
-            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'camera_01_link'],
-            output='screen'
-        ),
-        Node(
-            package='tf2_ros',
-            executable='static_transform_publisher',
-            name='camera_02_tf',
-            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'camera_02_link'],
-            output='screen'
-        )
-    ])
-
-    return ld
-```
-
-将此配置保存为 `multi_camera_tf.launch.py` 在Orbbec摄像头包的启动目录中。运行它，请使用：
-
-```bash
-ros2 launch orbbec_camera multi_camera_tf.launch.py
-```

OrbbecSDK_ROS2/docs/point_cloud.MD

@@ -1,53 +0,0 @@
-## Enabling and Visualizing Point Cloud in ROS 2
-
-This section demonstrates how to enable point cloud data output from the camera node and visualize it using RViz2, similarly to the initial camera node setup discussed in the [Starting Camera Node](./start_camera_node.MD) document.
-
-### Enabling Depth Point Cloud
-
-#### Command to Enable Depth Point Cloud
-
-To activate the point cloud data stream for depth information, use the following command:
-
-```bash
-ros2 launch orbbec_camera gemini_330_series.launch.py enable_point_cloud:=true
-```
-
-#### Visualizing Depth Point Cloud in RViz2
-
-After running the above command, perform the following steps to visualize the depth point cloud:
-
-1. Open RViz2.
-2. Add a `PointCloud2` display.
-3. Select the `/camera/depth/points` topic for visualization.
-4. Set the fixed frame to `camera_link` to properly align the data.
-
-##### Example Visualization
-
-Here is what the depth point cloud might look like in RViz2:
-
-![Depth Point Cloud Visualization](./images/image5.jpg)
-
-### Enabling Colored Point Cloud
-
-#### Command to Enable Colored Point Cloud
-
-To enable the colored point cloud feature, enter the following command:
-
-```bash
-ros2 launch orbbec_camera gemini_330_series.launch.py enable_colored_point_cloud:=true
-```
-
-#### Visualizing Colored Point Cloud in RViz2
-
-To visualize the colored point cloud data:
-
-1. Launch RViz2 following the command execution.
-2. Add a `PointCloud2` display panel.
-3. Choose the `/camera/depth_registered/points` topic from the list.
-4. Ensure the fixed frame is set to `camera_link`.
-
-##### Example Visualization
-
-The result of the colored point cloud in RViz2 should look similar to this:
-
-![Colored Point Cloud Visualization](./images/image6.jpg)

OrbbecSDK_ROS2/docs/point_cloud_CN.MD

@@ -1,53 +0,0 @@
-## 在ROS 2中启用和可视化点云
-
-本节演示如何从相机节点启用点云数据输出，并使用RViz2进行可视化，类似于之前讨论的[启动相机节点](./start_camera_node.MD)文档中的初始相机节点设置。
-
-### 启用深度点云
-
-#### 启用深度点云的命令
-
-要激活深度信息的点云数据流，请使用以下命令：
-
-```bash
-ros2 launch orbbec_camera gemini_330_series.launch.py enable_point_cloud:=true
-```
-
-#### 在RViz2中可视化深度点云
-
-运行上述命令后，执行以下步骤来可视化深度点云：
-
-1. 打开RViz2。
-2. 添加一个`PointCloud2`显示。
-3. 选择`/camera/depth/points`主题进行可视化。
-4. 将固定帧设置为`camera_link`以正确对齐数据。
-
-##### 示例可视化
-
-以下是在RViz2中可能看到的深度点云的样子：
-
-![深度点云可视化](./images/image5.jpg)
-
-### 启用彩色点云
-
-#### 启用彩色点云的命令
-
-要启用彩色点云功能，请输入以下命令：
-
-```bash
-ros2 launch orbbec_camera gemini_330_series.launch.py enable_colored_point_cloud:=true
-```
-
-#### 在RViz2中可视化彩色点云
-
-要可视化彩色点云数据：
-
-1. 在执行命令后启动RViz2。
-2. 添加一个`PointCloud2`显示面板。
-3. 从列表中选择`/camera/depth_registered/points`主题。
-4. 确保固定帧设置为`camera_link`。
-
-##### 示例可视化
-
-在RViz2中彩色点云的结果应类似于这样：
-
-![彩色点云可视化](./images/image6.jpg)

OrbbecSDK_ROS2/docs/start_camera_node.MD

@@ -1,83 +0,0 @@
-## Starting the Camera Node in ROS 2
-
-This guide provides instructions on how to launch the camera node with a colored point cloud feature enabled using ROS 2.
-
-### Command to Start the Node
-
-To start the camera node, execute the following command in your terminal:
-
-```bash
-ros2 launch orbbec_camera gemini_330_series.launch.py enable_colored_point_cloud:=true
-```
-
-This command initiates the camera node and enables the colored point cloud.
-
-### Published Topics
-
-Once the camera node is running, it will publish data on several ROS topics. Below is a list of the available topics:
-
-- **IMU Data and IMU Information:**
-  - `camera/accel/imu_info`
-  - `camera/gyro/imu_info`
-  - `camera/gyro_accel/sample`
-
-- **Color Camera Topics:**
-  - `/camera/color/camera_info`
-  - `/camera/color/image_raw`
-  - `/camera/color/image_raw/compressed`
-  - `/camera/color/image_raw/compressedDepth`
-  - `/camera/color/image_raw/theora`
-  - `/camera/color/metadata`
-
-- **Depth Camera Topics:**
-  - `/camera/depth/camera_info`
-  - `/camera/depth/image_raw`
-  - `/camera/depth/image_raw/compressed`
-  - `/camera/depth/image_raw/compressedDepth`
-  - `/camera/depth/image_raw/theora`
-  - `/camera/depth/metadata`
-  - `/camera/depth/points`
-  - `/camera/depth_filter_status`
-  - `/camera/depth_registered/points`
-  - `/camera/depth_to_color`
-  - `/camera/depth_to_left_ir`
-  - `/camera/depth_to_right_ir`
-
-- **Infrared Camera Topics:**
-  - `/camera/left_ir/camera_info`
-  - `/camera/left_ir/image_raw`
-  - `/camera/left_ir/image_raw/compressed`
-  - `/camera/left_ir/image_raw/compressedDepth`
-  - `/camera/left_ir/image_raw/theora`
-  - `/camera/left_ir/metadata`
-  - `/camera/right_ir/camera_info`
-  - `/camera/right_ir/image_raw`
-  - `/camera/right_ir/image_raw/compressed`
-  - `/camera/right_ir/image_raw/compressedDepth`
-  - `/camera/right_ir/image_raw/theora`
-  - `/camera/right_ir/metadata`
-
-- **Miscellaneous Topics:**
-  - `/diagnostics`
-  - `/parameter_events`
-  - `/rosout`
-  - `/rosout_agg`
-
-### Visualizing Data in RViz2
-
-To view the PointCloud or Image data, use RViz2:
-
-1. Launch RViz2.
-2. Select the topic you wish to visualize from the list of published topics.
-3. Add the selected topic to RViz2 to start viewing the data.
-
-### Example Visualizations
-
-Here are examples of how the visualization might appear in RViz2:
-
-- **PointCloud Visualization**
-  ![PointCloud View](./images/image1.jpg)
-  
-- **Image Data Visualization**
-  ![Image Data View](./images/image2.jpg)
-

OrbbecSDK_ROS2/docs/start_camera_node_CN.MD

@@ -1,82 +0,0 @@
-## 在ROS 2中启动相机节点
-
-本指南提供了使用ROS 2启动启用彩色点云功能的相机节点的指令。
-
-### 启动节点的命令
-
-要启动相机节点，请在终端执行以下命令：
-
-```bash
-ros2 launch orbbec_camera gemini_330_series.launch.py enable_colored_point_cloud:=true
-```
-
-此命令将启动相机节点并启用彩色点云。
-
-### 发布的主题
-
-一旦相机节点运行，它将在多个ROS主题上发布数据。以下是可用主题的列表：
-
-- **IMU数据和IMU信息：**
-  - `camera/accel/imu_info`
-  - `camera/gyro/imu_info`
-  - `camera/gyro_accel/sample`
-
-- **彩色相机主题：**
-  - `/camera/color/camera_info`
-  - `/camera/color/image_raw`
-  - `/camera/color/image_raw/compressed`
-  - `/camera/color/image_raw/compressedDepth`
-  - `/camera/color/image_raw/theora`
-  - `/camera/color/metadata`
-
-- **深度相机主题：**
-  - `/camera/depth/camera_info`
-  - `/camera/depth/image_raw`
-  - `/camera/depth/image_raw/compressed`
-  - `/camera/depth/image_raw/compressedDepth`
-  - `/camera/depth/image_raw/theora`
-  - `/camera/depth/metadata`
-  - `/camera/depth/points`
-  - `/camera/depth_filter_status`
-  - `/camera/depth_registered/points`
-  - `/camera/depth_to_color`
-  - `/camera/depth_to_left_ir`
-  - `/camera/depth_to_right_ir`
-
-- **红外相机主题：**
-  - `/camera/left_ir/camera_info`
-  - `/camera/left_ir/image_raw`
-  - `/camera/left_ir/image_raw/compressed`
-  - `/camera/left_ir/image_raw/compressedDepth`
-  - `/camera/left_ir/image_raw/theora`
-  - `/camera/left_ir/metadata`
-  - `/camera/right_ir/camera_info`
-  - `/camera/right_ir/image_raw`
-  - `/camera/right_ir/image_raw/compressed`
-  - `/camera/right_ir/image_raw/compressedDepth`
-  - `/camera/right_ir/image_raw/theora`
-  - `/camera/right_ir/metadata`
-
-- **杂项主题：**
-  - `/diagnostics`
-  - `/parameter_events`
-  - `/rosout`
-  - `/rosout_agg`
-
-### 在RViz2中可视化数据
-
-要查看点云或图像数据，请使用RViz2：
-
-1. 启动RViz2。
-2. 从发布的主题列表中选择您希望可视化的主题。
-3. 将选定的主题添加到RViz2中开始查看数据。
-
-### 示例可视化
-
-以下是在RViz2中可视化可能出现的示例：
-
-- **点云可视化**
-  ![点云视图](./images/image1.jpg)
-  
-- **图像数据可视化**
-  ![图像数据视图](./images/image2.jpg)