Merge pull request #11 from elephantrobotics/humble-dev

fix(agv_pro_bringup): add launch with RViz condition

FAST_LIO/.github/stale.yml

@@ -0,0 +1,17 @@
+# 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

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

FAST_LIO/.gitmodules

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

FAST_LIO/CMakeLists.txt

@@ -0,0 +1,128 @@
+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

@@ -0,0 +1,339 @@
+                    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

@@ -0,0 +1,135 @@
+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

@@ -0,0 +1 @@
+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

@@ -0,0 +1,94 @@
+# 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

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

FAST_LIO/README.md

@@ -0,0 +1,199 @@
+> 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

@@ -0,0 +1,46 @@
+/**:
+    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

@@ -0,0 +1,46 @@
+/**:
+    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

@@ -0,0 +1,50 @@
+/**:
+    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

@@ -0,0 +1,47 @@
+/**:
+    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

@@ -0,0 +1,61 @@
+/**:
+    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

@@ -0,0 +1,48 @@
+/**:
+    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

@@ -0,0 +1,103 @@
+#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

@@ -0,0 +1,82 @@
+/*
+ *  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

@@ -0,0 +1,229 @@
+// 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

@@ -0,0 +1,123 @@
+// 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

@@ -0,0 +1,294 @@
+// 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

@@ -0,0 +1,168 @@
+
+/*
+ *  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

@@ -0,0 +1,328 @@
+// 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

@@ -0,0 +1,316 @@
+// 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

@@ -0,0 +1,317 @@
+// 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

@@ -0,0 +1,461 @@
+// 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

@@ -0,0 +1,113 @@
+/*
+ *  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

@@ -0,0 +1,270 @@
+#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

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

FAST_LIO/include/ikd-Tree/ikd_Tree.h

@@ -0,0 +1,344 @@
+#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

@@ -0,0 +1,111 @@
+#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

@@ -0,0 +1,126 @@
+#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

@@ -0,0 +1,22 @@
+<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

@@ -0,0 +1,70 @@
+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

@@ -0,0 +1,70 @@
+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

@@ -0,0 +1,7 @@
+# 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

@@ -0,0 +1,38 @@
+<?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

@@ -0,0 +1,304 @@
+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

@@ -0,0 +1,358 @@
+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

@@ -0,0 +1,379 @@
+#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

@@ -0,0 +1,196 @@
+// #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;
+};

OrbbecSDK_ROS2/.clang-format

@@ -0,0 +1,167 @@
+---
+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

@@ -0,0 +1,13 @@
+#!/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

@@ -0,0 +1,201 @@
+                                 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

@@ -0,0 +1,50 @@
+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

@@ -0,0 +1,903 @@
+# 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

@@ -0,0 +1,824 @@
+# 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

@@ -0,0 +1,40 @@
+## 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

@@ -0,0 +1,40 @@
+## 在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

@@ -0,0 +1,154 @@
+# 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

@@ -0,0 +1,108 @@
+## 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

@@ -0,0 +1,109 @@
+## 使用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

@@ -0,0 +1,53 @@
+## 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

@@ -0,0 +1,53 @@
+## 在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

@@ -0,0 +1,83 @@
+## 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

@@ -0,0 +1,82 @@
+## 在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)

OrbbecSDK_ROS2/orbbec_camera/.make_deb.sh

@@ -0,0 +1,9 @@
+#!/usr/bin/bash
+set -e
+CURR_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd -P)"
+rm -fr debian obj-x86_64-linux-gnu .obj-aarch64-linux-gnu || true
+export DEBIAN_INC=1
+export BUILDING_PACKAGE=1
+bloom-generate rosdebian --os-name ubuntu --ros-distro $ROS_DISTRO -i ${DEBIAN_INC}.$(date +%Y%m%d.%H%M%S)
+export DEB_BUILD_OPTIONS="parallel=$(($(nproc) - 1))"
+fakeroot debian/rules binary

OrbbecSDK_ROS2/orbbec_camera/CMakeLists.txt

@@ -0,0 +1,258 @@
+cmake_minimum_required(VERSION 3.8)
+project(orbbec_camera)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC -O3")
+set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG}  -fPIC -g3")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -O3")
+set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fPIC -g3")
+set(CMAKE_BUILD_TYPE "Release")
+option(USE_RK_HW_DECODER "Use Rockchip hardware decoder" OFF)
+option(USE_NV_HW_DECODER "Use Nvidia hardware decoder" OFF)
+
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Werror -Wno-pedantic -Wno-array-bounds)
+endif ()
+
+# find dependencies
+set(dependencies
+  ament_cmake
+  ament_index_cpp
+  Eigen3
+  builtin_interfaces
+  cv_bridge
+  camera_info_manager
+  image_transport
+  image_publisher
+  OpenCV
+  orbbec_camera_msgs
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  std_msgs
+  std_srvs
+  tf2
+  tf2_eigen
+  tf2_msgs
+  tf2_ros
+  tf2_sensor_msgs
+  Threads
+  diagnostic_updater
+  diagnostic_msgs
+  statistics_msgs
+  backward_ros
+)
+
+foreach (dep IN LISTS dependencies)
+  find_package(${dep} REQUIRED)
+endforeach ()
+
+find_package(PkgConfig REQUIRED)
+
+
+if (USE_RK_HW_DECODER)
+  pkg_search_module(RK_MPP REQUIRED rockchip_mpp)
+  if (NOT RK_MPP_FOUND)
+    message(FATAL_ERROR "rockchip_mpp is not found")
+  endif ()
+  pkg_search_module(RGA librga)
+  if (NOT RGA_FOUND)
+    add_definitions(-DUSE_LIBYUV)
+    message("librga is not found, use libyuv instead")
+  endif ()
+endif ()
+
+
+execute_process(COMMAND uname -m OUTPUT_VARIABLE MACHINES)
+execute_process(COMMAND getconf LONG_BIT OUTPUT_VARIABLE MACHINES_BIT)
+message(STATUS "ORRBEC Machine : ${MACHINES}")
+message(STATUS "ORRBEC Machine Bits : ${MACHINES_BIT}")
+
+if ((${MACHINES} MATCHES "x86_64") AND (${MACHINES_BIT} MATCHES "64"))
+  set(HOST_PLATFORM "x64")
+elseif (${MACHINES} MATCHES "arm" OR (${MACHINES} MATCHES "aarch64" AND ${MACHINES_BIT} MATCHES "32"))
+  set(HOST_PLATFORM "arm32")
+elseif ((${MACHINES} MATCHES "aarch64") AND (${MACHINES_BIT} MATCHES "64"))
+  set(HOST_PLATFORM "arm64")
+endif ()
+
+set(ORBBEC_LIBS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/SDK/lib/${HOST_PLATFORM})
+set(ORBBEC_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/SDK/include/)
+
+set(CMAKE_BUILD_RPATH "${CMAKE_BUILD_RPATH}:${ORBBEC_LIBS_DIR}")
+set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_RPATH}:${ORBBEC_LIBS_DIR}")
+if (USE_NV_HW_DECODER)
+  set(JETSON_MULTI_MEDIA_API_DIR /usr/src/jetson_multimedia_api)
+  set(JETSON_MULTI_MEDIA_API_CLASS_DIR ${JETSON_MULTI_MEDIA_API_DIR}/samples/common/classes)
+  set(JETSON_MULTI_MEDIA_API_INCLUDE_DIR ${JETSON_MULTI_MEDIA_API_DIR}/include/)
+  set(LIBJPEG8B_INCLUDE_DIR ${JETSON_MULTI_MEDIA_API_INCLUDE_DIR}/libjpeg-8b)
+  set(TEGRA_ARMABI /usr/lib/aarch64-linux-gnu/)
+  add_definitions(-DUSE_NV_HW_DECODER)
+  add_compile_options(-Wno-missing-field-initializers -Wno-unused-parameter)
+  set(NV_LIBRARIES
+    -lnvjpeg -lnvbufsurface -lnvbufsurftransform -lyuv -lv4l2
+  )
+  list(APPEND NV_LIBRARIES
+    -L${TEGRA_ARMABI} -L${TEGRA_ARMABI}/tegra)
+endif ()
+
+set(COMMON_INCLUDE_DIRS
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${ORBBEC_INCLUDE_DIR}
+  ${OpenCV_INCLUDED_DIRS}
+  ${CMAKE_CURRENT_SOURCE_DIR}/tools
+)
+
+set(COMMON_LIBRARIES
+  ${ORBBEC_SDK_LIBRARIES}
+  ${OpenCV_LIBS}
+  Eigen3::Eigen
+  -lOrbbecSDK
+  -L${ORBBEC_LIBS_DIR}
+  Threads::Threads
+  -lrt
+  -ldw
+)
+if (USE_RK_HW_DECODER)
+  list(APPEND COMMON_LIBRARIES
+    ${RK_MPP_LIBRARIES}
+    ${RGA_LIBRARIES}
+  )
+
+endif ()
+
+if (USE_NV_HW_DECODER)
+  list(APPEND COMMON_LIBRARIES
+    ${NV_LIBRARIES})
+endif ()
+
+
+set(SOURCE_FILES
+  src/d2c_viewer.cpp
+  src/dynamic_params.cpp
+  src/image_publisher.cpp
+  src/ob_camera_node_driver.cpp
+  src/ob_camera_node.cpp
+  src/ros_param_backend.cpp
+  src/ros_service.cpp
+  src/synced_imu_publisher.cpp
+  src/utils.cpp
+  src/jpeg_decoder.cpp
+)
+
+if (USE_RK_HW_DECODER)
+  add_definitions(-DUSE_RK_HW_DECODER)
+  list(APPEND SOURCE_FILES src/rk_mpp_decoder.cpp)
+  list(APPEND COMMON_INCLUDE_DIRS
+    ${RK_MPP_INCLUDE_DIRS}
+    ${RGA_INCLUDE_DIRS}
+  )
+  list(APPEND COMMON_LIBRARIES
+    ${RGA_LIBRARIES}
+    ${RK_MPP_LIBRARIES}
+  )
+  if (NOT RGA_FOUND)
+    list(APPEND COMMON_LIBRARIES
+      -lyuv
+    )
+  endif ()
+endif ()
+
+if (USE_NV_HW_DECODER)
+  list(APPEND SOURCE_FILES src/jetson_nv_decoder.cpp)
+  list(APPEND COMMON_INCLUDE_DIRS ${JETSON_MULTI_MEDIA_API_INCLUDE_DIR}
+    ${LIBJPEG8B_INCLUDE_DIR})
+  # append jetson_multimedia_api source files
+  list(APPEND SOURCE_FILES
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvBuffer.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvElement.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvElementProfiler.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvJpegDecoder.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvJpegEncoder.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvLogging.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvUtils.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvV4l2Element.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvV4l2ElementPlane.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvVideoDecoder.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvVideoEncoder.cpp
+    ${JETSON_MULTI_MEDIA_API_CLASS_DIR}/NvBufSurface.cpp
+  )
+endif ()
+
+macro(add_orbbec_executable TARGET SOURCE)
+  add_executable(${TARGET} ${SOURCE})
+  target_include_directories(${TARGET} PUBLIC ${COMMON_INCLUDE_DIRS})
+  target_link_libraries(${TARGET} ${COMMON_LIBRARIES} ${PROJECT_NAME})
+  ament_target_dependencies(${TARGET} ${dependencies})
+endmacro()
+
+# Define library and nodes
+add_library(${PROJECT_NAME} SHARED
+  ${SOURCE_FILES}
+)
+
+ament_target_dependencies(${PROJECT_NAME} ${dependencies})
+target_include_directories(${PROJECT_NAME} PUBLIC ${COMMON_INCLUDE_DIRS})
+target_link_libraries(${PROJECT_NAME} ${COMMON_LIBRARIES})
+
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "orbbec_camera::OBCameraNodeDriver"
+  EXECUTABLE orbbec_camera_node
+)
+# Add nodes using the macro
+add_orbbec_executable(list_devices_node tools/list_devices_node.cpp)
+add_orbbec_executable(list_depth_work_mode_node tools/list_depth_work_mode.cpp)
+add_orbbec_executable(list_camera_profile_mode_node tools/list_camera_profile.cpp)
+
+add_orbbec_executable(topic_statistics_node tools/topic_statistics.cpp)
+
+
+add_library(frame_latency SHARED tools/frame_latency.cpp)
+target_include_directories(frame_latency PUBLIC ${COMMON_INCLUDE_DIRS})
+target_link_libraries(frame_latency ${COMMON_LIBRARIES})
+ament_target_dependencies(frame_latency ${dependencies})
+
+rclcpp_components_register_node(frame_latency
+        PLUGIN "orbbec_camera::FrameLatencyNode"
+        EXECUTABLE frame_latency_node
+)
+# Install rules
+install(TARGETS ${PROJECT_NAME} frame_latency
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/ DESTINATION include)
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/)
+install(DIRECTORY ${ORBBEC_INCLUDE_DIR} DESTINATION include)
+install(DIRECTORY ${ORBBEC_LIBS_DIR}/ DESTINATION lib/ FILES_MATCHING PATTERN "*.so*")
+if (DEFINED ENV{BUILDING_PACKAGE})
+  # Install udev rules
+  install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/scripts/99-obsensor-libusb.rules
+    DESTINATION /etc/udev/rules.d
+  )
+endif ()
+
+install(TARGETS list_devices_node
+  list_depth_work_mode_node
+  list_camera_profile_mode_node
+  topic_statistics_node
+  DESTINATION lib/${PROJECT_NAME}/)
+
+if (BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif ()
+
+ament_export_include_directories(include ${ORBBEC_INCLUDE_DIR})
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(${dependencies} ${ORBBEC_LIBS})
+
+ament_package()

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/ObSensor.h

@@ -0,0 +1,22 @@
+/* License: Apache 2.0. See LICENSE file in root directory.
+    Copyright(c) 2020  Orbbec Corporation. All Rights Reserved. */
+
+/**
+ * \file ObSensor.h
+ * \brief This file serves as the C entrance for the OrbbecSDK library.
+ *        It includes all necessary header files for OrbbecSDK usage.
+ */
+#pragma once
+
+#include <libobsensor/h/Context.h>
+#include <libobsensor/h/Device.h>
+#include <libobsensor/h/Error.h>
+#include <libobsensor/h/Filter.h>
+#include <libobsensor/h/Frame.h>
+#include <libobsensor/h/ObTypes.h>
+#include <libobsensor/h/Pipeline.h>
+#include <libobsensor/h/Property.h>
+#include <libobsensor/h/RecordPlayback.h>
+#include <libobsensor/h/Sensor.h>
+#include <libobsensor/h/StreamProfile.h>
+#include <libobsensor/h/Version.h>

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/ObSensor.hpp

@@ -0,0 +1,21 @@
+/* License: Apache 2.0. See LICENSE file in root directory.
+    Copyright(c) 2020  Orbbec Corporation. All Rights Reserved. */
+
+/**
+ * \file ObSensor.hpp
+ * \brief This is the main entry point for the OrbbecSDK C++ library.
+ *        It includes all necessary header files for using the library.
+ */
+#pragma once
+
+#include <libobsensor/hpp/Context.hpp>
+#include <libobsensor/hpp/Device.hpp>
+#include <libobsensor/hpp/Error.hpp>
+#include <libobsensor/hpp/Filter.hpp>
+#include <libobsensor/hpp/Frame.hpp>
+#include <libobsensor/hpp/Pipeline.hpp>
+#include <libobsensor/hpp/RecordPlayback.hpp>
+#include <libobsensor/hpp/Sensor.hpp>
+#include <libobsensor/hpp/StreamProfile.hpp>
+#include <libobsensor/hpp/Types.hpp>
+#include <libobsensor/hpp/Version.hpp>

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/Context.h

@@ -0,0 +1,181 @@
+/**
+ * @file Context.h
+ * @brief Context is a management class that describes the runtime of the SDK and is responsible for resource allocation and release of the SDK.
+ * Context has the ability to manage multiple devices. It is responsible for enumerating devices, monitoring device callbacks, and enabling multi-device
+ * synchronization.
+ */
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ObTypes.h"
+
+/**
+ * @brief Create a context object
+ *
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during context creation
+ * @return Pointer to the created context object
+ */
+ob_context *ob_create_context(ob_error **error);
+
+/**
+ * @brief Create a context object with a specified configuration file
+ *
+ * @param[in] config_path Path to the configuration file. If NULL, the default configuration file will be used.
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during context creation
+ * @return Pointer to the created context object
+ */
+ob_context *ob_create_context_with_config(const char *config_path, ob_error **error);
+
+/**
+ * @brief Delete a context object
+ *
+ * @param[in] context Pointer to the context object to be deleted
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during context deletion
+ */
+void ob_delete_context(ob_context *context, ob_error **error);
+
+/**
+ * @brief Get a list of enumerated devices
+ *
+ * @param[in] context Pointer to the context object
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during device enumeration
+ * @return Pointer to the device list object
+ */
+ob_device_list *ob_query_device_list(ob_context *context, ob_error **error);
+
+/**
+ * @brief Enable or disable network device enumeration
+ * @brief After enabling, the network device will be automatically discovered and can be retrieved through @ref ob_query_device_list. The default state can be
+ * set in the configuration file.
+ *
+ * @attention Network device enumeration is performed through the GVCP protocol. If the device is not in the same subnet as the host, it will be discovered but
+ * cannot be connected.
+ *
+ * @param[in] context Pointer to the context object
+ * @param[in] enable true to enable, false to disable
+ * @param[out] error Pointer to an error object that will be populated if an error occurs.
+ */
+void ob_enable_net_device_enumeration(ob_context *context, bool enable, ob_error **error);
+
+/**
+ * @brief Create a network device object
+ *
+ * @param[in] context Pointer to the context object
+ * @param[in] address IP address of the device
+ * @param[in] port Port number of the device
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during device creation
+ * @return Pointer to the created device object
+ */
+ob_device *ob_create_net_device(ob_context *context, const char *address, uint16_t port, ob_error **error);
+
+/**
+ * @brief Set a device plug-in callback function
+ * @attention The added and removed device lists returned through the callback interface need to be released manually
+ *
+ * @param[in] context Pointer to the context object
+ * @param[in] callback Pointer to the callback function triggered when a device is plugged or unplugged
+ * @param[in] user_data Pointer to user data that can be passed to and retrieved from the callback function
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during callback function setting
+ */
+void ob_set_device_changed_callback(ob_context *context, ob_device_changed_callback callback, void *user_data, ob_error **error);
+
+/**
+ * @brief Activates device clock synchronization to synchronize the clock of the host and all created devices (if supported).
+ *
+ * @param[in] context Pointer to the context object
+ * @param[in] repeatInterval The interval for auto-repeated synchronization, in milliseconds. If the value is 0, synchronization is performed only once.
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during execution
+ */
+void ob_enable_device_clock_sync(ob_context *context, uint64_t repeatInterval, ob_error **error);
+#define ob_enable_multi_device_sync ob_enable_device_clock_sync
+
+/**
+ * @brief Free idle memory from the internal frame memory pool
+ *
+ * @param[in] context Pointer to the context object
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during memory freeing
+ */
+void ob_free_idle_memory(ob_context *context, ob_error **error);
+
+/**
+ * @brief Set the global log level
+ *
+ * @attention This interface setting will affect the output level of all logs (terminal, file, callback)
+ *
+ * @param[in] severity Log level to set
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during log level setting
+ */
+void ob_set_logger_severity(ob_log_severity severity, ob_error **error);
+
+/**
+ * @brief Set the log output to a file
+ *
+ * @param[in] severity Log level to output to file
+ * @param[in] directory Path to the log file output directory. If the path is empty, the existing settings will continue to be used (if the existing
+ * configuration is also empty, the log will not be output to the file)
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during log output setting
+ */
+void ob_set_logger_to_file(ob_log_severity severity, const char *directory, ob_error **error);
+
+/**
+ * @brief Set the log callback function
+ *
+ * @param[in] severity Log level to set for the callback function
+ * @param[in] callback Pointer to the callback function
+ * @param[in] user_data Pointer to user data that can be passed to and retrieved from the callback function
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during log callback function setting
+ */
+void ob_set_logger_callback(ob_log_severity severity, ob_log_callback callback, void *user_data, ob_error **error);
+
+/**
+ * @brief Set the log output to the console
+ *
+ * @param[in] severity Log level to output to the console
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during log output setting
+ */
+void ob_set_logger_to_console(ob_log_severity severity, ob_error **error);
+
+/**
+ * @brief Load a license file
+ *
+ * @param[in] filePath Path to the license file
+ * @param[in] key Decryption key. "OB_DEFAULT_DECRYPT_KEY" can be used to represent the default key.
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during license loading
+ */
+void ob_load_license(const char *filePath, const char *key, ob_error **error);
+
+/**
+ * @brief Load a license from data
+ *
+ * @param[in] data Pointer to the license data
+ * @param[in] dataLen Length of the license data
+ * @param[in] key Decryption key. "OB_DEFAULT_DECRYPT_KEY" can be used to represent the default key.
+ * @param[out] error Pointer to an error object that will be populated if an error occurs during license loading
+ */
+void ob_load_license_from_data(const char *data, uint32_t dataLen, const char *key, ob_error **error);
+
+/**
+ * @brief Set the UVC backend for the specified context
+ * This function configures the Universal Video Class (UVC) backend for the given context, allowing the selection of a specific backend for
+ * video capture operations.
+ *
+ * @attention This function is only supported on Linux (ARM) platforms.
+ * Some devices, like the Dabai series, do not support V4L2. Therefore, the default backend is LIBUVC. Ensure that the device
+ * supports V4L2 before setting it as the backend.
+ *
+ * @param[in] context Pointer to the context object
+ * @param[in] uvc_backend Specifies the UVC backend to use:
+ *                       - `UVC_BACKEND_AUTO`: Automatically selects between
+ *                         V4L2 or libuvc based on metadata support.
+ *                       - `UVC_BACKEND_LIBUVC`: Forces the use of libuvc.
+ *                       - `UVC_BACKEND_V4L2`: Forces the use of V4L2.
+ * @param[out] error Pointer to an error object that will be populated if an error occurs.
+ */
+void ob_set_uvc_backend(ob_context *context, ob_uvc_backend uvc_backend, ob_error **error);
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/Device.h

@@ -0,0 +1,982 @@
+/**
+ * @file Device.h
+ * @brief Device-related functions, including operations such as obtaining and creating a device, setting and obtaining device property, and obtaining sensors
+ */
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ObTypes.h"
+#include "Property.h"
+#include "MultipleDevices.h"
+
+/**
+ * @brief Get the number of devices
+ *
+ * @param[in] list Device list object
+ * @param[out] error Log error messages
+ * @return uint32_t return the number of devices
+ */
+uint32_t ob_device_list_device_count(ob_device_list *list, ob_error **error);
+
+/**
+ * @brief Get device name (DEPRECATED)
+ *
+ * @param[in] list Device list object
+ * @param[in] index Device index
+ * @param[out] error Log error messages
+ * @return const char* return device name
+ */
+const char *ob_device_list_get_device_name(ob_device_list *list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Get the pid of the specified device
+ *
+ * @param[in] list Device list object
+ * @param[in] index Device index
+ * @param[out] error Log error messages
+ * @return int return the device pid
+ */
+int ob_device_list_get_device_pid(ob_device_list *list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Get the vid of the specified device
+ *
+ * @param[in] list Device list object
+ * @param[in] index Device index
+ * @param[out] error Log error messages
+ * @return int return device vid
+ */
+int ob_device_list_get_device_vid(ob_device_list *list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Get the uid of the specified device
+ *
+ * @param[in] list Device list object
+ * @param[in] index Device index
+ * @param[out] error Log error messages
+ * @return const char* return the device uid
+ */
+const char *ob_device_list_get_device_uid(ob_device_list *list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Get the serial number of the specified device.
+ *
+ * @param[in] list Device list object.
+ * @param[in] index Device index.
+ * @param[out] error Log error messages.
+ * @return const char* The device UID.
+ */
+const char *ob_device_list_get_device_serial_number(ob_device_list *list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Get device connection type
+ *
+ * @param[in] list Device list object
+ * @param[in] index Device index
+ * @param[out] error Log error messages
+ * @return const char* returns the device connection type，currently supports："USB", "USB1.0", "USB1.1", "USB2.0", "USB2.1", "USB3.0", "USB3.1", "USB3.2",
+ * "Ethernet"
+ */
+const char *ob_device_list_get_device_connection_type(ob_device_list *list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Get device ip address
+ *
+ * @attention Only valid for network devices, otherwise it will return "0.0.0.0".
+ *
+ * @param list Device list object
+ * @param index Device index
+ * @param error Log error messages
+ * @return const char* returns the device ip address，such as "192.168.1.10"
+ */
+const char *ob_device_list_get_device_ip_address(ob_device_list *list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Get the device extension information.
+ *
+ * @param[in] list Device list object
+ * @param[in] index Device index
+ * @param[out] error Log error messages
+ * @return const char* The device extension information
+ */
+const char *ob_device_list_get_extension_info(ob_device_list *list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Create a device.
+ *
+ * @attention If the device has already been acquired and created elsewhere, repeated acquisitions will return an error.
+ *
+ * @param[in] list Device list object.
+ * @param[in] index The index of the device to create.
+ * @param[out] error Log error messages.
+ * @return ob_device* The created device.
+ *
+ */
+ob_device *ob_device_list_get_device(ob_device_list *list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Create a device.
+ *
+ * @attention If the device has already been acquired and created elsewhere, repeated acquisitions will return an error.
+ *
+ * @param[in] list Device list object.
+ * @param[in] serial_number The serial number of the device to create.
+ * @param[out] error Log error messages.
+ * @return ob_device* The created device.
+ */
+ob_device *ob_device_list_get_device_by_serial_number(ob_device_list *list, const char *serial_number, ob_error **error);
+
+/**
+ * @brief Create device by uid
+ * @brief On Linux platform, the uid of the device is composed of bus-port-dev, for example 1-1.2-1. But the SDK will remove the dev number and only keep the
+ * bus-port as the uid to create the device, for example 1-1.2, so that we can create a device connected to the specified USB port. Similarly, users can also
+ * directly pass in bus-port as uid to create device.
+ *
+ * @attention If the device has already been acquired and created elsewhere, repeated acquisitions will return an error.
+ *
+ * @param[in] list Device list object.
+ * @param[in] uid The UID of the device to create.
+ * @param[out] error Log error messages.
+ * @return ob_device* The created device.
+ */
+ob_device *ob_device_list_get_device_by_uid(ob_device_list *list, const char *uid, ob_error **error);
+
+/**
+ * @brief Delete a device.
+ *
+ * @param[in] device The device to be deleted.
+ * @param[out] error Log error messages.
+ */
+void ob_delete_device(ob_device *device, ob_error **error);
+
+/**
+ * @brief Delete device information.
+ *
+ * @param[in] info The device information to be deleted.
+ * @param[out] error Log error messages.
+ */
+void ob_delete_device_info(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Delete a device list.
+ *
+ * @param[in] list The device list object to be deleted.
+ * @param[out] error Log error messages.
+ */
+void ob_delete_device_list(ob_device_list *list, ob_error **error);
+
+/**
+ * @brief Get device information.
+ *
+ * @param[in] device The device to obtain information from.
+ * @param[out] error Log error messages.
+ * @return ob_device_info* The device information.
+ */
+ob_device_info *ob_device_get_device_info(ob_device *device, ob_error **error);
+
+/**
+ * @brief List all sensors.
+ *
+ * @param[in] device The device object.
+ * @param[out] error Log error messages.
+ * @return ob_sensor_list* The list of all sensors.
+ */
+ob_sensor_list *ob_device_get_sensor_list(ob_device *device, ob_error **error);
+
+/**
+ * @brief Get a device's sensor.
+ *
+ * @param[in] device The device object.
+ * @param[in] type The type of sensor to get.
+ * @param[out] error Log error messages.
+ * @return ob_sensor* The acquired sensor.
+ */
+ob_sensor *ob_device_get_sensor(ob_device *device, ob_sensor_type type, ob_error **error);
+
+/**
+ * @brief Set an integer type of device property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The ID of the property to be set.
+ * @param[in] property The property value to be set.
+ * @param[out] error Log error messages.
+ */
+void ob_device_set_int_property(ob_device *device, ob_property_id property_id, int32_t property, ob_error **error);
+
+/**
+ * @brief Get an integer type of device property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The property ID.
+ * @param[out] error Log error messages.
+ * @return int32_t The property value.
+ */
+int32_t ob_device_get_int_property(ob_device *device, ob_property_id property_id, ob_error **error);
+
+/**
+ * @brief Set a float type of device property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The ID of the property to be set.
+ * @param[in] property The property value to be set.
+ * @param[out] error Log error messages.
+ */
+void ob_device_set_float_property(ob_device *device, ob_property_id property_id, float property, ob_error **error);
+
+/**
+ * @brief Get a float type of device property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The property ID.
+ * @param[out] error Log error messages.
+ * @return float The property value.
+ */
+float ob_device_get_float_property(ob_device *device, ob_property_id property_id, ob_error **error);
+
+/**
+ * @brief Set a boolean type of device property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The ID of the property to be set.
+ * @param[in] property The property value to be set.
+ * @param[out] error Log error messages.
+ */
+void ob_device_set_bool_property(ob_device *device, ob_property_id property_id, bool property, ob_error **error);
+
+/**
+ * @brief Get a boolean type of device property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The property ID.
+ * @param[out] error Log error messages.
+ * @return bool The property value.
+ */
+bool ob_device_get_bool_property(ob_device *device, ob_property_id property_id, ob_error **error);
+
+/**
+ * @brief Set structured data.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The ID of the property to be set.
+ * @param[in] data The property data to be set.
+ * @param[in] data_size The size of the property to be set.
+ * @param[out] error Log error messages.
+ */
+void ob_device_set_structured_data(ob_device *device, ob_property_id property_id, const void *data, uint32_t data_size, ob_error **error);
+
+/**
+ * @brief Get structured data of a device property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The ID of the property.
+ * @param[out] data The obtained property data.
+ * @param[out] data_size The size of the obtained property data.
+ * @param[out] error Log error messages.
+ */
+void ob_device_get_structured_data(ob_device *device, ob_property_id property_id, void *data, uint32_t *data_size, ob_error **error);
+
+/**
+ * @brief Set structured data of a device property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The ID of the property.
+ * @param[in] data_bundle The target data to set.
+ * @param[in] cb The data callback.
+ * @param[in] user_data User-defined data that will be returned in the callback.
+ * @param[out] error Log error messages.
+ */
+void ob_device_set_structured_data_ext(ob_device *device, ob_property_id property_id, ob_data_bundle *data_bundle, ob_set_data_callback cb, void *user_data,
+                                       ob_error **error);
+
+/**
+ * @brief Get structured data of a device property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The ID of the property.
+ * @param[out] error Log error messages.
+ * @return ob_data_bundle. NOTE: ob_data_bundle must be freed by ob_delete_data_bundle() because it comes from OrbbecSDK's API.
+ */
+ob_data_bundle *ob_device_get_structured_data_ext(ob_device *device, ob_property_id property_id, ob_error **error);
+
+/**
+ * @brief Set raw data of a device property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The ID of the property to be set.
+ * @param[in] data The property data to be set.
+ * @param[in] data_size The size of the property data to be set.
+ * @param[in] cb The set data callback.
+ * @param[in] async Whether to execute asynchronously.
+ * @param[in] user_data User-defined data that will be returned in the callback.
+ * @param[out] error Log error messages.
+ */
+void ob_device_set_raw_data(ob_device *device, ob_property_id property_id, void *data, uint32_t data_size, ob_set_data_callback cb, bool async, void *user_data,
+                            ob_error **error);
+
+/**
+ * @brief Get raw data of a device property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The ID of the property.
+ * @param[in] cb The get data callback.
+ * @param[in] async Whether to execute asynchronously.
+ * @param[in] user_data User-defined data that will be returned in the callback.
+ * @param[out] error Log error messages.
+ */
+void ob_device_get_raw_data(ob_device *device, ob_property_id property_id, ob_get_data_callback cb, bool async, void *user_data, ob_error **error);
+
+/**
+ * @brief Get the protocol version of the device.
+ *
+ * @param[in] device The device object.
+ * @param[out] error Log error messages.
+ * @return The protocol version of the device.
+ */
+ob_protocol_version ob_device_get_protocol_version(ob_device *device, ob_error **error);
+
+/**
+ * @brief Get the cmdVersion of a property.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The property id.
+ * @param[out] error Log error messages.
+ * @return The cmdVersion of the property.
+ */
+ob_cmd_version ob_device_get_cmd_version(ob_device *device, ob_property_id property_id, ob_error **error);
+
+/**
+ * @brief Get the number of properties supported by the device.
+ *
+ * @param[in] device The device object.
+ * @param[out] error Log error messages.
+ * @return The number of properties supported by the device.
+ */
+uint32_t ob_device_get_supported_property_count(ob_device *device, ob_error **error);
+
+/**
+ * @brief Get the type of property supported by the device.
+ *
+ * @param[in] device The device object.
+ * @param[in] index The property index.
+ * @param[out] error Log error messages.
+ * @return The type of property supported by the device.
+ */
+ob_property_item ob_device_get_supported_property(ob_device *device, uint32_t index, ob_error **error);
+
+/**
+ * @brief Check if a device property permission is supported.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The property id.
+ * @param[in] permission The type of permission that needs to be interpreted.
+ * @param[out] error Log error messages.
+ * @return Whether the property permission is supported.
+ */
+bool ob_device_is_property_supported(ob_device *device, ob_property_id property_id, ob_permission_type permission, ob_error **error);
+
+/**
+ * @brief Get the integer type of device property range.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The property id.
+ * @param[out] error Log error messages.
+ * @return The property range.
+ */
+ob_int_property_range ob_device_get_int_property_range(ob_device *device, ob_property_id property_id, ob_error **error);
+
+/**
+ * @brief Get the float type of device property range.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The property id.
+ * @param[out] error Log error messages.
+ * @return The property range.
+ */
+ob_float_property_range ob_device_get_float_property_range(ob_device *device, ob_property_id property_id, ob_error **error);
+
+/**
+ * @brief Get the boolean type of device property range.
+ *
+ * @param[in] device The device object.
+ * @param[in] property_id The property id.
+ * @param[out] error Log error messages.
+ * @return The property range.
+ */
+ob_bool_property_range ob_device_get_bool_property_range(ob_device *device, ob_property_id property_id, ob_error **error);
+
+/**
+ * @brief Write to an AHB register.
+ *
+ * @param[in] device The device object.
+ * @param reg The register to be written.
+ * @param mask The mask.
+ * @param value The value to be written.
+ * @param[out] error Log error messages.
+ */
+void ob_device_write_ahb(ob_device *device, uint32_t reg, uint32_t mask, uint32_t value, ob_error **error);
+
+/**
+ * @brief Read an AHB register.
+ *
+ * @param[in] device The device object.
+ * @param reg The register to be read.
+ * @param mask The mask.
+ * @param value The value to be read.
+ * @param[out] error Log error messages.
+ */
+void ob_device_read_ahb(ob_device *device, uint32_t reg, uint32_t mask, uint32_t *value, ob_error **error);
+
+/**
+ * @brief Write to an I2C register.
+ *
+ * @param[in] device The device object.
+ * @param module_id The I2C module id to be written.
+ * @param reg The register to be written.
+ * @param mask The mask.
+ * @param value The value to be written.
+ * @param[out] error Log error messages.
+ */
+void ob_device_write_i2c(ob_device *device, uint32_t module_id, uint32_t reg, uint32_t mask, uint32_t value, ob_error **error);
+
+/**
+ * @brief Read an I2C register.
+ *
+ * @param[in] device The device object.
+ * @param module_id The id of the I2C module to be read.
+ * @param reg The register to be read.
+ * @param mask The mask.
+ * @param value The value to be read.
+ * @param[out] error Log error messages.
+ */
+void ob_device_read_i2c(ob_device *device, uint32_t module_id, uint32_t reg, uint32_t mask, uint32_t *value, ob_error **error);
+
+/**
+ * @brief Set the properties of writing to Flash [Asynchronous Callback].
+ *
+ * @param[in] device The device object.
+ * @param offset The flash offset address.
+ * @param data The property data to be written.
+ * @param data_size The size of the property to be written.
+ * @param cb The set data callback.
+ * @param[in] async Whether to execute asynchronously.
+ * @param[in] user_data User-defined data that will be returned in the callback.
+ * @param[out] error Log error messages.
+ */
+void ob_device_write_flash(ob_device *device, uint32_t offset, const void *data, uint32_t data_size, ob_set_data_callback cb, bool async, void *user_data,
+                           ob_error **error);
+
+/**
+ * @brief Read Flash properties [asynchronous callback].
+ *
+ * @param[in] device The device object.
+ * @param offset The flash offset address.
+ * @param data_size The size of the data to be read.
+ * @param cb The read flash data and progress callback.
+ * @param[in] async Whether to execute asynchronously.
+ * @param[in] user_data User-defined data that will be returned in the callback.
+ * @param[out] error Log error messages.
+ */
+void ob_device_read_flash(ob_device *device, uint32_t offset, uint32_t data_size, ob_get_data_callback cb, bool async, void *user_data, ob_error **error);
+
+/**
+ * @brief Set customer data.
+ *
+ * @param[in] device The device object.
+ * @param[in] data The property data to be set.
+ * @param[in] data_size The size of the property to be set,the maximum length cannot exceed 65532 bytes.
+ * @param[out] error Log error messages.
+ */
+void ob_device_write_customer_data(ob_device *device, const void *data, uint32_t data_size, ob_error **error);
+
+/**
+ * @brief Get customer data of a device property.
+ *
+ * @param[in] device The device object.
+ * @param[out] data The obtained property data.
+ * @param[out] data_size The size of the obtained property data.
+ * @param[out] error Log error messages.
+ */
+void ob_device_read_customer_data(ob_device *device, void *data, uint32_t *data_size, ob_error **error);
+
+/**
+ * @brief Upgrade the device firmware.
+ *
+ * @param[in] device The device object.
+ * @param[in] path The firmware path.
+ * @param[in] callback The firmware upgrade progress callback.
+ * @param[in] async Whether to execute asynchronously.
+ * @param[in] user_data User-defined data that will be returned in the callback.
+ * @param[out] error Log error messages.
+ */
+void ob_device_upgrade(ob_device *device, const char *path, ob_device_upgrade_callback callback, bool async, void *user_data, ob_error **error);
+
+/**
+ * @brief Upgrade the device firmware.
+ *
+ * @param[in] device The device object.
+ * @param[in] file_data The firmware file data.
+ * @param[in] file_size The firmware file size.
+ * @param[in] callback The firmware upgrade progress callback.
+ * @param[in] async Whether to execute asynchronously.
+ * @param[in] user_data User-defined data that will be returned in the callback.
+ * @param[out] error Log error messages.
+ */
+void ob_device_upgrade_from_data(ob_device *device, const char *file_data, uint32_t file_size, ob_device_upgrade_callback callback, bool async, void *user_data,
+                                 ob_error **error);
+
+/**
+ * @brief Get the current device status.
+ *
+ * @param[in] device The device object.
+ * @param[out] error Log error messages.
+ *
+ * @return ob_device_state The device state information.
+ */
+ob_device_state ob_device_get_device_state(ob_device *device, ob_error **error);
+
+/**
+ * @brief Monitor device state changes.
+ *
+ * @param[in] device The device object.
+ * @param[in] callback The callback function to be called when the device status changes.
+ * @param[in] user_data User-defined data that will be returned in the callback.
+ * @param[out] error Log error messages.
+ */
+void ob_device_state_changed(ob_device *device, ob_device_state_callback callback, void *user_data, ob_error **error);
+
+/**
+ * @brief Send files to the specified path on the device.
+ *
+ * @param[in] device The device object.
+ * @param[in] file_path The source file path.
+ * @param[in] dst_path The destination path on the device.
+ * @param[in] callback The file sending progress callback.
+ * @param[in] async Whether to execute asynchronously.
+ * @param[in] user_data User-defined data that will be returned in the callback.
+ * @param[out] error Log error messages.
+ */
+void ob_device_send_file_to_destination(ob_device *device, const char *file_path, const char *dst_path, ob_file_send_callback callback, bool async,
+                                        void *user_data, ob_error **error);
+
+/**
+ * @brief Verify the device authorization code.
+ *
+ * @param[in] device The device object.
+ * @param[in] auth_code The authorization code.
+ * @param[out] error Log error messages.
+ *
+ * @return bool Whether the activation is successful.
+ */
+bool ob_device_activate_authorization(ob_device *device, const char *auth_code, ob_error **error);
+
+/**
+ * @brief Write the device authorization code.
+ *
+ * @param[in] device The device object.
+ * @param[in] auth_code The authorization code.
+ * @param[out] error Log error messages.
+ */
+void ob_device_write_authorization_code(ob_device *device, const char *auth_code, ob_error **error);
+
+/**
+ * @brief Get the original parameter list of camera calibration saved on the device.
+ *
+ * @attention The parameters in the list do not correspond to the current open-stream configuration.You need to select the parameters according to the actual
+ * situation, and may need to do scaling, mirroring and other processing. Non-professional users are recommended to use the ob_pipeline_get_camera_param()
+ * interface.
+ *
+ * @param[in] device The device object.
+ * @param[out] error Log error messages.
+ *
+ * @return ob_camera_param_list The camera parameter list.
+ */
+ob_camera_param_list *ob_device_get_calibration_camera_param_list(ob_device *device, ob_error **error);
+
+/**
+ * @brief Get the current depth work mode.
+ *
+ * @param[in] device The device object.
+ * @param[out] error Log error messages.
+ *
+ * @return ob_depth_work_mode The current depth work mode.
+ */
+ob_depth_work_mode ob_device_get_current_depth_work_mode(ob_device *device, ob_error **error);
+
+/**
+ * @brief Switch the depth work mode by ob_depth_work_mode.
+ *        Prefer to use ob_device_switch_depth_work_mode_by_name to switch depth mode when the complete name of the depth work mode is known.
+ *
+ * @param[in] device The device object.
+ * @param[in] work_mode The depth work mode from ob_depth_work_mode_list which is returned by ob_device_get_depth_work_mode_list.
+ * @param[out] error Log error messages.
+ *
+ * @return ob_status The switch result. OB_STATUS_OK: success, other failed.
+ */
+ob_status ob_device_switch_depth_work_mode(ob_device *device, const ob_depth_work_mode *work_mode, ob_error **error);
+
+/**
+ * @brief Switch the depth work mode by work mode name.
+ *
+ * @param[in] device The device object.
+ * @param[in] mode_name The depth work mode name which is equal to ob_depth_work_mode.name.
+ * @param[out] error Log error messages.
+ *
+ * @return ob_status The switch result. OB_STATUS_OK: success, other failed.
+ */
+ob_status ob_device_switch_depth_work_mode_by_name(ob_device *device, const char *mode_name, ob_error **error);
+
+/**
+ * @brief Request the list of supported depth work modes.
+ *
+ * @param[in] device The device object.
+ * @param[out] error Log error messages.
+ *
+ * @return ob_depth_work_mode_list The list of ob_depth_work_mode.
+ */
+ob_depth_work_mode_list *ob_device_get_depth_work_mode_list(ob_device *device, ob_error **error);
+
+/**
+ * @brief Device reboot
+ * @attention The device will be disconnected and reconnected. After the device is disconnected, the interface access to the device handle may be abnormal.
+ * Please use the ob_delete_device interface to delete the handle directly. After the device is reconnected, it can be obtained again.
+ *
+ * @param[in] device Device object
+ * @param[out] error Log error messages
+ */
+void ob_device_reboot(ob_device *device, ob_error **error);
+
+/**
+ * @brief Get the current device synchronization configuration
+ * @brief Device synchronization: including exposure synchronization function and multi-camera synchronization function of different sensors within a single
+ * machine
+ *
+ * @param[in] device Device object
+ * @param[out] error Log error messages
+ * @return ob_device_sync_config Return the device synchronization configuration
+ */
+ob_device_sync_config ob_device_get_sync_config(ob_device *device, ob_error **error);
+
+/**
+ * @brief Set the device synchronization configuration
+ * @brief Used to configure the exposure synchronization function and multi-camera synchronization function of different sensors in a single machine
+ *
+ * @attention Calling this function will directly write the configuration to the device Flash, and it will still take effect after the device restarts. To avoid
+ * affecting the Flash lifespan, do not update the configuration frequently.
+ *
+ * @param[in] device Device object
+ * @param[out] device_sync_config Device synchronization configuration
+ * @param[out] error Log error messages
+ */
+void ob_device_set_sync_config(ob_device *device, ob_device_sync_config device_sync_config, ob_error **error);
+
+/**
+ * @brief Get device name
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return const char* return the device name
+ */
+const char *ob_device_info_name(ob_device_info *info, ob_error **error);
+
+/**
+* @brief Get device pid
+
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return int return the device pid
+*/
+int ob_device_info_pid(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get device vid
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return int return device vid
+ */
+int ob_device_info_vid(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get device uid
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return const char* return device uid
+ */
+const char *ob_device_info_uid(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get device serial number
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return const char* return device serial number
+ */
+const char *ob_device_info_serial_number(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get the firmware version number
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return int return the firmware version number
+ */
+const char *ob_device_info_firmware_version(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get the USB connection type (DEPRECATED)
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return const char* The USB connection type
+ */
+const char *ob_device_info_usb_type(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get the device connection type
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return const char* The connection type，currently supports："USB", "USB1.0", "USB1.1", "USB2.0", "USB2.1", "USB3.0", "USB3.1", "USB3.2", "Ethernet"
+ */
+const char *ob_device_info_connection_type(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get the device IP address
+ *
+ * @attention Only valid for network devices, otherwise it will return "0.0.0.0"
+ *
+ * @param info Device Information
+ * @param error Log error messages
+ * @return const char* The IP address，such as "192.168.1.10"
+ */
+const char *ob_device_info_ip_address(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get the hardware version number
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return const char* The hardware version number
+ */
+const char *ob_device_info_hardware_version(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get the device extension information.
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return const char* The device extension information
+ */
+const char *ob_device_info_get_extension_info(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get the minimum SDK version number supported by the device
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return const char* The minimum SDK version number supported by the device
+ */
+const char *ob_device_info_supported_min_sdk_version(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get the chip name
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return const char* The ASIC name
+ */
+const char *ob_device_info_asicName(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get the device type
+ *
+ * @param[in] info Device Information
+ * @param[out] error Log error messages
+ * @return ob_device_type The device type
+ */
+ob_device_type ob_device_info_device_type(ob_device_info *info, ob_error **error);
+
+/**
+ * @brief Get the number of camera parameter lists
+ *
+ * @param param_list Camera parameter list
+ * @param error Log error messages
+ * @return uint32_t The number of lists
+ */
+uint32_t ob_camera_param_list_count(ob_camera_param_list *param_list, ob_error **error);
+
+/**
+ * @brief Get camera parameters from the camera parameter list
+ *
+ * @param param_list Camera parameter list
+ * @param index Parameter index
+ * @param error Log error messages
+ * @return ob_camera_param The camera parameters. Since it returns the structure object directly, there is no need to provide a delete interface.
+ */
+ob_camera_param ob_camera_param_list_get_param(ob_camera_param_list *param_list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Delete the camera parameter list
+ *
+ * @param param_list Camera parameter list
+ * @param error Log error messages
+ */
+void ob_delete_camera_param_list(ob_camera_param_list *param_list, ob_error **error);
+
+/**
+ * \if English
+ * @brief Get the depth work mode count that ob_depth_work_mode_list hold
+ * @param[in] work_mode_list data struct contain list of ob_depth_work_mode
+ * @param[out] error Log error messages
+ * @return The total number contained in ob_depth_work_mode_list
+ *
+ */
+uint32_t ob_depth_work_mode_list_count(ob_depth_work_mode_list *work_mode_list, ob_error **error);
+
+/**
+ * @brief Get the index target of ob_depth_work_mode from work_mode_list
+ *
+ * @param[in] work_mode_list Data structure containing a list of ob_depth_work_mode
+ * @param[in] index Index of the target ob_depth_work_mode
+ * @param[out] error Log error messages
+ * @return ob_depth_work_mode
+ *
+ */
+ob_depth_work_mode ob_depth_work_mode_list_get_item(ob_depth_work_mode_list *work_mode_list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Free the resources of ob_depth_work_mode_list
+ *
+ * @param[in] work_mode_list Data structure containing a list of ob_depth_work_mode
+ * @param[out] error Log error messages
+ *
+ */
+void ob_delete_depth_work_mode_list(ob_depth_work_mode_list *work_mode_list, ob_error **error);
+
+/**
+ * @brief Free the resources of data_bundle which come from OrbbecSDK's API
+ *
+ * @param data_bundle Data bundle
+ * @param[out] error Log error messages
+ */
+void ob_delete_data_bundle(ob_data_bundle *data_bundle, ob_error **error);
+
+/**
+ * @brief Check if the device supports global timestamp.
+ *
+ * @param[in] device The device object.
+ * @param[out] error Log error messages.
+ * @return bool Whether the device supports global timestamp.
+ */
+bool ob_device_is_global_timestamp_supported(ob_device *device, ob_error **error);
+
+/**
+ * @brief Load depth filter config from file.
+ * @param[in] device The device object.
+ * @param[in] file_path Path of the config file.
+ * @param[out] error Log error messages.
+ */
+void ob_device_load_depth_filter_config(ob_device *device, const char *file_path, ob_error **error);
+
+/**
+ * @brief Reset depth filter config to device default define.
+ * @param[in] device The device object.
+ * @param[out] error Log error messages.
+ */
+void ob_device_reset_default_depth_filter_config(ob_device *device, ob_error **error);
+
+/**
+ * @breif Get the current preset name.
+ * @brief The preset mean a set of parameters or configurations that can be applied to the device to achieve a specific effect or function.
+ *
+ * @param device The device object.
+ * @param error  Log error messages.
+ * @return  The current preset name, it should be one of the preset names returned by @ref ob_device_get_available_preset_list.
+ */
+const char *ob_device_get_current_preset_name(ob_device *device, ob_error **error);
+
+/**
+ * @brief Get the available preset list.
+ * @attention After loading the preset, the settings in the preset will set to the device immediately. Therefore, it is recommended to re-read the device
+ * settings to update the user program temporarily.
+ *
+ * @param device The device object.
+ * @param preset_name  Log error messages. The name should be one of the preset names returned by @ref ob_device_get_available_preset_list.
+ * @param error  Log error messages.
+ */
+void ob_device_load_preset(ob_device *device, const char *preset_name, ob_error **error);
+
+/**
+ * @brief Load preset from json string.
+ * @brief After loading the custom preset, the settings in the custom preset will set to the device immediately.
+ * @brief After loading the custom preset, the available preset list will be appended with the custom preset and named as the file name.
+ *
+ * @param device   The device object.
+ * @param json_file_path  The json file path.
+ * @param error   Log error messages.
+ */
+void ob_device_load_preset_from_json_file(ob_device *device, const char *json_file_path, ob_error **error);
+
+/**
+ * @brief Export current settings as a preset json file.
+ * @brief After exporting the custom preset, the available preset list will be appended with the custom preset and named as the file name.
+ *
+ * @param device   The device object.
+ * @param json_file_path  The json file path.
+ * @param error   Log error messages.
+ */
+void ob_device_export_current_settings_as_preset_json_file(ob_device *device, const char *json_file_path, ob_error **error);
+
+/**
+ * @brief Get the available preset list.
+ *
+ * @param device The device object.
+ * @param error  Log error messages.
+ * @return  The available preset list.
+ */
+ob_device_preset_list *ob_device_get_available_preset_list(ob_device *device, ob_error **error);
+
+/**
+ * @brief Delete the available preset list.
+ *
+ * @param preset_list The available preset list.
+ * @param error  Log error messages.
+ */
+void ob_delete_preset_list(ob_device_preset_list *preset_list, ob_error **error);
+
+/**
+ * @brief Get the number of preset in the preset list.
+ *
+ * @param preset_list The available preset list.
+ * @param error  Log error messages.
+ * @return  The number of preset in the preset list.
+ */
+uint32_t ob_device_preset_list_count(ob_device_preset_list *preset_list, ob_error **error);
+
+/**
+ * @brief Get the name of the preset in the preset list.
+ *
+ * @param preset_list The available preset list.
+ * @param index  The index of the preset in the preset list.
+ * @param error  Log error messages.
+ * @return  The name of the preset in the preset list.
+ */
+const char *ob_device_preset_list_get_name(ob_device_preset_list *preset_list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Check if the preset list has the preset.
+ *
+ * @param preset_list The available preset list.
+ * @param preset_name  The name of the preset.
+ * @param error  Log error messages.
+ * @return  Whether the preset list has the preset. If true, the preset list has the preset. If false, the preset list does not have the preset.
+ */
+bool ob_device_preset_list_has_preset(ob_device_preset_list *preset_list, const char *preset_name, ob_error **error);
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/Error.h

@@ -0,0 +1,62 @@
+/**
+ * @file Error.h
+ * @brief Functions for handling errors, mainly used for obtaining error messages.
+ */
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ObTypes.h"
+
+/**
+ * @brief Get the error status.
+ *
+ * @param[in] error The error object.
+ * @return The error status.
+ */
+ob_status ob_error_status(ob_error *error);
+
+/**
+ * @brief Get the error message.
+ *
+ * @param[in] error The error object.
+ * @return The error message.
+ */
+const char *ob_error_message(const ob_error *error);
+
+/**
+ * @brief Get the name of the API function that caused the error.
+ *
+ * @param[in] error The error object.
+ * @return The name of the API function.
+ */
+const char *ob_error_function(ob_error *error);
+
+/**
+ * @brief Get the error parameters.
+ *
+ * @param[in] error The error object.
+ * @return The error parameters.
+ */
+const char *ob_error_args(ob_error *error);
+
+/**
+ * @brief Get the type of exception that caused the error.
+ *
+ * @param[in] error The error object.
+ * @return The type of exception.
+ */
+ob_exception_type ob_error_exception_type(ob_error *error);
+
+/**
+ * @brief Delete the error object.
+ *
+ * @param[in] error The error object to delete.
+ */
+void ob_delete_error(ob_error *error);
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/Filter.h

@@ -0,0 +1,652 @@
+/**
+ * @file Filter.h
+ * @brief The processing unit of the SDK can perform point cloud generation, format conversion and other functions.
+ */
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ObTypes.h"
+
+/**
+ * @brief Create a PointCloud Filter.
+ *
+ * @param[out] error Log error messages.
+ *
+ * @return A pointcloud_filter object.
+ */
+ob_filter *ob_create_pointcloud_filter(ob_error **error);
+
+/**
+ * @brief Set the camera parameters for the PointCloud Filter.
+ *
+ * @param[in] filter A pointcloud_filter object.
+ * @param[in] param Camera parameters.
+ * @param[out] error Log error messages.
+ */
+void ob_pointcloud_filter_set_camera_param(ob_filter *filter, ob_camera_param param, ob_error **error);
+
+/**
+ * @brief Set the point cloud type parameters for the PointCloud Filter.
+ *
+ * @param[in] filter A pointcloud_filter object.
+ * @param[in] type Point cloud type: depth point cloud or RGBD point cloud.
+ * @param[out] error Log error messages.
+ */
+void ob_pointcloud_filter_set_point_format(ob_filter *filter, ob_format type, ob_error **error);
+
+/**
+ * @brief Set the alignment state of the frames that will be input to produce the point cloud.
+ *
+ * @param[in] filter A pointcloud_filter object.
+ * @param[in] state Alignment status, True: aligned; False: unaligned.
+ * @param[out] error Log error messages.
+ */
+void ob_pointcloud_filter_set_frame_align_state(ob_filter *filter, bool state, ob_error **error);
+
+/**
+ * @brief Set the point cloud data scaling factor.
+ *
+ * @attention Calling this function to set the scale will change the point coordinate scaling factor of the output point cloud frame: posScale = posScale /
+ * scale. The point coordinate scaling factor for the output point cloud frame can be obtained via the @ref ob_points_frame_get_position_value_scale function.
+ *
+ * @param[in] filter A pointcloud_filter object.
+ * @param[in] scale Set the point cloud coordinate data zoom factor.
+ * @param[out] error Log error messages.
+ */
+void ob_pointcloud_filter_set_position_data_scale(ob_filter *filter, float scale, ob_error **error);
+
+/**
+ * @brief Set the point cloud color data normalization.
+ *
+ * @param[in] filter A pointcloud_filter object.
+ * @param[in] state Sets whether the point cloud color data is normalized.
+ * @param[out] error Log error messages.
+ */
+void ob_pointcloud_filter_set_color_data_normalization(ob_filter *filter, bool state, ob_error **error);
+
+/**
+ * @brief Set the point cloud coordinate system.
+ *
+ * @param[in] filter A pointcloud_filter object.
+ * @param[in] type Coordinate system type.
+ * @param[out] error Log error messages.
+ */
+void ob_pointcloud_filter_set_coordinate_system(ob_filter *filter, ob_coordinate_system_type type, ob_error **error);
+
+/**
+ * @brief Create a format convert Filter.
+ *
+ * @param[out] error Log error messages.
+ *
+ * @return A format_convert object.
+ */
+ob_filter *ob_create_format_convert_filter(ob_error **error);
+
+/**
+ * @brief Set the type of format conversion for the format convert Filter.
+ *
+ * @param[in] filter A format convert filter object.
+ * @param[in] type Format conversion type.
+ * @param[out] error Log error messages.
+ */
+void ob_format_convert_filter_set_format(ob_filter *filter, ob_convert_format type, ob_error **error);
+
+/**
+ * @brief Create a compression Filter.
+ *
+ * @param[out] error Log error messages.
+ *
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_compression_filter(ob_error **error);
+
+/**
+ * @brief Set the compression parameters for the compression Filter.
+ *
+ * @param[in] filter A compression_filter object.
+ * @param[in] mode Compression mode OB_COMPRESSION_LOSSLESS or OB_COMPRESSION_LOSSY.
+ * @param[in] params Compression params, struct ob_compression_params, when mode is OB_COMPRESSION_LOSSLESS, params is NULL.
+ * @param[out] error Log error messages.
+ */
+void ob_compression_filter_set_compression_params(ob_filter *filter, ob_compression_mode mode, void *params, ob_error **error);
+
+/**
+ * @brief Create a decompression Filter.
+ *
+ * @param[out] error Log error messages.
+ *
+ * @return A decompression Filter object.
+ */
+ob_filter *ob_create_decompression_filter(ob_error **error);
+
+/**
+ * @brief Create a HoleFilling Filter.
+ *
+ * @param[out] error Log error messages.
+ *
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_holefilling_filter(ob_error **error);
+
+/**
+ * @brief Set the HoleFillingFilter mode.
+ *
+ * @param[in] filter A holefilling_filter object.
+ * @param[in] mode holefilling mode OB_HOLE_FILL_TOP,OB_HOLE_FILL_NEAREST or OB_HOLE_FILL_FAREST.
+ * @param[out] error Log error messages.
+ */
+void ob_holefilling_filter_set_mode(ob_filter *filter, ob_hole_filling_mode mode, ob_error **error);
+
+/**
+ * @brief Get the HoleFillingFilter mode.
+ *
+ * @param[in] filter A holefilling_filter object.
+ * @param[out] error Log error messages.
+ * @return ob_hole_filling_mode
+ */
+ob_hole_filling_mode ob_holefilling_filter_get_mode(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Create a Temporal Filter.
+ *
+ * @param[out] error Log error messages.
+ *
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_temporal_filter(ob_error **error);
+
+/**
+ * @brief Get the TemporalFilter diffscale range.
+ *
+ * @param[in] filter A temporal_filter object.
+ * @param[out] error Log error messages.
+ * @return ob_float_property_range the value of property range.
+ */
+ob_float_property_range ob_temporal_filter_get_diffscale_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Set the TemporalFilter diffscale value.
+ *
+ * @param[in] filter A temporal_filter object.
+ * @param[in] value diffscale value.
+ * @param[out] error Log error messages.
+ */
+void ob_temporal_filter_set_diffscale_value(ob_filter *filter, float value, ob_error **error);
+
+/**
+ * @brief Get the TemporalFilter weight range.
+ *
+ * @param[in] filter A temporal filter object.
+ * @param[out] error Log error messages.
+ */
+ob_float_property_range ob_temporal_filter_get_weight_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Set the TemporalFilter weight range.
+ *
+ * @param[in] filter A temporal_filter object.
+ * @param[in] value weight value.
+ * @param[out] error Log error messages.
+ */
+void ob_temporal_filter_set_weight_value(ob_filter *filter, float value, ob_error **error);
+
+/**
+ * @brief Create a spatial advanced filter.
+ * @param[out] error Log error messages.
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_spatial_advanced_filter(ob_error **error);
+
+/**
+ * @brief Get the spatial advanced filter alpha range.
+ *
+ * @param[in] filter A spatial advanced filter object.
+ * @param[out] error Log error messages.
+ * @return ob_float_property_range the alpha value of property range.
+ */
+ob_float_property_range ob_spatial_advanced_filter_get_alpha_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Get the spatial advanced filter disp diff range.
+ *
+ * @param[in] filter A spatial advanced filter object.
+ * @param[out] error Log error messages.
+ * @return ob_uint16_property_range the dispdiff value of property range.
+ */
+ob_uint16_property_range ob_spatial_advanced_filter_get_disp_diff_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Get the spatial advanced filter radius range.
+ *
+ * @param[in] filter A spatial advanced filter object.
+ * @param[out] error Log error messages.
+ * @return ob_uint16_property_range the radius value of property range.
+ */
+ob_uint16_property_range ob_spatial_advanced_filter_get_radius_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Get the spatial advanced filter magnitude range.
+ *
+ * @param[in] filter A spatial advanced filter object.
+ * @param[out] error Log error messages.
+ * @return ob_int_property_range the magnitude value of property range.
+ */
+ob_int_property_range ob_spatial_advanced_filter_get_magnitude_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Get the spatial advanced filter params.
+ *
+ * @param[in] filter A spatial advanced filter object.
+ * @param[out] error Log error messages.
+ * @return ob_spatial_advanced_filter_params.
+ */
+ob_spatial_advanced_filter_params ob_spatial_advanced_filter_get_filter_params(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Set the spatial advanced filter params.
+ *
+ * @param[in] filter A spatial advanced filter object.
+ * @param[in] params ob_spatial_advanced_filter_params.
+ * @param[out] error Log error messages.
+ */
+void ob_spatial_advanced_filter_set_filter_params(ob_filter *filter, ob_spatial_advanced_filter_params params, ob_error **error);
+
+/**
+ * @brief Create a spatial fast filter.
+ * @param[out] error Log error messages.
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_spatial_fast_filter(ob_error **error);
+
+/**
+ * @brief Get the spatial fast filter window size range.
+ *
+ * @param[in] filter A spatial fast filter object.
+ * @param[out] error Log error messages.
+ * @return ob_uint8_property_range the filter window size value of property range.
+ */
+ob_uint8_property_range ob_spatial_fast_filter_get_size_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Set the spatial fast filter params.
+ *
+ * @param[in] filter A spatial fast filter object.
+ * @param[in] params ob_spatial_fast_filter_params.
+ * @param[out] error Log error messages.
+ */
+void ob_spatial_fast_filter_set_filter_params(ob_filter *filter, ob_spatial_fast_filter_params params, ob_error **error);
+
+/**
+ * @brief Get the spatial fast filter params.
+ *
+ * @param[in] filter A spatial fast filter object.
+ * @param[out] error Log error messages.
+ * @return ob_spatial_fast_filter_params.
+ */
+ob_spatial_fast_filter_params ob_spatial_fast_filter_get_filter_params(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Create a spatial moderate filter.
+ * @param[out] error Log error messages.
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_spatial_moderate_filter(ob_error **error);
+/**
+ * @brief Get the spatial moderate filter disp diff range.
+ *
+ * @param[in] filter A spatial moderate filter object.
+ * @param[out] error Log error messages.
+ * @return ob_uint16_property_range the dispdiff value of property range.
+ */
+ob_uint16_property_range ob_spatial_moderate_filter_get_disp_diff_range(ob_filter *filter, ob_error **error);
+/**
+ * @brief Get the spatial moderate filter magnitude range.
+ *
+ * @param[in] filter A spatial moderate filter object.
+ * @param[out] error Log error messages.
+ * @return ob_uint8_property_range the magnitude value of property range.
+ */
+ob_uint8_property_range ob_spatial_moderate_filter_get_magnitude_range(ob_filter *filter, ob_error **error);
+/**
+ * @brief Get the spatial moderate filter window size range.
+ *
+ * @param[in] filter A spatial moderate filter object.
+ * @param[out] error Log error messages.
+ * @return ob_uint8_property_range the filter window size value of property range.
+ */
+ob_uint8_property_range ob_spatial_moderate_filter_get_size_range(ob_filter *filter, ob_error **error);
+/**
+ * @brief Get the spatial moderate filter params.
+ *
+ * @param[in] filter A spatial moderate filter object.
+ * @param[out] error Log error messages.
+ * @return ob_spatial_moderate_filter_params.
+ */
+ob_spatial_moderate_filter_params ob_spatial_moderate_filter_get_filter_params(ob_filter *filter, ob_error **error);
+/**
+ * @brief Set the spatial moderate filter params.
+ *
+ * @param[in] filter A spatial moderate filter object.
+ * @param[in] params ob_spatial_moderate_filter_params.
+ * @param[out] error Log error messages.
+ */
+void ob_spatial_moderate_filter_set_filter_params(ob_filter *filter, ob_spatial_moderate_filter_params params, ob_error **error);
+
+/**
+ * @brief Create a noise removal filter.
+ * @param[out] error Log error messages.
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_noise_removal_filter(ob_error **error);
+
+/**
+ * @brief Get the noise removal filter disp diff range.
+ *
+ * @param[in] filter A noise removal filter object.
+ * @param[out] error Log error messages.
+ * @return ob_uint16_property_range the disp_diff value of property range.
+ */
+ob_uint16_property_range ob_noise_removal_filter_get_disp_diff_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Get the noise removal filter max size range.
+ *
+ * @param[in] filter noise removal filter object.
+ * @param[out] error Log error messages.
+ * @return ob_int_property_range the _max_size value of property range.
+ */
+ob_int_property_range ob_noise_removal_filter_get_max_size_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Set the noise removal filter params.
+ *
+ * @param[in] filter noise removal filter object.
+ * @param[in] params ob_noise_removal_filter_params.
+ * @param[out] error Log error messages.
+ */
+void ob_noise_removal_filter_set_filter_params(ob_filter *filter, ob_noise_removal_filter_params params, ob_error **error);
+
+/**
+ * @brief Get the noise removal filter params.
+ *
+ * @param[in] filter noise removal filter object.
+ * @param[out] error Log error messages.
+ * @return ob_noise_removal_filter_params.
+ */
+ob_noise_removal_filter_params ob_noise_removal_filter_get_filter_params(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Create a edge noise removal filter.
+ * @param[out] error Log error messages.
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_edge_noise_removal_filter(ob_error **error);
+
+/**
+ * @brief Set the edge noise removal filter params.
+ *
+ * @param[in] filter edge noise removal filter object.
+ * @param[in] params ob_edge_noise_removal_filter_params.
+ * @param[out] error Log error messages.
+ */
+void ob_edge_noise_removal_filter_set_filter_params(ob_filter *filter, ob_edge_noise_removal_filter_params params, ob_error **error);
+
+/**
+ * @brief Get the edge noise removal filter params.
+ *
+ * @param[in] filter edge noise removal filter object.
+ * @param[out] error Log error messages.
+ * @return ob_edge_noise_removal_filter_params.
+ */
+ob_edge_noise_removal_filter_params ob_edge_noise_removal_filter_get_filter_params(ob_filter *filter, ob_error **error);
+
+
+/**
+ * @brief Get the noise removal filter margin left th range.
+ *
+ * @param[in] filter A edge noise removal filter object.
+ * @param[out] error Log error messages.
+ * @return ob_uint16_property_range the margin_left_th value of property range.
+ */
+ob_uint16_property_range ob_edge_noise_removal_filter_get_margin_left_th_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Get the noise removal filter margin right th range.
+ *
+ * @param[in] filter A edge noise removal filter object.
+ * @param[out] error Log error messages.
+ * @return ob_uint16_property_range the margin_right_th value of property range.
+ */
+ob_uint16_property_range ob_edge_noise_removal_filter_get_margin_right_th_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Get the noise removal filter margin top th range.
+ *
+ * @param[in] filter A edge noise removal filter object.
+ * @param[out] error Log error messages.
+ * @return ob_uint16_property_range the margin_top_th value of property range.
+ */
+ob_uint16_property_range ob_edge_noise_removal_filter_get_margin_top_th_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Get the noise removal filter margin bottom th range.
+ *
+ * @param[in] filter A edge noise removal filter object.
+ * @param[out] error Log error messages.
+ * @return ob_uint16_property_range the margin_bottom_th value of property range.
+ */
+ob_uint16_property_range ob_edge_noise_removal_filter_get_margin_bottom_th_range(ob_filter *filter, ob_error **error);
+
+
+/**
+ * @brief Create a decimation filter.
+ * @param[out] error Log error messages.
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_decimation_filter(ob_error **error);
+
+/**
+ * @brief Get the decimation filter scale range.
+ *
+ * @param[in] filter A decimation filter object.
+ * @param[out] error Log error messages.
+ */
+ob_uint8_property_range ob_decimation_filter_get_scale_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Set the decimation filter scale value.
+ *
+ * @param[in] filter A decimation object.
+ * @param[in] value decimation filter scale value.
+ * @param[out] error Log error messages.
+ */
+void ob_decimation_filter_set_scale_value(ob_filter *filter, uint8_t value, ob_error **error);
+
+/**
+ * @brief Get the decimation filter scale value.
+ *
+ * @param[in] filter A decimation object.
+ * @param[out] error Log error messages.
+ * @return decimation filter scale value.
+ */
+uint8_t ob_decimation_filter_get_scale_value(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Create a threshold filter.
+ * @param[out] error Log error messages.
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_threshold_filter(ob_error **error);
+
+/**
+ * @brief Get the threshold filter min range.
+ *
+ * @param[in] filter A threshold filter object.
+ * @param[out] error Log error messages.
+ */
+ob_int_property_range ob_threshold_filter_get_min_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Get the threshold filter max range.
+ *
+ * @param[in] filter A threshold filter object.
+ * @param[out] error Log error messages.
+ */
+ob_int_property_range ob_threshold_filter_get_max_range(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Set the threshold filter scale range.
+ *
+ * @param[in] filter A threshold object.
+ * @param[in] min threshold filter scale min value.
+ * @param[in] max threshold filter scale max value.
+ * @param[out] error Log error messages.
+ */
+bool ob_threshold_filter_set_scale_value(ob_filter *filter, uint16_t min, uint16_t max, ob_error **error);
+
+/**
+ * @brief Create a SequenceId filter.
+ * @param[out] error Log error messages.
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_sequenceId_filter(ob_error **error);
+
+/**
+ * @brief Set the sequence id filter select sequence id.
+ *
+ * @param[in] filter A sequence id object.
+ * @param[in] sequence_id sequence id to pass the filter.
+ * @param[out] error Log error messages.
+ */
+void ob_sequence_id_filter_select_sequence_id(ob_filter *filter, int sequence_id, ob_error **error);
+
+/**
+ * @brief Get the current sequence id.
+ *
+ * @param[in] filter A sequence id object.
+ * @param[out] error Log error messages.
+ * @return sequence id to pass the filter.
+ */
+int ob_sequence_id_filter_get_sequence_id(ob_filter *filter, ob_error **error);
+
+
+/**
+ * @brief Get the current sequence id list.
+ *
+ * @param[in] filter A sequence id object.
+ * @param[out] error Log error messages.
+ */
+ob_sequence_id_item *ob_sequence_id_filter_get_sequence_id_list(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Get the current sequence id list size.
+ *
+ * @param[in] filter A sequence id object.
+ * @param[out] error Log error messages.
+ */
+int ob_sequence_id_filter_get_sequence_id_list_size(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Create a hdr merge.
+ * @param[out] error Log error messages.
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_hdr_merge(ob_error **error);
+
+/**
+ * @brief Create a align.
+ * @param[in] align_to_stream ob_stream_type.
+ * @param[out] error Log error messages.
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_align(ob_error **error, ob_stream_type align_to_stream);
+
+/**
+ * @brief Get the algin stream type.
+ * 
+ * @param[in] filter A align object.
+ * @param[out] error Log error messages.
+ * @return A ob_stream_type.
+ */
+ob_stream_type ob_align_get_to_stream_type(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Create a disparity transform.
+ * @param[in] depth_to_disparity disparity to depth, depth to disparity Conversion.
+ * @param[out] error Log error messages.
+ * @return A depth_filter object.
+ */
+ob_filter *ob_create_disparity_transform(ob_error **error, bool depth_to_disparity);
+
+/**
+ * @brief Reset the filter, clears the cache, and resets the state. If the asynchronous interface is used, the processing thread will also be stopped and the
+ * pending cache frames will be cleared.
+ *
+ * @param[in] filter A filter object.
+ * @param[out] error Log error messages.
+ */
+void ob_filter_reset(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Process the frame (synchronous interface).
+ *
+ * @param[in] filter A filter object.
+ * @param[in] frame Pointer to the frame object to be processed.
+ * @param[out] error Log error messages.
+ *
+ * @return The frame object processed by the filter.
+ */
+ob_frame *ob_filter_process(ob_filter *filter, ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Enable the frame post processing
+ *
+ * @param[in] filter A filter object.
+ * @param[in] enable enable status
+ * @param[out] error Log error messages.
+ */
+void ob_filter_enable(ob_filter *filter, bool enable, ob_error **error);
+
+/**
+ * @brief Get the enable status of the frame post processing
+ *
+ * @param[in] filter A filter object.
+ * @param[out] error Log error messages.
+ *
+ * @return The post processing filter status.
+ */
+bool ob_filter_is_enable(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Set the processing result callback function for the filter (asynchronous callback interface).
+ *
+ * @param[in] filter A filter object.
+ * @param[in] callback Callback function.
+ * @param[in] user_data Arbitrary user data pointer can be passed in and returned from the callback.
+ * @param[out] error Log error messages.
+ */
+void ob_filter_set_callback(ob_filter *filter, ob_filter_callback callback, void *user_data, ob_error **error);
+
+/**
+ * @brief Push the frame into the pending cache for the filter (asynchronous callback interface).
+ *
+ * @param[in] filter A filter object.
+ * @param[in] frame Pointer to the frame object to be processed.
+ * @param[out] error Log error messages.
+ */
+void ob_filter_push_frame(ob_filter *filter, ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Delete the filter.
+ *
+ * @param[in] filter A filter object.
+ * @param[out] error Log error messages.
+ */
+void ob_delete_filter(ob_filter *filter, ob_error **error);
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/Frame.h

@@ -0,0 +1,436 @@
+/**
+ * @file Frame.h
+ * @brief Frame related function is mainly used to obtain frame data and frame information
+ *
+ */
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ObTypes.h"
+
+/**
+ * @brief Get the frame index
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log wrong message
+ * @return uint64_t return the frame index
+ */
+uint64_t ob_frame_index(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the frame format
+ *
+ * @param[in] frame Frame object
+ * @param[out] error  Log error messages
+ * @return ob_format return the frame format
+ */
+ob_format ob_frame_format(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the frame type
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return ob_frame_type return the frame type
+ */
+ob_frame_type ob_frame_get_type(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the hardware timestamp of the frame in milliseconds.
+ * @brief The hardware timestamp is the time point when the frame was captured by the device, on device clock domain.
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return uint64_t return the frame hardware timestamp in milliseconds
+ */
+uint64_t ob_frame_time_stamp(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the hardware timestamp of the frame in microseconds.
+ * @brief The hardware timestamp is the time point when the frame was captured by the device, on device clock domain.
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return uint64_t return the frame hardware timestamp in microseconds
+ */
+uint64_t ob_frame_time_stamp_us(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the system timestamp of the frame in milliseconds.
+ * @brief The system timestamp is the time point when the frame was received by the host, on host clock domain.
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return uint64_t return the frame system timestamp in milliseconds
+ */
+uint64_t ob_frame_system_time_stamp(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the system timestamp of the frame in microseconds.
+ * @brief The system timestamp is the time point when the frame was received by the host, on host clock domain.
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return uint64_t return the frame system timestamp in microseconds
+ */
+uint64_t ob_frame_system_time_stamp_us(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the global timestamp of the frame in microseconds.
+ * @brief The global timestamp is the time point when the frame was was captured by the device, and has been converted to the host clock domain. The
+ * conversion process base on the device timestamp and can eliminate the timer drift of the device
+ *
+ * @attention Only some devices support getting the global timestamp. If the device does not support it, this function will return 0. Check the device support
+ * status by @ref ob_device_is_global_timestamp_supported() function.
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return uint64_t The global timestamp of the frame in microseconds.
+ */
+uint64_t ob_frame_global_time_stamp_us(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get frame data
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return void* * return frame data pointer
+ */
+void *ob_frame_data(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the frame data size
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return uint32_t return the frame data size
+ * If it is point cloud data, it return the number of bytes occupied by all point sets. If you need to find the number of points, you need to divide dataSize
+ * by the structure size of the corresponding point type.
+ */
+uint32_t ob_frame_data_size(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the metadata of the frame
+ *
+ * @param[in] frame frame object
+ * @param[out] error Log error messages
+ * @return void* return the metadata pointer of the frame
+ */
+void *ob_frame_metadata(ob_frame *frame, ob_error **error);
+#define ob_video_frame_metadata ob_frame_metadata  // for compatibility
+
+/**
+ * @brief Get the metadata size of the frame
+ *
+ * @param[in] frame frame object
+ * @param[out] error Log error messages
+ * @return uint32_t return the metadata size of the frame
+ */
+uint32_t ob_frame_metadata_size(ob_frame *frame, ob_error **error);
+#define ob_video_frame_metadata_size ob_frame_metadata_size  // for compatibility
+
+/**
+ * @brief check if the frame contains the specified metadata
+ *
+ * @param[in] frame frame object
+ * @param[in] type metadata type, refer to @ref ob_frame_metadata_type
+ * @param[out] error Log error messages
+ */
+bool ob_frame_has_metadata(ob_frame *frame, ob_frame_metadata_type type, ob_error **error);
+
+/**
+ * @brief Get the metadata value of the frame
+ *
+ * @param[in] frame frame object
+ * @param[in] type  metadata type, refer to @ref ob_frame_metadata_type
+ * @param[out] error Log error messages
+ * @return int64_t return the metadata value of the frame
+ */
+int64_t ob_frame_get_metadata_value(ob_frame *frame, ob_frame_metadata_type type, ob_error **error);
+
+/**
+ * @brief Get the stream profile of the frame
+ *
+ * @attention Require @ref ob_delete_stream_profile() to release the return stream profile.
+ *
+ * @param frame frame object
+ * @param error Log error messages
+ * @return ob_stream_profile* Return the stream profile of the frame, if the frame is not captured by a sensor stream, it will return NULL
+ */
+ob_stream_profile* ob_frame_get_stream_profile(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the sensor of the frame
+ *
+ * @attention Require @ref ob_delete_sensor() to release the return sensor.
+ *
+ * @param[in] frame frame object
+ * @param[out] error Log error messages
+ * @return ob_sensor* return the sensor of the frame, if the frame is not captured by a sensor or the sensor stream has been destroyed, it will return NULL
+ */
+ob_sensor* ob_frame_get_sensor(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the device of the frame
+ *
+ * @attention Require @ref ob_delete_device() to release the return device.
+ *
+ * @param frame frame object
+ * @param[out] error Log error messages
+ * @return ob_device* return the device of the frame, if the frame is not captured by a sensor stream or the device has been destroyed, it will return NULL
+ */
+ob_device* ob_frame_get_device(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get video frame width
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return uint32_t return the frame width
+ */
+uint32_t ob_video_frame_width(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get video frame height
+ *
+ * @param[in] frame  Frame object
+ * @param[out] error Log error messages
+ * @return uint32_t return the frame height
+ */
+uint32_t ob_video_frame_height(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief  Get the effective number of pixels (such as Y16 format frame, but only the lower 10 bits are effective bits, and the upper 6 bits are filled with 0)
+ * @attention Only valid for Y8/Y10/Y11/Y12/Y14/Y16 format
+ *
+ * @param[in] frame video frame object
+ * @param[out] error log error messages
+ * @return uint8_t return the effective number of pixels in the pixel, or 0 if it is an unsupported format
+ */
+uint8_t ob_video_frame_pixel_available_bit_size(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the source sensor type of the ir frame (left or right for dual camera)
+ *
+ * @param frame Frame object
+ * @param ob_error Log error messages
+ * @return ob_sensor_type return the source sensor type of the ir frame
+ */
+ob_sensor_type ob_ir_frame_get_source_sensor_type(ob_frame *frame, ob_error **ob_error);
+
+/**
+ * @brief Get the value scale of the depth frame. The pixel value of the depth frame is multiplied by the scale to give a depth value in millimeters.
+ * For example, if valueScale=0.1 and a certain coordinate pixel value is pixelValue=10000, then the depth value = pixelValue*valueScale = 10000*0.1=1000mm.
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return float The value scale of the depth frame
+ */
+float ob_depth_frame_get_value_scale(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the point position value scale of the points frame. The point position value of the points frame is multiplied by the scale to give a position
+ * value in millimeters. For example, if scale=0.1, the x-coordinate value of a point is x = 10000, which means that the actual x-coordinate value = x*scale =
+ * 10000*0.1 = 1000mm.
+ *
+ * @param[in] frame Frame object
+ * @param[out] error Log error messages
+ * @return float The position value scale of the points frame
+ */
+float ob_points_frame_get_position_value_scale(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Delete a frame object
+ *
+ * @param[in] frame The frame object to delete
+ * @param[out] error Log error messages
+ */
+void ob_delete_frame(ob_frame *frame, ob_error **error);
+/**
+ * @brief Get the number of frames contained in the frameset
+ *
+ * @param[in] frameset frameset object
+ * @param[out] error Log error messages
+ * @return uint32_t return the number of frames
+ */
+uint32_t ob_frameset_frame_count(ob_frame *frameset, ob_error **error);
+/**
+ * @brief Get the depth frame from the frameset.
+ *
+ * @param[in] frameset Frameset object.
+ * @param[out] error Log error messages.
+ * @return ob_frame* Return the depth frame.
+ */
+ob_frame *ob_frameset_depth_frame(ob_frame *frameset, ob_error **error);
+
+/**
+ * @brief Get the color frame from the frameset.
+ *
+ * @param[in] frameset Frameset object.
+ * @param[out] error Log error messages.
+ * @return ob_frame* Return the color frame.
+ */
+ob_frame *ob_frameset_color_frame(ob_frame *frameset, ob_error **error);
+
+/**
+ * @brief Get the infrared frame from the frameset.
+ *
+ * @param[in] frameset Frameset object.
+ * @param[out] error Log error messages.
+ * @return ob_frame* Return the infrared frame.
+ */
+ob_frame *ob_frameset_ir_frame(ob_frame *frameset, ob_error **error);
+
+/**
+ * @brief Get point cloud data from the frameset.
+ *
+ * @param[in] frameset Frameset object.
+ * @param[out] error Log error messages.
+ * @return ob_frame* Return the point cloud frame.
+ */
+ob_frame *ob_frameset_points_frame(ob_frame *frameset, ob_error **error);
+
+/**
+ * @brief Get a frame of a specific type from the frameset.
+ *
+ * @param[in] frameset Frameset object.
+ * @param[in] frame_type Frame type.
+ * @param[out] error Log error messages.
+ * @return ob_frame* Return the frame of the specified type, or nullptr if it does not exist.
+ */
+ob_frame *ob_frameset_get_frame(ob_frame *frameset, ob_frame_type frame_type, ob_error **error);
+
+/**
+ * @brief Get a frame at a specific index from the FrameSet
+ *
+ * @param[in] frameset Frameset object.
+ * @param[in] index The index of the frame.
+ * @param[out] error Log error messages.
+ * @return ob_frame* Return the frame at the specified index, or nullptr if it does not exist.
+ */
+ob_frame *ob_frameset_get_frame_by_index(ob_frame *frameset, int index, ob_error **error);
+
+/**
+ * @brief Get accelerometer frame data.
+ *
+ * @param[in] frame Accelerometer frame.
+ * @param[out] error Log error messages.
+ * @return ob_accel_value Return the accelerometer data.
+ */
+ob_accel_value ob_accel_frame_value(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the temperature when acquiring the accelerometer frame.
+ *
+ * @param[in] frame Accelerometer frame.
+ * @param[out] error Log error messages.
+ * @return float Return the temperature value.
+ */
+float ob_accel_frame_temperature(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get gyroscope frame data.
+ *
+ * @param[in] frame Gyroscope frame.
+ * @param[out] error Log error messages.
+ * @return ob_gyro_value Return the gyroscope data.
+ */
+ob_gyro_value ob_gyro_frame_value(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Get the temperature when acquiring the gyroscope frame.
+ *
+ * @param[in] frame Gyroscope frame.
+ * @param[out] error Log error messages.
+ * @return float Return the temperature value.
+ */
+float ob_gyro_frame_temperature(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Increase the reference count of a frame object.
+ *
+ * @param[in] frame Frame object to increase the reference count.
+ * @param[out] error Log error messages.
+ */
+void ob_frame_add_ref(ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Create an empty frame object based on the specified parameters.
+ *
+ * @param[in] frame_format Frame object format.
+ * @param[in] width Frame object width.
+ * @param[in] height Frame object height.
+ * @param[in] stride_bytes Buffer row span.
+ * @param[in] frame_type Frame object type.
+ * @param[out] error Log error messages.
+ * @return ob_frame* Return an empty frame object.
+ */
+ob_frame *ob_create_frame(ob_format frame_format, int width, int height, int stride_bytes, ob_frame_type frame_type, ob_error **error);
+
+/**
+ * @brief Create a frame object based on an externally created buffer.
+ *
+ * @param[in] frame_format Frame object format.
+ * @param[in] frame_width Frame object width.
+ * @param[in] frame_height Frame object height.
+ * @param[in] buffer Frame object buffer.
+ * @param[in] buffer_size Frame object buffer size.
+ * @param[in] buffer_destroy_cb Destroy callback.
+ * @param[in] buffer_destroy_context Destroy context.
+ * @param[out] error Log error messages.
+ * @return ob_frame* Return the frame object.
+ */
+ob_frame *ob_create_frame_from_buffer(ob_format frame_format, uint32_t frame_width, uint32_t frame_height, uint8_t *buffer, uint32_t buffer_size,
+                                      ob_frame_destroy_callback *buffer_destroy_cb, void *buffer_destroy_context, ob_error **error);
+
+/**
+ * @brief Create an empty frameset object.
+ *
+ * @param[out] error Log error messages.
+ * @return ob_frame* Return the frameset object.
+ */
+ob_frame *ob_create_frameset(ob_error **error);
+
+/**
+ * @brief Add a frame of the specified type to the frameset.
+ *
+ * @param[in] frameset Frameset object.
+ * @param[in] type Type of frame to add.
+ * @param[in] frame Frame object to add.
+ * @param[out] error Log error messages.
+ */
+void ob_frameset_push_frame(ob_frame *frameset, ob_frame_type type, ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Set the system timestamp of a frame object.
+ *
+ * @param[in] frame Frame object to set the system timestamp for.
+ * @param[in] system_timestamp System timestamp to set in milliseconds.
+ * @param[out] error Log error messages.
+ */
+void ob_frame_set_system_time_stamp(ob_frame *frame, uint64_t system_timestamp, ob_error **error);
+
+/**
+ * @brief Set the device timestamp of a frame object.
+ *
+ * @param[in] frame Frame object to set the device timestamp.
+ * @param[in] device_timestamp Device timestamp to set in milliseconds.
+ * @param[out] error Log error messages.
+ */
+void ob_frame_set_device_time_stamp(ob_frame *frame, uint64_t device_timestamp, ob_error **error);
+
+/**
+ * @brief Set the device timestamp of a frame object.
+ *
+ * @param[in] frame Frame object to set the device timestamp for.
+ * @param[in] device_timestamp_us Device timestamp to set in microseconds.
+ * @param[out] error Log error messages.
+ */
+void ob_frame_set_device_time_stamp_us(ob_frame *frame, uint64_t device_timestamp_us, ob_error **error);
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/MultipleDevices.h

@@ -0,0 +1,124 @@
+/**
+ * @file MultipleDevices.h
+ * @brief This file contains the multiple devices related API witch is used to control the synchronization between multiple devices and the synchronization
+ * between different sensor within single device.
+ * @brief The synchronization between multiple devices is complex, and different models have different synchronization modes and limitations. please refer to
+ * the product manual for details.
+ * @brief As the Depth and Infrared are the same sensor physically, the behavior of the Infrared is same as the Depth in the synchronization mode.
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ObTypes.h"
+#include "Device.h"
+
+/**
+ * @brief Get the supported multi device sync mode bitmap of the device.
+ * @brief For example, if the return value is 0b00001100, it means the device supports @ref OB_MULTI_DEVICE_SYNC_MODE_PRIMARY and @ref
+ * OB_MULTI_DEVICE_SYNC_MODE_SECONDARY. User can check the supported mode by the code:
+ * ```c
+ *   if(supported_mode_bitmap & OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN){
+ *      //support OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN
+ *   }
+ *   if(supported_mode_bitmap & OB_MULTI_DEVICE_SYNC_MODE_STANDALONE){
+ *     //support OB_MULTI_DEVICE_SYNC_MODE_STANDALONE
+ *   }
+ *   // and so on
+ * ```
+ * @param[in] device The device handle.
+ * @param[out] error The error information.
+ * @return uint16_t return the supported multi device sync mode bitmap of the device.
+ */
+uint16_t ob_device_get_supported_multi_device_sync_mode_bitmap(ob_device *device, ob_error **error);
+
+/**
+ * @brief set the multi device sync configuration of the device.
+ *
+ * @param[in] device The device handle.
+ * @param[in] config The multi device sync configuration.
+ * @param[out] error The error information.
+ */
+void ob_device_set_multi_device_sync_config(ob_device *device, const ob_multi_device_sync_config *config, ob_error **error);
+
+/**
+ * @brief get the multi device sync configuration of the device.
+ *
+ * @param[in] device The device handle.
+ * @param[out] error The error information.
+ * @return ob_multi_device_sync_config return the multi device sync configuration of the device.
+ */
+ob_multi_device_sync_config ob_device_get_multi_device_sync_config(ob_device *device, ob_error **error);
+
+/**
+ * @brief send the capture command to the device.
+ * @brief The device will start one time image capture after receiving the capture command when it is in the @ref OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING
+ *
+ * @attention The frequency of the user call this function multiplied by the number of frames per trigger should be less than the frame rate of the stream. The
+ * number of frames per trigger can be set by @ref framesPerTrigger.
+ * @attention For some models，receive and execute the capture command will have a certain delay and performance consumption, so the frequency of calling this
+ * function should not be too high, please refer to the product manual for the specific supported frequency.
+ * @attention If the device is not in the @ref OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING mode, device will ignore the capture command.
+ *
+ * @param[in] device The device handle.
+ * @param[out] error The error information.
+ */
+void ob_device_trigger_capture(ob_device *device, ob_error **error);
+
+/**
+ * @brief set the timestamp reset configuration of the device.
+ *
+ * @param[in] device The device handle.
+ * @param[in] config The timestamp reset configuration.
+ * @param[out] error The error information.
+ */
+void ob_device_set_timestamp_reset_config(ob_device *device, const ob_device_timestamp_reset_config *config, ob_error **error);
+
+/**
+ * @brief get the timestamp reset configuration of the device.
+ *
+ * @param[in] device The device handle.
+ * @param[out] error The error information.
+ * @return ob_device_timestamp_reset_config return the timestamp reset configuration of the device.
+ */
+ob_device_timestamp_reset_config ob_device_get_timestamp_reset_config(ob_device *device, ob_error **error);
+
+/**
+ * @brief send the timestamp reset command to the device.
+ * @brief The device will reset the timer for calculating the timestamp for output frames to 0 after receiving the timestamp reset command when the timestamp
+ * reset function is enabled. The timestamp reset function can be enabled by call @ref ob_device_set_timestamp_reset_config.
+ *
+ * @attention If the stream of the device is started, the timestamp of the continuous frames output by the stream will jump once after the timestamp reset.
+ * @attention Due to the timer of device is not high-accuracy, the timestamp of the continuous frames output by the stream will drift after a long time. User
+ * can call this function periodically to reset the timer to avoid the timestamp drift, the recommended interval time is 60 minutes.
+ *
+ * @param[in] device The device handle.
+ * @param[out] error The error information.
+ */
+void ob_device_timestamp_reset(ob_device *device, ob_error **error);
+
+/**
+ * @brief Alias for @ref ob_device_timestamp_reset since it is more accurate.
+ */
+#define ob_device_timer_reset ob_device_timestamp_reset
+
+/**
+ * @brief synchronize the timer of the device with the host.
+ * @brief After calling this function, the timer of the device will be synchronized with the host. User can call this function to multiple devices to
+ * synchronize all timers of the devices.
+ *
+ * @attention If the stream of the device is started, the timestamp of the continuous frames output by the stream will may jump once after the timer sync.
+ * @attention Due to the timer of device is not high-accuracy, the timestamp of the continuous frames output by the stream will drift after a long time. User
+ * can call this function periodically to synchronize the timer to avoid the timestamp drift, the recommended interval time is 60 minutes.
+ *
+ * @param[in] device The device handle.
+ * @param[out] error The error information.
+ */
+void ob_device_timer_sync_with_host(ob_device *device, ob_error **error);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/Pipeline.h

@@ -0,0 +1,389 @@
+/**
+ * @file Pipeline.h
+ * @brief The SDK's advanced API can quickly implement functions such as switching streaming, frame synchronization, software filtering, etc., suitable for
+ * applications, and the algorithm focuses on rgbd data stream scenarios. If you are on real-time or need to handle synchronization separately, align the scene.
+ * Please use the interface of Device's Lower API.
+ */
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ObTypes.h"
+
+/**
+ * @brief Create a pipeline object
+ *
+ * @param[out] error Log error messages
+ * @return ob_pipeline* return the pipeline object
+ */
+ob_pipeline *ob_create_pipeline(ob_error **error);
+
+/**
+ * @brief Using device objects to create pipeline objects
+ *
+ * @param[in] dev Device object used to create pipeline
+ * @param[out] error  Log error messages
+ * @return ob_pipeline* return the pipeline object
+ */
+ob_pipeline *ob_create_pipeline_with_device(ob_device *dev, ob_error **error);
+
+/**
+ * @brief Use the playback file to create a pipeline object
+ *
+ * @param[in] file_name The playback file path used to create the pipeline
+ * @param[out] error Log error messages
+ * @return ob_pipeline* return the pipeline object
+ */
+ob_pipeline *ob_create_pipeline_with_playback_file(const char *file_name, ob_error **error);
+
+/**
+ * @brief Delete pipeline objects
+ *
+ * @param[in] pipeline  The pipeline object to be deleted
+ * @param[out] error Log error messages
+ */
+void ob_delete_pipeline(ob_pipeline *pipeline, ob_error **error);
+
+/**
+ * @brief Start the pipeline with default parameters
+ *
+ * @param[in] pipeline  pipeline object
+ * @param[out] error Log error messages
+ */
+void ob_pipeline_start(ob_pipeline *pipeline, ob_error **error);
+
+/**
+ * @brief Start the pipeline with configuration parameters
+ *
+ * @param[in] pipeline  pipeline object
+ * @param[in] config Parameters to be configured
+ * @param[out] error Log error messages
+ */
+void ob_pipeline_start_with_config(ob_pipeline *pipeline, ob_config *config, ob_error **error);
+
+/**
+ * @brief Start the pipeline and set the frame collection data callback
+ *
+ * @param[in] pipeline pipeline object
+ * @param[in] config  Parameters to be configured
+ * @param[in] callback Trigger a callback when all frame data in the frameset arrives
+ * @param[in] user_data  Pass in any user data and get it from the callback
+ * @param[out] error  Log error messages
+ */
+void ob_pipeline_start_with_callback(ob_pipeline *pipeline, ob_config *config, ob_frameset_callback callback, void *user_data, ob_error **error);
+
+/**
+ * @brief Stop pipeline
+ *
+ * @param[in] pipeline pipeline object
+ * @param[out] error  Log error messages
+ */
+void ob_pipeline_stop(ob_pipeline *pipeline, ob_error **error);
+
+/**
+ * @brief Get the configuration object associated with the pipeline
+ * @brief Returns default configuration if the user has not configured
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[out] error Log error messages
+ * @return ob_config* The configuration object
+ */
+ob_config *ob_pipeline_get_config(ob_pipeline *pipeline, ob_error **error);
+
+/**
+ * @brief Wait for a set of frames to be returned synchronously
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[in] timeout_ms The timeout for waiting (in milliseconds)
+ * @param[out] error Log error messages
+ * @return ob_frame* The frameset that was waited for. A frameset is a special frame that can be used to obtain independent frames from the set.
+ */
+ob_frame *ob_pipeline_wait_for_frameset(ob_pipeline *pipeline, uint32_t timeout_ms, ob_error **error);
+
+/**
+ * @brief Get the device object associated with the pipeline
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[out] error Log error messages
+ * @return ob_device* The device object
+ */
+ob_device *ob_pipeline_get_device(ob_pipeline *pipeline, ob_error **error);
+
+/**
+ * @brief Get the playback object associated with the pipeline
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[out] error Log error messages
+ * @return ob_playback* The playback object
+ */
+ob_playback *ob_pipeline_get_playback(ob_pipeline *pipeline, ob_error **error);
+
+/**
+ * @brief Get the stream profile list associated with the pipeline
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[in] sensorType The sensor type. The supported sensor types can be obtained through the ob_device_get_sensor_list() interface.
+ * @param[out] error Log error messages
+ * @return ob_stream_profile_list* The stream profile list
+ */
+ob_stream_profile_list *ob_pipeline_get_stream_profile_list(ob_pipeline *pipeline, ob_sensor_type sensorType, ob_error **error);
+
+/**
+ * @brief Enable frame synchronization
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[out] error Log error messages
+ */
+void ob_pipeline_enable_frame_sync(ob_pipeline *pipeline, ob_error **error);
+
+/**
+ * @brief Disable frame synchronization
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[out] error Log error messages
+ */
+void ob_pipeline_disable_frame_sync(ob_pipeline *pipeline, ob_error **error);
+
+/**
+ * @brief Dynamically switch the corresponding configuration
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[in] config The pipeline configuration
+ * @param[out] error Log error messages
+ */
+void ob_pipeline_switch_config(ob_pipeline *pipeline, ob_config *config, ob_error **error);
+
+/**
+ * @brief Get the current camera parameters
+ *
+ * @param[in] pipeline pipeline object
+ * @param[in] colorWidth color width
+ * @param[in] colorHeight color height
+ * @param[in] depthWidth depth width
+ * @param[in] depthHeight depth height
+ * @param[out] error Log error messages
+ * @return ob_camera_param returns camera internal parameters
+ */
+ob_camera_param ob_pipeline_get_camera_param_with_profile(ob_pipeline *pipeline, uint32_t colorWidth, uint32_t colorHeight, uint32_t depthWidth,
+                                                          uint32_t depthHeight, ob_error **error);
+
+/**
+ * @brief Get current camera parameters
+ * @attention If D2C is enabled, it will return the camera parameters after D2C, if not, it will return to the default parameters
+ *
+ * @param[in] pipeline pipeline object
+ * @param[out] error Log error messages
+ * @return ob_camera_param The camera internal parameters
+ */
+ob_camera_param ob_pipeline_get_camera_param(ob_pipeline *pipeline, ob_error **error);
+
+/**
+ * @brief Get device calibration parameters with the specified configuration
+ *
+ * @param[in] pipeline pipeline object
+ * @param[in] config The pipeline configuration
+ * @param[out] error Log error messages
+ * @return ob_calibration_param The calibration parameters
+ */
+ob_calibration_param ob_pipeline_get_calibration_param(ob_pipeline *pipeline, ob_config *config, ob_error **error);
+
+/**
+ * @brief Return a list of D2C-enabled depth sensor resolutions corresponding to the input color sensor resolution
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[in] color_profile The input profile of the color sensor
+ * @param[in] align_mode The input align mode
+ * @param[out] error Log error messages
+ * @return ob_stream_profile_list* The list of D2C-enabled depth sensor resolutions
+ */
+ob_stream_profile_list *ob_get_d2c_depth_profile_list(ob_pipeline *pipeline, ob_stream_profile *color_profile, ob_align_mode align_mode, ob_error **error);
+
+/**
+ * @brief Get the valid area after D2C (DEPRECATED)
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[in] distance The working distance
+ * @param[out] error Log error messages
+ * @return ob_rect The area information that is valid after D2C at the working distance
+ */
+ob_rect ob_get_d2c_valid_area(ob_pipeline *pipeline, uint32_t distance, ob_error **error);
+
+/**
+ * @brief Get the valid area between the minimum distance and maximum distance after D2C
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[in] minimum_distance The minimum working distance
+ * @param[in] maximum_distance The maximum working distance
+ * @param[out] error Log error messages
+ * @return ob_rect The area information that is valid after D2C at the working distance
+ */
+ob_rect ob_get_d2c_range_valid_area(ob_pipeline *pipeline, uint32_t minimum_distance, uint32_t maximum_distance, ob_error **error);
+
+/**
+ * @brief Start recording
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[in] file_name The recorded file path
+ * @param[out] error Log error messages
+ */
+void ob_pipeline_start_record(ob_pipeline *pipeline, const char *file_name, ob_error **error);
+
+/**
+ * @brief Stop recording
+ *
+ * @param[in] pipeline The pipeline object
+ * @param[out] error Log error messages
+ */
+void ob_pipeline_stop_record(ob_pipeline *pipeline, ob_error **error);
+
+/**
+ * @brief Create the pipeline configuration
+ *
+ * @param[out] error Log error messages
+ * @return ob_config* The configuration object
+ */
+ob_config *ob_create_config(ob_error **error);
+
+/**
+ * @brief Delete the pipeline configuration
+ *
+ * @param[in] config The configuration to be deleted
+ * @param[out] error Log error messages
+ */
+void ob_delete_config(ob_config *config, ob_error **error);
+
+/**
+ * @brief Enable the specified stream in the pipeline configuration
+ *
+ * @param[in] config The pipeline configuration
+ * @param[in] profile The stream configuration to be enabled
+ * @param[out] error Log error messages
+ */
+void ob_config_enable_stream(ob_config *config, ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Enable a video stream to be used in the configuration.
+ *
+ * This function configures and enables a video stream with specific parameters.
+ * Users must specify all parameters explicitly as C does not support default arguments.
+ * Refer to the product manual for details on supported resolutions and formats for different camera models.
+ *
+ * @param config Pointer to the configuration structure.
+ * @param type The video stream type.
+ * @param width The video stream width.
+ * @param height The video stream height.
+ * @param fps The video stream frame rate.
+ * @param format The video stream format.
+ * @param error Pointer to store the error if operation fails.
+ */
+void ob_config_enable_video_stream(ob_config *config, ob_stream_type type, int width, int height, int fps, ob_format format, ob_error **error);
+
+/**
+ * @brief Enable an accelerometer stream to be used in the configuration.
+ *
+ * This function configures and enables an accelerometer stream with specific parameters.
+ * Users must specify all parameters explicitly. For details on available full-scale ranges and sample rates,
+ * please refer to the product manual.
+ *
+ * @param config Pointer to the configuration structure.
+ * @param fullScaleRange The full-scale range of the accelerometer.
+ * @param sampleRate The sample rate of the accelerometer.
+ * @param error Pointer to store the error if operation fails.
+ */
+void ob_config_enable_accel_stream(ob_config *config, ob_accel_full_scale_range full_scale_range, ob_accel_sample_rate sample_rate, ob_error **error);
+
+/**
+ * @brief Enable a gyroscope stream to be used in the configuration.
+ *
+ * This function configures and enables a gyroscope stream with specific parameters.
+ * Users must specify all parameters explicitly. For details on available full-scale ranges and sample rates,
+ * please refer to the product manual.
+ *
+ * @param config Pointer to the configuration structure.
+ * @param fullScaleRange The full-scale range of the gyroscope.
+ * @param sampleRate The sample rate of the gyroscope.
+ * @param error Pointer to store the error if operation fails.
+ */
+void ob_config_enable_gyro_stream(ob_config *config, ob_gyro_full_scale_range full_scale_range, ob_gyro_sample_rate sample_rate, ob_error **error);
+
+/**
+ * @deprecated Use @ref ob_config_enable_stream instead
+ * @brief Enable all streams in the pipeline configuration
+ *
+ * @param[in] config The pipeline configuration
+ * @param[out] error Log error messages
+ */
+void ob_config_enable_all_stream(ob_config *config, ob_error **error);
+
+/**
+ * @brief  Get the enabled stream profile list in the pipeline configuration
+ *
+ * @param config The pipeline configuration
+ * @param error Log error messages
+ * @return ob_stream_profile_list* The enabled stream profile list, should be released by @ref ob_delete_stream_profile_list after use
+ */
+ob_stream_profile_list *ob_config_get_enabled_stream_profile_list(ob_config *config, ob_error **error);
+
+/**
+ * @brief Disable a specific stream in the pipeline configuration
+ *
+ * @param[in] config The pipeline configuration
+ * @param[in] type The type of stream to be disabled
+ * @param[out] error Log error messages
+ */
+void ob_config_disable_stream(ob_config *config, ob_stream_type type, ob_error **error);
+
+/**
+ * @brief Disable all streams in the pipeline configuration
+ *
+ * @param[in] config The pipeline configuration
+ * @param[out] error Log error messages
+ */
+void ob_config_disable_all_stream(ob_config *config, ob_error **error);
+
+/**
+ * @brief Set the alignment mode for the pipeline configuration
+ *
+ * @param[in] config The pipeline configuration
+ * @param[in] mode The alignment mode to be set
+ * @param[out] error Log error messages
+ */
+void ob_config_set_align_mode(ob_config *config, ob_align_mode mode, ob_error **error);
+
+/**
+ * @brief Set whether depth scaling is required after setting D2C
+ *
+ * @param[in] config The pipeline configuration
+ * @param[in] enable Whether scaling is required
+ * @param[out] error Log error messages
+ */
+void ob_config_set_depth_scale_require(ob_config *config, bool enable, ob_error **error);
+
+/**
+ * @brief Set the target resolution for D2C, which is applicable when the color stream is not enabled using the OrbbecSDK and the depth needs to be D2C
+ * Note: When using the OrbbecSDK to enable the color stream, this interface should also be used to set the D2C target resolution. The configuration of the
+ * enabled color stream is preferred for D2C.
+ *
+ * @param[in] config The pipeline configuration
+ * @param[in] d2c_target_width The target width for D2C
+ * @param[in] d2c_target_height The target height for D2C
+ * @param[out] error Log error messages
+ */
+void ob_config_set_d2c_target_resolution(ob_config *config, uint32_t d2c_target_width, uint32_t d2c_target_height, ob_error **error);
+
+/**
+ * @brief Set the frame aggregation output mode for the pipeline configuration
+ * @brief The processing strategy when the FrameSet generated by the frame aggregation function does not contain the frames of all opened streams (which
+ * can be caused by different frame rates of each stream, or by the loss of frames of one stream): drop directly or output to the user.
+ *
+ * @param[in] config The pipeline configuration
+ * @param[in] mode The frame aggregation output mode to be set (default mode is @ref OB_FRAME_AGGREGATE_OUTPUT_FULL_FRAME_REQUIRE)
+ * @param[out] error Log error messages
+ */
+void ob_config_set_frame_aggregate_output_mode(ob_config *config, ob_frame_aggregate_output_mode mode, ob_error **error);
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/Property.h

@@ -0,0 +1,780 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2020 Orbbec  Corporation. All Rights Reserved.
+
+/**
+ * @file Property.h
+ * @brief Control command property list maintenance
+ */
+
+#ifdef OB_SENSOR_SDK_DEVELOPER
+#include "libobsensor/internal/InternalProperty.h"
+#else  // not define OB_SENSOR_SDK_DEVELOPER
+#ifndef _OB_PROPERTY_H_
+#define _OB_PROPERTY_H_
+
+#include "ObTypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Enumeration value describing all attribute control commands of the device
+ */
+typedef enum {
+    /**
+     * @brief LDP switch
+     */
+    OB_PROP_LDP_BOOL = 2,
+
+    /**
+     * @brief Laser switch
+     */
+    OB_PROP_LASER_BOOL = 3,
+
+    /**
+     * @brief laser pulse width
+     */
+    OB_PROP_LASER_PULSE_WIDTH_INT = 4,
+
+    /**
+     * @brief Laser current (uint: mA)
+     */
+    OB_PROP_LASER_CURRENT_FLOAT = 5,
+
+    /**
+     * @brief IR flood switch
+     */
+    OB_PROP_FLOOD_BOOL = 6,
+
+    /**
+     * @brief IR flood level
+     */
+    OB_PROP_FLOOD_LEVEL_INT = 7,
+    /**
+     * @brief temperature compensation switch
+     */
+    OB_PROP_TEMPERATURE_COMPENSATION_BOOL = 8,
+
+    /**
+     * @brief Depth mirror
+     */
+    OB_PROP_DEPTH_MIRROR_BOOL = 14,
+
+    /**
+     * @brief Depth flip
+     */
+    OB_PROP_DEPTH_FLIP_BOOL = 15,
+
+    /**
+     * @brief Depth Postfilter
+     */
+    OB_PROP_DEPTH_POSTFILTER_BOOL = 16,
+
+    /**
+     * @brief Depth Holefilter
+     */
+    OB_PROP_DEPTH_HOLEFILTER_BOOL = 17,
+
+    /**
+     * @brief IR mirror
+     */
+    OB_PROP_IR_MIRROR_BOOL = 18,
+
+    /**
+     * @brief IR flip
+     */
+    OB_PROP_IR_FLIP_BOOL = 19,
+
+    /**
+     * @brief Minimum depth threshold
+     */
+    OB_PROP_MIN_DEPTH_INT = 22,
+
+    /**
+     * @brief Maximum depth threshold
+     */
+    OB_PROP_MAX_DEPTH_INT = 23,
+
+    /**
+     * @brief Software filter switch
+     */
+    OB_PROP_DEPTH_SOFT_FILTER_BOOL = 24,
+
+    /**
+     * @brief LDP status
+     */
+    OB_PROP_LDP_STATUS_BOOL = 32,
+
+    /**
+     * @brief soft filter maxdiff param
+     */
+    OB_PROP_DEPTH_MAX_DIFF_INT = 40,
+
+    /**
+     * @brief soft filter maxSpeckleSize
+     */
+    OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT = 41,
+
+    /**
+     * @brief Hardware d2c is on
+     */
+    OB_PROP_DEPTH_ALIGN_HARDWARE_BOOL = 42,
+
+    /**
+     * @brief Timestamp adjustment
+     */
+    OB_PROP_TIMESTAMP_OFFSET_INT = 43,
+
+    /**
+     * @brief Hardware distortion switch Rectify
+     */
+    OB_PROP_HARDWARE_DISTORTION_SWITCH_BOOL = 61,
+
+    /**
+     * @brief Fan mode switch
+     */
+    OB_PROP_FAN_WORK_MODE_INT = 62,
+
+    /**
+     * @brief Multi-resolution D2C mode
+     */
+    OB_PROP_DEPTH_ALIGN_HARDWARE_MODE_INT = 63,
+
+    /**
+     * @brief Anti_collusion activation status
+     */
+    OB_PROP_ANTI_COLLUSION_ACTIVATION_STATUS_BOOL = 64,
+
+    /**
+     * @brief the depth precision level, which may change the depth frame data unit, needs to be confirmed through the ValueScale interface of
+     * DepthFrame
+     */
+    OB_PROP_DEPTH_PRECISION_LEVEL_INT = 75,
+
+    /**
+     * @brief tof filter range configuration
+     */
+    OB_PROP_TOF_FILTER_RANGE_INT = 76,
+
+    /**
+     * @brief laser mode, the firmware terminal currently only return 1: IR Drive, 2: Torch
+     */
+    OB_PROP_LASER_MODE_INT = 79,
+
+    /**
+     * @brief brt2r-rectify function switch (brt2r is a special module on mx6600), 0: Disable, 1: Rectify Enable
+     */
+    OB_PROP_RECTIFY2_BOOL = 80,
+
+    /**
+     * @brief Color mirror
+     */
+    OB_PROP_COLOR_MIRROR_BOOL = 81,
+
+    /**
+     * @brief Color flip
+     */
+    OB_PROP_COLOR_FLIP_BOOL = 82,
+
+    /**
+     * @brief Indicator switch, 0: Disable, 1: Enable
+     */
+    OB_PROP_INDICATOR_LIGHT_BOOL = 83,
+
+    /**
+     * @brief Disparity to depth switch, false: switch to software disparity convert to depth, true: switch to hardware disparity convert to depth
+     */
+    OB_PROP_DISPARITY_TO_DEPTH_BOOL = 85,
+
+    /**
+     * @brief BRT function switch (anti-background interference), 0: Disable, 1: Enable
+     */
+    OB_PROP_BRT_BOOL = 86,
+
+    /**
+     * @brief Watchdog function switch, 0: Disable, 1: Enable
+     */
+    OB_PROP_WATCHDOG_BOOL = 87,
+
+    /**
+     * @brief External signal trigger restart function switch, 0: Disable, 1: Enable
+     */
+    OB_PROP_EXTERNAL_SIGNAL_RESET_BOOL = 88,
+
+    /**
+     * @brief Heartbeat monitoring function switch, 0: Disable, 1: Enable
+     */
+    OB_PROP_HEARTBEAT_BOOL = 89,
+
+    /**
+     * @brief Depth cropping mode device: OB_DEPTH_CROPPING_MODE
+     */
+    OB_PROP_DEPTH_CROPPING_MODE_INT = 90,
+
+    /**
+     * @brief D2C preprocessing switch (such as RGB cropping), 0: off, 1: on
+     */
+    OB_PROP_D2C_PREPROCESS_BOOL = 91,
+
+    /**
+     * @brief Custom RGB cropping switch, 0 is off, 1 is on custom cropping, and the ROI cropping area is issued
+     */
+    OB_PROP_RGB_CUSTOM_CROP_BOOL = 94,
+
+    /**
+     * @brief Device operating mode (power consumption)
+     */
+    OB_PROP_DEVICE_WORK_MODE_INT = 95,
+
+    /**
+     * @brief Device communication type, 0: USB; 1: Ethernet(RTSP)
+     */
+    OB_PROP_DEVICE_COMMUNICATION_TYPE_INT = 97,
+
+    /**
+     * @brief Switch infrared imaging mode, 0: active IR mode, 1: passive IR mode
+     */
+    OB_PROP_SWITCH_IR_MODE_INT = 98,
+
+    /**
+     * @brief Laser power level
+     */
+    OB_PROP_LASER_POWER_LEVEL_CONTROL_INT = 99,
+
+    /**
+     * @brief LDP's measure distance, unit: mm
+     */
+    OB_PROP_LDP_MEASURE_DISTANCE_INT = 100,
+
+    /**
+     * @brief Reset device time to zero
+     */
+    OB_PROP_TIMER_RESET_SIGNAL_BOOL = 104,
+
+    /**
+     * @brief Enable send reset device time signal to other device. true: enable, false: disable
+     */
+    OB_PROP_TIMER_RESET_TRIGGER_OUT_ENABLE_BOOL = 105,
+
+    /**
+     * @brief Delay to reset device time, unit: us
+     */
+    OB_PROP_TIMER_RESET_DELAY_US_INT = 106,
+
+    /**
+     * @brief Signal to capture image
+     */
+    OB_PROP_CAPTURE_IMAGE_SIGNAL_BOOL = 107,
+
+    /**
+     * @brief Right IR sensor mirror state
+     */
+    OB_PROP_IR_RIGHT_MIRROR_BOOL = 112,
+
+    /**
+     * @brief Number frame to capture once a 'OB_PROP_CAPTURE_IMAGE_SIGNAL_BOOL' effect. range: [1, 255]
+     */
+    OB_PROP_CAPTURE_IMAGE_FRAME_NUMBER_INT = 113,
+
+    /**
+     * @brief Right IR sensor flip state. true: flip image, false: origin, default: false
+     */
+    OB_PROP_IR_RIGHT_FLIP_BOOL = 114,
+
+    /**
+     * @brief Color sensor rotation, angle{0, 90, 180, 270}
+     */
+    OB_PROP_COLOR_ROTATE_INT = 115,
+
+    /**
+     * @brief IR/Left-IR sensor rotation, angle{0, 90, 180, 270}
+     */
+    OB_PROP_IR_ROTATE_INT = 116,
+
+    /**
+     * @brief Right IR sensor rotation, angle{0, 90, 180, 270}
+     */
+    OB_PROP_IR_RIGHT_ROTATE_INT = 117,
+
+    /**
+     * @brief Depth sensor rotation, angle{0, 90, 180, 270}
+     */
+    OB_PROP_DEPTH_ROTATE_INT = 118,
+
+    /**
+     * @brief Get hardware laser power actual level which real state of laser element. OB_PROP_LASER_POWER_LEVEL_CONTROL_INT99）will effect this command
+     * which it setting and changed the hardware laser energy level.
+     */
+    OB_PROP_LASER_POWER_ACTUAL_LEVEL_INT = 119,
+
+    /**
+     * @brief USB's power state, enum type: OBUSBPowerState
+     */
+    OB_PROP_USB_POWER_STATE_INT = 121,
+
+    /**
+     * @brief DC's power state, enum type: OBDCPowerState
+     */
+    OB_PROP_DC_POWER_STATE_INT = 122,
+
+    /**
+     * @brief Device development mode switch, optional modes can refer to the definition in @ref OBDeviceDevelopmentMode,the default mode is
+     * @ref OB_USER_MODE
+     * @attention The device takes effect after rebooting when switching modes.
+     */
+    OB_PROP_DEVICE_DEVELOPMENT_MODE_INT = 129,
+
+    /**
+     * @brief Multi-DeviceSync synchronized signal trigger out is enable state. true: enable, false: disable
+     */
+    OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL = 130,
+
+    /**
+     * @brief Restore factory settings and factory parameters
+     * @attention This command can only be written, and the parameter value must be true. The command takes effect after restarting the device.
+     */
+    OB_PROP_RESTORE_FACTORY_SETTINGS_BOOL = 131,
+
+    /**
+     * @brief Enter recovery mode (flashing mode) when boot the device
+     * @attention The device will take effect after rebooting with the enable option. After entering recovery mode, you can upgrade the device system. Upgrading
+     * the system may cause system damage, please use it with caution.
+     */
+    OB_PROP_BOOT_INTO_RECOVERY_MODE_BOOL = 132,
+
+    /**
+     * @brief Query whether the current device is running in recovery mode (read-only)
+     */
+    OB_PROP_DEVICE_IN_RECOVERY_MODE_BOOL = 133,
+
+    /**
+     * @brief Capture interval mode, 0:time interval, 1:number interval
+     */
+    OB_PROP_CAPTURE_INTERVAL_MODE_INT = 134,
+
+    /**
+     * @brief Capture time interval
+     */
+    OB_PROP_CAPTURE_IMAGE_TIME_INTERVAL_INT = 135,
+
+    /**
+     * @brief Capture number interval
+     */
+    OB_PROP_CAPTURE_IMAGE_NUMBER_INTERVAL_INT = 136,
+
+    /*
+     * @brief Timer reset function enable
+     */
+    OB_PROP_TIMER_RESET_ENABLE_BOOL = 140,
+
+    /**
+     * @brief Enable or disable the device to retry USB2.0 re-identification when the device is connected to a USB2.0 port.
+     * @brief This feature ensures that the device is not mistakenly identified as a USB 2.0 device when connected to a USB 3.0 port.
+     */
+    OB_PROP_DEVICE_USB2_REPEAT_IDENTIFY_BOOL = 141,
+
+    /**
+     * @brief Reboot device delay mode. Delay time unit: ms, range: [0, 8000).
+     */
+    OB_PROP_DEVICE_REBOOT_DELAY_INT = 142,
+
+    /**
+     * @brief Query the status of laser overcurrent protection (read-only)
+     */
+    OB_PROP_LASER_OVERCURRENT_PROTECTION_STATUS_BOOL = 148,
+
+    /**
+     * @brief Query the status of laser pulse width protection (read-only)
+     */
+    OB_PROP_LASER_PULSE_WIDTH_PROTECTION_STATUS_BOOL = 149,
+
+    /**
+     * @brief Laser always on, true: always on, false: off, laser will be turned off when out of exposure time
+     */
+    OB_PROP_LASER_ALWAYS_ON_BOOL = 174,
+
+    /**
+     * @brief Laser on/off alternate mode, 0: off, 1: on-off alternate, 2: off-on alternate
+     * @attention When turn on this mode, the laser will turn on and turn off alternately each frame.
+     */
+    OB_PROP_LASER_ON_OFF_PATTERN_INT = 175,
+
+    /**
+     * @brief Depth unit flexible adjustment\
+     * @brief This property allows continuous adjustment of the depth unit, unlike @ref OB_PROP_DEPTH_PRECISION_LEVEL_INT must be set to some fixed value.
+     */
+    OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT = 176,
+
+    /**
+     * @brief Laser control, 0: off, 1: on, 2: auto
+     *
+     */
+    OB_PROP_LASER_CONTROL_INT = 182,
+
+    /**
+     * @brief IR brightness
+     */
+    OB_PROP_IR_BRIGHTNESS_INT = 184,
+
+    /**
+     * @brief slave device sync status
+     */
+    OB_PROP_SLAVE_DEVICE_SYNC_STATUS_BOOL = 188,
+
+    /**
+     * @brief Color AE max exposure
+     */
+    OB_PROP_COLOR_AE_MAX_EXPOSURE_INT = 189,
+
+    /**
+     * @brief IR AE max exposure
+     */
+    OB_PROP_IR_AE_MAX_EXPOSURE_INT = 190,
+
+    /**
+     * @brief disparity search range mode
+     */
+    OB_PROP_DISP_SEARCH_RANGE_MODE_INT = 191,
+
+    /**
+     * @brief cpu temperature correction . true: calibrate temperature
+     */
+    OB_PROP_CPU_TEMPERATURE_CALIBRATION_BOOL = 199,
+
+    /**
+     * @brief Baseline calibration parameters
+     */
+    OB_STRUCT_BASELINE_CALIBRATION_PARAM = 1002,
+
+    /**
+     * @brief Device temperature information
+     */
+    OB_STRUCT_DEVICE_TEMPERATURE = 1003,
+
+    /**
+     * @brief TOF exposure threshold range
+     */
+    OB_STRUCT_TOF_EXPOSURE_THRESHOLD_CONTROL = 1024,
+
+    /**
+     * @brief get/set serial number
+     */
+    OB_STRUCT_DEVICE_SERIAL_NUMBER = 1035,
+
+    /**
+     * @brief get/set device time
+     */
+    OB_STRUCT_DEVICE_TIME = 1037,
+
+    /**
+     * @brief Multi-device synchronization mode and parameter configuration
+     */
+    OB_STRUCT_MULTI_DEVICE_SYNC_CONFIG = 1038,
+
+    /**
+     * @brief RGB cropping ROI
+     */
+    OB_STRUCT_RGB_CROP_ROI = 1040,
+
+    /**
+     * @brief Device IP address configuration
+     */
+    OB_STRUCT_DEVICE_IP_ADDR_CONFIG = 1041,
+
+    /**
+     * @brief The current camera depth mode
+     */
+    OB_STRUCT_CURRENT_DEPTH_ALG_MODE = 1043,
+
+    /**
+     * @brief A list of depth accuracy levels, returning an array of uin16_t, corresponding to the enumeration
+     */
+    OB_STRUCT_DEPTH_PRECISION_SUPPORT_LIST = 1045,
+
+    /**
+     * @brief Device network static ip config record
+     * @brief Using for get last static ip config，witch is record in device flash when user set static ip config
+     *
+     * @attention read only
+     */
+    OB_STRUCT_DEVICE_STATIC_IP_CONFIG_RECORD = 1053,
+
+    /**
+     * @brief Using to configure the depth sensor's HDR mode
+     * @brief The Value type is @ref OBHdrConfig
+     *
+     * @attention After enable HDR mode, the depth sensor auto exposure will be disabled.
+     */
+    OB_STRUCT_DEPTH_HDR_CONFIG = 1059,
+
+    /**
+     * @brief Color Sensor AE ROI configuration
+     * @brief The Value type is @ref OBRegionOfInterest
+     */
+    OB_STRUCT_COLOR_AE_ROI = 1060,
+
+    /**
+     * @brief Depth Sensor AE ROI configuration
+     * @brief The Value type is @ref OBRegionOfInterest
+     * @brief Since the ir sensor is the same physical sensor as the depth sensor, this property will also effect the ir sensor.
+     */
+    OB_STRUCT_DEPTH_AE_ROI = 1061,
+
+    /**
+     * @brief ASIC serial number
+     */
+    OB_STRUCT_ASIC_SERIAL_NUMBER = 1063,
+
+    /**
+     * @brief Color camera auto exposure
+     */
+    OB_PROP_COLOR_AUTO_EXPOSURE_BOOL = 2000,
+
+    /**
+     * @brief Color camera exposure adjustment
+     */
+    OB_PROP_COLOR_EXPOSURE_INT = 2001,
+
+    /**
+     * @brief Color camera gain adjustment
+     */
+    OB_PROP_COLOR_GAIN_INT = 2002,
+
+    /**
+     * @brief Color camera automatic white balance
+     */
+    OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL = 2003,
+
+    /**
+     * @brief Color camera white balance adjustment
+     */
+    OB_PROP_COLOR_WHITE_BALANCE_INT = 2004,
+
+    /**
+     * @brief Color camera brightness adjustment
+     */
+    OB_PROP_COLOR_BRIGHTNESS_INT = 2005,
+
+    /**
+     * @brief Color camera sharpness adjustment
+     */
+    OB_PROP_COLOR_SHARPNESS_INT = 2006,
+
+    /**
+     * @brief Color camera shutter adjustment
+     */
+    OB_PROP_COLOR_SHUTTER_INT = 2007,
+
+    /**
+     * @brief Color camera saturation adjustment
+     */
+    OB_PROP_COLOR_SATURATION_INT = 2008,
+
+    /**
+     * @brief Color camera contrast adjustment
+     */
+    OB_PROP_COLOR_CONTRAST_INT = 2009,
+
+    /**
+     * @brief Color camera gamma adjustment
+     */
+    OB_PROP_COLOR_GAMMA_INT = 2010,
+
+    /**
+     * @brief Color camera image rotation
+     */
+    OB_PROP_COLOR_ROLL_INT = 2011,
+
+    /**
+     * @brief Color camera auto exposure priority
+     */
+    OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT = 2012,
+
+    /**
+     * @brief Color camera brightness compensation
+     */
+    OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT = 2013,
+
+    /**
+     * @brief Color camera color tint
+     */
+    OB_PROP_COLOR_HUE_INT = 2014,
+
+    /**
+     * @brief Color Camera Power Line Frequency
+     */
+    OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT = 2015,
+
+    /**
+     * @brief Automatic exposure of depth camera (infrared camera will be set synchronously under some models of devices)
+     */
+    OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL = 2016,
+
+    /**
+     * @brief Depth camera exposure adjustment (infrared cameras will be set synchronously under some models of devices)
+     */
+    OB_PROP_DEPTH_EXPOSURE_INT = 2017,
+
+    /**
+     * @brief Depth camera gain adjustment (infrared cameras will be set synchronously under some models of devices)
+     */
+    OB_PROP_DEPTH_GAIN_INT = 2018,
+
+    /**
+     * @brief Infrared camera auto exposure (depth camera will be set synchronously under some models of devices)
+     */
+    OB_PROP_IR_AUTO_EXPOSURE_BOOL = 2025,
+
+    /**
+     * @brief Infrared camera exposure adjustment (some models of devices will set the depth camera synchronously)
+     */
+    OB_PROP_IR_EXPOSURE_INT = 2026,
+
+    /**
+     * @brief Infrared camera gain adjustment (the depth camera will be set synchronously under some models of devices)
+     */
+    OB_PROP_IR_GAIN_INT = 2027,
+
+    /**
+     * @brief Select Infrared camera data source channel. If not support throw exception. 0 : IR stream from IR Left sensor; 1 : IR stream from IR Right sensor;
+     */
+    OB_PROP_IR_CHANNEL_DATA_SOURCE_INT = 2028,
+
+    /**
+     * @brief Depth effect dedistortion, true: on, false: off. mutually exclusive with D2C function, RM_Filter disable When hardware or software D2C is enabled.
+     */
+    OB_PROP_DEPTH_RM_FILTER_BOOL = 2029,
+
+    /**
+     * @brief Color camera maximal gain
+     */
+    OB_PROP_COLOR_MAXIMAL_GAIN_INT = 2030,
+
+    /**
+     * @brief Color camera shutter gain
+     */
+    OB_PROP_COLOR_MAXIMAL_SHUTTER_INT = 2031,
+
+    /**
+     * @brief The enable/disable switch for IR short exposure function, supported only by a few devices.
+     */
+    OB_PROP_IR_SHORT_EXPOSURE_BOOL = 2032,
+
+    /**
+     * @brief Color camera HDR
+     */
+    OB_PROP_COLOR_HDR_BOOL = 2034,
+
+    /**
+     * @brief IR long exposure mode switch read and write.
+     */
+    OB_PROP_IR_LONG_EXPOSURE_BOOL = 2035,
+
+    /**
+     * @brief Setting and getting the USB device frame skipping mode status, true: frame skipping mode, false: non-frame skipping mode.
+     */
+    OB_PROP_SKIP_FRAME_BOOL = 2036,
+
+    /**
+     * @brief Depth HDR merge, true: on, false: off.
+     */
+    OB_PROP_HDR_MERGE_BOOL = 2037,
+
+    /**
+     * @brief Color camera FOCUS
+     */
+    OB_PROP_COLOR_FOCUS_INT = 2038,
+
+    /**
+     * @brief Software disparity to depth
+     */
+    OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL = 3004,
+
+    /**
+     * @brief Depth data unpacking function switch (each open stream will be turned on by default, support RLE/Y10/Y11/Y12/Y14 format)
+     */
+    OB_PROP_SDK_DEPTH_FRAME_UNPACK_BOOL = 3007,
+
+    /**
+     * @brief IR data unpacking function switch (each current will be turned on by default, support RLE/Y10/Y11/Y12/Y14 format)
+     */
+    OB_PROP_SDK_IR_FRAME_UNPACK_BOOL = 3008,
+
+    /**
+     * @brief Accel data conversion function switch (on by default)
+     */
+    OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL = 3009,
+
+    /**
+     * @brief Gyro data conversion function switch (on by default)
+     */
+    OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL = 3010,
+
+    /**
+     * @brief Left IR frame data unpacking function switch (each current will be turned on by default, support RLE/Y10/Y11/Y12/Y14 format)
+     */
+    OB_PROP_SDK_IR_LEFT_FRAME_UNPACK_BOOL = 3011,
+
+    /**
+     * @brief Right IR frame data unpacking function switch (each current will be turned on by default, support RLE/Y10/Y11/Y12/Y14 format)
+     */
+    OB_PROP_SDK_IR_RIGHT_FRAME_UNPACK_BOOL = 3012,
+
+    /**
+     * @brief depth Margin Filter
+     */
+    OB_PROP_SDK_DEPTH_RECTIFY_MG_FILTER_BOOL = 3013,
+
+    /**
+     * @brief Depth Stream Industry Working Mode Settings, currently only supported by DCW2.
+     */
+    OB_PROP_DEPTH_INDUSTRY_MODE_INT = 3024,
+
+    /**
+     * @brief "OpenNI device setting data stream packet size, such as DCW2.
+     */
+    OB_PROP_STREAM_PACK_UNIT_INT = 3025,
+
+    /**
+     * @brief Calibration JSON file read from device (Femto Mega, read only)
+     */
+    OB_RAW_DATA_CAMERA_CALIB_JSON_FILE = 4029,
+} OBPropertyID,
+    ob_property_id;
+
+// For backward compatibility
+#define OB_PROP_TIMER_RESET_TRIGGLE_OUT_ENABLE_BOOL OB_PROP_TIMER_RESET_TRIGGER_OUT_ENABLE_BOOL
+#define OB_PROP_LASER_ON_OFF_MODE_INT OB_PROP_LASER_ON_OFF_PATTERN_INT
+#define OB_PROP_LASER_ENERGY_LEVEL_INT OB_PROP_LASER_POWER_LEVEL_CONTROL_INT
+#define OB_PROP_LASER_HW_ENERGY_LEVEL_INT OB_PROP_LASER_POWER_ACTUAL_LEVEL_INT
+#define OB_PROP_DEVICE_USB3_REPEAT_IDENTIFY_BOOL OB_PROP_DEVICE_USB2_REPEAT_IDENTIFY_BOOL
+#define OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL OB_PROP_DEPTH_SOFT_FILTER_BOOL
+
+/**
+ * @brief The data type used to describe all property settings
+ */
+typedef enum OBPropertyType {
+    OB_BOOL_PROPERTY   = 0, /**< Boolean property */
+    OB_INT_PROPERTY    = 1, /**< Integer property */
+    OB_FLOAT_PROPERTY  = 2, /**< Floating-point property */
+    OB_STRUCT_PROPERTY = 3, /**< Struct property */
+} OBPropertyType,
+    ob_property_type;
+
+/**
+ * @brief Used to describe the characteristics of each property
+ */
+typedef struct OBPropertyItem {
+    OBPropertyID     id;         /**< Property ID */
+    const char      *name;       /**< Property name */
+    OBPropertyType   type;       /**< Property type */
+    OBPermissionType permission; /**< Property read and write permission */
+} OBPropertyItem, ob_property_item;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // _OB_PROPERTY_H_
+#endif  // OB_SENSOR_SDK_DEVELOPER

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/RecordPlayback.h

@@ -0,0 +1,132 @@
+/**
+ * @file RecordPlayback.h
+ * @brief Header file for recording and playback functions.
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ObTypes.h"
+
+/**
+ * @brief Create a recorder for data recording.
+ *
+ * @param[out] error Pointer to log error messages.
+ * @return Pointer to the recorder object.
+ */
+ob_recorder *ob_create_recorder(ob_error **error);
+
+/**
+ * @brief Create a recorder for data recording.
+ *
+ * @param dev The device object used to create the recorder.
+ * @param[out] error Pointer to log error messages.
+ * @return Pointer to the recorder object.
+ */
+ob_recorder *ob_create_recorder_with_device(ob_device *dev, ob_error **error);
+
+/**
+ * @brief Delete the recorder object.
+ *
+ * @param recorder Pointer to the recorder object.
+ * @param[out] error Pointer to log error messages.
+ */
+void ob_delete_recorder(ob_recorder *recorder, ob_error **error);
+
+/**
+ * @brief Start recording.
+ *
+ * @param[in] recorder Pointer to the recorder object.
+ * @param[in] filename Recorded file name.
+ * @param[in] async Whether to record asynchronously.
+ * @param[out] error Pointer to log error messages.
+ */
+void ob_recorder_start(ob_recorder *recorder, const char *filename, bool async, ob_error **error);
+
+/**
+ * @brief Stop recording.
+ *
+ * @param[in] recorder Pointer to the recorder object.
+ * @param[out] error Pointer to log error messages.
+ */
+void ob_recorder_stop(ob_recorder *recorder, ob_error **error);
+
+/**
+ * @brief Write frame data to the recorder.
+ *
+ * @param[in] recorder Pointer to the recorder object.
+ * @param[in] frame Pointer to the frame data to write.
+ * @param[out] error Pointer to log error messages.
+ */
+void ob_recorder_write_frame(ob_recorder *recorder, ob_frame *frame, ob_error **error);
+
+/**
+ * @brief Create a playback object.
+ *
+ * @param[in] filename Playback filename.
+ * @param[out] error Pointer to log error messages.
+ * @return Pointer to the playback object.
+ */
+ob_playback *ob_create_playback(const char *filename, ob_error **error);
+
+/**
+ * @brief Delete the playback object.
+ *
+ * @param[in] playback Pointer to the playback object.
+ * @param[out] error Pointer to log error messages.
+ */
+void ob_delete_playback(ob_playback *playback, ob_error **error);
+
+/**
+ * @brief Start playback, with data returned from the callback.
+ *
+ * @param[in] playback Pointer to the playback object.
+ * @param[in] callback Callback function for playback data.
+ * @param[in] user_data User data.
+ * @param[in] type Type of playback data.
+ * @param[out] error Pointer to log error messages.
+ */
+void ob_playback_start(ob_playback *playback, ob_playback_callback callback, void *user_data, ob_media_type type, ob_error **error);
+
+/**
+ * @brief Stop playback.
+ *
+ * @param[in] playback Pointer to the playback object.
+ * @param[out] error Pointer to log error messages.
+ */
+void ob_playback_stop(ob_playback *playback, ob_error **error);
+
+/**
+ * @brief Set the playback state.
+ *
+ * @param[in] playback Pointer to the playback object.
+ * @param[in] callback Playback status callback function.
+ * @param[in] user_data User data.
+ * @param[out] error Pointer to log error messages.
+ */
+void ob_set_playback_state_callback(ob_playback *playback, ob_media_state_callback callback, void *user_data, ob_error **error);
+
+/**
+ * @brief Get the device information in the recording file.
+ *
+ * @param[in] playback Pointer to the playback object.
+ * @param[out] error Pointer to log error messages.
+ * @return Pointer to the device information.
+ */
+ob_device_info *ob_playback_get_device_info(ob_playback *playback, ob_error **error);
+
+/**
+ * @brief Get the intrinsic and extrinsic parameter information in the recording file.
+ *
+ * @param[in] playback Pointer to the playback object.
+ * @param[out] error Pointer to log error messages.
+ * @return  Camera intrinsic and extrinsic parameter.
+ */
+ob_camera_param ob_playback_get_camera_param(ob_playback *playback, ob_error **error);
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/Sensor.h

@@ -0,0 +1,162 @@
+/**
+ * @file Sensor.h
+ * @brief Defines types related to sensors, used for obtaining stream configurations, opening and closing streams, and setting and getting sensor properties.
+ */
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ObTypes.h"
+
+/**
+ * @brief Get the type of the sensor.
+ *
+ * @param[in] sensor The sensor object.
+ * @param[out] error Logs error messages.
+ * @return The sensor type.
+ */
+ob_sensor_type ob_sensor_get_type(ob_sensor *sensor, ob_error **error);
+
+/**
+ * @brief Get a list of all supported stream profiles.
+ *
+ * @param[in] sensor The sensor object.
+ * @param[out] error Logs error messages.
+ * @return A list of stream profiles.
+ */
+ob_stream_profile_list *ob_sensor_get_stream_profile_list(ob_sensor *sensor, ob_error **error);
+
+/**
+ * @brief Request the list of recommended filter list
+ *
+ * @param[in] sensor The ob_sensor object.
+ * @param[out] error Log error messages.
+ *
+ * @return ob_filter_list
+ */
+ob_filter_list *ob_sensor_get_recommended_filter_list(ob_sensor *sensor, ob_error **error);
+
+/**
+ * @brief Get the number of recommended filter list
+ *
+ * @param filter_list Recommended filter list
+ * @param error Log error messages
+ * @return uint32_t The number of list
+ */
+uint32_t ob_filter_list_get_count(ob_filter_list *filter_list, ob_error **error);
+
+/**
+ * @brief Get the number of recommended filter list
+ *
+ * @param filter_list Recommended filter list
+ * @param index Recommended filter index
+ * @param error Log error messages
+ * @return ob_filter The index of ob_filter
+ */
+ob_filter *ob_get_filter(ob_filter_list *filter_list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Get the name of ob_filter
+ *
+ * @param filter ob_filter object
+ * @param error Log error messages
+ * @return char The filter of name
+ */
+const char *ob_get_filter_name(ob_filter *filter, ob_error **error);
+
+/**
+ * @brief Delete a list of ob_filter objects.
+ *
+ * @param[in] filter_list The list of ob_filter objects to delete.
+ * @param[out] error Logs error messages.
+ */
+void ob_delete_filter_list(ob_filter_list *filter_list, ob_error **error);
+
+/**
+ * @brief Open the current sensor and set the callback data frame.
+ *
+ * @param[in] sensor The sensor object.
+ * @param[in] profile The stream configuration information.
+ * @param[in] callback The callback function triggered when frame data arrives.
+ * @param[in] user_data Any user data to pass in and get from the callback.
+ * @param[out] error Logs error messages.
+ */
+void ob_sensor_start(ob_sensor *sensor, ob_stream_profile *profile, ob_frame_callback callback, void *user_data, ob_error **error);
+
+/**
+ * @brief Stop the sensor stream.
+ *
+ * @param[in] sensor The sensor object.
+ * @param[out] error Logs error messages.
+ */
+void ob_sensor_stop(ob_sensor *sensor, ob_error **error);
+
+/**
+ * @brief Dynamically switch resolutions.
+ *
+ * @param[in] sensor The sensor object.
+ * @param[in] profile The stream configuration information.
+ * @param[out] error Logs error messages.
+ */
+void ob_sensor_switch_profile(ob_sensor *sensor, ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Delete a list of sensor objects.
+ *
+ * @param[in] sensor_list The list of sensor objects to delete.
+ * @param[out] error Logs error messages.
+ */
+void ob_delete_sensor_list(ob_sensor_list *sensor_list, ob_error **error);
+
+/**
+ * @brief Get the number of sensors in the sensor list.
+ *
+ * @param[in] sensor_list The list of sensor objects.
+ * @param[out] error Logs error messages.
+ * @return The number of sensors in the list.
+ */
+uint32_t ob_sensor_list_get_sensor_count(ob_sensor_list *sensor_list, ob_error **error);
+
+/**
+ * @brief Get the sensor type.
+ *
+ * @param[in] sensor_list The list of sensor objects.
+ * @param[in] index The index of the sensor on the list.
+ * @param[out] error Logs error messages.
+ * @return The sensor type.
+ */
+ob_sensor_type ob_sensor_list_get_sensor_type(ob_sensor_list *sensor_list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Get a sensor by sensor type.
+ *
+ * @param[in] sensor_list The list of sensor objects.
+ * @param[in] sensorType The sensor type to be obtained.
+ * @param[out] error Logs error messages.
+ * @return The sensor pointer. If the specified type of sensor does not exist, it will return null.
+ */
+ob_sensor *ob_sensor_list_get_sensor_by_type(ob_sensor_list *sensor_list, ob_sensor_type sensorType, ob_error **error);
+
+/**
+ * @brief Get a sensor by index number.
+ *
+ * @param[in] sensor_list The list of sensor objects.
+ * @param[in] index The index of the sensor on the list.
+ * @param[out] error Logs error messages.
+ * @return The sensor object.
+ */
+ob_sensor *ob_sensor_list_get_sensor(ob_sensor_list *sensor_list, uint32_t index, ob_error **error);
+
+/**
+ * @brief Delete a sensor object.
+ *
+ * @param[in] sensor The sensor object to delete.
+ * @param[out] error Logs error messages.
+ */
+void ob_delete_sensor(ob_sensor *sensor, ob_error **error);
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/StreamProfile.h

@@ -0,0 +1,219 @@
+/**
+ * @file StreamProfile.h
+ * @brief The stream profile related type is used to get information such as the width, height, frame rate, and format of the stream.
+ *
+ */
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ObTypes.h"
+
+/**
+ * @brief Get stream profile format
+ *
+ * @param[in] profile  Stream profile object
+ * @param[out] error  Log error messages
+ * @return ob_format return the format of the stream
+ */
+ob_format ob_stream_profile_format(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get stream profile type
+ *
+ * @param[in] profile Stream profile object
+ * @param[out] error Log error messages
+ * @return ob_stream_type stream type
+ */
+ob_stream_type ob_stream_profile_type(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get the extrinsic for source stream to target stream
+ *
+ * @param source Source stream profile
+ * @param target Target stream profile
+ * @param error Log error messages
+ * @return ob_extrinsic The extrinsic
+ */
+ob_extrinsic ob_stream_profile_get_extrinsic_to(ob_stream_profile *source, ob_stream_profile *target, ob_error **error);
+
+/**
+ * @brief Get the frame rate of the video stream
+ *
+ * @param[in] profile Stream profile object
+ * @param[out] error Log error messages
+ * @return uint32_t return the frame rate of the stream
+ */
+uint32_t ob_video_stream_profile_fps(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get the width of the video stream
+ *
+ * @param[in] profile Stream profile object , If the profile is not a video stream configuration, an error will be returned
+ * @param[out] error Log error messages
+ * @return uint32_t return the width of the stream
+ */
+uint32_t ob_video_stream_profile_width(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get the height of the video stream
+ *
+ * @param[in] profile Stream profile object , If the profile is not a video stream configuration, an error will be returned
+ * @param[out] error  Log error messages
+ * @return uint32_t return the height of the stream
+ */
+uint32_t ob_video_stream_profile_height(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get the intrinsic of the video stream
+ *
+ * @param profile Stream profile object
+ * @param error   Log error messages
+ * @return ob_camera_intrinsic Return the intrinsic of the stream
+ */
+ob_camera_intrinsic ob_video_stream_get_intrinsic(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get the distortion of the video stream
+ *
+ * @param profile Stream profile object
+ * @param error   Log error messages
+ * @return ob_camera_distortion Return the distortion of the stream
+ */
+ob_camera_distortion ob_video_stream_get_distortion(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get the full-scale range of the accelerometer stream.
+ *
+ * @param[in] profile Stream profile object. If the profile is not for the accelerometer stream, an error will be returned.
+ * @param[out] error Log error messages.
+ * @return The full-scale range of the accelerometer stream.
+ */
+ob_accel_full_scale_range ob_accel_stream_profile_full_scale_range(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get the sampling frequency of the accelerometer frame.
+ *
+ * @param[in] profile Stream profile object. If the profile is not for the accelerometer stream, an error will be returned.
+ * @param[out] error Log error messages.
+ * @return The sampling frequency of the accelerometer frame.
+ */
+ob_accel_sample_rate ob_accel_stream_profile_sample_rate(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get the intrinsic of the accelerometer stream.
+ *
+ * @param profile Stream profile object. If the profile is not for the accelerometer stream, an error will be returned.
+ * @param error Log error messages.
+ * @return ob_accel_intrinsic Return the intrinsic of the accelerometer stream.
+ */
+ob_accel_intrinsic ob_accel_stream_profile_get_intrinsic(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get the full-scale range of the gyroscope stream.
+ *
+ * @param[in] profile Stream profile object. If the profile is not for the gyroscope stream, an error will be returned.
+ * @param[out] error Log error messages.
+ * @return The full-scale range of the gyroscope stream.
+ */
+ob_gyro_full_scale_range ob_gyro_stream_profile_full_scale_range(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get the intrinsic of the gyroscope stream.
+ *
+ * @param profile Stream profile object. If the profile is not for the gyroscope stream, an error will be returned.
+ * @param error Log error messages.
+ * @return ob_gyro_intrinsic Return the intrinsic of the gyroscope stream.
+ */
+ob_gyro_intrinsic ob_gyro_stream_get_intrinsic(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Get the sampling frequency of the gyroscope stream.
+ *
+ * @param[in] profile Stream profile object. If the profile is not for the gyroscope stream, an error will be returned.
+ * @param[out] error Log error messages.
+ * @return The sampling frequency of the gyroscope stream.
+ */
+ob_gyro_sample_rate ob_gyro_stream_profile_sample_rate(ob_stream_profile *profile, ob_error **error);
+
+/**
+ * @brief Match the corresponding ob_stream_profile through the passed parameters. If there are multiple matches,
+ * the first one in the list will be returned by default. If no matched profile is found, an error will be returned.
+ *
+ * @param[in] profile_list Resolution list.
+ * @param[in] width Width. If you don't need to add matching conditions, you can pass OB_WIDTH_ANY.
+ * @param[in] height Height. If you don't need to add matching conditions, you can pass OB_HEIGHT_ANY.
+ * @param[in] format Format. If you don't need to add matching conditions, you can pass OB_FORMAT_ANY.
+ * @param[in] fps Frame rate. If you don't need to add matching conditions, you can pass OB_FPS_ANY.
+ * @param[out] error Log error messages.
+ * @return The matching profile.
+ */
+ob_stream_profile *ob_stream_profile_list_get_video_stream_profile(ob_stream_profile_list *profile_list, int width, int height, ob_format format, int fps,
+                                                                   ob_error **error);
+
+/**
+ * @brief Match the corresponding ob_stream_profile through the passed parameters. If there are multiple matches,
+ * the first one in the list will be returned by default. If no matched profile is found, an error will be returned.
+ *
+ * @param[in] profile_list Resolution list.
+ * @param[in] fullScaleRange Full-scale range. If you don't need to add matching conditions, you can pass 0.
+ * @param[in] sampleRate Sample rate. If you don't need to add matching conditions, you can pass 0.
+ * @param[out] error Log error messages.
+ * @return The matching profile.
+ */
+ob_stream_profile *ob_stream_profile_list_get_accel_stream_profile(ob_stream_profile_list *profile_list, ob_accel_full_scale_range fullScaleRange,
+                                                                   ob_accel_sample_rate sampleRate, ob_error **error);
+
+/**
+ * @brief Match the corresponding ob_stream_profile through the passed parameters. If there are multiple matches,
+ * the first one in the list will be returned by default. If no matched profile is found, an error will be returned.
+ *
+ * @param[in] profile_list Resolution list.
+ * @param[in] fullScaleRange Full-scale range. If you don't need to add matching conditions, you can pass 0.
+ * @param[in] sampleRate Sample rate. If you don't need to add matching conditions, you can pass 0.
+ * @param[out] error Log error messages.
+ * @return The matching profile.
+ */
+ob_stream_profile *ob_stream_profile_list_get_gyro_stream_profile(ob_stream_profile_list *profile_list, ob_gyro_full_scale_range fullScaleRange,
+                                                                  ob_gyro_sample_rate sampleRate, ob_error **error);
+
+/**
+ * @brief Get the corresponding StreamProfile by subscripting.
+ *
+ * @param[in] profile_list StreamProfile lists.
+ * @param[in] index Index.
+ * @param[out] error Log error messages.
+ * @return The matching profile.
+ */
+ob_stream_profile *ob_stream_profile_list_get_profile(ob_stream_profile_list *profile_list, int index, ob_error **error);
+
+/**
+ * @brief Get the number of StreamProfile lists.
+ *
+ * @param[in] profile_list StreamProfile list.
+ * @param[out] error Log error messages.
+ * @return The number of StreamProfile lists.
+ */
+uint32_t ob_stream_profile_list_count(ob_stream_profile_list *profile_list, ob_error **error);
+
+/**
+ * @brief Delete the stream profile list.
+ *
+ * @param[in] profile_list Stream configuration list.
+ * @param[out] error Log error messages.
+ */
+void ob_delete_stream_profile_list(ob_stream_profile_list *profile_list, ob_error **error);
+
+/**
+ * @brief Delete the stream configuration.
+ *
+ * @param[in] profile Stream profile object .
+ * @param[out] error Log error messages.
+ */
+void ob_delete_stream_profile(ob_stream_profile *profile, ob_error **error);
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/Utils.h

@@ -0,0 +1,142 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "ObTypes.h"
+
+/**
+ * @brief Transform a 3d point of a source coordinate system into a 3d point of the target coordinate system.
+ *
+ * @param[in] calibration_param Device calibration param,see pipeline::getCalibrationParam
+ * @param[in] source_point3f Source 3d point value
+ * @param[in] source_sensor_type Source sensor type
+ * @param[in] target_sensor_type Target sensor type
+ * @param[out] target_point3f Target 3d point value
+ * @param[out] error Log error messages
+ *
+ * @return bool Transform result
+ */
+bool ob_calibration_3d_to_3d(const ob_calibration_param calibration_param, const ob_point3f source_point3f, const ob_sensor_type source_sensor_type,
+                             const ob_sensor_type target_sensor_type, ob_point3f *target_point3f, ob_error **error);
+
+/**
+ * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 3d point of the target coordinate system.
+ *
+ * @param[in] calibration_param Device calibration param,see pipeline::getCalibrationParam
+ * @param[in] source_point2f Source 2d point value
+ * @param[in] source_depth_pixel_value The depth of sourcePoint2f in millimeters
+ * @param[in] source_sensor_type Source sensor type
+ * @param[in] target_sensor_type Target sensor type
+ * @param[out] target_point3f Target 3d point value
+ * @param[out] error Log error messages
+ *
+ * @return bool Transform result
+ */
+bool ob_calibration_2d_to_3d(const ob_calibration_param calibration_param, const ob_point2f source_point2f, const float source_depth_pixel_value,
+                             const ob_sensor_type source_sensor_type, const ob_sensor_type target_sensor_type, ob_point3f *target_point3f, ob_error **error);
+
+/**
+ * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 3d point of the target coordinate system.
+ *
+ * @param[in] calibration_param Device calibration param,see pipeline::getCalibrationParam
+ * @param[in] source_point2f Source 2d point value
+ * @param[in] source_depth_pixel_value The depth of sourcePoint2f in millimeters
+ * @param[in] source_sensor_type Source sensor type
+ * @param[in] target_sensor_type Target sensor type
+ * @param[out] target_point3f Target 3d point value
+ * @param[out] error Log error messages
+ *
+ * @return bool Transform result
+ */
+bool ob_calibration_2d_to_3d_undistortion(const ob_calibration_param calibration_param, const ob_point2f source_point2f, const float source_depth_pixel_value,
+                                          const ob_sensor_type source_sensor_type, const ob_sensor_type target_sensor_type, ob_point3f *target_point3f,
+                                          ob_error **error);
+
+/**
+ * @brief Transform a 3d point of a source coordinate system into a 2d pixel coordinate of the target camera.
+ *
+ * @param[in] calibration_param Device calibration param,see pipeline::getCalibrationParam
+ * @param[in] source_point3f Source 3d point value
+ * @param[in] source_sensor_type Source sensor type
+ * @param[in] target_sensor_type Target sensor type
+ * @param[out] target_point2f Target 2d point value
+ * @param[out] error Log error messages
+ *
+ * @return bool Transform result
+ */
+bool ob_calibration_3d_to_2d(const ob_calibration_param calibration_param, const ob_point3f source_point3f, const ob_sensor_type source_sensor_type,
+                             const ob_sensor_type target_sensor_type, ob_point2f *target_point2f, ob_error **error);
+
+/**
+ * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 2d pixel coordinate of the target camera
+ *
+ * @param[in] calibration_param Device calibration param,see pipeline::getCalibrationParam
+ * @param[in] source_point2f Source 2d point value
+ * @param[in] source_depth_pixel_value The depth of sourcePoint2f in millimeters
+ * @param[in] source_sensor_type Source sensor type
+ * @param[in] target_sensor_type Target sensor type
+ * @param[out] target_point2f Target 2d point value
+ * @param[out] error Log error messages
+ *
+ * @return bool Transform result
+ */
+bool ob_calibration_2d_to_2d(const ob_calibration_param calibration_param, const ob_point2f source_point2f, const float source_depth_pixel_value,
+                             const ob_sensor_type source_sensor_type, const ob_sensor_type target_sensor_type, ob_point2f *target_point2f, ob_error **error);
+
+/**
+ * @brief Transforms the depth frame into the geometry of the color camera.
+ *
+ * @param[in] device Device handle
+ * @param[in] depth_frame Input depth frame
+ * @param[in] target_color_camera_width Target color camera width
+ * @param[in] target_color_camera_height Target color camera height
+ * @param[out] error Log error messages
+ *
+ * @return ob_frame* Transformed depth frame
+ */
+ob_frame *transformation_depth_frame_to_color_camera(ob_device *device, ob_frame *depth_frame, uint32_t target_color_camera_width,
+                                                     uint32_t target_color_camera_height, ob_error **error);
+
+/**
+ * @brief Init transformation tables
+ *
+ * @param[in] calibration_param Device calibration param,see pipeline::getCalibrationParam
+ * @param[in] sensor_type sensor type
+ * @param[in] data input data,needs to be allocated externally.During initialization, the external allocation size is 'data_size', for example, data_size = 1920
+ * * 1080 * 2*sizeof(float) (1920 * 1080 represents the image resolution, and 2 represents two LUTs, one for x-coordinate and one for y-coordinate).
+ * @param[in] data_size input data size
+ * @param[out] xy_tables output xy tables
+ * @param[out] error Log error messages
+ *
+ * @return bool Transform result
+ */
+bool transformation_init_xy_tables(const ob_calibration_param calibration_param, const ob_sensor_type sensor_type, float *data, uint32_t *data_size,
+                                   ob_xy_tables *xy_tables, ob_error **error);
+
+/**
+ * @brief Transform depth image to point cloud data
+ *
+ * @param[in] xy_tables input xy tables,see transformation_init_xy_tables
+ * @param[in] depth_image_data input depth image data
+ * @param[out] pointcloud_data output point cloud data
+ * @param[out] error Log error messages
+ */
+void transformation_depth_to_pointcloud(ob_xy_tables *xy_tables, const void *depth_image_data, void *pointcloud_data, ob_error **error);
+
+/**
+ * @brief Transform depth image to point cloud data
+ *
+ * @param[in] xy_tables input xy tables,see transformation_init_xy_tables
+ * @param[in] depth_image_data input depth image data
+ * @param[in] color_image_data input color image data (only RGB888 support)
+ * @param[out] pointcloud_data output point cloud data
+ * @param[out] error Log error messages
+ */
+void transformation_depth_to_rgbd_pointcloud(ob_xy_tables *xy_tables, const void *depth_image_data, const void *color_image_data, void *pointcloud_data,
+                                             ob_error **error);
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/h/Version.h

@@ -0,0 +1,50 @@
+/**
+ * @file Version.h
+ * @brief Functions for retrieving the SDK version number information.
+ *
+ */
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Get the SDK version number.
+ *
+ * @return int The SDK version number.
+ */
+int ob_get_version();
+
+/**
+ * @brief Get the SDK major version number.
+ *
+ * @return int The SDK major version number.
+ */
+int ob_get_major_version();
+
+/**
+ * @brief Get the SDK minor version number.
+ *
+ * @return int The SDK minor version number.
+ */
+int ob_get_minor_version();
+
+/**
+ * @brief Get the SDK patch version number.
+ *
+ * @return int The SDK patch version number.
+ */
+int ob_get_patch_version();
+
+/**
+ * @brief Get the SDK stage version.
+ * @attention The returned char* does not need to be freed.
+ *
+ * @return const char* The SDK stage version.
+ */
+const char *ob_get_stage_version();
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/Context.hpp

@@ -0,0 +1,164 @@
+/**
+ * @file Context.hpp
+ * @brief The SDK context class, which serves as the entry point to the underlying SDK. It is used to query device lists, handle device callbacks, and set the
+ * log level.
+ *
+ */
+#pragma once
+
+#include "Types.hpp"
+
+#include <functional>
+#include <memory>
+
+struct ContextImpl;
+
+namespace ob {
+class Device;
+class DeviceInfo;
+class DeviceList;
+
+class OB_EXTENSION_API Context {
+private:
+    std::unique_ptr<ContextImpl> impl_;
+
+public:
+    /**
+     * @brief The Context class is a management class that describes the runtime of the SDK. It is responsible for applying and releasing resources for the SDK.
+     * The context has the ability to manage multiple devices, enumerate devices, monitor device callbacks, and enable functions such as multi-device
+     * synchronization.
+     */
+    Context(const char *configPath = "");
+    virtual ~Context() noexcept;
+
+    /**
+     * @brief Queries the enumerated device list.
+     *
+     * @return std::shared_ptr<DeviceList> A pointer to the device list class.
+     */
+    std::shared_ptr<DeviceList> queryDeviceList();
+
+    /**
+     * @brief enable or disable net device enumeration.
+     * @brief after enable, the net device will be discovered automatically and can be retrieved by @ref queryDeviceList. The default state can be set in the
+     * configuration file.
+     *
+     * @attention Net device enumeration by gvcp protocol, if the device is not in the same subnet as the host, it will be discovered but cannot be connected.
+     *
+     * @param[out] enable true to enable, false to disable
+     */
+    void enableNetDeviceEnumeration(bool enable);
+
+    /**
+     * @brief Creates a network device object.
+     *
+     * @param address The IP address.
+     * @param port The port.
+     * @return std::shared_ptr<Device> The created device object.
+     */
+    std::shared_ptr<Device> createNetDevice(const char *address, uint16_t port);
+
+    /**
+     * @brief Changes the IP configuration of a network device.
+     *
+     * @param deviceUid The device unique ID, which is the network device MAC address. It can be obtained through the @ref DeviceList::uid() function.
+     * @param config The new IP configuration.
+     */
+    void changeNetDeviceIpConfig(const char *deviceUid, const OBNetIpConfig &config);
+
+    using DeviceChangedCallback = std::function<void(std::shared_ptr<DeviceList> removedList, std::shared_ptr<DeviceList> addedList)>;
+
+    /**
+     * @brief Set the device plug-in callback function.
+     *
+     * @param callback The function triggered when the device is plugged and unplugged.
+     */
+    void setDeviceChangedCallback(DeviceChangedCallback callback);
+
+    /**
+     * @brief Activates device clock synchronization to synchronize the clock of the host and all created devices (if supported).
+     *
+     * @param repeatInterval The interval for auto-repeated synchronization, in milliseconds. If the value is 0, synchronization is performed only once.
+     */
+    void enableDeviceClockSync(uint64_t repeatInterval);
+#define enableMultiDeviceSync enableDeviceClockSync
+
+    /**
+     * @brief Frees idle memory from the internal frame memory pool.
+     */
+    void freeIdleMemory();
+
+    /**
+     * @brief Set the level of the global log, which affects both the log level output to the terminal and output to the file.
+     *
+     * @param severity The log output level.
+     */
+    static void setLoggerSeverity(OBLogSeverity severity);
+
+    /**
+     * @brief Set log output to a file.
+     *
+     * @param severity The log level output to the file.
+     * @param directory The log file output path. If the path is empty, the existing settings will continue to be used (if the existing configuration is also
+     * empty, the log will not be output to the file).
+     */
+    static void setLoggerToFile(OBLogSeverity severity, const char *directory);
+
+    /**
+     * @brief Set log output to the terminal.
+     *
+     * @param severity The log level output to the terminal.
+     */
+    static void setLoggerToConsole(OBLogSeverity severity);
+
+    /**
+     * @brief Log output callback function.
+     *
+     * @param severity The current callback log level.
+     * @param logMsg The log message.
+     */
+    using LogCallback = std::function<void(OBLogSeverity severity, const char *logMsg)>;
+
+    /**
+     * @brief Set the logger to callback.
+     *
+     * @param severity The callback log level.
+     * @param callback The callback function.
+     */
+    static void setLoggerToCallback(OBLogSeverity severity, LogCallback callback);
+
+    /**
+     * @brief Loads a license file.
+     *
+     * @param filePath The license file path.
+     * @param key The decryption key.
+     */
+    static void loadLicense(const char *filePath, const char *key = OB_DEFAULT_DECRYPT_KEY);
+
+    /**
+     * @brief Loads a license from data.
+     *
+     * @param data The license data.
+     * @param dataLen The license data length.
+     * @param key The decryption key.
+     */
+    static void loadLicenseFromData(const char *data, uint32_t dataLen, const char *key = OB_DEFAULT_DECRYPT_KEY);
+
+    /**
+     * @brief Set the UVC backend for the specified context
+     * This function configures the Universal Video Class (UVC) backend for the given context, allowing the selection of a specific backend for
+     * video capture operations.
+     *
+     * @attention This function is only supported on Linux (ARM) platforms.
+     * Some devices, like the Dabai series, do not support V4L2. Therefore, the default backend is LIBUVC. Ensure that the device
+     * supports V4L2 before setting it as the backend.
+     *
+     * @param[in] uvcBackend Specifies the UVC backend to use:
+     *                       - `UVC_BACKEND_AUTO`: Automatically selects between
+     *                         V4L2 or libuvc based on metadata support.
+     *                       - `UVC_BACKEND_LIBUVC`: Forces the use of libuvc.
+     *                       - `UVC_BACKEND_V4L2`: Forces the use of V4L2.
+     */
+    void setUVCBackend(OBUvcBackend uvcBackend);
+};
+}  // namespace ob

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/Device.hpp

@@ -0,0 +1,969 @@
+/**
+ * @file Device.hpp
+ * @brief Device related types, including operations such as getting and creating a device, setting and obtaining device attributes, and obtaining sensors
+ *
+ */
+#pragma once
+#include "Types.hpp"
+
+#include "libobsensor/h/Property.h"
+#include "libobsensor/hpp/Filter.hpp"
+#include <memory>
+#include <string>
+#include <vector>
+
+struct DeviceImpl;
+struct DeviceInfoImpl;
+struct DeviceListImpl;
+struct CameraParamListImpl;
+
+namespace ob {
+class SensorList;
+class Context;
+class DeviceInfo;
+class Sensor;
+class CameraParamList;
+class OBDepthWorkModeList;
+class DevicePresetList;
+
+class OB_EXTENSION_API Device {
+protected:
+    std::unique_ptr<DeviceImpl> impl_;
+
+    Device(Device &&device);
+
+public:
+    /**
+     * @brief Describe the entity of the RGBD camera, representing a specific model of RGBD camera
+     */
+    Device(std::unique_ptr<DeviceImpl> impl);
+    virtual ~Device() noexcept;
+
+    /**
+     * @brief Get device information
+     *
+     * @return std::shared_ptr<DeviceInfo> return device information
+     */
+    std::shared_ptr<DeviceInfo> getDeviceInfo();
+
+    /**
+     * @brief Get device sensor list
+     *
+     * @return std::shared_ptr<SensorList> return the sensor list
+     */
+    std::shared_ptr<SensorList> getSensorList();
+
+    /**
+     * @brief Get specific type of sensor
+     * if device not open, SDK will automatically open the connected device and return to the instance
+     *
+     * @return std::shared_ptr<Sensor> return the sensor example, if the device does not have the device,return nullptr
+     */
+    std::shared_ptr<Sensor> getSensor(OBSensorType type);
+
+    /**
+     * @brief Set int type of device property
+     *
+     * @param propertyId Property id
+     * @param property Property to be set
+     */
+    void setIntProperty(OBPropertyID propertyId, int32_t property);
+
+    /**
+     * @brief Set float type of device property
+     *
+     * @param propertyId Property id
+     * @param property Property to be set
+     */
+    void setFloatProperty(OBPropertyID propertyId, float property);
+
+    /**
+     * @brief Set bool type of device property
+     *
+     * @param propertyId Property id
+     * @param property Property to be set
+     */
+    void setBoolProperty(OBPropertyID propertyId, bool property);
+
+    /**
+     * @brief Get int type of device property
+     *
+     * @param propertyId Property id
+     * @return int32_t Property to get
+     */
+    int32_t getIntProperty(OBPropertyID propertyId);
+
+    /**
+     * @brief Get float type of device property
+     *
+     * @param propertyId Property id
+     * @return float Property to get
+     */
+    float getFloatProperty(OBPropertyID propertyId);
+
+    /**
+     * @brief Get bool type of device property
+     *
+     * @param propertyId Property id
+     * @return bool Property to get
+     */
+    bool getBoolProperty(OBPropertyID propertyId);
+
+    /**
+     * @brief Get int type device property range (including current value and default value)
+     *
+     * @param propertyId Property id
+     * @return OBIntPropertyRange Property range
+     */
+    OBIntPropertyRange getIntPropertyRange(OBPropertyID propertyId);
+
+    /**
+     * @brief Get float type device property range((including current value and default value)
+     *
+     * @param propertyId Property id
+     * @return OBFloatPropertyRange Property range
+     */
+    OBFloatPropertyRange getFloatPropertyRange(OBPropertyID propertyId);
+
+    /**
+     * @brief Get bool type device property range (including current value and default value)
+     *
+     * @param propertyId The ID of the property
+     * @return OBBoolPropertyRange The range of the property
+     */
+    OBBoolPropertyRange getBoolPropertyRange(OBPropertyID propertyId);
+
+    /**
+     * @brief Write to an AHB register
+     *
+     * @param reg The register to write to
+     * @param mask The mask to apply
+     * @param value The value to write
+     */
+    void writeAHB(uint32_t reg, uint32_t mask, uint32_t value);
+
+    /**
+     * @brief Read from an AHB register
+     *
+     * @param reg The register to read from
+     * @param mask The mask to apply
+     * @param value The value to return
+     */
+    void readAHB(uint32_t reg, uint32_t mask, uint32_t *value);
+
+    /**
+     * @brief Write to an I2C register
+     *
+     * @param moduleId The ID of the I2C module to write to
+     * @param reg The register to write to
+     * @param mask The mask to apply
+     * @param value The value to write
+     */
+    void writeI2C(uint32_t moduleId, uint32_t reg, uint32_t mask, uint32_t value);
+
+    /**
+     * @brief Read from an I2C register
+     *
+     * @param moduleId The ID of the I2C module to read from
+     * @param reg The register to read from
+     * @param mask The mask to apply
+     * @param value The value to return
+     */
+    void readI2C(uint32_t moduleId, uint32_t reg, uint32_t mask, uint32_t *value);
+
+    /**
+     * @brief Set the properties for writing to Flash
+     *
+     * @param offset The offset address in Flash
+     * @param data The data to write
+     * @param dataSize The size of the data to write
+     * @param callback The callback for write progress
+     * @param async Whether to execute asynchronously
+     */
+    void writeFlash(uint32_t offset, const void *data, uint32_t dataSize, SetDataCallback callback, bool async = false);
+
+    /**
+     * @brief Read a property from Flash
+     *
+     * @param offset The offset address in Flash
+     * @param dataSize The size of the property to read
+     * @param callback The callback for read progress and data
+     * @param async Whether to execute asynchronously
+     */
+    void readFlash(uint32_t offset, uint32_t dataSize, GetDataCallback callback, bool async = false);
+
+    /**
+     * @brief Set the customer data type of a device property
+     *
+     * @param data The data to set
+     * @param dataSize The size of the data to set,the maximum length cannot exceed 65532 bytes.
+     */
+    void writeCustomerData(const void *data, uint32_t dataSize);
+
+    /**
+     * @brief Get the customer data type of a device property
+     *
+     * @param data The property data obtained
+     * @param dataSize The size of the data obtained
+     */
+    void readCustomerData(void *data, uint32_t *dataSize);
+
+    /**
+     * @brief Set the raw data type of a device property (with asynchronous callback)
+     *
+     * @param propertyId The ID of the property
+     * @param data The data to set
+     * @param dataSize The size of the data to set
+     * @param callback The callback for set progress
+     * @param async Whether to execute asynchronously
+     */
+    void setRawData(OBPropertyID propertyId, const void *data, uint32_t dataSize, SetDataCallback callback, bool async = false);
+
+    /**
+     * @brief Get the raw data type of a device property (with asynchronous callback)
+     *
+     * @param propertyId The ID of the property
+     * @param callback The callback for getting the data and progress
+     * @param async Whether to execute asynchronously
+     */
+    void getRawData(OBPropertyID propertyId, GetDataCallback callback, bool async = false);
+
+    /**
+     * @brief Set the structured data type of a device property
+     *
+     * @param propertyId The ID of the property
+     * @param data The data to set
+     * @param dataSize The size of the data to set
+     */
+    void setStructuredData(OBPropertyID propertyId, const void *data, uint32_t dataSize);
+
+    /**
+     * @brief Get the structured data type of a device property
+     *
+     * @param propertyId The ID of the property
+     * @param data The property data obtained
+     * @param dataSize The size of the data obtained
+     */
+    void getStructuredData(OBPropertyID propertyId, void *data, uint32_t *dataSize);
+
+    /**
+     * @brief Set the structured data type of a device property (with extended data bundle)
+     *
+     * @param propertyId The ID of the property
+     * @param dataBundle The target data bundle
+     * @param callback The callback for setting
+     */
+    void setStructuredDataExt(OBPropertyID propertyId, std::shared_ptr<OBDataBundle> dataBundle, SetDataCallback callback);
+
+    /**
+     * @brief Get the structured data type of a device property (with extended data bundle)
+     *
+     * @param propertyId The ID of the property
+     * @return The data bundle
+     */
+    std::shared_ptr<OBDataBundle> getStructuredDataExt(OBPropertyID propertyId);
+
+    /**
+     * @brief Get the property protocol version
+     *
+     * @return The protocol version
+     */
+    OBProtocolVersion getProtocolVersion();
+
+    /**
+     * @brief Get the cmdVersion of a property
+     *
+     * @param propertyId The ID of the property
+     * @return The cmdVersion
+     */
+    OBCmdVersion getCmdVersion(OBPropertyID propertyId);
+
+    /**
+     * @brief Get the number of properties supported by the device
+     *
+     * @return The number of supported properties
+     */
+    uint32_t getSupportedPropertyCount();
+
+    /**
+     * @brief Get the supported properties of the device
+     *
+     * @param index The index of the property
+     * @return The type of supported property
+     */
+    OBPropertyItem getSupportedProperty(uint32_t index);
+
+    /**
+     * @brief Check if a property permission is supported
+     *
+     * @param propertyId The ID of the property
+     * @param permission The read and write permissions to check
+     * @return Whether the property permission is supported
+     */
+    bool isPropertySupported(OBPropertyID propertyId, OBPermissionType permission);
+
+    /**
+     * @brief Check if the global timestamp is supported for the device
+     *
+     * @return Whether the global timestamp is supported
+     */
+    bool isGlobalTimestampSupported();
+
+    /**
+     * @brief Upgrade the device firmware
+     *
+     * @param filePath Firmware path
+     * @param callback  Firmware upgrade progress and status callback
+     * @param async    Whether to execute asynchronously
+     */
+    void deviceUpgrade(const char *filePath, DeviceUpgradeCallback callback, bool async = true);
+
+    /**
+     * \if English
+     * @brief Upgrade the device firmware
+     *
+     * @param fileData Firmware file data
+     * @param fileSize Firmware file size
+     * @param callback  Firmware upgrade progress and status callback
+     * @param async    Whether to execute asynchronously
+     */
+    void deviceUpgradeFromData(const char *fileData, uint32_t fileSize, DeviceUpgradeCallback callback, bool async = true);
+
+    /**
+     * @brief Send files to the specified path on the device side [Asynchronouscallback]
+     *
+     * @param filePath Original file path
+     * @param dstPath  Accept the save path on the device side
+     * @param callback File transfer callback
+     * @param async    Whether to execute asynchronously
+     */
+    void sendFile(const char *filePath, const char *dstPath, SendFileCallback callback, bool async = true);
+
+    /**
+     * @brief Get the current state
+     * @return OBDeviceState device state information
+     */
+    OBDeviceState getDeviceState();
+
+    /**
+     * @brief Set the device state changed callbacks
+     *
+     * @param callback The callback function that is triggered when the device status changes (for example, the frame rate is automatically reduced or the
+     * stream is closed due to high temperature, etc.)
+     */
+    void setDeviceStateChangedCallback(DeviceStateChangedCallback callback);
+
+    /**
+     * @brief Verify device authorization code
+     *
+     * @param authCode Authorization code
+     * @return bool whether the activation is successful
+     */
+    bool activateAuthorization(const char *authCode);
+
+    /**
+     * @brief Write authorization code
+     * @param[in] authCodeStr  Authorization code
+     */
+    void writeAuthorizationCode(const char *authCodeStr);
+
+    /**
+     * @brief Get the original parameter list of camera calibration saved in the device.
+     *
+     * @attention The parameters in the list do not correspond to the current open-current configuration. You need to select the parameters according to the
+     * actual situation, and may need to do scaling, mirroring and other processing. Non-professional users are recommended to use the
+     * Pipeline::getCameraParam() interface.
+     *
+     * @return std::shared_ptr<CameraParamList> camera parameter list
+     */
+    std::shared_ptr<CameraParamList> getCalibrationCameraParamList();
+
+    /**
+     * @brief Get current depth work mode
+     *
+     * @return ob_depth_work_mode Current depth work mode
+     */
+    OBDepthWorkMode getCurrentDepthWorkMode();
+
+    /**
+     * @brief Switch depth work mode by OBDepthWorkMode. Prefer invoke switchDepthWorkMode(const char *modeName) to switch depth mode
+     *        when known the complete name of depth work mode.
+     * @param[in] workMode Depth work mode come from ob_depth_work_mode_list which return by ob_device_get_depth_work_mode_list
+     */
+    OBStatus switchDepthWorkMode(const OBDepthWorkMode &workMode);
+
+    /**
+     * @brief Switch depth work mode by work mode name.
+     *
+     * @param[in] modeName Depth work mode name which equals to OBDepthWorkMode.name
+     */
+    OBStatus switchDepthWorkMode(const char *modeName);
+
+    /**
+     * @brief Request support depth work mode list
+     * @return OBDepthWorkModeList list of ob_depth_work_mode
+     */
+    std::shared_ptr<OBDepthWorkModeList> getDepthWorkModeList();
+
+    /**
+     * @brief Device restart
+     * @attention The device will be disconnected and reconnected. After the device is disconnected, the access to the Device object interface may be abnormal.
+     *   Please delete the object directly and obtain it again after the device is reconnected.
+     */
+    void reboot();
+
+    /**
+     * @brief Device restart delay mode
+     * @attention The device will be disconnected and reconnected. After the device is disconnected, the access to the Device object interface may be abnormal.
+     *   Please delete the object directly and obtain it again after the device is reconnected.
+     * Support devices: Gemini2 L
+     *
+     * @param[in] delayMs Time unit：ms。delayMs == 0：No delay；delayMs > 0, Delay millisecond connect to host device after reboot
+     */
+    void reboot(uint32_t delayMs);
+
+    /**
+     * @brief Synchronize the device time (synchronize local system time to device)
+     * @deprecated This interface is deprecated, please use @ref timerSyncWithHost instead.
+     * @return The command (round trip time, rtt)
+     */
+    DEPRECATED uint64_t syncDeviceTime();
+
+    /**
+     * @brief get the current device synchronization configuration
+     * @brief Device synchronization: including exposure synchronization function and multi-camera synchronization function of different sensors within a single
+     * machine
+     *
+     * @deprecated This interface is deprecated, please use @ref getMultiDeviceSyncConfig instead.
+     *
+     * @return OBDeviceSyncConfig return the device synchronization configuration
+     */
+    DEPRECATED OBDeviceSyncConfig getSyncConfig();
+
+    /**
+     * @brief Set the device synchronization configuration
+     * @brief Used to configure the exposure synchronization function and multi-camera synchronization function of different sensors in a single machine
+     *
+     * @deprecated This interface is deprecated, please use @ref setMultiDeviceSyncConfig instead.
+     *
+     * @attention Calling this function will directly write the configuration to the device Flash, and it will still take effect after the device restarts. To
+     * avoid affecting the Flash lifespan, do not update the configuration frequently.
+     *
+     * @param deviceSyncConfig Device synchronization configuration
+     */
+    DEPRECATED void setSyncConfig(const OBDeviceSyncConfig &deviceSyncConfig);
+
+    /**
+     * @brief Get the supported multi device sync mode bitmap of the device.
+     * @brief For example, if the return value is 0b00001100, it means the device supports @ref OB_MULTI_DEVICE_SYNC_MODE_PRIMARY and @ref
+     * OB_MULTI_DEVICE_SYNC_MODE_SECONDARY. User can check the supported mode by the code:
+     * ```c
+     *   if(supported_mode_bitmap & OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN){
+     *      //support OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN
+     *   }
+     *   if(supported_mode_bitmap & OB_MULTI_DEVICE_SYNC_MODE_STANDALONE){
+     *     //support OB_MULTI_DEVICE_SYNC_MODE_STANDALONE
+     *   }
+     *   // and so on
+     * ```
+     * @return uint16_t return the supported multi device sync mode bitmap of the device.
+     */
+    uint16_t getSupportedMultiDeviceSyncModeBitmap();
+
+    /**
+     * @brief set the multi device sync configuration of the device.
+     *
+     * @param[in] config The multi device sync configuration.
+     */
+    void setMultiDeviceSyncConfig(const OBMultiDeviceSyncConfig &config);
+
+    /**
+     * @brief get the multi device sync configuration of the device.
+     *
+     * @return OBMultiDeviceSyncConfig return the multi device sync configuration of the device.
+     */
+    OBMultiDeviceSyncConfig getMultiDeviceSyncConfig();
+
+    /**
+     * @brief send the capture command to the device.
+     * @brief The device will start one time image capture after receiving the capture command when it is in the @ref
+     * OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING
+     *
+     * @attention The frequency of the user call this function multiplied by the number of frames per trigger should be less than the frame rate of the stream.
+     * The number of frames per trigger can be set by @ref framesPerTrigger.
+     * @attention For some models，receive and execute the capture command will have a certain delay and performance consumption, so the frequency of calling
+     * this function should not be too high, please refer to the product manual for the specific supported frequency.
+     * @attention If the device is not in the @ref OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING mode, device will ignore the capture command.
+     */
+    void triggerCapture();
+
+    /**
+     * @brief set the timestamp reset configuration of the device.
+     */
+    void setTimestampResetConfig(const OBDeviceTimestampResetConfig &config);
+
+    /**
+     * @brief get the timestamp reset configuration of the device.
+     *
+     * @return OBDeviceTimestampResetConfig return the timestamp reset configuration of the device.
+     */
+    OBDeviceTimestampResetConfig getTimestampResetConfig();
+
+    /**
+     * @brief send the timestamp reset command to the device.
+     * @brief The device will reset the timer for calculating the timestamp for output frames to 0 after receiving the timestamp reset command when the
+     * timestamp reset function is enabled. The timestamp reset function can be enabled by call @ref ob_device_set_timestamp_reset_config.
+     * @brief Before calling this function, user should call @ref ob_device_set_timestamp_reset_config to disable the timestamp reset function (It is not
+     * required for some models, but it is still recommended to do so for code compatibility).
+     *
+     * @attention If the stream of the device is started, the timestamp of the continuous frames output by the stream will jump once after the timestamp reset.
+     * @attention Due to the timer of device is not high-accuracy, the timestamp of the continuous frames output by the stream will drift after a long time.
+     * User can call this function periodically to reset the timer to avoid the timestamp drift, the recommended interval time is 60 minutes.
+     */
+    void timestampReset();
+
+    /**
+     *  @brief Alias for @ref timestampReset since it is more accurate.
+     */
+#define timerReset timestampReset
+
+    /**
+     * @brief synchronize the timer of the device with the host.
+     * @brief After calling this function, the timer of the device will be synchronized with the host. User can call this function to multiple devices to
+     * synchronize all timers of the devices.
+     *
+     * @attention If the stream of the device is started, the timestamp of the continuous frames output by the stream will may jump once after the timer
+     * sync.
+     * @attention Due to the timer of device is not high-accuracy, the timestamp of the continuous frames output by the stream will drift after a long time.
+     * User can call this function periodically to synchronize the timer to avoid the timestamp drift, the recommended interval time is 60 minutes.
+     *
+     */
+    void timerSyncWithHost();
+
+    /**
+     * @brief Load depth filter config from file.
+     * @param filePath Path of the config file.
+     */
+    void loadDepthFilterConfig(const char *filePath);
+
+    /**
+     * @brief Reset depth filter config to device default define.
+     */
+    void resetDefaultDepthFilterConfig();
+
+    /**
+     * @brief Get current preset name
+     * @brief The preset mean a set of parameters or configurations that can be applied to the device to achieve a specific effect or function.
+     * @return const char* return the current preset name, it should be one of the preset names returned by @ref getAvailablePresetList.
+     */
+    const char *getCurrentPresetName();
+
+    /**
+     * @brief Get current depth mode name
+     * @brief According the current preset name to return current depth mode name 
+     * @return const char* return the current depth mode name.
+     */
+    const char *getCurrentDepthModeName();
+
+    /**
+     * @brief load the preset according to the preset name.
+     * @attention After loading the preset, the settings in the preset will set to the device immediately. Therefore, it is recommended to re-read the device
+     * settings to update the user program temporarily.
+     * @param presetName The preset name to set. The name should be one of the preset names returned by @ref getAvailablePresetList.
+     */
+    void loadPreset(const char *presetName);
+
+    /**
+     * @brief Get available preset list
+     * @brief The available preset list usually defined by the device manufacturer and restores on the device.
+     * @brief User can load the custom preset by calling @ref loadPresetFromJsonFile to append the available preset list.
+     *
+     * @return DevicePresetList return the available preset list.
+     */
+    std::shared_ptr<DevicePresetList> getAvailablePresetList();
+
+    /**
+     * @brief Load custom preset from file.
+     * @brief After loading the custom preset, the settings in the custom preset will set to the device immediately.
+     * @brief After loading the custom preset, the available preset list will be appended with the custom preset and named as the file name.
+     *
+     * @attention The user should ensure that the custom preset file is adapted to the device and the settings in the file are valid.
+     * @attention It is recommended to re-read the device settings to update the user program temporarily after successfully loading the custom preset.
+     *
+     * @param filePath The path of the custom preset file.
+     */
+    void loadPresetFromJsonFile(const char *filePath);
+
+    /**
+     * @brief Load custom preset from data.
+     * @brief After loading the custom preset, the settings in the custom preset will set to the device immediately.
+     * @brief After loading the custom preset, the available preset list will be appended with the custom preset and named as the @ref presetName.
+     *
+     * @attention The user should ensure that the custom preset data is adapted to the device and the settings in the data are valid.
+     * @attention It is recommended to re-read the device settings to update the user program temporarily after successfully loading the custom preset.
+     *
+     * @param data The custom preset data.
+     * @param size The size of the custom preset data.
+     */
+    void loadPresetFromJsonData(const char *presetName, const uint8_t *data, uint32_t size);
+
+    /**
+     * @brief Export current device settings as a preset json file.
+     * @brief The exported preset file can be loaded by calling @ref loadPresetFromJsonFile to restore the device setting.
+     * @brief After exporting the preset, a new preset named as the @ref filePath will be added to the available preset list.
+     *
+     * @param filePath The path of the preset file to be exported.
+     */
+    void exportSettingsAsPresetJsonFile(const char *filePath);
+
+    /**
+     * @brief Export current device settings as a preset json data.
+     * @brief After exporting the preset, a new preset named as the @ref presetName will be added to the available preset list.
+     *
+     * @attention The memory of the data is allocated by the SDK, and will automatically be released by the SDK.
+     * @attention The memory of the data will be reused by the SDK on the next call, so the user should copy the data to a new buffer if it needs to be
+     * preserved.
+     *
+     * @param[out] data return the preset json data.
+     * @param[out] dataSize return the size of the preset json data.
+     */
+    void exportSettingsAsPresetJsonData(const char *presetName, const uint8_t **data, uint32_t *dataSize);
+
+    friend class Pipeline;
+    friend class Recorder;
+    friend class CoordinateTransformHelper;
+};
+
+/**
+ * @brief A class describing device information, representing the name, id, serial number and other basic information of an RGBD camera.
+ */
+class OB_EXTENSION_API DeviceInfo {
+private:
+    std::unique_ptr<DeviceInfoImpl> impl_;
+
+public:
+    DeviceInfo(std::unique_ptr<DeviceInfoImpl> impl);
+    virtual ~DeviceInfo() noexcept;
+
+    /**
+     * @brief Get device name
+     *
+     * @return const char * return the device name
+     */
+    const char *name();
+
+    /**
+     * @brief Get the pid of the device
+     *
+     * @return int return the pid of the device
+     */
+    int pid();
+
+    /**
+     * @brief Get the vid of the device
+     *
+     * @return int return the vid of the device
+     */
+    int vid();
+
+    /**
+     * @brief Get system assigned uid for distinguishing between different devices
+     *
+     * @return const char * return the uid of the device
+     */
+    const char *uid();
+
+    /**
+     * @brief Get the serial number of the device
+     *
+     * @return const char * return the serial number of the device
+     */
+    const char *serialNumber();
+
+    /**
+     * @brief Get the version number of the firmware
+     *
+     * @return const char* return the version number of the firmware
+     */
+    const char *firmwareVersion();
+
+    /**
+     * @brief Get the USB connection type of the device (DEPRECATED)
+     *
+     * @return const char* the USB connection type of the device
+     */
+    DEPRECATED const char *usbType();
+
+    /**
+     * @brief Get the connection type of the device
+     *
+     * @return const char* the connection type of the device，currently supports："USB", "USB1.0", "USB1.1", "USB2.0", "USB2.1", "USB3.0", "USB3.1", "USB3.2",
+     * "Ethernet"
+     */
+    const char *connectionType();
+
+    /**
+     * @brief Get the IP address of the device
+     *
+     * @attention Only valid for network devices, otherwise it will return "0.0.0.0".
+     *
+     * @return const char* the IP address of the device, such as "192.168.1.10"
+     */
+    const char *ipAddress();
+
+    /**
+     * @brief Get the version number of the hardware
+     *
+     * @return const char* the version number of the hardware
+     */
+    const char *hardwareVersion();
+
+    /**
+     * @brief Get the minimum version number of the SDK supported by the device
+     *
+     * @return const char* the minimum SDK version number supported by the device
+     */
+    const char *supportedMinSdkVersion();
+
+    /**
+     * @brief Get information about extensions obtained from SDK supported by the device
+     *
+     * @return const char* Returns extended information about the device
+     */
+    const char *extensionInfo();
+
+    /**
+     * @brief Get chip type name
+     *
+     * @return const char* the chip type name
+     */
+    const char *asicName();
+
+    /**
+     * @brief Get the device type
+     *
+     * @return OBDeviceType the device type
+     */
+    OBDeviceType deviceType();
+
+    friend class Context;
+    friend class DeviceList;
+    friend class Pipeline;
+};
+
+/**
+ * @brief Class representing a list of devices
+ */
+class OB_EXTENSION_API DeviceList {
+private:
+    std::unique_ptr<DeviceListImpl> impl_;
+
+public:
+    DeviceList(std::unique_ptr<DeviceListImpl> impl);
+    ~DeviceList() noexcept;
+
+    /**
+     * @brief Get the number of devices in the list
+     *
+     * @return uint32_t the number of devices in the list
+     */
+    uint32_t deviceCount();
+
+    /**
+     * @brief Get the name of the device at the specified index (DEPRECATED)
+     *
+     * @param index the index of the device
+     * @return int the name of the device
+     */
+    DEPRECATED const char *name(uint32_t index);
+
+    /**
+     * @brief Get the PID of the device at the specified index
+     *
+     * @param index the index of the device
+     * @return int the PID of the device
+     */
+    int pid(uint32_t index);
+
+    /**
+     * @brief Get the VID of the device at the specified index
+     *
+     * @param index the index of the device
+     * @return int the VID of the device
+     */
+    int vid(uint32_t index);
+
+    /**
+     * @brief Get the UID of the device at the specified index
+     *
+     * @param index the index of the device
+     * @return const char* the UID of the device
+     */
+    const char *uid(uint32_t index);
+
+    /**
+     * @brief Get the serial number of the device at the specified index
+     *
+     * @param index the index of the device
+     * @return const char* the serial number of the device
+     */
+    const char *serialNumber(uint32_t index);
+
+    /**
+     * @brief Get device connection type
+     *
+     * @param index device index
+     * @return const char* returns connection type，currently supports："USB", "USB1.0", "USB1.1", "USB2.0", "USB2.1", "USB3.0", "USB3.1", "USB3.2", "Ethernet"
+     */
+    const char *connectionType(uint32_t index);
+
+    /**
+     * @brief get the ip address of the device at the specified index
+     *
+     * @attention Only valid for network devices, otherwise it will return "0.0.0.0".
+     *
+     * @param index the index of the device
+     * @return const char* the ip address of the device
+     */
+    const char *ipAddress(uint32_t index);
+
+    /**
+     * @brief Get the device object at the specified index
+     *
+     * @attention If the device has already been acquired and created elsewhere, repeated acquisition will throw an exception
+     *
+     * @param index the index of the device to create
+     * @return std::shared_ptr<Device> the device object
+     */
+    std::shared_ptr<Device> getDevice(uint32_t index);
+
+    /**
+     * @brief Get the device object with the specified serial number
+     *
+     * @attention If the device has already been acquired and created elsewhere, repeated acquisition will throw an exception
+     *
+     * @param serialNumber the serial number of the device to create
+     * @return std::shared_ptr<Device> the device object
+     */
+    std::shared_ptr<Device> getDeviceBySN(const char *serialNumber);
+
+    /**
+     * @brief Get the specified device object from the device list by uid
+     * @brief On Linux platform, the uid of the device is composed of bus-port-dev, for example 1-1.2-1. But the SDK will remove the dev number and only keep
+     * the bus-port as the uid to create the device, for example 1-1.2, so that we can create a device connected to the specified USB port. Similarly, users can
+     * also directly pass in bus-port as uid to create device.
+     *
+     * @attention If the device has been acquired and created elsewhere, repeated acquisition will throw an exception
+     *
+     * @param uid The uid of the device to be created
+     * @return std::shared_ptr<Device> returns the device object
+     */
+    std::shared_ptr<Device> getDeviceByUid(const char *uid);
+};
+
+/**
+ * @brief Class representing a list of camera parameters
+ */
+class OB_EXTENSION_API CameraParamList {
+private:
+    std::unique_ptr<CameraParamListImpl> impl_;
+
+public:
+    CameraParamList(std::unique_ptr<CameraParamListImpl> impl);
+    ~CameraParamList() noexcept;
+
+    /**
+     * @brief Get the number of camera parameter groups
+     *
+     * @return uint32_t the number of camera parameter groups
+     */
+    uint32_t count();
+
+    /**
+     * @brief Get the camera parameters for the specified index
+     *
+     * @param index the index of the parameter group
+     * @return OBCameraParam the corresponding group parameters
+     */
+    OBCameraParam getCameraParam(uint32_t index);
+};
+
+/**
+ * @brief Class representing a list of OBDepthWorkMode
+ */
+class OB_EXTENSION_API OBDepthWorkModeList {
+private:
+    std::unique_ptr<OBDepthWorkModeListImpl> impl_;
+
+public:
+    OBDepthWorkModeList(std::unique_ptr<OBDepthWorkModeListImpl> impl_);
+    ~OBDepthWorkModeList();
+
+    /**
+     * @brief Get the number of OBDepthWorkMode objects in the list
+     *
+     * @return uint32_t the number of OBDepthWorkMode objects in the list
+     */
+    uint32_t count();
+
+    /**
+     * @brief Get the OBDepthWorkMode object at the specified index
+     *
+     * @param index the index of the target OBDepthWorkMode object
+     * @return OBDepthWorkMode the OBDepthWorkMode object at the specified index
+     */
+    OBDepthWorkMode getOBDepthWorkMode(uint32_t index);
+
+    /**
+     * @brief Get the name of the depth work mode at the specified index
+     *
+     * @param index the index of the depth work mode
+     * @return const char* the name of the depth work mode
+     */
+    const char *getName(uint32_t index);
+
+    /**
+     * @brief Get the OBDepthWorkMode object at the specified index
+     *
+     * @param index the index of the target OBDepthWorkMode object
+     * @return OBDepthWorkMode the OBDepthWorkMode object at the specified index
+     */
+    OBDepthWorkMode operator[](uint32_t index);
+};
+
+/**
+ * @brief Class representing a list of device presets
+ * @breif A device preset is a set of parameters or configurations that can be applied to the device to achieve a specific effect or function.
+ */
+class OB_EXTENSION_API DevicePresetList {
+private:
+    std::unique_ptr<DevicePresetListImpl> impl_;
+
+public:
+    DevicePresetList(std::unique_ptr<DevicePresetListImpl> impl);
+    ~DevicePresetList() noexcept;
+
+    /**
+     * @brief Get the number of device presets in the list
+     *
+     * @return uint32_t the number of device presets in the list
+     */
+    uint32_t count();
+
+    /**
+     * @brief Get the name of the device preset at the specified index
+     *
+     * @param index the index of the device preset
+     * @return const char* the name of the device preset
+     */
+    const char *getName(uint32_t index);
+
+    /**
+     * @breif check if the preset list contains the special name preset.
+     * @param name The name of the preset
+     * @return bool Returns true if the special name is found in the preset list, otherwise returns false.
+     */
+    bool hasPreset(const char *name);
+};
+
+}  // namespace ob

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/Error.hpp

@@ -0,0 +1,49 @@
+/**
+ * @file  Error.hpp
+ * @brief This file defines the Error class, which describes abnormal errors within the SDK.
+ *        Detailed information about the exception can be obtained through this class.
+ */
+#pragma once
+
+#include "Types.hpp"
+#include <memory>
+
+struct ErrorImpl;
+
+namespace ob {
+class OB_EXTENSION_API Error {
+private:
+    std::unique_ptr<ErrorImpl> impl_;
+
+public:
+    Error(std::unique_ptr<ErrorImpl> impl) noexcept;
+
+    Error(const Error &error) noexcept;
+
+    ~Error() noexcept;
+
+    /**
+     * @brief Get the detailed error logs of SDK internal exceptions.
+     * @return A C-style string containing the error message.
+     */
+    const char *getMessage() const noexcept;
+
+    /**
+     * @brief Get the exception type of the error, which can be used to determine which module is abnormal.
+     * @return The OBExceptionType enum value.
+     */
+    OBExceptionType getExceptionType() const noexcept;
+
+    /**
+     * @brief Get the name of the error function.
+     * @return A C-style string containing the name of the error function.
+     */
+    const char *getName() const noexcept;
+
+    /**
+     * @brief Get the parameter passed to the error interface.
+     * @return A C-style string containing the error interface parameter.
+     */
+    const char *getArgs() const noexcept;
+};
+}  // namespace ob

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/Filter.hpp

@@ -0,0 +1,651 @@
+/**
+ * @file  Filter.hpp
+ * @brief This file contains the Filter class, which is the processing unit of the SDK that can perform point cloud generation, format conversion, and other
+ * functions.
+ */
+#pragma once
+
+#include "Types.hpp"
+#include <functional>
+#include <memory>
+#include <map>
+#include <string>
+#include <iostream>
+
+namespace ob {
+class Frame;
+class OBFilterList;
+
+/**
+ * @brief A callback function that takes a shared pointer to a Frame object as its argument.
+ */
+typedef std::function<void(std::shared_ptr<Frame>)> FilterCallback;
+
+/**
+ * @brief The Filter class is the base class for all filters in the SDK.
+ */
+class OB_EXTENSION_API Filter : public std::enable_shared_from_this<Filter> {
+public:
+    Filter();
+
+    Filter(std::shared_ptr<FilterImpl> impl);
+
+    virtual ~Filter() = default;
+
+    /**
+     * @brief ReSet the filter, freeing the internal cache, stopping the processing thread, and clearing the pending buffer frame when asynchronous processing
+     * is used.
+     */
+    virtual void reset();
+
+    /**
+     * @brief enable the filter
+     */
+    void enable(bool enable);
+
+    /**
+     * @brief Return Enable State
+     */
+    bool isEnabled();
+
+    /**
+     * @brief Processes a frame synchronously.
+     *
+     * @param frame The frame to be processed.
+     * @return std::shared_ptr< Frame > The processed frame.
+     */
+    virtual std::shared_ptr<Frame> process(std::shared_ptr<Frame> frame);
+
+    /**
+     * @brief Pushes the pending frame into the cache for asynchronous processing.
+     *
+     * @param frame The pending frame. The processing result is returned by the callback function.
+     */
+    virtual void pushFrame(std::shared_ptr<Frame> frame);
+
+    /**
+     * @brief Set the callback function for asynchronous processing.
+     *
+     * @param callback The processing result callback.
+     */
+    virtual void setCallBack(FilterCallback callback);
+
+    /**
+     * @brief Get the type of filter.
+     *
+     * @return string The type of filte.
+     */
+    virtual const char *type();
+
+    /**
+     * @brief Check if the runtime type of the filter object is compatible with a given type.
+     *
+     * @tparam T The given type.
+     * @return bool The result.
+     */
+    template <typename T> bool is();
+
+    /**
+     * @brief Convert the filter object to a target type.
+     *
+     * @tparam T The target type.
+     * @return std::shared_ptr<T> The result. If it cannot be converted, an exception will be thrown.
+     */
+    template <typename T> std::shared_ptr<T> as() {
+        if(!is<T>()) {
+            throw std::runtime_error("unsupported operation, object's type is not require type");
+        }
+
+        return std::static_pointer_cast<T>(shared_from_this());
+    }
+
+protected:
+    std::shared_ptr<FilterImpl> impl_;
+    std::string                 type_;
+
+    friend class OBFilterList;
+};
+
+/**
+ * @brief The PointCloudFilter class is a subclass of Filter that generates point clouds.
+ */
+class OB_EXTENSION_API PointCloudFilter : public Filter {
+public:
+    PointCloudFilter();
+
+    /**
+     * @brief Set the point cloud type parameters.
+     *
+     * @param type The point cloud type: depth point cloud or RGBD point cloud.
+     */
+    void setCreatePointFormat(OBFormat type);
+
+    /**
+     * @brief Set the camera parameters.
+     *
+     * @param param The camera internal and external parameters.
+     */
+    void setCameraParam(OBCameraParam param);
+
+    /**
+     * @brief Set the frame alignment state that will be input to generate point cloud.
+     *
+     * @param state The alignment status. True: enable alignment; False: disable alignment.
+     */
+    void setFrameAlignState(bool state);
+
+    /**
+     * @brief Set the point cloud coordinate data zoom factor.
+     *
+     * @attention Calling this function to set the scale will change the point coordinate scaling factor of the output point cloud frame: posScale = posScale /
+     * scale. The point coordinate scaling factor for the output point cloud frame can be obtained via @ref PointsFrame::getPositionValueScale function.
+     *
+     * @param scale The zoom factor.
+     */
+    void setPositionDataScaled(float scale);
+
+    /**
+     * @brief Set point cloud color data normalization.
+     *
+     * @param state Whether normalization is required.
+     */
+    void setColorDataNormalization(bool state);
+
+    /**
+     * @brief Set the point cloud coordinate system.
+     *
+     * @param type The coordinate system type.
+     */
+    void setCoordinateSystem(OBCoordinateSystemType type);
+};
+
+/**
+ * @brief The FormatConvertFilter class is a subclass of Filter that performs format conversion.
+ */
+class OB_EXTENSION_API FormatConvertFilter : public Filter {
+public:
+    FormatConvertFilter();
+
+    /**
+     * @brief Set the format conversion type.
+     *
+     * @param type The format conversion type.
+     */
+    void setFormatConvertType(OBConvertFormat type);
+};
+
+/**
+ * @brief The CompressionFilter class is a subclass of Filter that performs compression.
+ */
+class OB_EXTENSION_API CompressionFilter : public Filter {
+public:
+    CompressionFilter();
+
+    /**
+     * @brief Set the compression parameters.
+     *
+     * @param mode The compression mode: OB_COMPRESSION_LOSSLESS or OB_COMPRESSION_LOSSY.
+     * @param params The compression parameters. When mode is OB_COMPRESSION_LOSSLESS, params is NULL.
+     */
+    void setCompressionParams(OBCompressionMode mode, void *params);
+};
+
+/**
+ * @brief The DecompressionFilter class is a subclass of Filter that performs decompression.
+ */
+class OB_EXTENSION_API DecompressionFilter : public Filter {
+public:
+    DecompressionFilter();
+};
+
+/**
+ * @brief Hole filling filter,the processing performed depends on the selected hole filling mode.
+ */
+class OB_EXTENSION_API HoleFillingFilter : public Filter {
+public:
+    HoleFillingFilter();
+
+    /**
+     * @brief Set the HoleFillingFilter mode.
+     *
+     * @param[in] filter A holefilling_filter object.
+     * @param mode OBHoleFillingMode, OB_HOLE_FILL_TOP,OB_HOLE_FILL_NEAREST or OB_HOLE_FILL_FAREST.
+     */
+    void setFilterMode(OBHoleFillingMode mode);
+
+    /**
+     * @brief Get the HoleFillingFilter mode.
+     *
+     * @return OBHoleFillingMode
+     */
+    OBHoleFillingMode getFilterMode();
+};
+
+/**
+ * @brief Temporal filter
+ */
+class OB_EXTENSION_API TemporalFilter : public Filter {
+public:
+    TemporalFilter();
+
+    /**
+     * @brief Get the TemporalFilter diffscale range.
+     *
+     * @return OBFloatPropertyRange the diffscale value of property range.
+     */
+    OBFloatPropertyRange getDiffScaleRange();
+
+    /**
+     * @brief Set the TemporalFilter diffscale value.
+     *
+     * @param value diffscale value.
+     */
+    void setDiffScale(float value);
+
+    /**
+     * @brief Get the TemporalFilter weight range.
+     *
+     * @return OBFloatPropertyRange the weight value of property range.
+     */
+    OBFloatPropertyRange getWeightRange();
+
+    /**
+     * @brief Set the TemporalFilter weight value.
+     *
+     * @param value weight value.
+     */
+    void setWeight(float value);
+};
+
+/**
+ * @brief Spatial advanced filter smooths the image by calculating frame with alpha and delta settings
+ * alpha defines the weight of the current pixel for smoothing,
+ * delta defines the depth gradient below which the smoothing will occur as number of depth levels.
+ */
+class OB_EXTENSION_API SpatialAdvancedFilter : public Filter {
+public:
+    SpatialAdvancedFilter();
+
+    /**
+     * @brief Get the spatial advanced filter alpha range.
+     *
+     * @return OBFloatPropertyRange the alpha value of property range.
+     */
+    OBFloatPropertyRange getAlphaRange();
+
+    /**
+     * @brief Get the spatial advanced filter dispdiff range.
+     *
+     * @return OBUint16PropertyRange the dispdiff value of property range.
+     */
+    OBUint16PropertyRange getDispDiffRange();
+
+    /**
+     * @brief Get the spatial advanced filter radius range.
+     *
+     * @return OBUint16PropertyRange the radius value of property range.
+     */
+    OBUint16PropertyRange getRadiusRange();
+
+    /**
+     * @brief Get the spatial advanced filter magnitude range.
+     *
+     * @return OBIntPropertyRange the magnitude value of property range.
+     */
+    OBIntPropertyRange getMagnitudeRange();
+
+    /**
+     * @brief Get the spatial advanced filter params.
+     *
+     * @return OBSpatialAdvancedFilterParams
+     */
+    OBSpatialAdvancedFilterParams getFilterParams();
+
+    /**
+     * @brief Set the spatial advanced filter params.
+     *
+     * @param params OBSpatialAdvancedFilterParams.
+     */
+    void setFilterParams(OBSpatialAdvancedFilterParams params);
+};
+
+/**
+ * @brief Spatial fast filter smooths the image by calculating frame with filter window size settings
+ */
+class OB_EXTENSION_API SpatialFastFilter : public Filter {
+public:
+    SpatialFastFilter();
+    /**
+     * @brief Get the spatial fast filter window size range
+     *
+     * @return OBUint8PropertyRange the windows size value of property range.
+     */
+    OBUint8PropertyRange getSizeRange();
+
+     /**
+     * @brief Get the spatial fast filter params.
+     *
+     * @return OBSpatialFastFilterParams
+     */
+    OBSpatialFastFilterParams getFilterParams();
+    /**
+     * @brief Set the spatial fast filter params.
+     *
+     * @param params OBSpatialFastFilterParams.
+     */
+    void setFilterParams(OBSpatialFastFilterParams params);
+};
+
+/**
+ * @brief Spatial moderate filter smooths the image by calculating frame with filter window size,magnitude and disp diff settings
+ */
+class OB_EXTENSION_API SpatialModerateFilter : public Filter {
+public:
+    SpatialModerateFilter();
+    /**
+     * @brief Get the spatial moderate filter window size range
+     *
+     * @return OBUint8PropertyRange the windows size value of property range.
+     */
+    OBUint8PropertyRange          getSizeRange();
+
+    /**
+     * @brief Get the spatial moderate filter magnitude range.
+     *
+     * @return OBUint8PropertyRange the magnitude value of property range.
+     */
+    OBUint8PropertyRange getMagnitudeRange();
+    /**
+     * @brief Get the spatial moderate filter dispdiff range.
+     *
+     * @return OBUint16PropertyRange the dispdiff value of property range.
+     */
+    OBUint16PropertyRange getDispDiffRange();
+
+     /**
+     * @brief Get the spatial moderate filter params.
+     *
+     * @return OBSpatialModerateFilterParams
+     */
+    OBSpatialModerateFilterParams getFilterParams();
+    /**
+     * @brief Set the spatial moderate filter params.
+     *
+     * @param params OBSpatialModerateFilterParams.
+     */
+    void setFilterParams(OBSpatialModerateFilterParams params);
+};
+
+    /**
+ * @brief Depth to disparity or disparity to depth
+ */
+class OB_EXTENSION_API DisparityTransform : public Filter {
+public:
+
+    /**
+     * @brief Create a disparity transform.
+     * @param depth_to_disparity disparity to depth or depth to disparity Conversion.
+     */
+    DisparityTransform(bool depth_to_disparity);
+};
+
+/**
+ * @brief HdrMerge processing block,
+ * the processing merges between depth frames with
+ * different sub-preset sequence ids.
+ */
+class OB_EXTENSION_API HdrMerge : public Filter {
+public:
+    HdrMerge();
+};
+
+/**
+ * @brief Align for depth to other or other to depth.
+ */
+class OB_EXTENSION_API Align : public Filter {
+public:
+    /**
+     * @brief Creaet Align filter.
+     * @param OBStreamType alignment is performed between a depth image and another image.
+     */
+    Align(OBStreamType align_to_stream);
+
+    /**
+     * @brief Get the stream type to be aligned with.
+     *
+     * @return OBStreamType The stream type of align to.
+     */
+    OBStreamType getAlignToStreamType();
+};
+
+/**
+ * @brief Creates depth Thresholding filter
+ * By controlling min and max options on the block
+ */
+class OB_EXTENSION_API ThresholdFilter : public Filter {
+public:
+    ThresholdFilter();
+
+    /**
+     * @brief Get the threshold filter min range.
+     *
+     * @return OBIntPropertyRange The range of the threshold filter min.
+     */
+    OBIntPropertyRange getMinRange();
+
+    /**
+     * @brief Get the threshold filter max range.
+     *
+     * @return OBIntPropertyRange The range of the threshold filter max.
+     */
+    OBIntPropertyRange getMaxRange();
+
+    /**
+     * @brief Get the threshold filter max and min range.
+     */
+    bool setValueRange(uint16_t min, uint16_t max);
+};
+
+/**
+ * @brief Create SequenceIdFilter processing block.
+ */
+class OB_EXTENSION_API SequenceIdFilter : public Filter {
+public:
+    SequenceIdFilter();
+
+    /**
+     * @brief Set the sequenceId filter params.
+     *
+     * @param sequence id to pass the filter.
+     */
+    void selectSequenceId(int sequence_id);
+
+    /**
+     * @brief Get the current sequence id.
+     *
+     * @return sequence id to pass the filter.
+     */
+    int getSelectSequenceId();
+
+    /**
+     * @brief Get the current sequence id list.
+     *
+     * @return OBSequenceIdItem.
+     */
+    OBSequenceIdItem *getSequenceIdList();
+
+    /**
+     * @brief Get the sequenceId list size.
+     *
+     * @return the size of sequenceId list.
+     */
+    int getSequenceIdListSize();
+};
+
+/**
+ * @brief The noise removal filter,removing scattering depth pixels.
+ */
+class OB_EXTENSION_API NoiseRemovalFilter : public Filter {
+public:
+    NoiseRemovalFilter();
+
+    /**
+     * @brief Set the noise removal filter params.
+     *
+     * @param[in] params ob_noise_removal_filter_params.
+     */
+    void setFilterParams(OBNoiseRemovalFilterParams filterParams);
+
+    /**
+     * @brief Get the noise removal filter params.
+     *
+     * @return OBNoiseRemovalFilterParams.
+     */
+    OBNoiseRemovalFilterParams getFilterParams();
+
+    /**
+     * @brief Get the noise removal filter disp diff range.
+     * @return OBUint16PropertyRange The disp diff of property range.
+     */
+    OBUint16PropertyRange getDispDiffRange();
+
+    /**
+     * @brief Get the noise removal filter max size range.
+     * @return OBUint16PropertyRange The max size of property range.
+     */
+    OBUint16PropertyRange getMaxSizeRange();
+};
+
+/**
+ * @brief Decimation filter,reducing complexity by subsampling depth maps and losing depth details.
+ */
+class OB_EXTENSION_API DecimationFilter : public Filter {
+public:
+    DecimationFilter();
+
+    /**
+     * @brief Set the decimation filter scale value.
+     *
+     * @param type The decimation filter scale value.
+     */
+    void setScaleValue(uint8_t value);
+
+    /**
+     * @brief Get the decimation filter scale value.
+     */
+    uint8_t getScaleValue();
+
+    /**
+     * @brief Get the property range of the decimation filter scale value.
+     */
+    OBUint8PropertyRange getScaleRange();
+};
+
+/**
+ * @brief The edge noise removal filter,removing scattering depth pixels.
+ */
+class OB_EXTENSION_API EdgeNoiseRemovalFilter : public Filter {
+public:
+    EdgeNoiseRemovalFilter();
+
+    /**
+     * @brief Set the edge noise removal filter params.
+     *
+     * @param[in] params ob_edge_noise_removal_filter_params.
+     */
+    void setFilterParams(OBEdgeNoiseRemovalFilterParams filterParams);
+
+    /**
+     * @brief Get the edge noise removal filter params.
+     *
+     * @return OBEdgeNoiseRemovalFilterParams.
+     */
+    OBEdgeNoiseRemovalFilterParams getFilterParams();
+
+    /**
+     * @brief Get the edge noise removal filter margin left th range.
+     * @return OBUint16PropertyRange The disp diff of property range.
+     */
+    OBUint16PropertyRange getMarginLeftThRange();
+
+    /**
+     * @brief Get the edge noise removal filter margin right th range.
+     * @return OBUint16PropertyRange The max size of property range.
+     */
+    OBUint16PropertyRange getMarginRightThRange();
+
+    /**
+     * @brief Get the edge noise removal filter margin top th range.
+     * @return OBUint16PropertyRange The disp diff of property range.
+     */
+    OBUint16PropertyRange getMarginTopThRange();
+
+    /**
+     * @brief Get the edge noise removal filter margin bottom th range.
+     * @return OBUint16PropertyRange The max size of property range.
+     */
+    OBUint16PropertyRange getMarginBottomThRange();
+};
+
+
+// Define the is() template function for the Filter class
+template <typename T> bool Filter::is() {
+    std::string name = type();
+    if(name == "HDRMerge") {
+        return typeid(T) == typeid(HdrMerge);
+    }
+    if(name == "SequenceIdFilter") {
+        return typeid(T) == typeid(SequenceIdFilter);
+    }
+    if(name == "ThresholdFilter") {
+        return typeid(T) == typeid(ThresholdFilter);
+    }
+    if(name == "DisparityTransform") {
+        return typeid(T) == typeid(DisparityTransform);
+    }
+    if(name == "NoiseRemovalFilter") {
+        return typeid(T) == typeid(NoiseRemovalFilter);
+    }
+    if(name == "SpatialAdvancedFilter") {
+        return typeid(T) == typeid(SpatialAdvancedFilter);
+    }
+
+    if(name == "SpatialFastFilter") {
+        return typeid(T) == typeid(SpatialFastFilter);
+    }
+
+    if(name == "SpatialModerateFilter") {
+        return typeid(T) == typeid(SpatialModerateFilter);
+    }
+
+    if(name == "TemporalFilter") {
+        return typeid(T) == typeid(TemporalFilter);
+    }
+    if(name == "HoleFillingFilter") {
+        return typeid(T) == typeid(HoleFillingFilter);
+    }
+    if(name == "DecimationFilter") {
+        return typeid(T) == typeid(DecimationFilter);
+    }
+    if(name == "PointCloudFilter") {
+        return typeid(T) == typeid(PointCloudFilter);
+    }
+    if(name == "CompressionFilter") {
+        return typeid(T) == typeid(CompressionFilter);
+    }
+    if(name == "DecompressionFilter") {
+        return typeid(T) == typeid(DecompressionFilter);
+    }
+    if(name == "FormatConverter") {
+        return typeid(T) == typeid(FormatConvertFilter);
+    }
+    if(name == "Align") {
+        return typeid(T) == typeid(Align);
+    }
+    if(name == "EdgeNoiseRemovalFilter") {
+        return typeid(T) == typeid(EdgeNoiseRemovalFilter);
+    }
+    return false;
+}
+
+}  // namespace ob

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/Frame.hpp

@@ -0,0 +1,538 @@
+/**
+ * @file Frame.hpp
+ * @brief Frame related type, which is mainly used to obtain frame data and frame information.
+ *
+ */
+#pragma once
+
+#include "Types.hpp"
+
+#include <memory>
+#include <iostream>
+#include <typeinfo>
+
+/**
+ *    Frame class：
+ *                 Frame
+ *                   |
+ *            +------+----------+----------+-----------+
+ *            |      |          |         |            |
+ *      VideoFrame PointsFrame AccelFrame GyroFrame FrameSet
+ *          |
+ *       +--+------+---------+
+ *      |          |         |
+ *  ColorFrame DepthFrame IRFrame
+ *                           |
+ *                     +-----+-----+
+ *                     |           |
+ *                IRLeftFrame IRRightFrame
+ */
+
+struct FrameImpl;
+
+namespace ob {
+class Device;
+class Sensor;
+class StreamProfile;
+class Filter;
+class FrameHelper;
+
+typedef std::function<void(void *buffer, void *context)> BufferDestroyCallback;
+
+class OB_EXTENSION_API Frame : public std::enable_shared_from_this<Frame> {
+protected:
+    std::unique_ptr<FrameImpl> impl_;
+
+public:
+    explicit Frame(std::unique_ptr<FrameImpl> impl);
+    Frame(Frame &frame);
+
+    virtual ~Frame() noexcept;
+
+    /**
+     * @brief Get the type of frame.
+     *
+     * @return OBFrameType The type of frame.
+     */
+    virtual OBFrameType type();
+
+    /**
+     * @brief Get the format of the frame.
+     *
+     * @return OBFormat The format of the frame.
+     */
+    virtual OBFormat format();
+
+    /**
+     * @brief Get the sequence number of the frame.
+     *
+     * @return uint64_t The sequence number of the frame.
+     */
+    virtual uint64_t index();
+
+    /**
+     * @brief Get the frame data.
+     *
+     * @return void* The frame data.
+     */
+    virtual void *data();
+
+    /**
+     * @brief Get the size of the frame data.
+     *
+     * @return uint32_t The size of the frame data.
+     * For point cloud data, this returns the number of bytes occupied by all point sets. To find the number of points, divide the dataSize by the structure
+     * size of the corresponding point type.
+     */
+    virtual uint32_t dataSize();
+
+    /**
+     * @brief Get the hardware timestamp of the frame in milliseconds.
+     * @brief The hardware timestamp is the time point when the frame was captured by the device, on device clock domain.
+     *
+     * @return uint64_t The hardware timestamp of the frame in milliseconds.
+     */
+    uint64_t timeStamp();
+
+    /**
+     * @brief Get the hardware timestamp of the frame in microseconds.
+     * @brief The hardware timestamp is the time point when the frame was captured by the device, on device clock domain.
+     *
+     * @return uint64_t The hardware timestamp of the frame in microseconds.
+     */
+    uint64_t timeStampUs();
+
+    /**
+     * @brief Get the system timestamp of the frame in milliseconds.
+     * @brief The system timestamp is the time point when the frame was received by the host, on host clock domain.
+     *
+     * @return uint64_t The system timestamp of the frame in milliseconds.
+     */
+    uint64_t systemTimeStamp();
+
+    /**
+     * @brief Get the system timestamp of the frame in microseconds.
+     * @brief The system timestamp is the time point when the frame was received by the host, on host clock domain.
+     *
+     * @return uint64_t The system timestamp of the frame in microseconds.
+     */
+    uint64_t systemTimeStampUs();
+
+    /**
+     * @brief Get the global timestamp of the frame in microseconds.
+     * @brief The global timestamp is the time point when the frame was was captured by the device, and has been converted to the host clock domain. The
+     * conversion process base on the device timestamp and can eliminate the timer drift of the device
+     *
+     * @attention Only some devices support getting the global timestamp. If the device does not support it, this function will return 0. Check the device
+     * support status by @ref Device::isGlobalTimestampSupported() function.
+     *
+     * @return uint64_t The global timestamp of the frame in microseconds.
+     */
+    uint64_t globalTimeStampUs();
+
+    /**
+     * @brief Get the metadata of the frame.
+     *
+     * @return void* The metadata of the frame.
+     */
+    void *metadata();
+
+    /**
+     * @brief Get the size of the metadata of the frame.
+     *
+     * @return uint32_t The size of the metadata of the frame.
+     */
+    uint32_t metadataSize();
+
+    /**
+     * @brief Check if the frame object has metadata of a given type.
+     *
+     * @param type The metadata type. refer to @ref OBFrameMetadataType
+     * @return bool The result.
+     */
+    bool hasMetadata(OBFrameMetadataType type);
+
+    /**
+     * @brief Get the metadata value
+     *
+     * @param type The metadata type. refer to @ref OBFrameMetadataType
+     * @return int64_t The metadata value.
+     */
+    int64_t getMetadataValue(OBFrameMetadataType type);
+
+    /**
+     * @brief get StreamProfile of the frame
+     *
+     * @return std::shared_ptr<StreamProfile> The StreamProfile of the frame, may return nullptr if the frame is not captured from a stream.
+     */
+    std::shared_ptr<StreamProfile> getStreamProfile();
+
+    /**
+     * @brief get owner sensor of the frame
+     *
+     * @return std::shared_ptr<Sensor> The owner sensor of the frame, return nullptr if the frame is not owned by any sensor or the sensor is destroyed
+     */
+    std::shared_ptr<Sensor> getSensor();
+
+    /**
+     * @brief get owner device of the frame
+     *
+     * @return std::shared_ptr<Device> The owner device of the frame, return nullptr if the frame is not owned by any device or the device is destroyed
+     */
+    std::shared_ptr<Device> getDevice();
+
+    /**
+     * @brief Check if the runtime type of the frame object is compatible with a given type.
+     *
+     * @tparam T The given type.
+     * @return bool The result.
+     */
+    template <typename T> bool is();
+
+    /**
+     * @brief Convert the frame object to a target type.
+     *
+     * @tparam T The target type.
+     * @return std::shared_ptr<T> The result. If it cannot be converted, an exception will be thrown.
+     */
+    template <typename T> std::shared_ptr<T> as() {
+        if(!is<T>()) {
+            throw std::runtime_error("unsupported operation, object's type is not require type");
+        }
+
+        return std::dynamic_pointer_cast<T>(shared_from_this());
+    }
+
+private:
+    friend class Filter;
+    friend class Recorder;
+    friend class FrameHelper;
+    friend class CoordinateTransformHelper;
+};
+
+class OB_EXTENSION_API VideoFrame : public Frame {
+public:
+    explicit VideoFrame(Frame &frame);
+
+    explicit VideoFrame(std::unique_ptr<FrameImpl> impl);
+
+    ~VideoFrame() noexcept override = default;
+
+    /**
+     * @brief Get the width of the frame.
+     *
+     * @return uint32_t The width of the frame.
+     */
+    uint32_t width();
+
+    /**
+     * @brief Get the height of the frame.
+     *
+     * @return uint32_t The height of the frame.
+     */
+    uint32_t height();
+
+    /**
+     * @brief Get the effective number of pixels in the frame.
+     * @attention Only valid for Y8/Y10/Y11/Y12/Y14/Y16 format.
+     *
+     * @return uint8_t The effective number of pixels in the frame, or 0 if it is an unsupported format.
+     */
+    uint8_t pixelAvailableBitSize();
+};
+
+class OB_EXTENSION_API ColorFrame : public VideoFrame {
+public:
+    explicit ColorFrame(Frame &frame);
+
+    explicit ColorFrame(std::unique_ptr<FrameImpl> impl);
+
+    ~ColorFrame() noexcept override = default;
+};
+
+class OB_EXTENSION_API DepthFrame : public VideoFrame {
+public:
+    explicit DepthFrame(Frame &frame);
+
+    explicit DepthFrame(std::unique_ptr<FrameImpl> impl);
+
+    ~DepthFrame() noexcept override = default;
+
+    /**
+     * @brief Get the value scale of the depth frame. The pixel value of depth frame is multiplied by the scale to give a depth value in millimeters.
+     *      For example, if valueScale=0.1 and a certain coordinate pixel value is pixelValue=10000, then the depth value = pixelValue*valueScale =
+     *      10000*0.1=1000mm.
+     *
+     * @return float The scale.
+     */
+    float getValueScale();
+};
+
+class OB_EXTENSION_API IRFrame : public VideoFrame {
+public:
+    explicit IRFrame(Frame &frame);
+
+    explicit IRFrame(std::unique_ptr<FrameImpl> impl);
+
+    ~IRFrame() noexcept override = default;
+
+public:
+    OBSensorType getDataSource();
+};
+
+class OB_EXTENSION_API PointsFrame : public Frame {
+public:
+    explicit PointsFrame(Frame &frame);
+
+    explicit PointsFrame(std::unique_ptr<FrameImpl> impl);
+
+    ~PointsFrame() noexcept override = default;
+
+    /**
+     * @brief Get the point position value scale of the points frame. The point position value of the points frame is multiplied by the scale to give a position
+     * value in millimeters. For example, if scale=0.1, the x-coordinate value of a point is x = 10000, which means that the actual x-coordinate value = x*scale
+     * = 10000*0.1 = 1000mm.
+     *
+     * @return float The position value scale.
+     */
+    float getPositionValueScale();
+};
+
+/**
+ * @brief Define the FrameSet class, which inherits from the Frame class
+ *
+ */
+class OB_EXTENSION_API FrameSet : public Frame {
+
+public:
+    explicit FrameSet(std::unique_ptr<FrameImpl> impl);
+
+    explicit FrameSet(Frame &frame);
+
+    ~FrameSet() noexcept override;
+
+    /**
+     * @brief Get the number of frames in the FrameSet
+     *
+     * @return uint32_t The number of frames
+     */
+    uint32_t frameCount();
+
+    /**
+     * @brief Get the depth frame in the FrameSet
+     *
+     * @return std::shared_ptr<DepthFrame> The depth frame
+     */
+    std::shared_ptr<DepthFrame> depthFrame();
+
+    /**
+     * @brief Get the color frame in the FrameSet
+     *
+     * @return std::shared_ptr<ColorFrame> The color frame
+     */
+    std::shared_ptr<ColorFrame> colorFrame();
+
+    /**
+     * @brief Get the infrared frame in the FrameSet
+     *
+     * @return std::shared_ptr<IRFrame> The infrared frame
+     */
+    std::shared_ptr<IRFrame> irFrame();
+
+    /**
+     * @brief Get the point cloud frame in the FrameSet
+     *
+     * @return  std::shared_ptr<PointsFrame> The point cloud data frame
+     */
+    std::shared_ptr<PointsFrame> pointsFrame();
+
+    /**
+     * @brief Get a frame of a specific type from the FrameSet
+     *
+     * @param frameType The type of sensor
+     * @return std::shared_ptr<Frame> The corresponding type of frame
+     */
+    std::shared_ptr<Frame> getFrame(OBFrameType frameType);
+
+    /**
+     * @brief Get a frame at a specific index from the FrameSet
+     *
+     * @param index The index of the frame
+     * @return std::shared_ptr<Frame> The frame at the specified index
+     */
+    std::shared_ptr<Frame> getFrame(int index);
+
+    // Declare Pipeline and Filter classes as friends
+    friend class Pipeline;
+    friend class Filter;
+};
+
+/**
+ * @brief Define the AccelFrame class, which inherits from the Frame class
+ *
+ */
+class OB_EXTENSION_API AccelFrame : public Frame {
+public:
+    explicit AccelFrame(Frame &frame);
+
+    explicit AccelFrame(std::unique_ptr<FrameImpl> impl);
+
+    ~AccelFrame() noexcept override = default;
+
+    /**
+     * @brief Get the accelerometer frame data
+     *
+     * @return OBAccelValue The accelerometer frame data
+     */
+    OBAccelValue value();
+
+    /**
+     * @brief Get the temperature when the frame was sampled
+     *
+     * @return float The temperature value
+     */
+    float temperature();
+};
+
+/**
+ * @brief Define the GyroFrame class, which inherits from the Frame class
+ */
+class OB_EXTENSION_API GyroFrame : public Frame {
+public:
+    explicit GyroFrame(Frame &frame);
+
+    explicit GyroFrame(std::unique_ptr<FrameImpl> impl);
+
+    ~GyroFrame() noexcept override = default;
+
+    /**
+     * @brief Get the gyro frame data
+     *
+     * @return OBAccelValue The gyro frame data
+     */
+    OBGyroValue value();
+
+    /**
+     * @brief Get the temperature when the frame was sampled
+     *
+     * @return float The temperature value
+     */
+    float temperature();
+};
+
+/**
+ * @brief Define the RawPhaseFrame class, which inherits from the VideoFrame class
+ */
+class OB_EXTENSION_API RawPhaseFrame : public VideoFrame {
+public:
+    explicit RawPhaseFrame(Frame &frame);
+
+    explicit RawPhaseFrame(std::unique_ptr<FrameImpl> impl);
+
+    ~RawPhaseFrame() noexcept override = default;
+};
+
+/**
+ * @brief Define the FrameHelper class
+ */
+class OB_EXTENSION_API FrameHelper {
+public:
+    /**
+     * @brief Create a Frame object.
+     *
+     * @param[in] type The type of frame. See @ref OBFrameType.
+     * @param[in] format The format of the frame. See @ref OBFormat.
+     * @param[in] width The width of the frame.
+     * @param[in] height The height of the frame.
+     * @param[in] strideBytes The stride of the frame in bytes. If strideBytes > 0, the frame data size = height * strideBytes. If strideBytes = 0, the frame
+     * datasize = height * width * pixelSize (pixelSize according to the format).
+     *
+     * @return std::shared_ptr<Frame> The created frame object.
+     */
+    static std::shared_ptr<Frame> createFrame(OBFrameType type, OBFormat format, uint32_t width, uint32_t height, uint32_t strideBytes);
+
+    /**
+     * @brief Create a frame object based on an externally created buffer
+     *
+     * @param[in] format The format of the frame. See @ref OBFormat.
+     * @param[in] width The width of the frame.
+     * @param[in] height The height of the frame.
+     * @param[in] buffer The frame object buffer
+     * @param[in] bufferSize The frame object buffer size
+     * @param[in] destroyCallback The frame object buffer destroy callback
+     * @param[in] destroyCallbackContext The frame object buffer destroy callback context
+     *
+     * @return std::shared_ptr<Frame> The created frame object
+     */
+    static std::shared_ptr<Frame> createFrameFromBuffer(OBFormat format, uint32_t width, uint32_t height, uint8_t *buffer, uint32_t bufferSize,
+                                                        BufferDestroyCallback destroyCallback, void *destroyCallbackContext);
+
+    /**
+     * @brief Create an empty FrameSet object
+     *
+     * @return std::shared_ptr<Frame> The FrameSet object
+     */
+    static std::shared_ptr<FrameSet> createFrameSet();
+
+    /**
+     * @brief Add a frame of a specific type to the FrameSet
+     *
+     * @param frameSet The FrameSet object
+     * @param frameType The type of frame to add
+     * @param frame The frame object to add
+     */
+    static void pushFrame(std::shared_ptr<Frame> frameSet, OBFrameType frameType, std::shared_ptr<Frame> frame);
+
+    /**
+     * @brief Set the system timestamp of the frame.
+     *
+     * @param frame The frame object.
+     * @param systemTimestamp The system timestamp to set in milliseconds.
+     */
+    static void setFrameSystemTimestamp(std::shared_ptr<Frame> frame, uint64_t systemTimestamp);
+
+    /**
+     * @brief Set the device timestamp of the frame.
+     *
+     * @param frame The frame object.
+     * @param deviceTimestamp The device timestamp to set in milliseconds.
+     */
+    static void setFrameDeviceTimestamp(std::shared_ptr<Frame> frame, uint64_t deviceTimestamp);
+
+    /**
+     * @brief Set the device timestamp of the frame.
+     *
+     * @param frame The frame object.
+     * @param deviceTimestampUs The device timestamp to set in microseconds.
+     */
+    static void setFrameDeviceTimestampUs(std::shared_ptr<Frame> frame, uint64_t deviceTimestampUs);
+};
+
+// Define the is() template function for the Frame class
+template <typename T> bool Frame::is() {
+    switch(this->type()) {
+    case OB_FRAME_IR_LEFT:   // Follow
+    case OB_FRAME_IR_RIGHT:  // Follow
+    case OB_FRAME_IR:
+        return (typeid(T) == typeid(IRFrame) || typeid(T) == typeid(VideoFrame));
+    case OB_FRAME_DEPTH:
+        return (typeid(T) == typeid(DepthFrame) || typeid(T) == typeid(VideoFrame));
+    case OB_FRAME_COLOR:
+        return (typeid(T) == typeid(ColorFrame) || typeid(T) == typeid(VideoFrame));
+    case OB_FRAME_GYRO:
+        return (typeid(T) == typeid(GyroFrame));
+    case OB_FRAME_ACCEL:
+        return (typeid(T) == typeid(AccelFrame));
+    case OB_FRAME_SET:
+        return (typeid(T) == typeid(FrameSet));
+    case OB_FRAME_POINTS:
+        return (typeid(T) == typeid(PointsFrame));
+    case OB_FRAME_RAW_PHASE:
+        return (typeid(T) == typeid(RawPhaseFrame) || typeid(T) == typeid(VideoFrame));
+    default:
+        std::cout << "ob::Frame::is() did not catch frame type: " << (int)this->type() << std::endl;
+        break;
+    }
+    return false;
+}
+}  // namespace ob

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/Pipeline.hpp

@@ -0,0 +1,335 @@
+/**
+ * @file Pipeline.hpp
+ * @brief The SDK's advanced API type can quickly implement switching streaming and frame synchronization
+ * operations.
+ */
+#pragma once
+
+#include "Types.hpp"
+
+#include <functional>
+#include <memory>
+
+struct PipelineImpl;
+struct ConfigImpl;
+
+namespace ob {
+class FrameSet;
+class Frame;
+class Device;
+class Playback;
+class DeviceInfo;
+class Config;
+class StreamProfile;
+class StreamProfileList;
+
+typedef std::function<void(std::shared_ptr<FrameSet> frame)> FrameSetCallback;
+
+class OB_EXTENSION_API Pipeline {
+private:
+    std::unique_ptr<PipelineImpl> impl_;
+
+public:
+    /**
+     * @brief Pipeline is a high-level interface for applications, algorithms related RGBD data streams. Pipeline can provide alignment inside and synchronized
+     * FrameSet. Pipeline() no parameter version, which opens the first device in the list of devices connected to the OS by default. If the application has
+     * obtained the device through the DeviceList, opening the Pipeline() at this time will throw an exception that the device has been created.
+     */
+    Pipeline();
+
+    /**
+     * @brief
+     * Pipeline(std::shared_ptr< Device > device ) Function for multi-device operations. Multiple devices need to be obtained through DeviceList, and the device
+     * and pipeline are bound through this interface.
+     */
+    Pipeline(std::shared_ptr<Device> device);
+
+    /**
+     * @brief Construct a pipeline for playback of recorded stream files
+     *
+     * @param filename The file path of the recorded stream file to be played back
+     */
+    Pipeline(const char *filename);
+
+    /**
+     * @brief Destroy the pipeline object
+     */
+    ~Pipeline() noexcept;
+
+    /**
+     * @brief Start the pipeline with configuration parameters
+     *
+     * @param config The parameter configuration of the pipeline
+     */
+    void start(std::shared_ptr<Config> config);
+
+    /**
+     * @brief Start the pipeline with default configuration parameters
+     */
+    void start();
+
+    /**
+     * @brief Start the pipeline and set the frameset data callback
+     *
+     * @param config The configuration of the pipeline
+     * @param callback The callback to be triggered when all frame data in the frameset arrives
+     */
+    void start(std::shared_ptr<Config> config, FrameSetCallback callback);
+
+    /**
+     * @brief Stop the pipeline
+     */
+    void stop();
+
+    /**
+     * @brief get Enabled Stream Profile List,It must be called after the pipeline is started, otherwise it will return an empty list
+     */
+    std::shared_ptr<StreamProfileList> getEnabledStreamProfileList();
+    /**
+     * @brief Get the pipeline configuration parameters
+     * @brief Returns the default configuration if the user has not configured it
+     *
+     * @return std::shared_ptr<Config> The configured parameters
+     */
+    std::shared_ptr<Config> getConfig();
+
+    /**
+     * @brief Wait for frameset data
+     *
+     * @param timeout_ms The waiting timeout in milliseconds
+     * @return std::shared_ptr<FrameSet> The waiting frameset data
+     */
+    std::shared_ptr<FrameSet> waitForFrames(uint32_t timeout_ms = 1000);
+
+    /**
+     * @brief Get the device object
+     *
+     * @return std::shared_ptr<Device> The device object
+     */
+    std::shared_ptr<Device> getDevice();
+
+    /**
+     * @brief Get the playback object
+     *
+     * @return std::shared_ptr<Playback> The playback object
+     */
+    std::shared_ptr<Playback> getPlayback();
+
+    /**
+     * @brief Get the stream profile of the specified sensor
+     *
+     * @param sensorType The type of sensor
+     * @return std::shared_ptr<StreamProfileList> The stream profile list
+     */
+    std::shared_ptr<StreamProfileList> getStreamProfileList(OBSensorType sensorType);
+
+    /**
+     * @brief Turn on frame synchronization
+     */
+    void enableFrameSync();
+
+    /**
+     * @brief Turn off frame synchronization
+     */
+    void disableFrameSync();
+
+    /**
+     * @brief Get the camera parameters
+     *
+     * @note If D2C is enabled, it will return the camera parameters after D2C. If not, it will return the default parameters.
+     *
+     * @return OBCameraParam The camera parameters
+     */
+    OBCameraParam getCameraParam();
+
+    /**
+     * @brief Get camera parameters by entering color and depth resolution
+     * @attention If D2C is enabled, it will return the camera parameters after D2C, if not, it will return to the default parameters
+     *
+     * @param colorWidth Width of color resolution
+     * @param colorHeight High of color resolution
+     * @param depthWidth Width of depth resolution
+     * @param depthHeight High of depth resolution
+     *
+     * @return  OBCameraParam returns camera parameters
+     */
+    OBCameraParam getCameraParamWithProfile(uint32_t colorWidth, uint32_t colorHeight, uint32_t depthWidth, uint32_t depthHeight);
+
+    /**
+     * @brief Get the calibration parameters
+     *
+     * @param config The configured parameters
+     *
+     * @return OBCalibrationParam The calibration parameters
+     */
+    OBCalibrationParam getCalibrationParam(std::shared_ptr<Config> config);
+
+    /**
+     * @brief Return a list of D2C-enabled depth sensor resolutions corresponding to the input color sensor resolution
+     *
+     * @param colorProfile The input color sensor resolution
+     * @param alignMode The input align mode
+     * @return std::shared_ptr<StreamProfileList> A list of depth sensor resolutions
+     */
+    std::shared_ptr<StreamProfileList> getD2CDepthProfileList(std::shared_ptr<StreamProfile> colorProfile, OBAlignMode alignMode);
+
+    /**
+     * @brief Get the valid area between the minimum distance and maximum distance after D2C
+     *
+     * @param minimumDistance The minimum working distance
+     * @param maximumDistance The maximum working distance (optional)
+     * @return OBRect The area information valid after D2C at the working distance
+     */
+    OBRect getD2CValidArea(uint32_t minimumDistance, uint32_t maximumDistance = 0);
+
+    /**
+     * @brief Dynamically switch the corresponding config configuration
+     *
+     * @param config The updated config configuration
+     */
+    void switchConfig(std::shared_ptr<Config> config);
+
+    /**
+     * @brief Start recording
+     *
+     * @param filename The name of the record file
+     */
+    void startRecord(const char *filename);
+
+    /**
+     * @brief Stop recording
+     */
+    void stopRecord();
+};
+
+/**
+ * @brief Config class for configuring pipeline parameters
+ *
+ * The Config class provides an interface for configuring pipeline parameters.
+ */
+class OB_EXTENSION_API Config {
+private:
+    std::unique_ptr<ConfigImpl> impl_;
+
+public:
+    /**
+     * @brief Construct a new Config object
+     */
+    Config();
+
+    /**
+     * @brief Destroy the Config object
+     */
+    ~Config() noexcept;
+
+    /**
+     * @brief Enable a stream to be used in the pipeline
+     *
+     * @param streamProfile The stream configuration to be enabled
+     */
+    void enableStream(std::shared_ptr<StreamProfile> streamProfile);
+
+    /**
+     * @deprecated Use enableStream(std::shared_ptr<StreamProfile> streamProfile) instead
+     * @brief Enable all streams to be used in the pipeline
+     */
+    void enableAllStream();
+
+    /**
+     * @brief Enable a video stream to be used in the pipeline.
+     *
+     * This function allows users to enable a video stream with customizable parameters.
+     * If no parameters are specified, the stream will be enabled with default resolution settings.
+     * Users who wish to set custom resolutions should refer to the product manual, as available resolutions vary by camera model.
+     *
+     * @param type The video stream type.
+     * @param width The video stream width (default is OB_WIDTH_ANY, which selects the default resolution).
+     * @param height The video stream height (default is OB_HEIGHT_ANY, which selects the default resolution).
+     * @param fps The video stream frame rate (default is OB_FPS_ANY, which selects the default frame rate).
+     * @param format The video stream format (default is OB_FORMAT_ANY, which selects the default format).
+     */
+    void enableVideoStream(ob_stream_type type, int width = OB_WIDTH_ANY, int height = OB_HEIGHT_ANY, int fps = OB_FPS_ANY, OBFormat format = OB_FORMAT_ANY);
+
+    /**
+     * @brief Enable an accelerometer stream to be used in the pipeline.
+     *
+     * This function allows users to enable an accelerometer stream with customizable parameters.
+     * If no parameters are specified, the stream will be enabled with default settings.
+     * Users who wish to set custom full-scale ranges or sample rates should refer to the product manual, as available settings vary by device model.
+     *
+     * @param fullScaleRange The full-scale range of the accelerometer (default is OB_ACCEL_FULL_SCALE_RANGE_ANY, which selects the default range).
+     * @param sampleRate The sample rate of the accelerometer (default is OB_ACCEL_SAMPLE_RATE_ANY, which selects the default rate).
+     */
+    void enableAccelStream(ob_accel_full_scale_range fullScaleRange = OB_ACCEL_FULL_SCALE_RANGE_ANY,
+                           ob_accel_sample_rate      sampleRate     = OB_ACCEL_SAMPLE_RATE_ANY);
+
+    /**
+     * @brief Enable a gyroscope stream to be used in the pipeline.
+     *
+     * This function allows users to enable a gyroscope stream with customizable parameters.
+     * If no parameters are specified, the stream will be enabled with default settings.
+     * Users who wish to set custom full-scale ranges or sample rates should refer to the product manual, as available settings vary by device model.
+     *
+     * @param fullScaleRange The full-scale range of the gyroscope (default is OB_GYRO_FULL_SCALE_RANGE_ANY, which selects the default range).
+     * @param sampleRate The sample rate of the gyroscope (default is OB_GYRO_SAMPLE_RATE_ANY, which selects the default rate).
+     */
+    void enableGyroStream(ob_gyro_full_scale_range fullScaleRange = OB_GYRO_FULL_SCALE_RANGE_ANY, ob_gyro_sample_rate sampleRate = OB_GYRO_SAMPLE_RATE_ANY);
+
+    /**
+     * @brief Disable a stream to be used in the pipeline
+     *
+     * @param streamType The stream configuration to be disabled
+     */
+    void disableStream(OBStreamType streamType);
+
+    /**
+     * @brief Disable all streams to be used in the pipeline
+     */
+    void disableAllStream();
+
+    /**
+     * @brief Get the Enabled Stream Profile List
+     *
+     * @return std::shared_ptr<StreamProfileList>
+     */
+    std::shared_ptr<StreamProfileList> getEnabledStreamProfileList() const;
+
+    /**
+     * @brief Set the alignment mode
+     *
+     * @param mode The alignment mode
+     */
+    void setAlignMode(OBAlignMode mode);
+
+    /**
+     * @brief Set whether the depth needs to be scaled after setting D2C
+     *
+     * @param enable Whether scaling is required
+     */
+    void setDepthScaleRequire(bool enable);
+
+    /**
+     * @brief Set the D2C target resolution
+     * @brief The D2C target resolution is applicable to cases where the color stream is not enabled using the OrbbecSDK and the depth needs to be D2C.
+     *
+     * @note When you use OrbbecSDK to enable the color stream, you also use this interface to set the D2C target resolution. The configuration of the
+     * enabled Color stream is preferred for D2C.
+     *
+     * @param d2cTargetWidth The D2C target width resolution
+     * @param d2cTargetHeight The D2C target height resolution
+     */
+    void setD2CTargetResolution(uint32_t d2cTargetWidth, uint32_t d2cTargetHeight);
+
+    /**
+     * @brief Set the frame aggregation output mode for the pipeline configuration
+     * @brief The processing strategy when the FrameSet generated by the frame aggregation function does not contain the frames of all opened streams (which
+     * can be caused by different frame rates of each stream, or by the loss of frames of one stream): drop directly or output to the user.
+     *
+     * @param mode The frame aggregation output mode to be set (default mode is @ref OB_FRAME_AGGREGATE_OUTPUT_ANY_SITUATION)
+     */
+    void setFrameAggregateOutputMode(OBFrameAggregateOutputMode mode);
+
+    friend class Pipeline;
+};
+
+}  // namespace ob

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/RecordPlayback.hpp

@@ -0,0 +1,109 @@
+/**
+ * @file RecordPlayback.hpp
+ * @brief Header file for recording and playback functions.
+ */
+
+#pragma once
+
+#include "Types.hpp"
+#include <memory>
+
+struct RecorderImpl;
+struct PlaybackImpl;
+
+namespace ob {
+
+class Device;
+class Frame;
+class DeviceInfo;
+
+using PlaybackCallback   = std::function<void(std::shared_ptr<Frame> frame)>;
+using MediaStateCallback = std::function<void(OBMediaState state)>;
+
+class OB_EXTENSION_API Recorder {
+private:
+    std::unique_ptr<RecorderImpl> impl_;
+
+public:
+    /**
+     * @brief Create a recorder for data recording.
+     */
+    Recorder();
+    Recorder(std::unique_ptr<RecorderImpl> impl);
+
+    /**
+     * @brief Create a recorder for data recording.
+     * @param device The device for which to record device information.
+     */
+    Recorder(std::shared_ptr<Device> device);
+
+    virtual ~Recorder() noexcept;
+
+    /**
+     * @brief Enable the recorder. Throws an exception on failure.
+     *
+     * @param filename The name of the recorded file.
+     * @param async    Whether to execute asynchronously.
+     */
+    void start(const char *filename, bool async = false);
+
+    /**
+     * @brief Stop the recorder. Throws an exception on failure.
+     */
+    void stop();
+
+    /**
+     * @brief Write frame data to the recorder.
+     *
+     * @param frame The frame data to write.
+     */
+    void write(std::shared_ptr<Frame> frame);
+};
+
+class OB_EXTENSION_API Playback {
+private:
+    std::unique_ptr<PlaybackImpl> impl_;
+
+public:
+    /**
+     * @brief Create a playback object.
+     * @param filename The name of the playback file.
+     */
+    Playback(const char *filename);
+    Playback(std::unique_ptr<PlaybackImpl> impl);
+    virtual ~Playback() noexcept;
+
+    /**
+     * @brief Start playback. The playback data is returned from the callback. Throws an exception on failure.
+     * @param callback The callback for playback data.
+     * @param type     	The type of playback data.
+     */
+    void start(PlaybackCallback callback, OBMediaType type = OB_MEDIA_ALL);
+
+    /**
+     * @brief Stop playback. Throws an exception on failure.
+     */
+    void stop();
+
+    /**
+     * @brief Set the playback state.
+     * @param state The playback status callback.
+     */
+    void setPlaybackStateCallback(MediaStateCallback state);
+
+    /**
+     * @brief Get the device information in the recording file.
+     *
+     * @return DeviceInfo The device information.
+     */
+    std::shared_ptr<DeviceInfo> getDeviceInfo();
+
+    /**
+     * @brief Get the intrinsic and extrinsic parameter in the recording file.
+     *
+     * @return OBCameraParam Camera intrinsic and extrinsic parameter
+     */
+    OBCameraParam getCameraParam();
+};
+
+}  // namespace ob

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/Sensor.hpp

@@ -0,0 +1,149 @@
+/**
+ * @file Sensor.hpp
+ * @brief Defines types related to sensors, which are used to obtain stream configurations, open and close streams, and set and get sensor properties.
+ */
+#pragma once
+
+#include "Types.hpp"
+#include "libobsensor/hpp/Filter.hpp"
+
+#include <functional>
+#include <memory>
+
+struct SensorImpl;
+struct SensorListImpl;
+
+namespace ob {
+class StreamProfile;
+class StreamProfileList;
+class Device;
+class Frame;
+class ImuFrame;
+class OBFilterList;
+
+/**
+ * @brief Callback function for frame data.
+ *
+ * @param frame The frame data.
+ */
+using FrameCallback = std::function<void(std::shared_ptr<Frame> frame)>;
+
+class OB_EXTENSION_API Sensor {
+protected:
+    std::unique_ptr<SensorImpl> impl_;
+
+public:
+    Sensor(std::unique_ptr<SensorImpl> impl);
+    virtual ~Sensor() noexcept;
+
+    /**
+     * @brief Get the sensor type.
+     *
+     * @return OBSensorType The sensor type.
+     */
+    OBSensorType type();
+
+    /**
+     * @brief Get the list of stream profiles.
+     *
+     * @return std::shared_ptr<StreamProfileList> The stream profile list.
+     */
+    const std::shared_ptr<StreamProfileList> getStreamProfileList();
+
+    /**
+     * @brief Request recommended filters
+     * @return OBFilterList list of frame processing block
+     */
+    const std::shared_ptr<OBFilterList> getRecommendedFilters();
+
+    /**
+     * @brief Open a frame data stream and set up a callback.
+     *
+     * @param streamProfile The stream configuration.
+     * @param callback The callback to set when frame data arrives.
+     */
+    void start(std::shared_ptr<StreamProfile> streamProfile, FrameCallback callback);
+
+    /**
+     * @brief Stop the stream.
+     */
+    void stop();
+
+    /**
+     * @brief Dynamically switch resolutions.
+     *
+     * @param streamProfile The resolution to switch to.
+     */
+    void switchProfile(std::shared_ptr<StreamProfile> streamProfile);
+};
+
+class OB_EXTENSION_API SensorList {
+private:
+    std::unique_ptr<SensorListImpl> impl_;
+
+public:
+    SensorList(std::unique_ptr<SensorListImpl> impl);
+
+    virtual ~SensorList() noexcept;
+
+    /**
+     * @brief Get the number of sensors.
+     *
+     * @return uint32_t The number of sensors.
+     */
+    uint32_t count();
+
+    /**
+     * @brief Get the type of the specified sensor.
+     *
+     * @param index The sensor index.
+     * @return OBSensorType The sensor type.
+     */
+    OBSensorType type(uint32_t index);
+
+    /**
+     * @brief Get a sensor by index number.
+     *
+     * @param index The sensor index. The range is [0, count-1]. If the index exceeds the range, an exception will be thrown.
+     * @return std::shared_ptr<Sensor> The sensor object.
+     */
+    std::shared_ptr<Sensor> getSensor(uint32_t index);
+
+    /**
+     * @brief Get a sensor by sensor type.
+     *
+     * @param sensorType The sensor type to obtain.
+     * @return std::shared_ptr<Sensor> A sensor object. If the specified sensor type does not exist, it will return empty.
+     */
+    std::shared_ptr<Sensor> getSensor(OBSensorType sensorType);
+};
+
+
+/**
+ * @brief Class representing a list of FrameProcessingBlock
+ */
+class OB_EXTENSION_API OBFilterList {
+private:
+    std::unique_ptr<OBFilterListImpl> impl_;
+
+public:
+    OBFilterList(std::unique_ptr<OBFilterListImpl> impl_);
+    ~OBFilterList() noexcept;
+
+    /**
+     * @brief Get the number of OBDepthWorkMode FrameProcessingBlock in the list
+     *
+     * @return uint32_t the number of FrameProcessingBlock objects in the list
+     */
+    uint32_t count();
+
+    /**
+     * @brief Get the Filter object at the specified index
+     *
+     * @param index the index of the target Filter object
+     * @return the Filter object at the specified index
+     */
+    std::shared_ptr<Filter> getFilter(uint32_t index);
+};
+
+}  // namespace ob

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/StreamProfile.hpp

@@ -0,0 +1,267 @@
+/**
+ * @file StreamProfile.hpp
+ * @brief The stream profile related type is used to get information such as the width, height, frame rate, and format of the stream.
+ */
+#pragma once
+
+#include "Types.hpp"
+
+#include <iostream>
+#include <memory>
+
+struct StreamProfileImpl;
+struct StreamProfileListImpl;
+
+namespace ob {
+
+class VideoStreamProfile;
+class GyroStreamProfile;
+class AccelStreamProfile;
+class Config;
+
+class OB_EXTENSION_API StreamProfile : public std::enable_shared_from_this<StreamProfile> {
+protected:
+    std::unique_ptr<StreamProfileImpl> impl_;
+
+public:
+    StreamProfile(std::unique_ptr<StreamProfileImpl> impl);
+    StreamProfile(StreamProfile &streamProfile);
+    virtual ~StreamProfile() noexcept;
+
+    /**
+     * @brief Get the format of the stream
+     *
+     * @return OBFormat return the format of the stream
+     */
+    OBFormat format() const;
+
+    /**
+     * @brief Get the type of stream
+     *
+     * @return OBStreamType return the type of the stream
+     */
+    OBStreamType type() const;
+
+    /**
+     * @brief Get the extrinsic parameters from current stream profile to the given target stream profile
+     *
+     * @return OBExtrinsic Return the extrinsic parameters.
+     */
+    OBExtrinsic getExtrinsicTo(std::shared_ptr<StreamProfile> target);
+
+    /**
+     * @brief Check if frame object is compatible with the given type
+     *
+     * @tparam T  Given type
+     * @return bool return result
+     */
+    template <typename T> bool is();
+
+    /**
+     * @brief Converts object type to target type
+     *
+     * @tparam T Target type
+     * @return std::shared_ptr<T> Return the result. Throws an exception if conversion is not possible.
+     */
+    template <typename T> std::shared_ptr<T> as() {
+        if(!is<T>()) {
+            throw std::runtime_error("Unsupported operation. Object's type is not the required type.");
+        }
+
+        return std::static_pointer_cast<T>(std::const_pointer_cast<StreamProfile>(shared_from_this()));
+    }
+
+    friend class Sensor;
+    friend class Config;
+    friend class Pipeline;
+};
+
+/**
+ * @brief Class representing a video stream profile.
+ */
+class OB_EXTENSION_API VideoStreamProfile : public StreamProfile {
+public:
+    explicit VideoStreamProfile(StreamProfile &profile);
+
+    explicit VideoStreamProfile(std::unique_ptr<StreamProfileImpl> impl);
+
+    ~VideoStreamProfile() noexcept override;
+
+    /**
+     * @brief Return the frame rate of the stream.
+     *
+     * @return uint32_t Return the frame rate of the stream.
+     */
+    uint32_t fps() const;
+
+    /**
+     * @brief Return the width of the stream.
+     *
+     * @return uint32_t Return the width of the stream.
+     */
+    uint32_t width() const;
+
+    /**
+     * @brief Return the height of the stream.
+     *
+     * @return uint32_t Return the height of the stream.
+     */
+    uint32_t height() const;
+
+    /**
+     * @brief Get the intrinsic parameters of the stream.
+     *
+     * @return OBCameraIntrinsic Return the intrinsic parameters.
+     */
+    OBCameraIntrinsic getIntrinsic();
+
+    /**
+     * @brief Get the distortion parameters of the stream.
+     * @brief Brown distortion model
+     *
+     * @return OBCameraDistortion Return the distortion parameters.
+     */
+    OBCameraDistortion getDistortion();
+};
+
+/**
+ * @brief Class representing an accelerometer stream profile.
+ */
+class OB_EXTENSION_API AccelStreamProfile : public StreamProfile {
+public:
+    explicit AccelStreamProfile(StreamProfile &profile);
+
+    explicit AccelStreamProfile(std::unique_ptr<StreamProfileImpl> impl);
+
+    ~AccelStreamProfile() noexcept override;
+
+    /**
+     * @brief Return the full scale range.
+     *
+     * @return OBAccelFullScaleRange Return the scale range value.
+     */
+    OBAccelFullScaleRange fullScaleRange() const;
+
+    /**
+     * @brief Return the sampling frequency.
+     *
+     * @return OBAccelFullScaleRange Return the sampling frequency.
+     */
+    OBAccelSampleRate sampleRate() const;
+
+    /**
+     * @brief get the intrinsic parameters of the stream.
+     *
+     * @return OBAccelIntrinsic Return the intrinsic parameters.
+     */
+    OBAccelIntrinsic getIntrinsic();
+};
+
+/**
+ * @brief Class representing a gyroscope stream profile.
+ */
+class OB_EXTENSION_API GyroStreamProfile : public StreamProfile {
+public:
+    explicit GyroStreamProfile(StreamProfile &profile);
+
+    explicit GyroStreamProfile(std::unique_ptr<StreamProfileImpl> impl);
+    ~GyroStreamProfile() noexcept override;
+
+    /**
+     * @brief Return the full scale range.
+     *
+     * @return OBAccelFullScaleRange Return the scale range value.
+     */
+    OBGyroFullScaleRange fullScaleRange() const;
+
+    /**
+     * @brief Return the sampling frequency.
+     *
+     * @return OBAccelFullScaleRange Return the sampling frequency.
+     */
+    OBGyroSampleRate sampleRate() const;
+
+    /**
+     * @brief get the intrinsic parameters of the stream.
+     *
+     * @return OBGyroIntrinsic Return the intrinsic parameters.
+     */
+    OBGyroIntrinsic getIntrinsic();
+};
+
+template <typename T> bool StreamProfile::is() {
+    switch(this->type()) {
+    case OB_STREAM_VIDEO:
+    case OB_STREAM_IR:
+    case OB_STREAM_IR_LEFT:
+    case OB_STREAM_IR_RIGHT:
+    case OB_STREAM_COLOR:
+    case OB_STREAM_DEPTH:
+    case OB_STREAM_RAW_PHASE:
+        return typeid(T) == typeid(VideoStreamProfile);
+    case OB_STREAM_ACCEL:
+        return typeid(T) == typeid(AccelStreamProfile);
+    case OB_STREAM_GYRO:
+        return typeid(T) == typeid(GyroStreamProfile);
+    default:
+        break;
+    }
+    return false;
+}
+
+class OB_EXTENSION_API StreamProfileList {
+protected:
+    std::unique_ptr<StreamProfileListImpl> impl_;
+
+public:
+    explicit StreamProfileList(std::unique_ptr<StreamProfileListImpl> impl);
+    ~StreamProfileList() noexcept;
+
+    /**
+     * @brief Return the number of StreamProfile objects.
+     *
+     * @return uint32_t Return the number of StreamProfile objects.
+     */
+    uint32_t count() const;
+
+    /**
+     * @brief Return the StreamProfile object at the specified index.
+     *
+     * @param index The index of the StreamProfile object to be retrieved. Must be in the range [0, count-1]. Throws an exception if the index is out of range.
+     * @return std::shared_ptr<StreamProfile> Return the StreamProfile object.
+     */
+    const std::shared_ptr<StreamProfile> getProfile(uint32_t index);
+
+    /**
+     * @brief Match the corresponding video stream profile based on the passed-in parameters. If multiple Match are found, the first one in the list is
+     * returned by default. Throws an exception if no matching profile is found.
+     *
+     * @param width The width of the stream. Pass OB_WIDTH_ANY if no matching condition is required.
+     * @param height The height of the stream. Pass OB_HEIGHT_ANY if no matching condition is required.
+     * @param format The type of the stream. Pass OB_FORMAT_ANY if no matching condition is required.
+     * @param fps The frame rate of the stream. Pass OB_FPS_ANY if no matching condition is required.
+     * @return std::shared_ptr<VideoStreamProfile> Return the matching resolution.
+     */
+    const std::shared_ptr<VideoStreamProfile> getVideoStreamProfile(int width = OB_WIDTH_ANY, int height = OB_HEIGHT_ANY, OBFormat format = OB_FORMAT_ANY,
+                                                                    int fps = OB_FPS_ANY);
+
+    /**
+     * @brief Match the corresponding accelerometer stream profile based on the passed-in parameters. If multiple Match are found, the first one in the list
+     * is returned by default. Throws an exception if no matching profile is found.
+     *
+     * @param fullScaleRange The full scale range. Pass 0 if no matching condition is required.
+     * @param sampleRate The sampling frequency. Pass 0 if no matching condition is required.
+     */
+    const std::shared_ptr<AccelStreamProfile> getAccelStreamProfile(OBAccelFullScaleRange fullScaleRange, OBAccelSampleRate sampleRate);
+
+    /**
+     * @brief Match the corresponding gyroscope stream profile based on the passed-in parameters. If multiple Match are found, the first one in the list is
+     * returned by default. Throws an exception if no matching profile is found.
+     *
+     * @param fullScaleRange The full scale range. Pass 0 if no matching condition is required.
+     * @param sampleRate The sampling frequency. Pass 0 if no matching condition is required.
+     */
+    const std::shared_ptr<GyroStreamProfile> getGyroStreamProfile(OBGyroFullScaleRange fullScaleRange, OBGyroSampleRate sampleRate);
+};
+
+}  // namespace ob

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/Types.hpp

@@ -0,0 +1,60 @@
+/**
+ * @file Types.hpp
+ * @brief Provides SDK structure and enumeration constant definitions (depending on libobsensor/h/ObTypes.h).
+ */
+
+#pragma once
+
+#include "libobsensor/h/ObTypes.h"
+
+#include <functional>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Callback function for file transfer status updates.
+ *
+ * @param state The file transfer status.
+ * @param message Status information.
+ * @param percent The percentage of the file that has been transferred.
+ */
+using SendFileCallback = std::function<void(OBFileTranState state, const char *message, uint8_t percent)>;
+
+/**
+ * @brief Callback function for device upgrade status updates.
+ *
+ * @param state The device upgrade status.
+ * @param message Status information.
+ * @param percent The percentage of the upgrade that has been completed.
+ */
+using DeviceUpgradeCallback = std::function<void(OBUpgradeState state, const char *message, uint8_t percent)>;
+
+/**
+ * @brief Callback function for device status updates.
+ *
+ * @param state The device status.
+ * @param message Status information.
+ */
+using DeviceStateChangedCallback = std::function<void(OBDeviceState state, const char *message)>;
+
+/**
+ * @brief Callback function for getting raw data property data when data and progress callbacks are made.
+ *
+ * @param dataChunk The data chunk.
+ * @param state The status of getting the data.
+ */
+using GetDataCallback = std::function<void(OBDataTranState state, OBDataChunk *dataChunk)>;
+
+/**
+ * @brief Callback function for setting the raw data property when progress callbacks are made.
+ *
+ * @param percent The progress percentage.
+ * @param state The status of setting the data.
+ */
+using SetDataCallback = std::function<void(OBDataTranState state, uint8_t percent)>;
+
+#ifdef __cplusplus
+}
+#endif

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/Utils.hpp

@@ -0,0 +1,138 @@
+/**
+ * @file Utils.hpp
+ * @brief The SDK utils class
+ *
+ */
+#pragma once
+
+#include "Types.hpp"
+
+namespace ob {
+class Device;
+
+class OB_EXTENSION_API CoordinateTransformHelper {
+public:
+    /**
+     * @brief Transform a 3d point of a source coordinate system into a 3d point of the target coordinate system.
+     *
+     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+     * @param sourcePoint3f Source 3d point value
+     * @param sourceSensorType Source sensor type
+     * @param targetSensorType Target sensor type
+     * @param targetPoint3f Target 3d point value
+     *
+     * @return bool  Transform result
+     */
+    static bool calibration3dTo3d(const OBCalibrationParam calibrationParam, const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType,
+                                  const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
+
+    /**
+     * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 3d point of the target coordinate system.
+     *
+     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+     * @param sourcePoint2f Source 2d point value
+     * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
+     * @param sourceSensorType Source sensor type
+     * @param targetSensorType Target sensor type
+     * @param targetPoint3f Target 3d point value
+     *
+     * @return bool Transform result
+     */
+    static bool calibration2dTo3d(const OBCalibrationParam calibrationParam, const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
+                                  const OBSensorType sourceSensorType, const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
+
+    /**
+     * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 3d point of the target coordinate system.
+     * @brief This function uses undistortion, which may result in longer processing time.
+     *
+     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+     * @param sourcePoint2f Source 2d point value
+     * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
+     * @param sourceSensorType Source sensor type
+     * @param targetSensorType Target sensor type
+     * @param targetPoint3f Target 3d point value
+     *
+     * @return bool Transform result
+     */
+    static bool calibration2dTo3dUndistortion(const OBCalibrationParam calibrationParam, const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
+                                              const OBSensorType sourceSensorType, const OBSensorType targetSensorType, OBPoint3f *targetPoint3f);
+
+    /**
+     * @brief Transform a 3d point of a source coordinate system into a 2d pixel coordinate of the target camera.
+     *
+     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+     * @param sourcePoint3f Source 3d point value
+     * @param sourceSensorType Source sensor type
+     * @param targetSensorType Target sensor type
+     * @param targetPoint2f Target 2d point value
+     *
+     * @return bool Transform result
+     */
+    static bool calibration3dTo2d(const OBCalibrationParam calibrationParam, const OBPoint3f sourcePoint3f, const OBSensorType sourceSensorType,
+                                  const OBSensorType targetSensorType, OBPoint2f *targetPoint2f);
+
+    /**
+     * @brief Transform a 2d pixel coordinate with an associated depth value of the source camera into a 2d pixel coordinate of the target camera
+     *
+     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+     * @param sourcePoint2f Source 2d point value
+     * @param sourceDepthPixelValue The depth of sourcePoint2f in millimeters
+     * @param sourceSensorType Source sensor type
+     * @param targetSensorType Target sensor type
+     * @param targetPoint2f Target 2d point value
+     *
+     * @return bool Transform result
+     */
+    static bool calibration2dTo2d(const OBCalibrationParam calibrationParam, const OBPoint2f sourcePoint2f, const float sourceDepthPixelValue,
+                                  const OBSensorType sourceSensorType, const OBSensorType targetSensorType, OBPoint2f *targetPoint2f);
+
+    /**
+     * @brief Transforms the depth frame into the geometry of the color camera.
+     *
+     * @param device Device handle
+     * @param depthFrame Input depth frame
+     * @param targetColorCameraWidth Target color camera width
+     * @param targetColorCameraHeight Target color camera height
+     *
+     * @return std::shared_ptr<ob::Frame> Transformed depth frame
+     */
+    static std::shared_ptr<ob::Frame> transformationDepthFrameToColorCamera(std::shared_ptr<ob::Device> device, std::shared_ptr<ob::Frame> depthFrame,
+                                                                            uint32_t targetColorCameraWidth, uint32_t targetColorCameraHeight);
+
+    /**
+     * @brief Init transformation tables
+     *
+     * @param calibrationParam Device calibration param,see pipeline::getCalibrationParam
+     * @param sensorType sensor type
+     * @param data input data,needs to be allocated externally.During initialization, the external allocation size is 'dataSize', for example, dataSize = 1920 *
+     * 1080 * 2*sizeof(float) (1920 * 1080 represents the image resolution, and 2 represents two LUTs, one for x-coordinate and one for y-coordinate).
+     * @param dataSize input data size
+     * @param xyTables output xy tables
+     *
+     * @return bool Transform result
+     */
+    static bool transformationInitXYTables(const OBCalibrationParam calibrationParam, const OBSensorType sensorType, float *data, uint32_t *dataSize,
+                                           OBXYTables *xyTables);
+
+    /**
+     * @brief Transform depth image to point cloud data
+     *
+     * @param xyTables input xy tables,see CoordinateTransformHelper::transformationInitXYTables
+     * @param depthImageData input depth image data
+     * @param pointCloudData output point cloud data
+     *
+     */
+    static void transformationDepthToPointCloud(OBXYTables *xyTables, const void *depthImageData, void *pointCloudData);
+
+    /**
+     * @brief Transform depth image to RGBD point cloud data
+     *
+     * @param xyTables input xy tables,see CoordinateTransformHelper::transformationInitXYTables
+     * @param depthImageData input depth image data
+     * @param colorImageData input color image data (only RGB888 support)
+     * @param pointCloudData output RGBD point cloud data
+     *
+     */
+    static void transformationDepthToRGBDPointCloud(OBXYTables *xyTables, const void *depthImageData, const void *colorImageData, void *pointCloudData);
+};
+}  // namespace ob

OrbbecSDK_ROS2/orbbec_camera/SDK/include/libobsensor/hpp/Version.hpp

@@ -0,0 +1,45 @@
+/**
+ * @file Version.hpp
+ * @brief Provides functions to retrieve version information of the SDK.
+ */
+#pragma once
+
+namespace ob {
+class OB_EXTENSION_API Version {
+public:
+    /**
+     * @brief Get the major version number of the SDK.
+     *
+     * @return int The major version number of the SDK.
+     */
+    static int getMajor();
+
+    /**
+     * @brief Get the minor version number of the SDK.
+     *
+     * @return int The minor version number of the SDK.
+     */
+    static int getMinor();
+
+    /**
+     * @brief Get the patch version number of the SDK.
+     *
+     * @return int The patch version number of the SDK.
+     */
+    static int getPatch();
+
+    /**
+     * @brief Get the full version number of the SDK.
+     *
+     * @return int The full version number of the SDK.
+     */
+    static int getVersion();
+
+    /**
+     * @brief Get the stage version of the SDK.
+     *
+     * @return char* The stage version string of the SDK.
+     */
+    static char *getStageVersion();
+};
+}  // namespace ob

OrbbecSDK_ROS2/orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so

@@ -0,0 +1 @@
+libOrbbecSDK.so.1.10