diff --git a/FAST_LIO/.github/stale.yml b/FAST_LIO/.github/stale.yml
new file mode 100644
index 0000000..4d545fe
--- /dev/null
+++ b/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
diff --git a/FAST_LIO/.gitignore b/FAST_LIO/.gitignore
new file mode 100644
index 0000000..eb7be62
--- /dev/null
+++ b/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
diff --git a/FAST_LIO/.gitmodules b/FAST_LIO/.gitmodules
new file mode 100644
index 0000000..b8a0efa
--- /dev/null
+++ b/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
diff --git a/FAST_LIO/CMakeLists.txt b/FAST_LIO/CMakeLists.txt
new file mode 100644
index 0000000..e091f3a
--- /dev/null
+++ b/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()
diff --git a/FAST_LIO/LICENSE b/FAST_LIO/LICENSE
new file mode 100644
index 0000000..d159169
--- /dev/null
+++ b/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.
diff --git a/FAST_LIO/Log/fast_lio_time_log_analysis.m b/FAST_LIO/Log/fast_lio_time_log_analysis.m
new file mode 100644
index 0000000..8a0b679
--- /dev/null
+++ b/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')
+
diff --git a/FAST_LIO/Log/guide.md b/FAST_LIO/Log/guide.md
new file mode 100644
index 0000000..8ff3fc1
--- /dev/null
+++ b/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.
diff --git a/FAST_LIO/Log/plot.py b/FAST_LIO/Log/plot.py
new file mode 100644
index 0000000..e4aad85
--- /dev/null
+++ b/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()
diff --git a/FAST_LIO/PCD/1 b/FAST_LIO/PCD/1
new file mode 100644
index 0000000..d00491f
--- /dev/null
+++ b/FAST_LIO/PCD/1
@@ -0,0 +1 @@
+1
diff --git a/FAST_LIO/README.md b/FAST_LIO/README.md
new file mode 100644
index 0000000..412d1e1
--- /dev/null
+++ b/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).
diff --git a/FAST_LIO/config/avia.yaml b/FAST_LIO/config/avia.yaml
new file mode 100644
index 0000000..3bbc880
--- /dev/null
+++ b/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.
diff --git a/FAST_LIO/config/horizon.yaml b/FAST_LIO/config/horizon.yaml
new file mode 100644
index 0000000..2ff8654
--- /dev/null
+++ b/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.
diff --git a/FAST_LIO/config/mid360.yaml b/FAST_LIO/config/mid360.yaml
new file mode 100644
index 0000000..a987453
--- /dev/null
+++ b/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.
diff --git a/FAST_LIO/config/ouster64.yaml b/FAST_LIO/config/ouster64.yaml
new file mode 100644
index 0000000..6f47b82
--- /dev/null
+++ b/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.
diff --git a/FAST_LIO/config/unilidar_l2.yaml b/FAST_LIO/config/unilidar_l2.yaml
new file mode 100644
index 0000000..ef41db6
--- /dev/null
+++ b/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                          # 全帧保存（注意内存风险）
diff --git a/FAST_LIO/config/velodyne.yaml b/FAST_LIO/config/velodyne.yaml
new file mode 100644
index 0000000..24e6bf2
--- /dev/null
+++ b/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.
diff --git a/FAST_LIO/include/Exp_mat.h b/FAST_LIO/include/Exp_mat.h
new file mode 100644
index 0000000..7ab2897
--- /dev/null
+++ b/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
diff --git a/FAST_LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp b/FAST_LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp
new file mode 100755
index 0000000..7547584
--- /dev/null
+++ b/FAST_LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp
@@ -0,0 +1,2008 @@
+/*
+ *  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 ESEKFOM_EKF_HPP
+#define ESEKFOM_EKF_HPP
+
+
+#include <vector>
+#include <cstdlib>
+
+#include <boost/bind.hpp>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <Eigen/Dense>
+#include <Eigen/Sparse>
+
+#include "../mtk/types/vect.hpp"
+#include "../mtk/types/SOn.hpp"
+#include "../mtk/types/S2.hpp"
+#include "../mtk/startIdx.hpp"
+#include "../mtk/build_manifold.hpp"
+#include "util.hpp"
+
+//#define USE_sparse
+
+
+namespace esekfom {
+
+using namespace Eigen;
+
+//used for iterated error state EKF update
+//for the aim to calculate  measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
+//applied for measurement as a manifold.
+template<typename S, typename M, int measurement_noise_dof = M::DOF>
+struct share_datastruct
+{
+	bool valid;
+	bool converge;
+	M z;
+	Eigen::Matrix<typename S::scalar, M::DOF, measurement_noise_dof> h_v;
+	Eigen::Matrix<typename S::scalar, M::DOF, S::DOF> h_x;
+	Eigen::Matrix<typename S::scalar, measurement_noise_dof, measurement_noise_dof> R;
+};
+
+//used for iterated error state EKF update
+//for the aim to calculate  measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
+//applied for measurement as an Eigen matrix whose dimension is changing
+template<typename T>
+struct dyn_share_datastruct
+{
+	bool valid;
+	bool converge;
+	Eigen::Matrix<T, Eigen::Dynamic, 1> z;
+	Eigen::Matrix<T, Eigen::Dynamic, 1> h;
+	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_v;
+	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_x;
+	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> R;
+};
+
+//used for iterated error state EKF update
+//for the aim to calculate  measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
+//applied for measurement as a dynamic manifold whose dimension or type is changing
+template<typename T>
+struct dyn_runtime_share_datastruct
+{
+	bool valid;
+	bool converge;
+	//Z z;
+	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_v;
+	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_x;
+	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> R;
+};
+
+template<typename state, int process_noise_dof, typename input = state, typename measurement=state, int measurement_noise_dof=0>
+class esekf{
+
+	typedef esekf self;
+	enum{
+		n = state::DOF, m = state::DIM, l = measurement::DOF
+	};
+
+public:
+	
+	typedef typename state::scalar scalar_type;
+	typedef Matrix<scalar_type, n, n> cov;
+	typedef Matrix<scalar_type, m, n> cov_;
+	typedef SparseMatrix<scalar_type> spMt;
+	typedef Matrix<scalar_type, n, 1> vectorized_state;
+	typedef Matrix<scalar_type, m, 1> flatted_state;
+	typedef flatted_state processModel(state &, const input &);
+	typedef Eigen::Matrix<scalar_type, m, n> processMatrix1(state &, const input &);
+	typedef Eigen::Matrix<scalar_type, m, process_noise_dof> processMatrix2(state &, const input &);
+	typedef Eigen::Matrix<scalar_type, process_noise_dof, process_noise_dof> processnoisecovariance;
+	typedef measurement measurementModel(state &, bool &);
+	typedef measurement measurementModel_share(state &, share_datastruct<state, measurement, measurement_noise_dof> &);
+	typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, 1> measurementModel_dyn(state &, bool &);
+	//typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, 1> measurementModel_dyn_share(state &,  dyn_share_datastruct<scalar_type> &);
+	typedef void measurementModel_dyn_share(state &,  dyn_share_datastruct<scalar_type> &);
+	typedef Eigen::Matrix<scalar_type ,l, n> measurementMatrix1(state &, bool&);
+	typedef Eigen::Matrix<scalar_type , Eigen::Dynamic, n> measurementMatrix1_dyn(state &, bool&);
+	typedef Eigen::Matrix<scalar_type ,l, measurement_noise_dof> measurementMatrix2(state &, bool&);
+	typedef Eigen::Matrix<scalar_type ,Eigen::Dynamic, Eigen::Dynamic> measurementMatrix2_dyn(state &, bool&);
+	typedef Eigen::Matrix<scalar_type, measurement_noise_dof, measurement_noise_dof> measurementnoisecovariance;
+	typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> measurementnoisecovariance_dyn;
+
+	esekf(const state &x = state(),
+		const cov  &P = cov::Identity()): x_(x), P_(P){
+	#ifdef USE_sparse
+		SparseMatrix<scalar_type> ref(n, n);
+		ref.setIdentity();
+		l_ = ref;
+		f_x_2 = ref;
+		f_x_1 = ref;
+	#endif
+	};
+
+	//receive system-specific models and their differentions.
+	//for measurement as a manifold.
+	void init(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel h_in, measurementMatrix1 h_x_in, measurementMatrix2 h_v_in, int maximum_iteration, scalar_type limit_vector[n])
+	{
+		f = f_in;
+		f_x = f_x_in;
+		f_w = f_w_in;
+		h = h_in;
+		h_x = h_x_in;
+		h_v = h_v_in;
+
+		maximum_iter = maximum_iteration;
+		for(int i=0; i<n; i++)
+		{
+			limit[i] = limit_vector[i];
+		}
+
+		x_.build_S2_state();
+		x_.build_SO3_state();
+		x_.build_vect_state();
+	}
+
+	//receive system-specific models and their differentions.
+	//for measurement as an Eigen matrix whose dimention is chaing.
+	void init_dyn(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_dyn h_in, measurementMatrix1_dyn h_x_in, measurementMatrix2_dyn h_v_in, int maximum_iteration, scalar_type limit_vector[n])
+	{
+		f = f_in;
+		f_x = f_x_in;
+		f_w = f_w_in;
+		h_dyn = h_in;
+		h_x_dyn = h_x_in;
+		h_v_dyn = h_v_in;
+
+
+		maximum_iter = maximum_iteration;
+		for(int i=0; i<n; i++)
+		{
+			limit[i] = limit_vector[i];
+		}
+		
+		x_.build_S2_state();
+		x_.build_SO3_state();
+		x_.build_vect_state();
+	}
+
+	//receive system-specific models and their differentions.
+	//for measurement as a dynamic manifold whose dimension or type is changing.
+	void init_dyn_runtime(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementMatrix1_dyn h_x_in, measurementMatrix2_dyn h_v_in, int maximum_iteration, scalar_type limit_vector[n])
+	{
+		f = f_in;
+		f_x = f_x_in;
+		f_w = f_w_in;
+		h_x_dyn = h_x_in;
+		h_v_dyn = h_v_in;
+
+		maximum_iter = maximum_iteration;
+		for(int i=0; i<n; i++)
+		{
+			limit[i] = limit_vector[i];
+		}
+
+		x_.build_S2_state();
+		x_.build_SO3_state();
+		x_.build_vect_state();
+	}
+
+	//receive system-specific models and their differentions
+	//for measurement as a manifold.
+	//calculate  measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function (h_share_in).
+	void init_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_share h_share_in, int maximum_iteration, scalar_type limit_vector[n])
+	{
+		f = f_in;
+		f_x = f_x_in;
+		f_w = f_w_in;
+		h_share = h_share_in;
+
+		maximum_iter = maximum_iteration;
+		for(int i=0; i<n; i++)
+		{
+			limit[i] = limit_vector[i];
+		}
+
+		x_.build_S2_state();
+		x_.build_SO3_state();
+		x_.build_vect_state();
+	}
+
+	//receive system-specific models and their differentions
+	//for measurement as an Eigen matrix whose dimension is changing.
+	//calculate  measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function (h_dyn_share_in).
+	void init_dyn_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_dyn_share h_dyn_share_in, int maximum_iteration, scalar_type limit_vector[n])
+	{
+		f = f_in;
+		f_x = f_x_in;
+		f_w = f_w_in;
+		h_dyn_share = h_dyn_share_in;
+
+		maximum_iter = maximum_iteration;
+		for(int i=0; i<n; i++)
+		{
+			limit[i] = limit_vector[i];
+		}
+
+		x_.build_S2_state();
+		x_.build_SO3_state();
+		x_.build_vect_state();
+	}
+
+
+	//receive system-specific models and their differentions
+	//for measurement as a dynamic manifold whose dimension  or type is changing.
+	//calculate  measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function (h_dyn_share_in).
+	//for any scenarios where it is needed
+	void init_dyn_runtime_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, int maximum_iteration, scalar_type limit_vector[n])
+	{
+		f = f_in;
+		f_x = f_x_in;
+		f_w = f_w_in;
+
+		maximum_iter = maximum_iteration;
+		for(int i=0; i<n; i++)
+		{
+			limit[i] = limit_vector[i];
+		}
+
+		x_.build_S2_state();
+		x_.build_SO3_state();
+		x_.build_vect_state();
+	}
+
+	// iterated error state EKF propogation
+	void predict(double &dt, processnoisecovariance &Q, const input &i_in){
+		flatted_state f_ = f(x_, i_in);
+		cov_ f_x_ = f_x(x_, i_in);
+		cov f_x_final;
+
+		Matrix<scalar_type, m, process_noise_dof> f_w_ = f_w(x_, i_in);
+		Matrix<scalar_type, n, process_noise_dof> f_w_final;
+		state x_before = x_;
+		x_.oplus(f_, dt);
+
+		F_x1 = cov::Identity();
+		for (std::vector<std::pair<std::pair<int, int>, int> >::iterator it = x_.vect_state.begin(); it != x_.vect_state.end(); it++) {
+			int idx = (*it).first.first;
+			int dim = (*it).first.second;
+			int dof = (*it).second;
+			for(int i = 0; i < n; i++){
+				for(int j=0; j<dof; j++)
+				{f_x_final(idx+j, i) = f_x_(dim+j, i);}	
+			}
+			for(int i = 0; i < process_noise_dof; i++){
+				for(int j=0; j<dof; j++)
+				{f_w_final(idx+j, i) = f_w_(dim+j, i);}
+			}
+		}
+		Matrix<scalar_type, 3, 3> res_temp_SO3;
+		MTK::vect<3, scalar_type> seg_SO3;
+		for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+			int idx = (*it).first;
+			int dim = (*it).second;
+			for(int i = 0; i < 3; i++){
+				seg_SO3(i) = -1 * f_(dim + i) * dt;
+			}
+			MTK::SO3<scalar_type> res;
+			res.w() = MTK::exp<scalar_type, 3>(res.vec(), seg_SO3, scalar_type(1/2));
+		#ifdef USE_sparse
+			res_temp_SO3 = res.toRotationMatrix();
+			for(int i = 0; i < 3; i++){
+				for(int j = 0; j < 3; j++){
+					f_x_1.coeffRef(idx + i, idx + j) = res_temp_SO3(i, j);
+				}
+			}
+		#else
+			F_x1.template block<3, 3>(idx, idx) = res.toRotationMatrix();
+		#endif			
+			res_temp_SO3 = MTK::A_matrix(seg_SO3);
+			for(int i = 0; i < n; i++){
+				f_x_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_. template block<3, 1>(dim, i));	
+			}
+			for(int i = 0; i < process_noise_dof; i++){
+				f_w_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_w_. template block<3, 1>(dim, i));
+			}
+		}
+		
+		
+		Matrix<scalar_type, 2, 3> res_temp_S2;
+		Matrix<scalar_type, 2, 2> res_temp_S2_;
+		MTK::vect<3, scalar_type> seg_S2;
+		for (std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+			int idx = (*it).first;
+			int dim = (*it).second;
+			for(int i = 0; i < 3; i++){
+				seg_S2(i) = f_(dim + i) * dt;
+			}
+			MTK::vect<2, scalar_type> vec = MTK::vect<2, scalar_type>::Zero();
+			MTK::SO3<scalar_type> res;
+			res.w() = MTK::exp<scalar_type, 3>(res.vec(), seg_S2, scalar_type(1/2));
+			Eigen::Matrix<scalar_type, 2, 3> Nx;
+			Eigen::Matrix<scalar_type, 3, 2> Mx;
+			x_.S2_Nx_yy(Nx, idx);
+			x_before.S2_Mx(Mx, vec, idx);
+		#ifdef USE_sparse
+			res_temp_S2_ = Nx * res.toRotationMatrix() * Mx;
+			for(int i = 0; i < 2; i++){
+				for(int j = 0; j < 2; j++){
+					f_x_1.coeffRef(idx + i, idx + j) = res_temp_S2_(i, j);
+				}
+			}
+		#else
+			F_x1.template block<2, 2>(idx, idx) = Nx * res.toRotationMatrix() * Mx;
+		#endif
+
+			Eigen::Matrix<scalar_type, 3, 3> x_before_hat;
+			x_before.S2_hat(x_before_hat, idx);
+			res_temp_S2 = -Nx * res.toRotationMatrix() * x_before_hat*MTK::A_matrix(seg_S2).transpose();
+			
+			for(int i = 0; i < n; i++){
+				f_x_final. template block<2, 1>(idx, i) = res_temp_S2 * (f_x_. template block<3, 1>(dim, i));
+				
+			}
+			for(int i = 0; i < process_noise_dof; i++){
+				f_w_final. template block<2, 1>(idx, i) = res_temp_S2 * (f_w_. template block<3, 1>(dim, i));
+			}
+		}
+	
+	#ifdef USE_sparse
+		f_x_1.makeCompressed();
+		spMt f_x2 = f_x_final.sparseView();
+		spMt f_w1 = f_w_final.sparseView();
+		spMt xp = f_x_1 + f_x2 * dt;
+		P_ = xp * P_ * xp.transpose() + (f_w1 * dt) * Q * (f_w1 * dt).transpose();
+	#else
+		F_x1 += f_x_final * dt;
+		P_ = (F_x1) * P_ * (F_x1).transpose() + (dt * f_w_final) * Q * (dt * f_w_final).transpose();
+	#endif
+	}
+
+	//iterated error state EKF update for measurement as a manifold.
+	void update_iterated(measurement& z, measurementnoisecovariance &R) {
+		
+		if(!(is_same<typename measurement::scalar, scalar_type>())){
+			std::cerr << "the scalar type of measurment must be the same as the state" << std::endl;
+			std::exit(100);
+		}
+		int t = 0;
+		bool converg = true;   
+		bool valid = true;    
+		state x_propagated = x_;
+		cov P_propagated = P_;
+		
+		for(int i=-1; i<maximum_iter; i++)
+		{
+			vectorized_state dx, dx_new;
+			x_.boxminus(dx, x_propagated);
+			dx_new = dx;
+		#ifdef USE_sparse
+			spMt h_x_ = h_x(x_, valid).sparseView();
+			spMt h_v_ = h_v(x_, valid).sparseView();
+			spMt R_ = R.sparseView();
+		#else
+			Matrix<scalar_type, l, n> h_x_ = h_x(x_, valid);
+			Matrix<scalar_type, l, Eigen::Dynamic> h_v_ = h_v(x_, valid);
+		#endif	
+			if(! valid)
+			{
+				continue; 
+			}
+
+			P_ = P_propagated;
+			
+			Matrix<scalar_type, 3, 3> res_temp_SO3;
+			MTK::vect<3, scalar_type> seg_SO3;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 3; i++){
+					seg_SO3(i) = dx(idx+i);
+				}
+
+				res_temp_SO3 = A_matrix(seg_SO3).transpose();
+				dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) *  res_temp_SO3.transpose();	
+				}
+			}
+		
+		
+			Matrix<scalar_type, 2, 2> res_temp_S2;
+			MTK::vect<2, scalar_type> seg_S2;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 2; i++){
+					seg_S2(i) = dx(idx + i);
+				}
+				
+				Eigen::Matrix<scalar_type, 2, 3> Nx;
+				Eigen::Matrix<scalar_type, 3, 2> Mx;
+				x_.S2_Nx_yy(Nx, idx);
+				x_propagated.S2_Mx(Mx, seg_S2, idx);
+				res_temp_S2 = Nx * Mx; 
+				dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+				}
+			}
+
+			Matrix<scalar_type, n, l> K_;
+			if(n > l)
+			{
+			#ifdef USE_sparse
+				Matrix<scalar_type, l, l> K_temp = h_x_ * P_ * h_x_.transpose();
+				spMt R_temp = h_v_ * R_ * h_v_.transpose();
+				K_temp += R_temp;
+				K_ = P_ * h_x_.transpose() * K_temp.inverse();
+			#else
+				K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();
+			#endif
+			}
+			else
+			{
+			#ifdef USE_sparse
+				measurementnoisecovariance b = measurementnoisecovariance::Identity();
+				Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver; 
+				solver.compute(R_);
+				measurementnoisecovariance R_in_temp = solver.solve(b);
+				spMt R_in = R_in_temp.sparseView();
+				spMt K_temp = h_x_.transpose() * R_in * h_x_;
+				cov P_temp = P_.inverse();
+				P_temp += K_temp;
+				K_ = P_temp.inverse() * h_x_.transpose() * R_in;
+			#else
+				measurementnoisecovariance R_in = (h_v_*R*h_v_.transpose()).inverse();
+				K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;
+			#endif 
+			}
+			Matrix<scalar_type, l, 1> innovation; 
+			z.boxminus(innovation, h(x_, valid));
+			cov K_x = K_ * h_x_;
+			Matrix<scalar_type, n, 1> dx_ = K_ * innovation + (K_x  - Matrix<scalar_type, n, n>::Identity()) * dx_new;
+        	state x_before = x_;
+			x_.boxplus(dx_);
+
+			converg = true;
+			for(int i = 0; i < n ; i++)
+			{
+				if(std::fabs(dx_[i]) > limit[i])
+				{
+					converg = false;
+					break;
+				}
+			}
+
+			if(converg) t++;
+	        
+			if(t > 1 || i == maximum_iter - 1)
+			{
+				L_ = P_;
+		
+				Matrix<scalar_type, 3, 3> res_temp_SO3;
+				MTK::vect<3, scalar_type> seg_SO3;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+					int idx = (*it).first;
+					for(int i = 0; i < 3; i++){
+						seg_SO3(i) = dx_(i + idx);
+					}
+					res_temp_SO3 = A_matrix(seg_SO3).transpose();
+					for(int i = 0; i < n; i++){
+						L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); 
+					}
+					if(n > l)
+					{
+						for(int i = 0; i < l; i++){
+							K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+						P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+					}
+				}
+
+				Matrix<scalar_type, 2, 2> res_temp_S2;
+				MTK::vect<2, scalar_type> seg_S2;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+					int idx = (*it).first;
+			
+					for(int i = 0; i < 2; i++){
+						seg_S2(i) = dx_(i + idx);
+					}
+			
+					Eigen::Matrix<scalar_type, 2, 3> Nx;
+					Eigen::Matrix<scalar_type, 3, 2> Mx;
+					x_.S2_Nx_yy(Nx, idx);
+					x_propagated.S2_Mx(Mx, seg_S2, idx);
+					res_temp_S2 = Nx * Mx; 
+		
+					for(int i = 0; i < n; i++){
+						L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); 
+					}
+					if(n > l)
+					{
+						for(int i = 0; i < l; i++){
+							K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+						P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+					}
+				}
+				if(n > l)
+				{
+					P_ = L_ - K_ * h_x_ * P_;
+				}
+				else
+				{
+					P_ = L_ - K_x * P_;
+				}
+				return;
+			}
+		}
+	}
+
+	//iterated error state EKF update for measurement as a manifold.
+	//calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
+	void update_iterated_share() {
+		
+		if(!(is_same<typename measurement::scalar, scalar_type>())){
+			std::cerr << "the scalar type of measurment must be the same as the state" << std::endl;
+			std::exit(100);
+		}
+
+		int t = 0;
+		share_datastruct<state, measurement, measurement_noise_dof> _share;
+		_share.valid = true;
+		_share.converge = true;
+		state x_propagated = x_;
+		cov P_propagated = P_;
+		
+		for(int i=-1; i<maximum_iter; i++)
+		{
+			vectorized_state dx, dx_new;
+			x_.boxminus(dx, x_propagated);
+			dx_new = dx;
+			measurement h = h_share(x_, _share);
+			measurement z = _share.z;
+			measurementnoisecovariance R = _share.R;
+		#ifdef USE_sparse
+			spMt h_x_ = _share.h_x.sparseView();
+			spMt h_v_ = _share.h_v.sparseView();
+			spMt R_ = _share.R.sparseView();
+		#else
+			Matrix<scalar_type, l, n> h_x_ = _share.h_x;
+			Matrix<scalar_type, l, Eigen::Dynamic> h_v_ = _share.h_v;
+		#endif	
+			if(! _share.valid)
+			{
+				continue; 
+			}
+
+			P_ = P_propagated;
+			
+			Matrix<scalar_type, 3, 3> res_temp_SO3;
+			MTK::vect<3, scalar_type> seg_SO3;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 3; i++){
+					seg_SO3(i) = dx(idx+i);
+				}
+
+				res_temp_SO3 = A_matrix(seg_SO3).transpose();
+				dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) *  res_temp_SO3.transpose();	
+				}
+			}
+		
+		
+			Matrix<scalar_type, 2, 2> res_temp_S2;
+			MTK::vect<2, scalar_type> seg_S2;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 2; i++){
+					seg_S2(i) = dx(idx + i);
+				}
+				
+				Eigen::Matrix<scalar_type, 2, 3> Nx;
+				Eigen::Matrix<scalar_type, 3, 2> Mx;
+				x_.S2_Nx_yy(Nx, idx);
+				x_propagated.S2_Mx(Mx, seg_S2, idx);
+				res_temp_S2 = Nx * Mx; 
+				dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+				}
+			}
+
+			Matrix<scalar_type, n, l> K_;
+			if(n > l)
+			{
+			#ifdef USE_sparse
+				Matrix<scalar_type, l, l> K_temp = h_x_ * P_ * h_x_.transpose();
+				spMt R_temp = h_v_ * R_ * h_v_.transpose();
+				K_temp += R_temp;
+				K_ = P_ * h_x_.transpose() * K_temp.inverse();
+			#else
+				K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();
+			#endif
+			}
+			else
+			{
+			#ifdef USE_sparse
+				measurementnoisecovariance b = measurementnoisecovariance::Identity();
+				Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver; 
+				solver.compute(R_);
+				measurementnoisecovariance R_in_temp = solver.solve(b);
+				spMt R_in = R_in_temp.sparseView();
+				spMt K_temp = h_x_.transpose() * R_in * h_x_;
+				cov P_temp = P_.inverse();
+				P_temp += K_temp;
+				K_ = P_temp.inverse() * h_x_.transpose() * R_in;
+			#else
+				measurementnoisecovariance R_in = (h_v_*R*h_v_.transpose()).inverse();
+				K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;
+			#endif 
+			}
+			Matrix<scalar_type, l, 1> innovation; 
+			z.boxminus(innovation, h);
+			cov K_x = K_ * h_x_;
+			Matrix<scalar_type, n, 1> dx_ = K_ * innovation + (K_x  - Matrix<scalar_type, n, n>::Identity()) * dx_new;
+        	state x_before = x_;
+			x_.boxplus(dx_);
+
+			_share.converge = true;
+			for(int i = 0; i < n ; i++)
+			{
+				if(std::fabs(dx_[i]) > limit[i])
+				{
+					_share.converge = false;
+					break;
+				}
+			}
+
+			if(_share.converge) t++;
+	        
+			if(t > 1 || i == maximum_iter - 1)
+			{
+				L_ = P_;
+		
+				Matrix<scalar_type, 3, 3> res_temp_SO3;
+				MTK::vect<3, scalar_type> seg_SO3;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+					int idx = (*it).first;
+					for(int i = 0; i < 3; i++){
+						seg_SO3(i) = dx_(i + idx);
+					}
+					res_temp_SO3 = A_matrix(seg_SO3).transpose();
+					for(int i = 0; i < n; i++){
+						L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); 
+					}
+					if(n > l)
+					{
+						for(int i = 0; i < l; i++){
+							K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+						P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+					}
+				}
+
+				Matrix<scalar_type, 2, 2> res_temp_S2;
+				MTK::vect<2, scalar_type> seg_S2;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+					int idx = (*it).first;
+			
+					for(int i = 0; i < 2; i++){
+						seg_S2(i) = dx_(i + idx);
+					}
+			
+					Eigen::Matrix<scalar_type, 2, 3> Nx;
+					Eigen::Matrix<scalar_type, 3, 2> Mx;
+					x_.S2_Nx_yy(Nx, idx);
+					x_propagated.S2_Mx(Mx, seg_S2, idx);
+					res_temp_S2 = Nx * Mx; 
+		
+					for(int i = 0; i < n; i++){
+						L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); 
+					}
+					if(n > l)
+					{
+						for(int i = 0; i < l; i++){
+							K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+						P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+					}
+				}
+				if(n > l)
+				{
+					P_ = L_ - K_ * h_x_ * P_;
+				}
+				else
+				{
+					P_ = L_ - K_x * P_;
+				}
+				return;
+			}
+		}
+	}
+
+	//iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing.
+	void update_iterated_dyn(Eigen::Matrix<scalar_type, Eigen::Dynamic, 1> z, measurementnoisecovariance_dyn R) {
+	
+		int t = 0;
+		bool valid = true;
+		bool converg = true;
+		state x_propagated = x_;
+		cov P_propagated = P_;
+		int dof_Measurement;
+		int dof_Measurement_noise = R.rows();
+		for(int i=-1; i<maximum_iter; i++)
+		{
+			valid = true;
+		#ifdef USE_sparse
+			spMt h_x_ = h_x_dyn(x_, valid).sparseView();
+			spMt h_v_ = h_v_dyn(x_, valid).sparseView();
+			spMt R_ = R.sparseView();
+		#else
+			Matrix<scalar_type, Eigen::Dynamic, n> h_x_ = h_x_dyn(x_, valid);
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v_ = h_v_dyn(x_, valid);
+		#endif	
+			Matrix<scalar_type, Eigen::Dynamic, 1> h_ = h_dyn(x_, valid);
+			dof_Measurement = h_.rows();
+			vectorized_state dx, dx_new;
+			x_.boxminus(dx, x_propagated);
+			dx_new = dx;
+			if(! valid)
+			{
+				continue; 
+			}
+
+			P_ = P_propagated;
+			Matrix<scalar_type, 3, 3> res_temp_SO3;
+			MTK::vect<3, scalar_type> seg_SO3;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 3; i++){
+					seg_SO3(i) = dx(idx+i);
+				}
+
+				res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
+				dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) *  res_temp_SO3.transpose();	
+				}
+			}
+		
+			Matrix<scalar_type, 2, 2> res_temp_S2;
+			MTK::vect<2, scalar_type> seg_S2;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 2; i++){
+					seg_S2(i) = dx(idx + i);
+				}
+
+				Eigen::Matrix<scalar_type, 2, 3> Nx;
+				Eigen::Matrix<scalar_type, 3, 2> Mx;
+				x_.S2_Nx_yy(Nx, idx);
+				x_propagated.S2_Mx(Mx, seg_S2, idx);
+				res_temp_S2 = Nx * Mx; 
+				dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+				}
+			}
+
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_;
+			if(n > dof_Measurement)
+			{
+				#ifdef USE_sparse
+				Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x_ * P_ * h_x_.transpose();
+				spMt R_temp = h_v_ * R_ * h_v_.transpose();
+				K_temp += R_temp;
+				K_ = P_ * h_x_.transpose() * K_temp.inverse();
+			#else
+				K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();
+			#endif
+			}
+			else
+			{
+			#ifdef USE_sparse
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> b = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Identity(dof_Measurement_noise, dof_Measurement_noise);
+				Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver; 
+				solver.compute(R_);
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
+				spMt R_in = R_in_temp.sparseView();
+				spMt K_temp = h_x_.transpose() * R_in * h_x_;
+				cov P_temp = P_.inverse();
+				P_temp += K_temp;
+				K_ = P_temp.inverse() * h_x_.transpose() * R_in;
+			#else
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in = (h_v_*R*h_v_.transpose()).inverse();
+				K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;
+			#endif 
+			}
+			cov K_x = K_ * h_x_;
+			Matrix<scalar_type, n, 1> dx_ = K_ * (z - h_) + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new; 
+			state x_before = x_;
+			x_.boxplus(dx_);
+			converg = true;
+			for(int i = 0; i < n ; i++)
+			{
+				if(std::fabs(dx_[i]) > limit[i])
+				{
+					converg = false;
+					break;
+				}
+			}
+			if(converg) t++;
+			if(t > 1 || i == maximum_iter - 1)
+			{
+				L_ = P_;
+				std::cout << "iteration time:" << t << "," << i << std::endl;
+		
+				Matrix<scalar_type, 3, 3> res_temp_SO3;
+				MTK::vect<3, scalar_type> seg_SO3;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+					int idx = (*it).first;
+					for(int i = 0; i < 3; i++){
+						seg_SO3(i) = dx_(i + idx);
+					}
+					res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
+					for(int i = 0; i < n; i++){
+						L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); 
+					}
+					if(n > dof_Measurement)
+					{
+						for(int i = 0; i < dof_Measurement; i++){
+							K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+						P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+					}
+				}
+
+				Matrix<scalar_type, 2, 2> res_temp_S2;
+				MTK::vect<2, scalar_type> seg_S2;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+					int idx = (*it).first;
+			
+					for(int i = 0; i < 2; i++){
+						seg_S2(i) = dx_(i + idx);
+					}
+			
+					Eigen::Matrix<scalar_type, 2, 3> Nx;
+					Eigen::Matrix<scalar_type, 3, 2> Mx;
+					x_.S2_Nx_yy(Nx, idx);
+					x_propagated.S2_Mx(Mx, seg_S2, idx);
+					res_temp_S2 = Nx * Mx; 
+		
+					for(int i = 0; i < n; i++){
+						L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); 
+					}
+					if(n > dof_Measurement)
+					{
+						for(int i = 0; i < dof_Measurement; i++){
+							K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+						P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+					}
+				}
+				if(n > dof_Measurement)
+				{
+					P_ = L_ - K_*h_x_*P_;
+				}
+				else
+				{
+					P_ = L_ - K_x * P_;
+				}
+				return;
+			}
+		}
+	}
+	//iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing.
+	//calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
+	void update_iterated_dyn_share() {
+		
+		int t = 0;
+		dyn_share_datastruct<scalar_type> dyn_share;
+		dyn_share.valid = true;
+		dyn_share.converge = true;
+		state x_propagated = x_;
+		cov P_propagated = P_;
+		int dof_Measurement;
+		int dof_Measurement_noise;
+		for(int i=-1; i<maximum_iter; i++)
+		{
+			dyn_share.valid = true;
+			h_dyn_share (x_,  dyn_share);
+			//Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share (x_,  dyn_share);
+			Matrix<scalar_type, Eigen::Dynamic, 1> z = dyn_share.z;
+			Matrix<scalar_type, Eigen::Dynamic, 1> h = dyn_share.h;
+		#ifdef USE_sparse
+			spMt h_x = dyn_share.h_x.sparseView();
+			spMt h_v = dyn_share.h_v.sparseView();
+			spMt R_ = dyn_share.R.sparseView();
+		#else
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R = dyn_share.R; 
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x = dyn_share.h_x;
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v = dyn_share.h_v;
+		#endif	
+			dof_Measurement = h_x.rows();
+			dof_Measurement_noise = dyn_share.R.rows();
+			vectorized_state dx, dx_new;
+			x_.boxminus(dx, x_propagated);
+			dx_new = dx;
+			if(! (dyn_share.valid))
+			{
+				continue;
+			}
+
+			P_ = P_propagated;
+			Matrix<scalar_type, 3, 3> res_temp_SO3;
+			MTK::vect<3, scalar_type> seg_SO3;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 3; i++){
+					seg_SO3(i) = dx(idx+i);
+				}
+
+				res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
+				dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) *  res_temp_SO3.transpose();	
+				}
+			}
+		
+			Matrix<scalar_type, 2, 2> res_temp_S2;
+			MTK::vect<2, scalar_type> seg_S2;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 2; i++){
+					seg_S2(i) = dx(idx + i);
+				}
+
+				Eigen::Matrix<scalar_type, 2, 3> Nx;
+				Eigen::Matrix<scalar_type, 3, 2> Mx;
+				x_.S2_Nx_yy(Nx, idx);
+				x_propagated.S2_Mx(Mx, seg_S2, idx);
+				res_temp_S2 = Nx * Mx; 
+				dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+				}
+			}
+
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_;
+			if(n > dof_Measurement)
+			{
+			#ifdef USE_sparse
+				Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x * P_ * h_x.transpose();
+				spMt R_temp = h_v * R_ * h_v.transpose();
+				K_temp += R_temp;
+				K_ = P_ * h_x.transpose() * K_temp.inverse();
+			#else
+				K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();
+			#endif
+			}
+			else
+			{
+			#ifdef USE_sparse
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> b = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Identity(dof_Measurement_noise, dof_Measurement_noise);
+				Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver; 
+				solver.compute(R_);
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
+				spMt R_in = R_in_temp.sparseView();
+				spMt K_temp = h_x.transpose() * R_in * h_x;
+				cov P_temp = P_.inverse();
+				P_temp += K_temp;
+				K_ = P_temp.inverse() * h_x.transpose() * R_in;
+			#else
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in = (h_v*R*h_v.transpose()).inverse();
+				K_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in;
+			#endif 
+			}
+
+			cov K_x = K_ * h_x;
+			Matrix<scalar_type, n, 1> dx_ = K_ * (z - h) + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new; 
+			state x_before = x_;
+			x_.boxplus(dx_);
+			dyn_share.converge = true;
+			for(int i = 0; i < n ; i++)
+			{
+				if(std::fabs(dx_[i]) > limit[i])
+				{
+					dyn_share.converge = false;
+					break;
+				}
+			}
+			if(dyn_share.converge) t++;
+			if(t > 1 || i == maximum_iter - 1)
+			{
+				L_ = P_;
+				std::cout << "iteration time:" << t << "," << i << std::endl;
+		
+				Matrix<scalar_type, 3, 3> res_temp_SO3;
+				MTK::vect<3, scalar_type> seg_SO3;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+					int idx = (*it).first;
+					for(int i = 0; i < 3; i++){
+						seg_SO3(i) = dx_(i + idx);
+					}
+					res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
+					for(int i = 0; i < int(n); i++){
+						L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); 
+					}
+					if(n > dof_Measurement)
+					{
+						for(int i = 0; i < dof_Measurement; i++){
+							K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+						P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+					}
+				}
+
+				Matrix<scalar_type, 2, 2> res_temp_S2;
+				MTK::vect<2, scalar_type> seg_S2;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+					int idx = (*it).first;
+			
+					for(int i = 0; i < 2; i++){
+						seg_S2(i) = dx_(i + idx);
+					}
+			
+					Eigen::Matrix<scalar_type, 2, 3> Nx;
+					Eigen::Matrix<scalar_type, 3, 2> Mx;
+					x_.S2_Nx_yy(Nx, idx);
+					x_propagated.S2_Mx(Mx, seg_S2, idx);
+					res_temp_S2 = Nx * Mx; 
+		
+					for(int i = 0; i < n; i++){
+						L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); 
+					}
+					if(n > dof_Measurement)
+					{
+						for(int i = 0; i < dof_Measurement; i++){
+							K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+						P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+					}
+				}
+				if(n > dof_Measurement)
+				{
+					P_ = L_ - K_*h_x*P_;
+				}
+				else
+				{
+					P_ = L_ - K_x * P_;
+				}
+				return;
+			}
+		}
+	}
+
+	//iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing.
+	//the measurement and the measurement model are received in a dynamic manner.
+	template<typename measurement_runtime, typename measurementModel_runtime>
+	void update_iterated_dyn_runtime(measurement_runtime z, measurementnoisecovariance_dyn R, measurementModel_runtime h_runtime) {
+	
+		int t = 0;
+		bool valid = true;
+		bool converg = true;
+		state x_propagated = x_;
+		cov P_propagated = P_;
+		int dof_Measurement;
+		int dof_Measurement_noise;
+		for(int i=-1; i<maximum_iter; i++)
+		{
+			valid = true;
+		#ifdef USE_sparse
+			spMt h_x_ = h_x_dyn(x_, valid).sparseView();
+			spMt h_v_ = h_v_dyn(x_, valid).sparseView();
+			spMt R_ = R.sparseView();
+		#else
+			Matrix<scalar_type, Eigen::Dynamic, n> h_x_ = h_x_dyn(x_, valid);
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v_ = h_v_dyn(x_, valid);
+		#endif	
+			measurement_runtime h_ = h_runtime(x_, valid);
+			dof_Measurement = measurement_runtime::DOF;
+			dof_Measurement_noise = R.rows();
+			vectorized_state dx, dx_new;
+			x_.boxminus(dx, x_propagated);
+			dx_new = dx;
+			if(! valid)
+			{
+				continue; 
+			}
+
+			P_ = P_propagated;
+			Matrix<scalar_type, 3, 3> res_temp_SO3;
+			MTK::vect<3, scalar_type> seg_SO3;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 3; i++){
+					seg_SO3(i) = dx(idx+i);
+				}
+
+				res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
+				dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) *  res_temp_SO3.transpose();	
+				}
+			}
+		
+			Matrix<scalar_type, 2, 2> res_temp_S2;
+			MTK::vect<2, scalar_type> seg_S2;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 2; i++){
+					seg_S2(i) = dx(idx + i);
+				}
+
+				Eigen::Matrix<scalar_type, 2, 3> Nx;
+				Eigen::Matrix<scalar_type, 3, 2> Mx;
+				x_.S2_Nx_yy(Nx, idx);
+				x_propagated.S2_Mx(Mx, seg_S2, idx);
+				res_temp_S2 = Nx * Mx; 
+				dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+				}
+			}
+
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_;
+			if(n > dof_Measurement)
+			{
+			#ifdef USE_sparse
+				Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x_ * P_ * h_x_.transpose();
+				spMt R_temp = h_v_ * R_ * h_v_.transpose();
+				K_temp += R_temp;
+				K_ = P_ * h_x_.transpose() * K_temp.inverse();
+			#else
+				K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();
+			#endif
+			}
+			else
+			{
+			#ifdef USE_sparse
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> b = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Identity(dof_Measurement_noise, dof_Measurement_noise);
+				Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver; 
+				solver.compute(R_);
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
+				spMt R_in = R_in_temp.sparseView();
+				spMt K_temp = h_x_.transpose() * R_in * h_x_;
+				cov P_temp = P_.inverse();
+				P_temp += K_temp;
+				K_ = P_temp.inverse() * h_x_.transpose() * R_in;
+			#else
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in = (h_v_*R*h_v_.transpose()).inverse();
+				K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;
+			#endif 
+			}
+			cov K_x = K_ * h_x_;
+			Eigen::Matrix<scalar_type, measurement_runtime::DOF, 1> innovation;
+			z.boxminus(innovation, h_);
+			Matrix<scalar_type, n, 1> dx_ = K_ * innovation + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new; 
+			state x_before = x_;
+			x_.boxplus(dx_);
+			converg = true;
+			for(int i = 0; i < n ; i++)
+			{
+				if(std::fabs(dx_[i]) > limit[i])
+				{
+					converg = false;
+					break;
+				}
+			}
+			if(converg) t++;
+			if(t > 1 || i == maximum_iter - 1)
+			{
+				L_ = P_;
+				std::cout << "iteration time:" << t << "," << i << std::endl;
+		
+				Matrix<scalar_type, 3, 3> res_temp_SO3;
+				MTK::vect<3, scalar_type> seg_SO3;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+					int idx = (*it).first;
+					for(int i = 0; i < 3; i++){
+						seg_SO3(i) = dx_(i + idx);
+					}
+					res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
+					for(int i = 0; i < n; i++){
+						L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); 
+					}
+					if(n > dof_Measurement)
+					{
+						for(int i = 0; i < dof_Measurement; i++){
+							K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+						P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+					}
+				}
+
+				Matrix<scalar_type, 2, 2> res_temp_S2;
+				MTK::vect<2, scalar_type> seg_S2;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+					int idx = (*it).first;
+			
+					for(int i = 0; i < 2; i++){
+						seg_S2(i) = dx_(i + idx);
+					}
+			
+					Eigen::Matrix<scalar_type, 2, 3> Nx;
+					Eigen::Matrix<scalar_type, 3, 2> Mx;
+					x_.S2_Nx_yy(Nx, idx);
+					x_propagated.S2_Mx(Mx, seg_S2, idx);
+					res_temp_S2 = Nx * Mx; 
+		
+					for(int i = 0; i < n; i++){
+						L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); 
+					}
+					if(n > dof_Measurement)
+					{
+						for(int i = 0; i < dof_Measurement; i++){
+							K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+						P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+					}
+				}
+				if(n > dof_Measurement)
+				{
+					P_ = L_ - K_*h_x_*P_;
+				}
+				else
+				{
+					P_ = L_ - K_x * P_;
+				}
+				return;
+			}
+		}
+	}
+
+	//iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing.
+	//the measurement and the measurement model are received in a dynamic manner.
+	//calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
+	template<typename measurement_runtime, typename measurementModel_dyn_runtime_share>
+	void update_iterated_dyn_runtime_share(measurement_runtime z, measurementModel_dyn_runtime_share h) {
+		
+		int t = 0;
+		dyn_runtime_share_datastruct<scalar_type> dyn_share;
+		dyn_share.valid = true;
+		dyn_share.converge = true;
+		state x_propagated = x_;
+		cov P_propagated = P_;
+		int dof_Measurement;
+		int dof_Measurement_noise;
+		for(int i=-1; i<maximum_iter; i++)
+		{
+			dyn_share.valid = true;
+			measurement_runtime h_ = h(x_,  dyn_share); 
+			//measurement_runtime z = dyn_share.z;
+		#ifdef USE_sparse
+			spMt h_x = dyn_share.h_x.sparseView();
+			spMt h_v = dyn_share.h_v.sparseView();
+			spMt R_ = dyn_share.R.sparseView();
+		#else
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R = dyn_share.R; 
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x = dyn_share.h_x;
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v = dyn_share.h_v;
+		#endif	
+			dof_Measurement = measurement_runtime::DOF;
+			dof_Measurement_noise = dyn_share.R.rows();
+			vectorized_state dx, dx_new;
+			x_.boxminus(dx, x_propagated);
+			dx_new = dx;
+			if(! (dyn_share.valid))
+			{
+				continue;
+			}
+
+			P_ = P_propagated;
+			Matrix<scalar_type, 3, 3> res_temp_SO3;
+			MTK::vect<3, scalar_type> seg_SO3;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 3; i++){
+					seg_SO3(i) = dx(idx+i);
+				}
+
+				res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
+				dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) *  res_temp_SO3.transpose();	
+				}
+			}
+		
+			Matrix<scalar_type, 2, 2> res_temp_S2;
+			MTK::vect<2, scalar_type> seg_S2;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 2; i++){
+					seg_S2(i) = dx(idx + i);
+				}
+
+				Eigen::Matrix<scalar_type, 2, 3> Nx;
+				Eigen::Matrix<scalar_type, 3, 2> Mx;
+				x_.S2_Nx_yy(Nx, idx);
+				x_propagated.S2_Mx(Mx, seg_S2, idx);
+				res_temp_S2 = Nx * Mx; 
+				dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+				}
+			}
+
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_;
+			if(n > dof_Measurement)
+			{
+			#ifdef USE_sparse
+				Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x * P_ * h_x.transpose();
+				spMt R_temp = h_v * R_ * h_v.transpose();
+				K_temp += R_temp;
+				K_ = P_ * h_x.transpose() * K_temp.inverse();
+			#else
+				K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();
+			#endif
+			}
+			else
+			{
+			#ifdef USE_sparse
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> b = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Identity(dof_Measurement_noise, dof_Measurement_noise);
+				Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver; 
+				solver.compute(R_);
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
+				spMt R_in =R_in_temp.sparseView();
+				spMt K_temp = h_x.transpose() * R_in * h_x;
+				cov P_temp = P_.inverse();
+				P_temp += K_temp;
+				K_ = P_temp.inverse() * h_x.transpose() * R_in;
+			#else
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in = (h_v*R*h_v.transpose()).inverse();
+				K_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in;
+			#endif 
+			}
+			cov K_x = K_ * h_x;
+			Eigen::Matrix<scalar_type, measurement_runtime::DOF, 1> innovation;
+			z.boxminus(innovation, h_);
+			Matrix<scalar_type, n, 1> dx_ = K_ * innovation + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new; 
+			state x_before = x_;
+			x_.boxplus(dx_);
+			dyn_share.converge = true;
+			for(int i = 0; i < n ; i++)
+			{
+				if(std::fabs(dx_[i]) > limit[i])
+				{
+					dyn_share.converge = false;
+					break;
+				}
+			}
+			if(dyn_share.converge) t++;
+			if(t > 1 || i == maximum_iter - 1)
+			{
+				L_ = P_;
+				std::cout << "iteration time:" << t << "," << i << std::endl;
+		
+				Matrix<scalar_type, 3, 3> res_temp_SO3;
+				MTK::vect<3, scalar_type> seg_SO3;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+					int idx = (*it).first;
+					for(int i = 0; i < 3; i++){
+						seg_SO3(i) = dx_(i + idx);
+					}
+					res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
+					for(int i = 0; i < int(n); i++){
+						L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); 
+					}
+					if(n > dof_Measurement)
+					{
+						for(int i = 0; i < dof_Measurement; i++){
+							K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+						P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+					}
+				}
+
+				Matrix<scalar_type, 2, 2> res_temp_S2;
+				MTK::vect<2, scalar_type> seg_S2;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+					int idx = (*it).first;
+			
+					for(int i = 0; i < 2; i++){
+						seg_S2(i) = dx_(i + idx);
+					}
+			
+					Eigen::Matrix<scalar_type, 2, 3> Nx;
+					Eigen::Matrix<scalar_type, 3, 2> Mx;
+					x_.S2_Nx_yy(Nx, idx);
+					x_propagated.S2_Mx(Mx, seg_S2, idx);
+					res_temp_S2 = Nx * Mx; 
+		
+					for(int i = 0; i < n; i++){
+						L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); 
+					}
+					if(n > dof_Measurement)
+					{
+						for(int i = 0; i < dof_Measurement; i++){
+							K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
+						}
+					}
+					else
+					{
+						for(int i = 0; i < n; i++){
+							K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i));
+						}
+					}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+						P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+					}
+				}
+				if(n > dof_Measurement)
+				{
+					P_ = L_ - K_*h_x * P_;
+				}
+				else
+				{
+					P_ = L_ - K_x * P_;
+				}
+				return;
+			}
+		}
+	}
+	
+	//iterated error state EKF update modified for one specific system.
+	void update_iterated_dyn_share_modified(double R, double &solve_time) {
+		
+		dyn_share_datastruct<scalar_type> dyn_share;
+		dyn_share.valid = true;
+		dyn_share.converge = true;
+		int t = 0;
+		state x_propagated = x_;
+		cov P_propagated = P_;
+		int dof_Measurement; 
+		
+		Matrix<scalar_type, n, 1> K_h;
+		Matrix<scalar_type, n, n> K_x; 
+		
+		vectorized_state dx_new = vectorized_state::Zero();
+		for(int i=-1; i<maximum_iter; i++)
+		{
+			dyn_share.valid = true;	
+			h_dyn_share(x_, dyn_share);
+
+			if(! dyn_share.valid)
+			{
+				continue; 
+			}
+
+			//Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share(x_, dyn_share);
+			#ifdef USE_sparse
+				spMt h_x_ = dyn_share.h_x.sparseView();
+			#else
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, 12> h_x_ = dyn_share.h_x;
+			#endif
+			double solve_start = omp_get_wtime();
+			dof_Measurement = h_x_.rows();
+			vectorized_state dx;
+			x_.boxminus(dx, x_propagated);
+			dx_new = dx;
+			
+			
+			
+			P_ = P_propagated;
+			
+			Matrix<scalar_type, 3, 3> res_temp_SO3;
+			MTK::vect<3, scalar_type> seg_SO3;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 3; i++){
+					seg_SO3(i) = dx(idx+i);
+				}
+
+				res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
+				dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) *  res_temp_SO3.transpose();	
+				}
+			}
+
+			Matrix<scalar_type, 2, 2> res_temp_S2;
+			MTK::vect<2, scalar_type> seg_S2;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 2; i++){
+					seg_S2(i) = dx(idx + i);
+				}
+
+				Eigen::Matrix<scalar_type, 2, 3> Nx;
+				Eigen::Matrix<scalar_type, 3, 2> Mx;
+				x_.S2_Nx_yy(Nx, idx);
+				x_propagated.S2_Mx(Mx, seg_S2, idx);
+				res_temp_S2 = Nx * Mx; 
+				dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
+				for(int i = 0; i < n; i++){
+					P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));	
+				}
+				for(int i = 0; i < n; i++){
+					P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+				}
+			}
+			//Matrix<scalar_type, n, Eigen::Dynamic> K_;
+			//Matrix<scalar_type, n, 1> K_h;
+			//Matrix<scalar_type, n, n> K_x; 
+
+			/*
+			if(n > dof_Measurement)
+			{
+				K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose()/R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse()/R;
+			}
+			else
+			{
+				K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose();
+			}
+			*/
+
+			if(n > dof_Measurement)
+			{
+			//#ifdef USE_sparse
+				//Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x * P_ * h_x.transpose();
+				//spMt R_temp = h_v * R_ * h_v.transpose();
+				//K_temp += R_temp;
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
+				h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;
+				/*
+				h_x_cur.col(0) = h_x_.col(0);
+				h_x_cur.col(1) = h_x_.col(1);
+				h_x_cur.col(2) = h_x_.col(2);
+				h_x_cur.col(3) = h_x_.col(3);
+				h_x_cur.col(4) = h_x_.col(4);
+				h_x_cur.col(5) = h_x_.col(5);
+				h_x_cur.col(6) = h_x_.col(6);
+				h_x_cur.col(7) = h_x_.col(7);
+				h_x_cur.col(8) = h_x_.col(8);
+				h_x_cur.col(9) = h_x_.col(9);
+				h_x_cur.col(10) = h_x_.col(10);
+				h_x_cur.col(11) = h_x_.col(11);
+				*/
+				
+				Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose()/R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse()/R;
+				K_h = K_ * dyn_share.h;
+				K_x = K_ * h_x_cur;
+			//#else
+			//	K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();
+			//#endif
+			}
+			else
+			{
+			#ifdef USE_sparse
+				//Eigen::Matrix<scalar_type, n, n> b = Eigen::Matrix<scalar_type, n, n>::Identity();
+				//Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver; 
+				spMt A = h_x_.transpose() * h_x_;
+				cov P_temp = (P_/R).inverse();
+				P_temp. template block<12, 12>(0, 0) += A;
+				P_temp = P_temp.inverse();
+				/*
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
+				h_x_cur.col(0) = h_x_.col(0);
+				h_x_cur.col(1) = h_x_.col(1);
+				h_x_cur.col(2) = h_x_.col(2);
+				h_x_cur.col(3) = h_x_.col(3);
+				h_x_cur.col(4) = h_x_.col(4);
+				h_x_cur.col(5) = h_x_.col(5);
+				h_x_cur.col(6) = h_x_.col(6);
+				h_x_cur.col(7) = h_x_.col(7);
+				h_x_cur.col(8) = h_x_.col(8);
+				h_x_cur.col(9) = h_x_.col(9);
+				h_x_cur.col(10) = h_x_.col(10);
+				h_x_cur.col(11) = h_x_.col(11);
+				*/
+				K_ = P_temp. template block<n, 12>(0, 0) * h_x_.transpose();
+				K_x = cov::Zero();
+				K_x. template block<n, 12>(0, 0) = P_inv. template block<n, 12>(0, 0) * HTH;
+				/*
+				solver.compute(R_);
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
+				spMt R_in =R_in_temp.sparseView();
+				spMt K_temp = h_x.transpose() * R_in * h_x;
+				cov P_temp = P_.inverse();
+				P_temp += K_temp;
+				K_ = P_temp.inverse() * h_x.transpose() * R_in;
+				*/
+			#else
+				cov P_temp = (P_/R).inverse();
+				//Eigen::Matrix<scalar_type, 12, Eigen::Dynamic> h_T = h_x_.transpose();
+				Eigen::Matrix<scalar_type, 12, 12> HTH = h_x_.transpose() * h_x_; 
+				P_temp. template block<12, 12>(0, 0) += HTH;
+				/*
+				Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
+				//std::cout << "line 1767" << std::endl;
+				h_x_cur.col(0) = h_x_.col(0);
+				h_x_cur.col(1) = h_x_.col(1);
+				h_x_cur.col(2) = h_x_.col(2);
+				h_x_cur.col(3) = h_x_.col(3);
+				h_x_cur.col(4) = h_x_.col(4);
+				h_x_cur.col(5) = h_x_.col(5);
+				h_x_cur.col(6) = h_x_.col(6);
+				h_x_cur.col(7) = h_x_.col(7);
+				h_x_cur.col(8) = h_x_.col(8);
+				h_x_cur.col(9) = h_x_.col(9);
+				h_x_cur.col(10) = h_x_.col(10);
+				h_x_cur.col(11) = h_x_.col(11);
+				*/
+				cov P_inv = P_temp.inverse();
+				//std::cout << "line 1781" << std::endl;
+				K_h = P_inv. template block<n, 12>(0, 0) * h_x_.transpose() * dyn_share.h;
+				//std::cout << "line 1780" << std::endl;
+				//cov HTH_cur = cov::Zero();
+				//HTH_cur. template block<12, 12>(0, 0) = HTH;
+				K_x.setZero(); // = cov::Zero();
+				K_x. template block<n, 12>(0, 0) = P_inv. template block<n, 12>(0, 0) * HTH;
+				//K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose();
+			#endif 
+			}
+
+			//K_x = K_ * h_x_;
+			Matrix<scalar_type, n, 1> dx_ = K_h + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new; 
+			state x_before = x_;
+			x_.boxplus(dx_);
+			dyn_share.converge = true;
+			for(int i = 0; i < n ; i++)
+			{
+				if(std::fabs(dx_[i]) > limit[i])
+				{
+					dyn_share.converge = false;
+					break;
+				}
+			}
+			if(dyn_share.converge) t++;
+			
+			if(!t && i == maximum_iter - 2)
+			{
+				dyn_share.converge = true;
+			}
+
+			if(t > 1 || i == maximum_iter - 1)
+			{
+				L_ = P_;
+				//std::cout << "iteration time" << t << "," << i << std::endl; 
+				Matrix<scalar_type, 3, 3> res_temp_SO3;
+				MTK::vect<3, scalar_type> seg_SO3;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+					int idx = (*it).first;
+					for(int i = 0; i < 3; i++){
+						seg_SO3(i) = dx_(i + idx);
+					}
+					res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
+					for(int i = 0; i < n; i++){
+						L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); 
+					}
+					// if(n > dof_Measurement)
+					// {
+					// 	for(int i = 0; i < dof_Measurement; i++){
+					// 		K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
+					// 	}
+					// }
+					// else
+					// {
+						for(int i = 0; i < 12; i++){
+							K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i));
+						}
+					//}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+						P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+					}
+				}
+
+				Matrix<scalar_type, 2, 2> res_temp_S2;
+				MTK::vect<2, scalar_type> seg_S2;
+				for(typename std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+					int idx = (*it).first;
+
+					for(int i = 0; i < 2; i++){
+						seg_S2(i) = dx_(i + idx);
+					}
+
+					Eigen::Matrix<scalar_type, 2, 3> Nx;
+					Eigen::Matrix<scalar_type, 3, 2> Mx;
+					x_.S2_Nx_yy(Nx, idx);
+					x_propagated.S2_Mx(Mx, seg_S2, idx);
+					res_temp_S2 = Nx * Mx; 
+					for(int i = 0; i < n; i++){
+						L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); 
+					}
+					// if(n > dof_Measurement)
+					// {
+					// 	for(int i = 0; i < dof_Measurement; i++){
+					// 		K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
+					// 	}
+					// }
+					// else
+					// {
+						for(int i = 0; i < 12; i++){
+							K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i));
+						}
+					//}
+					for(int i = 0; i < n; i++){
+						L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+						P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+					}
+				}
+
+				// if(n > dof_Measurement)
+				// {
+				// 	Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
+				// 	h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;
+				// 	/*
+				// 	h_x_cur.col(0) = h_x_.col(0);
+				// 	h_x_cur.col(1) = h_x_.col(1);
+				// 	h_x_cur.col(2) = h_x_.col(2);
+				// 	h_x_cur.col(3) = h_x_.col(3);
+				// 	h_x_cur.col(4) = h_x_.col(4);
+				// 	h_x_cur.col(5) = h_x_.col(5);
+				// 	h_x_cur.col(6) = h_x_.col(6);
+				// 	h_x_cur.col(7) = h_x_.col(7);
+				// 	h_x_cur.col(8) = h_x_.col(8);
+				// 	h_x_cur.col(9) = h_x_.col(9);
+				// 	h_x_cur.col(10) = h_x_.col(10);
+				// 	h_x_cur.col(11) = h_x_.col(11);
+				// 	*/
+				// 	P_ = L_ - K_*h_x_cur * P_;
+				// }
+				// else
+				//{
+					P_ = L_ - K_x.template block<n, 12>(0, 0) * P_.template block<12, n>(0, 0);
+				//}
+				solve_time += omp_get_wtime() - solve_start;
+				return;
+			}
+			solve_time += omp_get_wtime() - solve_start;
+		}
+	}
+
+	void change_x(state &input_state)
+	{
+		x_ = input_state;
+		if((!x_.vect_state.size())&&(!x_.SO3_state.size())&&(!x_.S2_state.size()))
+		{
+			x_.build_S2_state();
+			x_.build_SO3_state();
+			x_.build_vect_state();
+		}
+	}
+
+	void change_P(cov &input_cov)
+	{
+		P_ = input_cov;
+	}
+
+	const state& get_x() const {
+		return x_;
+	}
+	const cov& get_P() const {
+		return P_;
+	}
+private:
+	state x_;
+	measurement m_;
+	cov P_;
+	spMt l_;
+	spMt f_x_1;
+	spMt f_x_2;
+	cov F_x1 = cov::Identity();
+	cov F_x2 = cov::Identity();
+	cov L_ = cov::Identity();
+
+	processModel *f;
+	processMatrix1 *f_x;
+	processMatrix2 *f_w;
+
+	measurementModel *h;
+	measurementMatrix1 *h_x;
+	measurementMatrix2 *h_v;
+
+	measurementModel_dyn *h_dyn;
+	measurementMatrix1_dyn *h_x_dyn;
+	measurementMatrix2_dyn *h_v_dyn;
+
+	measurementModel_share *h_share;
+	measurementModel_dyn_share *h_dyn_share;
+
+	int maximum_iter = 0;
+	scalar_type limit[n];
+	
+	template <typename T>
+    T check_safe_update( T _temp_vec )
+    {
+        T temp_vec = _temp_vec;
+        if ( std::isnan( temp_vec(0, 0) ) )
+        {
+            temp_vec.setZero();
+            return temp_vec;
+        }
+        double angular_dis = temp_vec.block( 0, 0, 3, 1 ).norm() * 57.3;
+        double pos_dis = temp_vec.block( 3, 0, 3, 1 ).norm();
+        if ( angular_dis >= 20 || pos_dis > 1 )
+        {
+            printf( "Angular dis = %.2f, pos dis = %.2f\r\n", angular_dis, pos_dis );
+            temp_vec.setZero();
+        }
+        return temp_vec;
+    }
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} // namespace esekfom
+
+#endif //  ESEKFOM_EKF_HPP
diff --git a/FAST_LIO/include/IKFoM_toolkit/esekfom/util.hpp b/FAST_LIO/include/IKFoM_toolkit/esekfom/util.hpp
new file mode 100755
index 0000000..ab39fc4
--- /dev/null
+++ b/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__
diff --git a/FAST_LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp b/FAST_LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp
new file mode 100755
index 0000000..2cdb106
--- /dev/null
+++ b/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_*/
diff --git a/FAST_LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp b/FAST_LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp
new file mode 100755
index 0000000..a1b13de
--- /dev/null
+++ b/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_ */
diff --git a/FAST_LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp b/FAST_LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp
new file mode 100755
index 0000000..e3420d1
--- /dev/null
+++ b/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_ */
diff --git a/FAST_LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp b/FAST_LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp
new file mode 100755
index 0000000..5025071
--- /dev/null
+++ b/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_ */
diff --git a/FAST_LIO/include/IKFoM_toolkit/mtk/startIdx.hpp b/FAST_LIO/include/IKFoM_toolkit/mtk/startIdx.hpp
new file mode 100755
index 0000000..4dc2958
--- /dev/null
+++ b/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_
diff --git a/FAST_LIO/include/IKFoM_toolkit/mtk/types/S2.hpp b/FAST_LIO/include/IKFoM_toolkit/mtk/types/S2.hpp
new file mode 100755
index 0000000..789ebd9
--- /dev/null
+++ b/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_*/
diff --git a/FAST_LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp b/FAST_LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp
new file mode 100755
index 0000000..31005e6
--- /dev/null
+++ b/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_*/
+
diff --git a/FAST_LIO/include/IKFoM_toolkit/mtk/types/vect.hpp b/FAST_LIO/include/IKFoM_toolkit/mtk/types/vect.hpp
new file mode 100755
index 0000000..0e5b9ce
--- /dev/null
+++ b/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_*/
diff --git a/FAST_LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp b/FAST_LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp
new file mode 100755
index 0000000..b6643f1
--- /dev/null
+++ b/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_ */
diff --git a/FAST_LIO/include/common_lib.h b/FAST_LIO/include/common_lib.h
new file mode 100644
index 0000000..69365e3
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/FAST_LIO/include/ikd-Tree/README.md b/FAST_LIO/include/ikd-Tree/README.md
new file mode 100644
index 0000000..e113a91
--- /dev/null
+++ b/FAST_LIO/include/ikd-Tree/README.md
@@ -0,0 +1,2 @@
+# ikd-Tree
+ikd-Tree is an incremental k-d tree for robotic applications.
diff --git a/FAST_LIO/include/ikd-Tree/ikd_Tree.cpp b/FAST_LIO/include/ikd-Tree/ikd_Tree.cpp
new file mode 100644
index 0000000..e8c4e86
--- /dev/null
+++ b/FAST_LIO/include/ikd-Tree/ikd_Tree.cpp
@@ -0,0 +1,1728 @@
+#include "ikd_Tree.h"
+
+/*
+Description: ikd-Tree: an incremental k-d tree for robotic applications 
+Author: Yixi Cai
+email: yixicai@connect.hku.hk
+*/
+
+template <typename PointType>
+KD_TREE<PointType>::KD_TREE(float delete_param, float balance_param, float box_length)
+{
+    delete_criterion_param = delete_param;
+    balance_criterion_param = balance_param;
+    downsample_size = box_length;
+    Rebuild_Logger.clear();
+    termination_flag = false;
+    start_thread();
+}
+
+template <typename PointType>
+KD_TREE<PointType>::~KD_TREE()
+{
+    stop_thread();
+    Delete_Storage_Disabled = true;
+    delete_tree_nodes(&Root_Node);
+    PointVector().swap(PCL_Storage);
+    Rebuild_Logger.clear();
+}
+
+
+
+template <typename PointType>
+void KD_TREE<PointType>::InitializeKDTree(float delete_param, float balance_param, float box_length)
+{
+    Set_delete_criterion_param(delete_param);
+    Set_balance_criterion_param(balance_param);
+    set_downsample_param(box_length);
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::InitTreeNode(KD_TREE_NODE *root)
+{
+    root->point.x = 0.0f;
+    root->point.y = 0.0f;
+    root->point.z = 0.0f;
+    root->node_range_x[0] = 0.0f;
+    root->node_range_x[1] = 0.0f;
+    root->node_range_y[0] = 0.0f;
+    root->node_range_y[1] = 0.0f;
+    root->node_range_z[0] = 0.0f;
+    root->node_range_z[1] = 0.0f;
+    root->radius_sq = 0.0f;
+    root->division_axis = 0;
+    root->father_ptr = nullptr;
+    root->left_son_ptr = nullptr;
+    root->right_son_ptr = nullptr;
+    root->TreeSize = 0;
+    root->invalid_point_num = 0;
+    root->down_del_num = 0;
+    root->point_deleted = false;
+    root->tree_deleted = false;
+    root->need_push_down_to_left = false;
+    root->need_push_down_to_right = false;
+    root->point_downsample_deleted = false;
+    root->working_flag = false;
+    pthread_mutex_init(&(root->push_down_mutex_lock), NULL);
+}
+
+template <typename PointType>
+int KD_TREE<PointType>::size()
+{
+    int s = 0;
+    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+    {
+        if (Root_Node != nullptr)
+        {
+            return Root_Node->TreeSize;
+        }
+        else
+        {
+            return 0;
+        }
+    }
+    else
+    {
+        if (!pthread_mutex_trylock(&working_flag_mutex))
+        {
+            s = Root_Node->TreeSize;
+            pthread_mutex_unlock(&working_flag_mutex);
+            return s;
+        }
+        else
+        {
+            return Treesize_tmp;
+        }
+    }
+}
+
+template <typename PointType>
+BoxPointType KD_TREE<PointType>::tree_range()
+{
+    BoxPointType range;
+    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+    {
+        if (Root_Node != nullptr)
+        {
+            range.vertex_min[0] = Root_Node->node_range_x[0];
+            range.vertex_min[1] = Root_Node->node_range_y[0];
+            range.vertex_min[2] = Root_Node->node_range_z[0];
+            range.vertex_max[0] = Root_Node->node_range_x[1];
+            range.vertex_max[1] = Root_Node->node_range_y[1];
+            range.vertex_max[2] = Root_Node->node_range_z[1];
+        }
+        else
+        {
+            memset(&range, 0, sizeof(range));
+        }
+    }
+    else
+    {
+        if (!pthread_mutex_trylock(&working_flag_mutex))
+        {
+            range.vertex_min[0] = Root_Node->node_range_x[0];
+            range.vertex_min[1] = Root_Node->node_range_y[0];
+            range.vertex_min[2] = Root_Node->node_range_z[0];
+            range.vertex_max[0] = Root_Node->node_range_x[1];
+            range.vertex_max[1] = Root_Node->node_range_y[1];
+            range.vertex_max[2] = Root_Node->node_range_z[1];
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+        else
+        {
+            memset(&range, 0, sizeof(range));
+        }
+    }
+    return range;
+}
+
+template <typename PointType>
+int KD_TREE<PointType>::validnum()
+{
+    int s = 0;
+    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+    {
+        if (Root_Node != nullptr)
+            return (Root_Node->TreeSize - Root_Node->invalid_point_num);
+        else
+            return 0;
+    }
+    else
+    {
+        if (!pthread_mutex_trylock(&working_flag_mutex))
+        {
+            s = Root_Node->TreeSize - Root_Node->invalid_point_num;
+            pthread_mutex_unlock(&working_flag_mutex);
+            return s;
+        }
+        else
+        {
+            return -1;
+        }
+    }
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::root_alpha(float &alpha_bal, float &alpha_del)
+{
+    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+    {
+        alpha_bal = Root_Node->alpha_bal;
+        alpha_del = Root_Node->alpha_del;
+        return;
+    }
+    else
+    {
+        if (!pthread_mutex_trylock(&working_flag_mutex))
+        {
+            alpha_bal = Root_Node->alpha_bal;
+            alpha_del = Root_Node->alpha_del;
+            pthread_mutex_unlock(&working_flag_mutex);
+            return;
+        }
+        else
+        {
+            alpha_bal = alpha_bal_tmp;
+            alpha_del = alpha_del_tmp;
+            return;
+        }
+    }
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::start_thread()
+{
+    pthread_mutex_init(&termination_flag_mutex_lock, NULL);
+    pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL);
+    pthread_mutex_init(&rebuild_logger_mutex_lock, NULL);
+    pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL);
+    pthread_mutex_init(&working_flag_mutex, NULL);
+    pthread_mutex_init(&search_flag_mutex, NULL);
+    pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void *)this);
+    printf("Multi thread started \n");
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::stop_thread()
+{
+    pthread_mutex_lock(&termination_flag_mutex_lock);
+    termination_flag = true;
+    pthread_mutex_unlock(&termination_flag_mutex_lock);
+    if (rebuild_thread)
+        pthread_join(rebuild_thread, NULL);
+    pthread_mutex_destroy(&termination_flag_mutex_lock);
+    pthread_mutex_destroy(&rebuild_logger_mutex_lock);
+    pthread_mutex_destroy(&rebuild_ptr_mutex_lock);
+    pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock);
+    pthread_mutex_destroy(&working_flag_mutex);
+    pthread_mutex_destroy(&search_flag_mutex);
+}
+
+template <typename PointType>
+void *KD_TREE<PointType>::multi_thread_ptr(void *arg)
+{
+    KD_TREE *handle = (KD_TREE *)arg;
+    handle->multi_thread_rebuild();
+    return nullptr;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::multi_thread_rebuild()
+{
+    bool terminated = false;
+    KD_TREE_NODE *father_ptr, **new_node_ptr;
+    pthread_mutex_lock(&termination_flag_mutex_lock);
+    terminated = termination_flag;
+    pthread_mutex_unlock(&termination_flag_mutex_lock);
+    while (!terminated)
+    {
+        pthread_mutex_lock(&rebuild_ptr_mutex_lock);
+        pthread_mutex_lock(&working_flag_mutex);
+        if (Rebuild_Ptr != nullptr)
+        {
+            /* Traverse and copy */
+            if (!Rebuild_Logger.empty())
+            {
+                printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n");
+            }
+            rebuild_flag = true;
+            if (*Rebuild_Ptr == Root_Node)
+            {
+                Treesize_tmp = Root_Node->TreeSize;
+                Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num;
+                alpha_bal_tmp = Root_Node->alpha_bal;
+                alpha_del_tmp = Root_Node->alpha_del;
+            }
+            KD_TREE_NODE *old_root_node = (*Rebuild_Ptr);
+            father_ptr = (*Rebuild_Ptr)->father_ptr;
+            PointVector().swap(Rebuild_PCL_Storage);
+            // Lock Search
+            pthread_mutex_lock(&search_flag_mutex);
+            while (search_mutex_counter != 0)
+            {
+                pthread_mutex_unlock(&search_flag_mutex);
+                usleep(1);
+                pthread_mutex_lock(&search_flag_mutex);
+            }
+            search_mutex_counter = -1;
+            pthread_mutex_unlock(&search_flag_mutex);
+            // Lock deleted points cache
+            pthread_mutex_lock(&points_deleted_rebuild_mutex_lock);
+            flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC);
+            // Unlock deleted points cache
+            pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock);
+            // Unlock Search
+            pthread_mutex_lock(&search_flag_mutex);
+            search_mutex_counter = 0;
+            pthread_mutex_unlock(&search_flag_mutex);
+            pthread_mutex_unlock(&working_flag_mutex);
+            /* Rebuild and update missed operations*/
+            Operation_Logger_Type Operation;
+            KD_TREE_NODE *new_root_node = nullptr;
+            if (int(Rebuild_PCL_Storage.size()) > 0)
+            {
+                BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size() - 1, Rebuild_PCL_Storage);
+                // Rebuild has been done. Updates the blocked operations into the new tree
+                pthread_mutex_lock(&working_flag_mutex);
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                int tmp_counter = 0;
+                while (!Rebuild_Logger.empty())
+                {
+                    Operation = Rebuild_Logger.front();
+                    max_queue_size = max(max_queue_size, Rebuild_Logger.size());
+                    Rebuild_Logger.pop();
+                    pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+                    pthread_mutex_unlock(&working_flag_mutex);
+                    run_operation(&new_root_node, Operation);
+                    tmp_counter++;
+                    if (tmp_counter % 10 == 0)
+                        usleep(1);
+                    pthread_mutex_lock(&working_flag_mutex);
+                    pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                }
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            /* Replace to original tree*/
+            // pthread_mutex_lock(&working_flag_mutex);
+            pthread_mutex_lock(&search_flag_mutex);
+            while (search_mutex_counter != 0)
+            {
+                pthread_mutex_unlock(&search_flag_mutex);
+                usleep(1);
+                pthread_mutex_lock(&search_flag_mutex);
+            }
+            search_mutex_counter = -1;
+            pthread_mutex_unlock(&search_flag_mutex);
+            if (father_ptr->left_son_ptr == *Rebuild_Ptr)
+            {
+                father_ptr->left_son_ptr = new_root_node;
+            }
+            else if (father_ptr->right_son_ptr == *Rebuild_Ptr)
+            {
+                father_ptr->right_son_ptr = new_root_node;
+            }
+            else
+            {
+                throw "Error: Father ptr incompatible with current node\n";
+            }
+            if (new_root_node != nullptr)
+                new_root_node->father_ptr = father_ptr;
+            (*Rebuild_Ptr) = new_root_node;
+            int valid_old = old_root_node->TreeSize - old_root_node->invalid_point_num;
+            int valid_new = new_root_node->TreeSize - new_root_node->invalid_point_num;
+            if (father_ptr == STATIC_ROOT_NODE)
+                Root_Node = STATIC_ROOT_NODE->left_son_ptr;
+            KD_TREE_NODE *update_root = *Rebuild_Ptr;
+            while (update_root != nullptr && update_root != Root_Node)
+            {
+                update_root = update_root->father_ptr;
+                if (update_root->working_flag)
+                    break;
+                if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left)
+                    break;
+                if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right)
+                    break;
+                Update(update_root);
+            }
+            pthread_mutex_lock(&search_flag_mutex);
+            search_mutex_counter = 0;
+            pthread_mutex_unlock(&search_flag_mutex);
+            Rebuild_Ptr = nullptr;
+            pthread_mutex_unlock(&working_flag_mutex);
+            rebuild_flag = false;
+            /* Delete discarded tree nodes */
+            delete_tree_nodes(&old_root_node);
+        }
+        else
+        {
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+        pthread_mutex_unlock(&rebuild_ptr_mutex_lock);
+        pthread_mutex_lock(&termination_flag_mutex_lock);
+        terminated = termination_flag;
+        pthread_mutex_unlock(&termination_flag_mutex_lock);
+        usleep(100);
+    }
+    printf("Rebuild thread terminated normally\n");
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation)
+{
+    switch (operation.op)
+    {
+    case ADD_POINT:
+        Add_by_point(root, operation.point, false, (*root)->division_axis);
+        break;
+    case ADD_BOX:
+        Add_by_range(root, operation.boxpoint, false);
+        break;
+    case DELETE_POINT:
+        Delete_by_point(root, operation.point, false);
+        break;
+    case DELETE_BOX:
+        Delete_by_range(root, operation.boxpoint, false, false);
+        break;
+    case DOWNSAMPLE_DELETE:
+        Delete_by_range(root, operation.boxpoint, false, true);
+        break;
+    case PUSH_DOWN:
+        (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted;
+        (*root)->point_downsample_deleted |= operation.tree_downsample_deleted;
+        (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted;
+        (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted;
+        if (operation.tree_downsample_deleted)
+            (*root)->down_del_num = (*root)->TreeSize;
+        if (operation.tree_deleted)
+            (*root)->invalid_point_num = (*root)->TreeSize;
+        else
+            (*root)->invalid_point_num = (*root)->down_del_num;
+        (*root)->need_push_down_to_left = true;
+        (*root)->need_push_down_to_right = true;
+        break;
+    default:
+        break;
+    }
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Build(PointVector point_cloud)
+{
+    if (Root_Node != nullptr)
+    {
+        delete_tree_nodes(&Root_Node);
+    }
+    if (point_cloud.size() == 0)
+        return;
+    STATIC_ROOT_NODE = new KD_TREE_NODE;
+    InitTreeNode(STATIC_ROOT_NODE);
+    BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size() - 1, point_cloud);
+    Update(STATIC_ROOT_NODE);
+    STATIC_ROOT_NODE->TreeSize = 0;
+    Root_Node = STATIC_ROOT_NODE->left_son_ptr;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector<float> &Point_Distance, float max_dist)
+{
+    MANUAL_HEAP q(2 * k_nearest);
+    q.clear();
+    vector<float>().swap(Point_Distance);
+    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+    {
+        Search(Root_Node, k_nearest, point, q, max_dist);
+    }
+    else
+    {
+        pthread_mutex_lock(&search_flag_mutex);
+        while (search_mutex_counter == -1)
+        {
+            pthread_mutex_unlock(&search_flag_mutex);
+            usleep(1);
+            pthread_mutex_lock(&search_flag_mutex);
+        }
+        search_mutex_counter += 1;
+        pthread_mutex_unlock(&search_flag_mutex);
+        Search(Root_Node, k_nearest, point, q, max_dist);
+        pthread_mutex_lock(&search_flag_mutex);
+        search_mutex_counter -= 1;
+        pthread_mutex_unlock(&search_flag_mutex);
+    }
+    int k_found = min(k_nearest, int(q.size()));
+    PointVector().swap(Nearest_Points);
+    vector<float>().swap(Point_Distance);
+    for (int i = 0; i < k_found; i++)
+    {
+        Nearest_Points.insert(Nearest_Points.begin(), q.top().point);
+        Point_Distance.insert(Point_Distance.begin(), q.top().dist);
+        q.pop();
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage)
+{
+    Storage.clear();
+    Search_by_range(Root_Node, Box_of_Point, Storage);
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Radius_Search(PointType point, const float radius, PointVector &Storage)
+{
+    Storage.clear();
+    Search_by_radius(Root_Node, point, radius, Storage);
+}
+
+template <typename PointType>
+int KD_TREE<PointType>::Add_Points(PointVector &PointToAdd, bool downsample_on)
+{
+    int NewPointSize = PointToAdd.size();
+    int tree_size = size();
+    BoxPointType Box_of_Point;
+    PointType downsample_result, mid_point;
+    bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH;
+    float min_dist, tmp_dist;
+    int tmp_counter = 0;
+    for (int i = 0; i < PointToAdd.size(); i++)
+    {
+        if (downsample_switch)
+        {
+            Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x / downsample_size) * downsample_size;
+            Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0] + downsample_size;
+            Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y / downsample_size) * downsample_size;
+            Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1] + downsample_size;
+            Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z / downsample_size) * downsample_size;
+            Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2] + downsample_size;
+            mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0] - Box_of_Point.vertex_min[0]) / 2.0;
+            mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1] - Box_of_Point.vertex_min[1]) / 2.0;
+            mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2] - Box_of_Point.vertex_min[2]) / 2.0;
+            PointVector().swap(Downsample_Storage);
+            Search_by_range(Root_Node, Box_of_Point, Downsample_Storage);
+            min_dist = calc_dist(PointToAdd[i], mid_point);
+            downsample_result = PointToAdd[i];
+            for (int index = 0; index < Downsample_Storage.size(); index++)
+            {
+                tmp_dist = calc_dist(Downsample_Storage[index], mid_point);
+                if (tmp_dist < min_dist)
+                {
+                    min_dist = tmp_dist;
+                    downsample_result = Downsample_Storage[index];
+                }
+            }
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+            {
+                if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result))
+                {
+                    if (Downsample_Storage.size() > 0)
+                        Delete_by_range(&Root_Node, Box_of_Point, true, true);
+                    Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis);
+                    tmp_counter++;
+                }
+            }
+            else
+            {
+                if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result))
+                {
+                    Operation_Logger_Type operation_delete, operation;
+                    operation_delete.boxpoint = Box_of_Point;
+                    operation_delete.op = DOWNSAMPLE_DELETE;
+                    operation.point = downsample_result;
+                    operation.op = ADD_POINT;
+                    pthread_mutex_lock(&working_flag_mutex);
+                    if (Downsample_Storage.size() > 0)
+                        Delete_by_range(&Root_Node, Box_of_Point, false, true);
+                    Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis);
+                    tmp_counter++;
+                    if (rebuild_flag)
+                    {
+                        pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                        if (Downsample_Storage.size() > 0)
+                            Rebuild_Logger.push(operation_delete);
+                        Rebuild_Logger.push(operation);
+                        pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+                    }
+                    pthread_mutex_unlock(&working_flag_mutex);
+                };
+            }
+        }
+        else
+        {
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+            {
+                Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis);
+            }
+            else
+            {
+                Operation_Logger_Type operation;
+                operation.point = PointToAdd[i];
+                operation.op = ADD_POINT;
+                pthread_mutex_lock(&working_flag_mutex);
+                Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis);
+                if (rebuild_flag)
+                {
+                    pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                    Rebuild_Logger.push(operation);
+                    pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+                }
+                pthread_mutex_unlock(&working_flag_mutex);
+            }
+        }
+    }
+    return tmp_counter;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Add_Point_Boxes(vector<BoxPointType> &BoxPoints)
+{
+    for (int i = 0; i < BoxPoints.size(); i++)
+    {
+        if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+        {
+            Add_by_range(&Root_Node, BoxPoints[i], true);
+        }
+        else
+        {
+            Operation_Logger_Type operation;
+            operation.boxpoint = BoxPoints[i];
+            operation.op = ADD_BOX;
+            pthread_mutex_lock(&working_flag_mutex);
+            Add_by_range(&Root_Node, BoxPoints[i], false);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(operation);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Delete_Points(PointVector &PointToDel)
+{
+    for (int i = 0; i < PointToDel.size(); i++)
+    {
+        if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+        {
+            Delete_by_point(&Root_Node, PointToDel[i], true);
+        }
+        else
+        {
+            Operation_Logger_Type operation;
+            operation.point = PointToDel[i];
+            operation.op = DELETE_POINT;
+            pthread_mutex_lock(&working_flag_mutex);
+            Delete_by_point(&Root_Node, PointToDel[i], false);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(operation);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    return;
+}
+
+template <typename PointType>
+int KD_TREE<PointType>::Delete_Point_Boxes(vector<BoxPointType> &BoxPoints)
+{
+    int tmp_counter = 0;
+    for (int i = 0; i < BoxPoints.size(); i++)
+    {
+        if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+        {
+            tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], true, false);
+        }
+        else
+        {
+            Operation_Logger_Type operation;
+            operation.boxpoint = BoxPoints[i];
+            operation.op = DELETE_BOX;
+            pthread_mutex_lock(&working_flag_mutex);
+            tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], false, false);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(operation);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    return tmp_counter;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::acquire_removed_points(PointVector &removed_points)
+{
+    pthread_mutex_lock(&points_deleted_rebuild_mutex_lock);
+    for (int i = 0; i < Points_deleted.size(); i++)
+    {
+        removed_points.push_back(Points_deleted[i]);
+    }
+    for (int i = 0; i < Multithread_Points_deleted.size(); i++)
+    {
+        removed_points.push_back(Multithread_Points_deleted[i]);
+    }
+    Points_deleted.clear();
+    Multithread_Points_deleted.clear();
+    pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock);
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage)
+{
+    if (l > r)
+        return;
+    *root = new KD_TREE_NODE;
+    InitTreeNode(*root);
+    int mid = (l + r) >> 1;
+    int div_axis = 0;
+    int i;
+    // Find the best division Axis
+    float min_value[3] = {INFINITY, INFINITY, INFINITY};
+    float max_value[3] = {-INFINITY, -INFINITY, -INFINITY};
+    float dim_range[3] = {0, 0, 0};
+    for (i = l; i <= r; i++)
+    {
+        min_value[0] = min(min_value[0], Storage[i].x);
+        min_value[1] = min(min_value[1], Storage[i].y);
+        min_value[2] = min(min_value[2], Storage[i].z);
+        max_value[0] = max(max_value[0], Storage[i].x);
+        max_value[1] = max(max_value[1], Storage[i].y);
+        max_value[2] = max(max_value[2], Storage[i].z);
+    }
+    // Select the longest dimension as division axis
+    for (i = 0; i < 3; i++)
+        dim_range[i] = max_value[i] - min_value[i];
+    for (i = 1; i < 3; i++)
+        if (dim_range[i] > dim_range[div_axis])
+            div_axis = i;
+    // Divide by the division axis and recursively build.
+
+    (*root)->division_axis = div_axis;
+    switch (div_axis)
+    {
+    case 0:
+        nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x);
+        break;
+    case 1:
+        nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_y);
+        break;
+    case 2:
+        nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_z);
+        break;
+    default:
+        nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x);
+        break;
+    }
+    (*root)->point = Storage[mid];
+    KD_TREE_NODE *left_son = nullptr, *right_son = nullptr;
+    BuildTree(&left_son, l, mid - 1, Storage);
+    BuildTree(&right_son, mid + 1, r, Storage);
+    (*root)->left_son_ptr = left_son;
+    (*root)->right_son_ptr = right_son;
+    Update((*root));
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Rebuild(KD_TREE_NODE **root)
+{
+    KD_TREE_NODE *father_ptr;
+    if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num)
+    {
+        if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock))
+        {
+            if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize))
+            {
+                Rebuild_Ptr = root;
+            }
+            pthread_mutex_unlock(&rebuild_ptr_mutex_lock);
+        }
+    }
+    else
+    {
+        father_ptr = (*root)->father_ptr;
+        int size_rec = (*root)->TreeSize;
+        PCL_Storage.clear();
+        flatten(*root, PCL_Storage, DELETE_POINTS_REC);
+        delete_tree_nodes(root);
+        BuildTree(root, 0, PCL_Storage.size() - 1, PCL_Storage);
+        if (*root != nullptr)
+            (*root)->father_ptr = father_ptr;
+        if (*root == Root_Node)
+            STATIC_ROOT_NODE->left_son_ptr = *root;
+    }
+    return;
+}
+
+template <typename PointType>
+int KD_TREE<PointType>::Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample)
+{
+    if ((*root) == nullptr || (*root)->tree_deleted)
+        return 0;
+    (*root)->working_flag = true;
+    Push_Down(*root);
+    int tmp_counter = 0;
+    if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1])
+        return 0;
+    if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1])
+        return 0;
+    if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1])
+        return 0;
+    if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1])
+    {
+        (*root)->tree_deleted = true;
+        (*root)->point_deleted = true;
+        (*root)->need_push_down_to_left = true;
+        (*root)->need_push_down_to_right = true;
+        tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num;
+        (*root)->invalid_point_num = (*root)->TreeSize;
+        if (is_downsample)
+        {
+            (*root)->tree_downsample_deleted = true;
+            (*root)->point_downsample_deleted = true;
+            (*root)->down_del_num = (*root)->TreeSize;
+        }
+        return tmp_counter;
+    }
+    if (!(*root)->point_deleted && boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z)
+    {
+        (*root)->point_deleted = true;
+        tmp_counter += 1;
+        if (is_downsample)
+            (*root)->point_downsample_deleted = true;
+    }
+    Operation_Logger_Type delete_box_log;
+    struct timespec Timeout;
+    if (is_downsample)
+        delete_box_log.op = DOWNSAMPLE_DELETE;
+    else
+        delete_box_log.op = DELETE_BOX;
+    delete_box_log.boxpoint = boxpoint;
+    if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)
+    {
+        tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample);
+    }
+    else
+    {
+        pthread_mutex_lock(&working_flag_mutex);
+        tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample);
+        if (rebuild_flag)
+        {
+            pthread_mutex_lock(&rebuild_logger_mutex_lock);
+            Rebuild_Logger.push(delete_box_log);
+            pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+        }
+        pthread_mutex_unlock(&working_flag_mutex);
+    }
+    if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)
+    {
+        tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample);
+    }
+    else
+    {
+        pthread_mutex_lock(&working_flag_mutex);
+        tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample);
+        if (rebuild_flag)
+        {
+            pthread_mutex_lock(&rebuild_logger_mutex_lock);
+            Rebuild_Logger.push(delete_box_log);
+            pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+        }
+        pthread_mutex_unlock(&working_flag_mutex);
+    }
+    Update(*root);
+    if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)
+        Rebuild_Ptr = nullptr;
+    bool need_rebuild = allow_rebuild & Criterion_Check((*root));
+    if (need_rebuild)
+        Rebuild(root);
+    if ((*root) != nullptr)
+        (*root)->working_flag = false;
+    return tmp_counter;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild)
+{
+    if ((*root) == nullptr || (*root)->tree_deleted)
+        return;
+    (*root)->working_flag = true;
+    Push_Down(*root);
+    if (same_point((*root)->point, point) && !(*root)->point_deleted)
+    {
+        (*root)->point_deleted = true;
+        (*root)->invalid_point_num += 1;
+        if ((*root)->invalid_point_num == (*root)->TreeSize)
+            (*root)->tree_deleted = true;
+        return;
+    }
+    Operation_Logger_Type delete_log;
+    struct timespec Timeout;
+    delete_log.op = DELETE_POINT;
+    delete_log.point = point;
+    if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z))
+    {
+        if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)
+        {
+            Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild);
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            Delete_by_point(&(*root)->left_son_ptr, point, false);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(delete_log);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    else
+    {
+        if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)
+        {
+            Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild);
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            Delete_by_point(&(*root)->right_son_ptr, point, false);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(delete_log);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    Update(*root);
+    if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)
+        Rebuild_Ptr = nullptr;
+    bool need_rebuild = allow_rebuild & Criterion_Check((*root));
+    if (need_rebuild)
+        Rebuild(root);
+    if ((*root) != nullptr)
+        (*root)->working_flag = false;
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild)
+{
+    if ((*root) == nullptr)
+        return;
+    (*root)->working_flag = true;
+    Push_Down(*root);
+    if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1])
+        return;
+    if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1])
+        return;
+    if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1])
+        return;
+    if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1])
+    {
+        (*root)->tree_deleted = false || (*root)->tree_downsample_deleted;
+        (*root)->point_deleted = false || (*root)->point_downsample_deleted;
+        (*root)->need_push_down_to_left = true;
+        (*root)->need_push_down_to_right = true;
+        (*root)->invalid_point_num = (*root)->down_del_num;
+        return;
+    }
+    if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z)
+    {
+        (*root)->point_deleted = (*root)->point_downsample_deleted;
+    }
+    Operation_Logger_Type add_box_log;
+    struct timespec Timeout;
+    add_box_log.op = ADD_BOX;
+    add_box_log.boxpoint = boxpoint;
+    if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)
+    {
+        Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild);
+    }
+    else
+    {
+        pthread_mutex_lock(&working_flag_mutex);
+        Add_by_range(&((*root)->left_son_ptr), boxpoint, false);
+        if (rebuild_flag)
+        {
+            pthread_mutex_lock(&rebuild_logger_mutex_lock);
+            Rebuild_Logger.push(add_box_log);
+            pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+        }
+        pthread_mutex_unlock(&working_flag_mutex);
+    }
+    if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)
+    {
+        Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild);
+    }
+    else
+    {
+        pthread_mutex_lock(&working_flag_mutex);
+        Add_by_range(&((*root)->right_son_ptr), boxpoint, false);
+        if (rebuild_flag)
+        {
+            pthread_mutex_lock(&rebuild_logger_mutex_lock);
+            Rebuild_Logger.push(add_box_log);
+            pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+        }
+        pthread_mutex_unlock(&working_flag_mutex);
+    }
+    Update(*root);
+    if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)
+        Rebuild_Ptr = nullptr;
+    bool need_rebuild = allow_rebuild & Criterion_Check((*root));
+    if (need_rebuild)
+        Rebuild(root);
+    if ((*root) != nullptr)
+        (*root)->working_flag = false;
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis)
+{
+    if (*root == nullptr)
+    {
+        *root = new KD_TREE_NODE;
+        InitTreeNode(*root);
+        (*root)->point = point;
+        (*root)->division_axis = (father_axis + 1) % 3;
+        Update(*root);
+        return;
+    }
+    (*root)->working_flag = true;
+    Operation_Logger_Type add_log;
+    struct timespec Timeout;
+    add_log.op = ADD_POINT;
+    add_log.point = point;
+    Push_Down(*root);
+    if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z))
+    {
+        if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)
+        {
+            Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis);
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            Add_by_point(&(*root)->left_son_ptr, point, false, (*root)->division_axis);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(add_log);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    else
+    {
+        if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)
+        {
+            Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild, (*root)->division_axis);
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            Add_by_point(&(*root)->right_son_ptr, point, false, (*root)->division_axis);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(add_log);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    Update(*root);
+    if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)
+        Rebuild_Ptr = nullptr;
+    bool need_rebuild = allow_rebuild & Criterion_Check((*root));
+    if (need_rebuild)
+        Rebuild(root);
+    if ((*root) != nullptr)
+        (*root)->working_flag = false;
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist)
+{
+    if (root == nullptr || root->tree_deleted)
+        return;
+    float cur_dist = calc_box_dist(root, point);
+    float max_dist_sqr = max_dist * max_dist;
+    if (cur_dist > max_dist_sqr)
+        return;
+    int retval;
+    if (root->need_push_down_to_left || root->need_push_down_to_right)
+    {
+        retval = pthread_mutex_trylock(&(root->push_down_mutex_lock));
+        if (retval == 0)
+        {
+            Push_Down(root);
+            pthread_mutex_unlock(&(root->push_down_mutex_lock));
+        }
+        else
+        {
+            pthread_mutex_lock(&(root->push_down_mutex_lock));
+            pthread_mutex_unlock(&(root->push_down_mutex_lock));
+        }
+    }
+    if (!root->point_deleted)
+    {
+        float dist = calc_dist(point, root->point);
+        if (dist <= max_dist_sqr && (q.size() < k_nearest || dist < q.top().dist))
+        {
+            if (q.size() >= k_nearest)
+                q.pop();
+            PointType_CMP current_point{root->point, dist};
+            q.push(current_point);
+        }
+    }
+    int cur_search_counter;
+    float dist_left_node = calc_box_dist(root->left_son_ptr, point);
+    float dist_right_node = calc_box_dist(root->right_son_ptr, point);
+    if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist)
+    {
+        if (dist_left_node <= dist_right_node)
+        {
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)
+            {
+                Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+            }
+            else
+            {
+                pthread_mutex_lock(&search_flag_mutex);
+                while (search_mutex_counter == -1)
+                {
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    usleep(1);
+                    pthread_mutex_lock(&search_flag_mutex);
+                }
+                search_mutex_counter += 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+                Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+                pthread_mutex_lock(&search_flag_mutex);
+                search_mutex_counter -= 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+            }
+            if (q.size() < k_nearest || dist_right_node < q.top().dist)
+            {
+                if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)
+                {
+                    Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+                }
+                else
+                {
+                    pthread_mutex_lock(&search_flag_mutex);
+                    while (search_mutex_counter == -1)
+                    {
+                        pthread_mutex_unlock(&search_flag_mutex);
+                        usleep(1);
+                        pthread_mutex_lock(&search_flag_mutex);
+                    }
+                    search_mutex_counter += 1;
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+                    pthread_mutex_lock(&search_flag_mutex);
+                    search_mutex_counter -= 1;
+                    pthread_mutex_unlock(&search_flag_mutex);
+                }
+            }
+        }
+        else
+        {
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)
+            {
+                Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+            }
+            else
+            {
+                pthread_mutex_lock(&search_flag_mutex);
+                while (search_mutex_counter == -1)
+                {
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    usleep(1);
+                    pthread_mutex_lock(&search_flag_mutex);
+                }
+                search_mutex_counter += 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+                Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+                pthread_mutex_lock(&search_flag_mutex);
+                search_mutex_counter -= 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+            }
+            if (q.size() < k_nearest || dist_left_node < q.top().dist)
+            {
+                if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)
+                {
+                    Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+                }
+                else
+                {
+                    pthread_mutex_lock(&search_flag_mutex);
+                    while (search_mutex_counter == -1)
+                    {
+                        pthread_mutex_unlock(&search_flag_mutex);
+                        usleep(1);
+                        pthread_mutex_lock(&search_flag_mutex);
+                    }
+                    search_mutex_counter += 1;
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+                    pthread_mutex_lock(&search_flag_mutex);
+                    search_mutex_counter -= 1;
+                    pthread_mutex_unlock(&search_flag_mutex);
+                }
+            }
+        }
+    }
+    else
+    {
+        if (dist_left_node < q.top().dist)
+        {
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)
+            {
+                Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+            }
+            else
+            {
+                pthread_mutex_lock(&search_flag_mutex);
+                while (search_mutex_counter == -1)
+                {
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    usleep(1);
+                    pthread_mutex_lock(&search_flag_mutex);
+                }
+                search_mutex_counter += 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+                Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+                pthread_mutex_lock(&search_flag_mutex);
+                search_mutex_counter -= 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+            }
+        }
+        if (dist_right_node < q.top().dist)
+        {
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)
+            {
+                Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+            }
+            else
+            {
+                pthread_mutex_lock(&search_flag_mutex);
+                while (search_mutex_counter == -1)
+                {
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    usleep(1);
+                    pthread_mutex_lock(&search_flag_mutex);
+                }
+                search_mutex_counter += 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+                Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+                pthread_mutex_lock(&search_flag_mutex);
+                search_mutex_counter -= 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+            }
+        }
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage)
+{
+    if (root == nullptr)
+        return;
+    Push_Down(root);
+    if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1])
+        return;
+    if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1])
+        return;
+    if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1])
+        return;
+    if (boxpoint.vertex_min[0] <= root->node_range_x[0] && boxpoint.vertex_max[0] > root->node_range_x[1] && boxpoint.vertex_min[1] <= root->node_range_y[0] && boxpoint.vertex_max[1] > root->node_range_y[1] && boxpoint.vertex_min[2] <= root->node_range_z[0] && boxpoint.vertex_max[2] > root->node_range_z[1])
+    {
+        flatten(root, Storage, NOT_RECORD);
+        return;
+    }
+    if (boxpoint.vertex_min[0] <= root->point.x && boxpoint.vertex_max[0] > root->point.x && boxpoint.vertex_min[1] <= root->point.y && boxpoint.vertex_max[1] > root->point.y && boxpoint.vertex_min[2] <= root->point.z && boxpoint.vertex_max[2] > root->point.z)
+    {
+        if (!root->point_deleted)
+            Storage.push_back(root->point);
+    }
+    if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr)
+    {
+        Search_by_range(root->left_son_ptr, boxpoint, Storage);
+    }
+    else
+    {
+        pthread_mutex_lock(&search_flag_mutex);
+        Search_by_range(root->left_son_ptr, boxpoint, Storage);
+        pthread_mutex_unlock(&search_flag_mutex);
+    }
+    if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr)
+    {
+        Search_by_range(root->right_son_ptr, boxpoint, Storage);
+    }
+    else
+    {
+        pthread_mutex_lock(&search_flag_mutex);
+        Search_by_range(root->right_son_ptr, boxpoint, Storage);
+        pthread_mutex_unlock(&search_flag_mutex);
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage)
+{
+    if (root == nullptr)
+        return;
+    Push_Down(root);
+    PointType range_center;
+    range_center.x = (root->node_range_x[0] + root->node_range_x[1]) * 0.5;
+    range_center.y = (root->node_range_y[0] + root->node_range_y[1]) * 0.5;
+    range_center.z = (root->node_range_z[0] + root->node_range_z[1]) * 0.5;
+    float dist = sqrt(calc_dist(range_center, point));
+    if (dist > radius + sqrt(root->radius_sq)) return;
+    if (dist <= radius - sqrt(root->radius_sq)) 
+    {
+        flatten(root, Storage, NOT_RECORD);
+        return;
+    }
+    if (!root->point_deleted && calc_dist(root->point, point) <= radius * radius){
+        Storage.push_back(root->point);
+    }
+    if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr)
+    {
+        Search_by_radius(root->left_son_ptr, point, radius, Storage);
+    }
+    else
+    {
+        pthread_mutex_lock(&search_flag_mutex);
+        Search_by_radius(root->left_son_ptr, point, radius, Storage);
+        pthread_mutex_unlock(&search_flag_mutex);
+    }
+    if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr)
+    {
+        Search_by_radius(root->right_son_ptr, point, radius, Storage);
+    }
+    else
+    {
+        pthread_mutex_lock(&search_flag_mutex);
+        Search_by_radius(root->right_son_ptr, point, radius, Storage);
+        pthread_mutex_unlock(&search_flag_mutex);
+    }    
+    return;
+}
+
+template <typename PointType>
+bool KD_TREE<PointType>::Criterion_Check(KD_TREE_NODE *root)
+{
+    if (root->TreeSize <= Minimal_Unbalanced_Tree_Size)
+    {
+        return false;
+    }
+    float balance_evaluation = 0.0f;
+    float delete_evaluation = 0.0f;
+    KD_TREE_NODE *son_ptr = root->left_son_ptr;
+    if (son_ptr == nullptr)
+        son_ptr = root->right_son_ptr;
+    delete_evaluation = float(root->invalid_point_num) / root->TreeSize;
+    balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize - 1);
+    if (delete_evaluation > delete_criterion_param)
+    {
+        return true;
+    }
+    if (balance_evaluation > balance_criterion_param || balance_evaluation < 1 - balance_criterion_param)
+    {
+        return true;
+    }
+    return false;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Push_Down(KD_TREE_NODE *root)
+{
+    if (root == nullptr)
+        return;
+    Operation_Logger_Type operation;
+    operation.op = PUSH_DOWN;
+    operation.tree_deleted = root->tree_deleted;
+    operation.tree_downsample_deleted = root->tree_downsample_deleted;
+    if (root->need_push_down_to_left && root->left_son_ptr != nullptr)
+    {
+        if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)
+        {
+            root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
+            root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
+            root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted;
+            root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted;
+            if (root->tree_downsample_deleted)
+                root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize;
+            if (root->tree_deleted)
+                root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize;
+            else
+                root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num;
+            root->left_son_ptr->need_push_down_to_left = true;
+            root->left_son_ptr->need_push_down_to_right = true;
+            root->need_push_down_to_left = false;
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
+            root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
+            root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted;
+            root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted;
+            if (root->tree_downsample_deleted)
+                root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize;
+            if (root->tree_deleted)
+                root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize;
+            else
+                root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num;
+            root->left_son_ptr->need_push_down_to_left = true;
+            root->left_son_ptr->need_push_down_to_right = true;
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(operation);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            root->need_push_down_to_left = false;
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    if (root->need_push_down_to_right && root->right_son_ptr != nullptr)
+    {
+        if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)
+        {
+            root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
+            root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
+            root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted;
+            root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted;
+            if (root->tree_downsample_deleted)
+                root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize;
+            if (root->tree_deleted)
+                root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize;
+            else
+                root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num;
+            root->right_son_ptr->need_push_down_to_left = true;
+            root->right_son_ptr->need_push_down_to_right = true;
+            root->need_push_down_to_right = false;
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
+            root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
+            root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted;
+            root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted;
+            if (root->tree_downsample_deleted)
+                root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize;
+            if (root->tree_deleted)
+                root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize;
+            else
+                root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num;
+            root->right_son_ptr->need_push_down_to_left = true;
+            root->right_son_ptr->need_push_down_to_right = true;
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(operation);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            root->need_push_down_to_right = false;
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Update(KD_TREE_NODE *root)
+{
+    KD_TREE_NODE *left_son_ptr = root->left_son_ptr;
+    KD_TREE_NODE *right_son_ptr = root->right_son_ptr;
+    float tmp_range_x[2] = {INFINITY, -INFINITY};
+    float tmp_range_y[2] = {INFINITY, -INFINITY};
+    float tmp_range_z[2] = {INFINITY, -INFINITY};
+    // Update Tree Size
+    if (left_son_ptr != nullptr && right_son_ptr != nullptr)
+    {
+        root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1;
+        root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0);
+        root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0);
+        root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;
+        root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted;
+        if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted))
+        {
+            tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0], right_son_ptr->node_range_x[0]), root->point.x);
+            tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1], right_son_ptr->node_range_x[1]), root->point.x);
+            tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0], right_son_ptr->node_range_y[0]), root->point.y);
+            tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1], right_son_ptr->node_range_y[1]), root->point.y);
+            tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0], right_son_ptr->node_range_z[0]), root->point.z);
+            tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1], right_son_ptr->node_range_z[1]), root->point.z);
+        }
+        else
+        {
+            if (!left_son_ptr->tree_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]);
+                tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]);
+                tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]);
+                tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]);
+                tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]);
+                tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]);
+            }
+            if (!right_son_ptr->tree_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]);
+                tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]);
+                tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]);
+                tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]);
+                tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]);
+                tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]);
+            }
+            if (!root->point_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], root->point.x);
+                tmp_range_x[1] = max(tmp_range_x[1], root->point.x);
+                tmp_range_y[0] = min(tmp_range_y[0], root->point.y);
+                tmp_range_y[1] = max(tmp_range_y[1], root->point.y);
+                tmp_range_z[0] = min(tmp_range_z[0], root->point.z);
+                tmp_range_z[1] = max(tmp_range_z[1], root->point.z);
+            }
+        }
+    }
+    else if (left_son_ptr != nullptr)
+    {
+        root->TreeSize = left_son_ptr->TreeSize + 1;
+        root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0);
+        root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0);
+        root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;
+        root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted;
+        if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted))
+        {
+            tmp_range_x[0] = min(left_son_ptr->node_range_x[0], root->point.x);
+            tmp_range_x[1] = max(left_son_ptr->node_range_x[1], root->point.x);
+            tmp_range_y[0] = min(left_son_ptr->node_range_y[0], root->point.y);
+            tmp_range_y[1] = max(left_son_ptr->node_range_y[1], root->point.y);
+            tmp_range_z[0] = min(left_son_ptr->node_range_z[0], root->point.z);
+            tmp_range_z[1] = max(left_son_ptr->node_range_z[1], root->point.z);
+        }
+        else
+        {
+            if (!left_son_ptr->tree_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]);
+                tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]);
+                tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]);
+                tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]);
+                tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]);
+                tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]);
+            }
+            if (!root->point_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], root->point.x);
+                tmp_range_x[1] = max(tmp_range_x[1], root->point.x);
+                tmp_range_y[0] = min(tmp_range_y[0], root->point.y);
+                tmp_range_y[1] = max(tmp_range_y[1], root->point.y);
+                tmp_range_z[0] = min(tmp_range_z[0], root->point.z);
+                tmp_range_z[1] = max(tmp_range_z[1], root->point.z);
+            }
+        }
+    }
+    else if (right_son_ptr != nullptr)
+    {
+        root->TreeSize = right_son_ptr->TreeSize + 1;
+        root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0);
+        root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0);
+        root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;
+        root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted;
+        if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted))
+        {
+            tmp_range_x[0] = min(right_son_ptr->node_range_x[0], root->point.x);
+            tmp_range_x[1] = max(right_son_ptr->node_range_x[1], root->point.x);
+            tmp_range_y[0] = min(right_son_ptr->node_range_y[0], root->point.y);
+            tmp_range_y[1] = max(right_son_ptr->node_range_y[1], root->point.y);
+            tmp_range_z[0] = min(right_son_ptr->node_range_z[0], root->point.z);
+            tmp_range_z[1] = max(right_son_ptr->node_range_z[1], root->point.z);
+        }
+        else
+        {
+            if (!right_son_ptr->tree_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]);
+                tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]);
+                tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]);
+                tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]);
+                tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]);
+                tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]);
+            }
+            if (!root->point_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], root->point.x);
+                tmp_range_x[1] = max(tmp_range_x[1], root->point.x);
+                tmp_range_y[0] = min(tmp_range_y[0], root->point.y);
+                tmp_range_y[1] = max(tmp_range_y[1], root->point.y);
+                tmp_range_z[0] = min(tmp_range_z[0], root->point.z);
+                tmp_range_z[1] = max(tmp_range_z[1], root->point.z);
+            }
+        }
+    }
+    else
+    {
+        root->TreeSize = 1;
+        root->invalid_point_num = (root->point_deleted ? 1 : 0);
+        root->down_del_num = (root->point_downsample_deleted ? 1 : 0);
+        root->tree_downsample_deleted = root->point_downsample_deleted;
+        root->tree_deleted = root->point_deleted;
+        tmp_range_x[0] = root->point.x;
+        tmp_range_x[1] = root->point.x;
+        tmp_range_y[0] = root->point.y;
+        tmp_range_y[1] = root->point.y;
+        tmp_range_z[0] = root->point.z;
+        tmp_range_z[1] = root->point.z;
+    }
+    memcpy(root->node_range_x, tmp_range_x, sizeof(tmp_range_x));
+    memcpy(root->node_range_y, tmp_range_y, sizeof(tmp_range_y));
+    memcpy(root->node_range_z, tmp_range_z, sizeof(tmp_range_z));
+    float x_L = (root->node_range_x[1] - root->node_range_x[0]) * 0.5;
+    float y_L = (root->node_range_y[1] - root->node_range_y[0]) * 0.5;
+    float z_L = (root->node_range_z[1] - root->node_range_z[0]) * 0.5;
+    root->radius_sq = x_L*x_L + y_L * y_L + z_L * z_L;
+    if (left_son_ptr != nullptr)
+        left_son_ptr->father_ptr = root;
+    if (right_son_ptr != nullptr)
+        right_son_ptr->father_ptr = root;
+    if (root == Root_Node && root->TreeSize > 3)
+    {
+        KD_TREE_NODE *son_ptr = root->left_son_ptr;
+        if (son_ptr == nullptr)
+            son_ptr = root->right_son_ptr;
+        float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize - 1);
+        root->alpha_del = float(root->invalid_point_num) / root->TreeSize;
+        root->alpha_bal = (tmp_bal >= 0.5 - EPSS) ? tmp_bal : 1 - tmp_bal;
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type)
+{
+    if (root == nullptr)
+        return;
+    Push_Down(root);
+    if (!root->point_deleted)
+    {
+        Storage.push_back(root->point);
+    }
+    flatten(root->left_son_ptr, Storage, storage_type);
+    flatten(root->right_son_ptr, Storage, storage_type);
+    switch (storage_type)
+    {
+    case NOT_RECORD:
+        break;
+    case DELETE_POINTS_REC:
+        if (root->point_deleted && !root->point_downsample_deleted)
+        {
+            Points_deleted.push_back(root->point);
+        }
+        break;
+    case MULTI_THREAD_REC:
+        if (root->point_deleted && !root->point_downsample_deleted)
+        {
+            Multithread_Points_deleted.push_back(root->point);
+        }
+        break;
+    default:
+        break;
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::delete_tree_nodes(KD_TREE_NODE **root)
+{
+    if (*root == nullptr)
+        return;
+    Push_Down(*root);
+    delete_tree_nodes(&(*root)->left_son_ptr);
+    delete_tree_nodes(&(*root)->right_son_ptr);
+
+    pthread_mutex_destroy(&(*root)->push_down_mutex_lock);
+    delete *root;
+    *root = nullptr;
+
+    return;
+}
+
+template <typename PointType>
+bool KD_TREE<PointType>::same_point(PointType a, PointType b)
+{
+    return (fabs(a.x - b.x) < EPSS && fabs(a.y - b.y) < EPSS && fabs(a.z - b.z) < EPSS);
+}
+
+template <typename PointType>
+float KD_TREE<PointType>::calc_dist(PointType a, PointType b)
+{
+    float dist = 0.0f;
+    dist = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z);
+    return dist;
+}
+
+template <typename PointType>
+float KD_TREE<PointType>::calc_box_dist(KD_TREE_NODE *node, PointType point)
+{
+    if (node == nullptr)
+        return INFINITY;
+    float min_dist = 0.0;
+    if (point.x < node->node_range_x[0])
+        min_dist += (point.x - node->node_range_x[0]) * (point.x - node->node_range_x[0]);
+    if (point.x > node->node_range_x[1])
+        min_dist += (point.x - node->node_range_x[1]) * (point.x - node->node_range_x[1]);
+    if (point.y < node->node_range_y[0])
+        min_dist += (point.y - node->node_range_y[0]) * (point.y - node->node_range_y[0]);
+    if (point.y > node->node_range_y[1])
+        min_dist += (point.y - node->node_range_y[1]) * (point.y - node->node_range_y[1]);
+    if (point.z < node->node_range_z[0])
+        min_dist += (point.z - node->node_range_z[0]) * (point.z - node->node_range_z[0]);
+    if (point.z > node->node_range_z[1])
+        min_dist += (point.z - node->node_range_z[1]) * (point.z - node->node_range_z[1]);
+    return min_dist;
+}
+template <typename PointType>
+bool KD_TREE<PointType>::point_cmp_x(PointType a, PointType b) { return a.x < b.x; }
+template <typename PointType>
+bool KD_TREE<PointType>::point_cmp_y(PointType a, PointType b) { return a.y < b.y; }
+template <typename PointType>
+bool KD_TREE<PointType>::point_cmp_z(PointType a, PointType b) { return a.z < b.z; }
+
+// Manual heap
+
+
+
+// manual queue
+
+
+// Manual Instatiations
+template class KD_TREE<pcl::PointXYZ>;
+template class KD_TREE<pcl::PointXYZI>;
+template class KD_TREE<pcl::PointXYZINormal>;
+
diff --git a/FAST_LIO/include/ikd-Tree/ikd_Tree.h b/FAST_LIO/include/ikd-Tree/ikd_Tree.h
new file mode 100644
index 0000000..d4b302e
--- /dev/null
+++ b/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);
diff --git a/FAST_LIO/include/matplotlibcpp.h b/FAST_LIO/include/matplotlibcpp.h
new file mode 100644
index 0000000..6855445
--- /dev/null
+++ b/FAST_LIO/include/matplotlibcpp.h
@@ -0,0 +1,2499 @@
+#pragma once
+
+// Python headers must be included before any system headers, since
+// they define _POSIX_C_SOURCE
+#include <Python.h>
+
+#include <vector>
+#include <map>
+#include <array>
+#include <numeric>
+#include <algorithm>
+#include <stdexcept>
+#include <iostream>
+#include <cstdint> // <cstdint> requires c++11 support
+#include <functional>
+
+#ifndef WITHOUT_NUMPY
+#  define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
+#  include <numpy/arrayobject.h>
+
+#  ifdef WITH_OPENCV
+#    include <opencv2/opencv.hpp>
+#  endif // WITH_OPENCV
+
+/*
+ * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so
+ * define the ones we need here.
+ */
+#  if CV_MAJOR_VERSION > 3
+#    define CV_BGR2RGB cv::COLOR_BGR2RGB
+#    define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA
+#  endif
+#endif // WITHOUT_NUMPY
+
+#if PY_MAJOR_VERSION >= 3
+#  define PyString_FromString PyUnicode_FromString
+#  define PyInt_FromLong PyLong_FromLong
+#  define PyString_FromString PyUnicode_FromString
+#endif
+
+
+namespace matplotlibcpp {
+namespace detail {
+
+static std::string s_backend;
+
+struct _interpreter {
+    PyObject* s_python_function_arrow;
+    PyObject *s_python_function_show;
+    PyObject *s_python_function_close;
+    PyObject *s_python_function_draw;
+    PyObject *s_python_function_pause;
+    PyObject *s_python_function_save;
+    PyObject *s_python_function_figure;
+    PyObject *s_python_function_fignum_exists;
+    PyObject *s_python_function_plot;
+    PyObject *s_python_function_quiver;
+    PyObject* s_python_function_contour;
+    PyObject *s_python_function_semilogx;
+    PyObject *s_python_function_semilogy;
+    PyObject *s_python_function_loglog;
+    PyObject *s_python_function_fill;
+    PyObject *s_python_function_fill_between;
+    PyObject *s_python_function_hist;
+    PyObject *s_python_function_imshow;
+    PyObject *s_python_function_scatter;
+    PyObject *s_python_function_boxplot;
+    PyObject *s_python_function_subplot;
+    PyObject *s_python_function_subplot2grid;
+    PyObject *s_python_function_legend;
+    PyObject *s_python_function_xlim;
+    PyObject *s_python_function_ion;
+    PyObject *s_python_function_ginput;
+    PyObject *s_python_function_ylim;
+    PyObject *s_python_function_title;
+    PyObject *s_python_function_axis;
+    PyObject *s_python_function_axvline;
+    PyObject *s_python_function_axvspan;
+    PyObject *s_python_function_xlabel;
+    PyObject *s_python_function_ylabel;
+    PyObject *s_python_function_gca;
+    PyObject *s_python_function_xticks;
+    PyObject *s_python_function_yticks;
+    PyObject* s_python_function_margins;
+    PyObject *s_python_function_tick_params;
+    PyObject *s_python_function_grid;
+    PyObject* s_python_function_cla;
+    PyObject *s_python_function_clf;
+    PyObject *s_python_function_errorbar;
+    PyObject *s_python_function_annotate;
+    PyObject *s_python_function_tight_layout;
+    PyObject *s_python_colormap;
+    PyObject *s_python_empty_tuple;
+    PyObject *s_python_function_stem;
+    PyObject *s_python_function_xkcd;
+    PyObject *s_python_function_text;
+    PyObject *s_python_function_suptitle;
+    PyObject *s_python_function_bar;
+    PyObject *s_python_function_colorbar;
+    PyObject *s_python_function_subplots_adjust;
+
+
+    /* For now, _interpreter is implemented as a singleton since its currently not possible to have
+       multiple independent embedded python interpreters without patching the python source code
+       or starting a separate process for each.
+        http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program
+       */
+
+    static _interpreter& get() {
+        static _interpreter ctx;
+        return ctx;
+    }
+
+    PyObject* safe_import(PyObject* module, std::string fname) {
+        PyObject* fn = PyObject_GetAttrString(module, fname.c_str());
+
+        if (!fn)
+            throw std::runtime_error(std::string("Couldn't find required function: ") + fname);
+
+        if (!PyFunction_Check(fn))
+            throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction."));
+
+        return fn;
+    }
+
+private:
+
+#ifndef WITHOUT_NUMPY
+#  if PY_MAJOR_VERSION >= 3
+
+    void *import_numpy() {
+        import_array(); // initialize C-API
+        return NULL;
+    }
+
+#  else
+
+    void import_numpy() {
+        import_array(); // initialize C-API
+    }
+
+#  endif
+#endif
+
+    _interpreter() {
+
+        // optional but recommended
+#if PY_MAJOR_VERSION >= 3
+        wchar_t name[] = L"plotting";
+#else
+        char name[] = "plotting";
+#endif
+        Py_SetProgramName(name);
+        Py_Initialize();
+
+#ifndef WITHOUT_NUMPY
+        import_numpy(); // initialize numpy C-API
+#endif
+
+        PyObject* matplotlibname = PyString_FromString("matplotlib");
+        PyObject* pyplotname = PyString_FromString("matplotlib.pyplot");
+        PyObject* cmname  = PyString_FromString("matplotlib.cm");
+        PyObject* pylabname  = PyString_FromString("pylab");
+        if (!pyplotname || !pylabname || !matplotlibname || !cmname) {
+            throw std::runtime_error("couldnt create string");
+        }
+
+        PyObject* matplotlib = PyImport_Import(matplotlibname);
+        Py_DECREF(matplotlibname);
+        if (!matplotlib) {
+            PyErr_Print();
+            throw std::runtime_error("Error loading module matplotlib!");
+        }
+
+        // matplotlib.use() must be called *before* pylab, matplotlib.pyplot,
+        // or matplotlib.backends is imported for the first time
+        if (!s_backend.empty()) {
+            PyObject_CallMethod(matplotlib, const_cast<char*>("use"), const_cast<char*>("s"), s_backend.c_str());
+        }
+
+        PyObject* pymod = PyImport_Import(pyplotname);
+        Py_DECREF(pyplotname);
+        if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); }
+
+        s_python_colormap = PyImport_Import(cmname);
+        Py_DECREF(cmname);
+        if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); }
+
+        PyObject* pylabmod = PyImport_Import(pylabname);
+        Py_DECREF(pylabname);
+        if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); }
+
+        s_python_function_arrow = safe_import(pymod, "arrow");
+        s_python_function_show = safe_import(pymod, "show");
+        s_python_function_close = safe_import(pymod, "close");
+        s_python_function_draw = safe_import(pymod, "draw");
+        s_python_function_pause = safe_import(pymod, "pause");
+        s_python_function_figure = safe_import(pymod, "figure");
+        s_python_function_fignum_exists = safe_import(pymod, "fignum_exists");
+        s_python_function_plot = safe_import(pymod, "plot");
+        s_python_function_quiver = safe_import(pymod, "quiver");
+        s_python_function_contour = safe_import(pymod, "contour");
+        s_python_function_semilogx = safe_import(pymod, "semilogx");
+        s_python_function_semilogy = safe_import(pymod, "semilogy");
+        s_python_function_loglog = safe_import(pymod, "loglog");
+        s_python_function_fill = safe_import(pymod, "fill");
+        s_python_function_fill_between = safe_import(pymod, "fill_between");
+        s_python_function_hist = safe_import(pymod,"hist");
+        s_python_function_scatter = safe_import(pymod,"scatter");
+        s_python_function_boxplot = safe_import(pymod,"boxplot");
+        s_python_function_subplot = safe_import(pymod, "subplot");
+        s_python_function_subplot2grid = safe_import(pymod, "subplot2grid");
+        s_python_function_legend = safe_import(pymod, "legend");
+        s_python_function_ylim = safe_import(pymod, "ylim");
+        s_python_function_title = safe_import(pymod, "title");
+        s_python_function_axis = safe_import(pymod, "axis");
+        s_python_function_axvline = safe_import(pymod, "axvline");
+        s_python_function_axvspan = safe_import(pymod, "axvspan");
+        s_python_function_xlabel = safe_import(pymod, "xlabel");
+        s_python_function_ylabel = safe_import(pymod, "ylabel");
+        s_python_function_gca = safe_import(pymod, "gca");
+        s_python_function_xticks = safe_import(pymod, "xticks");
+        s_python_function_yticks = safe_import(pymod, "yticks");
+        s_python_function_margins = safe_import(pymod, "margins");
+        s_python_function_tick_params = safe_import(pymod, "tick_params");
+        s_python_function_grid = safe_import(pymod, "grid");
+        s_python_function_xlim = safe_import(pymod, "xlim");
+        s_python_function_ion = safe_import(pymod, "ion");
+        s_python_function_ginput = safe_import(pymod, "ginput");
+        s_python_function_save = safe_import(pylabmod, "savefig");
+        s_python_function_annotate = safe_import(pymod,"annotate");
+        s_python_function_cla = safe_import(pymod, "cla");
+        s_python_function_clf = safe_import(pymod, "clf");
+        s_python_function_errorbar = safe_import(pymod, "errorbar");
+        s_python_function_tight_layout = safe_import(pymod, "tight_layout");
+        s_python_function_stem = safe_import(pymod, "stem");
+        s_python_function_xkcd = safe_import(pymod, "xkcd");
+        s_python_function_text = safe_import(pymod, "text");
+        s_python_function_suptitle = safe_import(pymod, "suptitle");
+        s_python_function_bar = safe_import(pymod,"bar");
+        s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar");
+        s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust");
+#ifndef WITHOUT_NUMPY
+        s_python_function_imshow = safe_import(pymod, "imshow");
+#endif
+        s_python_empty_tuple = PyTuple_New(0);
+    }
+
+    ~_interpreter() {
+        Py_Finalize();
+    }
+};
+
+} // end namespace detail
+
+/// Select the backend
+///
+/// **NOTE:** This must be called before the first plot command to have
+/// any effect.
+///
+/// Mainly useful to select the non-interactive 'Agg' backend when running
+/// matplotlibcpp in headless mode, for example on a machine with no display.
+///
+/// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use
+inline void backend(const std::string& name)
+{
+    detail::s_backend = name;
+}
+
+inline bool annotate(std::string annotation, double x, double y)
+{
+    detail::_interpreter::get();
+
+    PyObject * xy = PyTuple_New(2);
+    PyObject * str = PyString_FromString(annotation.c_str());
+
+    PyTuple_SetItem(xy,0,PyFloat_FromDouble(x));
+    PyTuple_SetItem(xy,1,PyFloat_FromDouble(y));
+
+    PyObject* kwargs = PyDict_New();
+    PyDict_SetItemString(kwargs, "xy", xy);
+
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, str);
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs);
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+namespace detail {
+
+#ifndef WITHOUT_NUMPY
+// Type selector for numpy array conversion
+template <typename T> struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default
+template <> struct select_npy_type<double> { const static NPY_TYPES type = NPY_DOUBLE; };
+template <> struct select_npy_type<float> { const static NPY_TYPES type = NPY_FLOAT; };
+template <> struct select_npy_type<bool> { const static NPY_TYPES type = NPY_BOOL; };
+template <> struct select_npy_type<int8_t> { const static NPY_TYPES type = NPY_INT8; };
+template <> struct select_npy_type<int16_t> { const static NPY_TYPES type = NPY_SHORT; };
+template <> struct select_npy_type<int32_t> { const static NPY_TYPES type = NPY_INT; };
+template <> struct select_npy_type<int64_t> { const static NPY_TYPES type = NPY_INT64; };
+template <> struct select_npy_type<uint8_t> { const static NPY_TYPES type = NPY_UINT8; };
+template <> struct select_npy_type<uint16_t> { const static NPY_TYPES type = NPY_USHORT; };
+template <> struct select_npy_type<uint32_t> { const static NPY_TYPES type = NPY_ULONG; };
+template <> struct select_npy_type<uint64_t> { const static NPY_TYPES type = NPY_UINT64; };
+
+// Sanity checks; comment them out or change the numpy type below if you're compiling on
+// a platform where they don't apply
+static_assert(sizeof(long long) == 8);
+template <> struct select_npy_type<long long> { const static NPY_TYPES type = NPY_INT64; };
+static_assert(sizeof(unsigned long long) == 8);
+template <> struct select_npy_type<unsigned long long> { const static NPY_TYPES type = NPY_UINT64; };
+// TODO: add int, long, etc.
+
+template<typename Numeric>
+PyObject* get_array(const std::vector<Numeric>& v)
+{
+    npy_intp vsize = v.size();
+    NPY_TYPES type = select_npy_type<Numeric>::type;
+    if (type == NPY_NOTYPE) {
+        size_t memsize = v.size()*sizeof(double);
+        double* dp = static_cast<double*>(::malloc(memsize));
+        for (size_t i=0; i<v.size(); ++i)
+            dp[i] = v[i];
+        PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, NPY_DOUBLE, dp);
+        PyArray_UpdateFlags(reinterpret_cast<PyArrayObject*>(varray), NPY_ARRAY_OWNDATA);
+        return varray;
+    }
+    
+    PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data()));
+    return varray;
+}
+
+
+template<typename Numeric>
+PyObject* get_2darray(const std::vector<::std::vector<Numeric>>& v)
+{
+    if (v.size() < 1) throw std::runtime_error("get_2d_array v too small");
+
+    npy_intp vsize[2] = {static_cast<npy_intp>(v.size()),
+                         static_cast<npy_intp>(v[0].size())};
+
+    PyArrayObject *varray =
+        (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE);
+
+    double *vd_begin = static_cast<double *>(PyArray_DATA(varray));
+
+    for (const ::std::vector<Numeric> &v_row : v) {
+      if (v_row.size() != static_cast<size_t>(vsize[1]))
+        throw std::runtime_error("Missmatched array size");
+      std::copy(v_row.begin(), v_row.end(), vd_begin);
+      vd_begin += vsize[1];
+    }
+
+    return reinterpret_cast<PyObject *>(varray);
+}
+
+#else // fallback if we don't have numpy: copy every element of the given vector
+
+template<typename Numeric>
+PyObject* get_array(const std::vector<Numeric>& v)
+{
+    PyObject* list = PyList_New(v.size());
+    for(size_t i = 0; i < v.size(); ++i) {
+        PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i)));
+    }
+    return list;
+}
+
+#endif // WITHOUT_NUMPY
+
+// sometimes, for labels and such, we need string arrays
+inline PyObject * get_array(const std::vector<std::string>& strings)
+{
+  PyObject* list = PyList_New(strings.size());
+  for (std::size_t i = 0; i < strings.size(); ++i) {
+    PyList_SetItem(list, i, PyString_FromString(strings[i].c_str()));
+  }
+  return list;
+}
+
+// not all matplotlib need 2d arrays, some prefer lists of lists
+template<typename Numeric>
+PyObject* get_listlist(const std::vector<std::vector<Numeric>>& ll)
+{
+  PyObject* listlist = PyList_New(ll.size());
+  for (std::size_t i = 0; i < ll.size(); ++i) {
+    PyList_SetItem(listlist, i, get_array(ll[i]));
+  }
+  return listlist;
+}
+
+} // namespace detail
+
+/// Plot a line through the given x and y data points..
+/// 
+/// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html
+template<typename Numeric>
+bool plot(const std::vector<Numeric> &x, const std::vector<Numeric> &y, const std::map<std::string, std::string>& keywords)
+{
+    assert(x.size() == y.size());
+
+    detail::_interpreter::get();
+
+    // using numpy arrays
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    // construct positional args
+    PyObject* args = PyTuple_New(2);
+    PyTuple_SetItem(args, 0, xarray);
+    PyTuple_SetItem(args, 1, yarray);
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+    {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs);
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+// TODO - it should be possible to make this work by implementing
+// a non-numpy alternative for `detail::get_2darray()`.
+#ifndef WITHOUT_NUMPY
+template <typename Numeric>
+void plot_surface(const std::vector<::std::vector<Numeric>> &x,
+                  const std::vector<::std::vector<Numeric>> &y,
+                  const std::vector<::std::vector<Numeric>> &z,
+                  const std::map<std::string, std::string> &keywords =
+                      std::map<std::string, std::string>())
+{
+  detail::_interpreter::get();
+
+  // We lazily load the modules here the first time this function is called
+  // because I'm not sure that we can assume "matplotlib installed" implies
+  // "mpl_toolkits installed" on all platforms, and we don't want to require
+  // it for people who don't need 3d plots.
+  static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr;
+  if (!mpl_toolkitsmod) {
+    detail::_interpreter::get();
+
+    PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits");
+    PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d");
+    if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); }
+
+    mpl_toolkitsmod = PyImport_Import(mpl_toolkits);
+    Py_DECREF(mpl_toolkits);
+    if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); }
+
+    axis3dmod = PyImport_Import(axis3d);
+    Py_DECREF(axis3d);
+    if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); }
+  }
+
+  assert(x.size() == y.size());
+  assert(y.size() == z.size());
+
+  // using numpy arrays
+  PyObject *xarray = detail::get_2darray(x);
+  PyObject *yarray = detail::get_2darray(y);
+  PyObject *zarray = detail::get_2darray(z);
+
+  // construct positional args
+  PyObject *args = PyTuple_New(3);
+  PyTuple_SetItem(args, 0, xarray);
+  PyTuple_SetItem(args, 1, yarray);
+  PyTuple_SetItem(args, 2, zarray);
+
+  // Build up the kw args.
+  PyObject *kwargs = PyDict_New();
+  PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1));
+  PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1));
+
+  PyObject *python_colormap_coolwarm = PyObject_GetAttrString(
+      detail::_interpreter::get().s_python_colormap, "coolwarm");
+
+  PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm);
+
+  for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
+       it != keywords.end(); ++it) {
+    PyDict_SetItemString(kwargs, it->first.c_str(),
+                         PyString_FromString(it->second.c_str()));
+  }
+
+
+  PyObject *fig =
+      PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,
+                          detail::_interpreter::get().s_python_empty_tuple);
+  if (!fig) throw std::runtime_error("Call to figure() failed.");
+
+  PyObject *gca_kwargs = PyDict_New();
+  PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d"));
+
+  PyObject *gca = PyObject_GetAttrString(fig, "gca");
+  if (!gca) throw std::runtime_error("No gca");
+  Py_INCREF(gca);
+  PyObject *axis = PyObject_Call(
+      gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs);
+
+  if (!axis) throw std::runtime_error("No axis");
+  Py_INCREF(axis);
+
+  Py_DECREF(gca);
+  Py_DECREF(gca_kwargs);
+
+  PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface");
+  if (!plot_surface) throw std::runtime_error("No surface");
+  Py_INCREF(plot_surface);
+  PyObject *res = PyObject_Call(plot_surface, args, kwargs);
+  if (!res) throw std::runtime_error("failed surface");
+  Py_DECREF(plot_surface);
+
+  Py_DECREF(axis);
+  Py_DECREF(args);
+  Py_DECREF(kwargs);
+  if (res) Py_DECREF(res);
+}
+#endif // WITHOUT_NUMPY
+
+template <typename Numeric>
+void plot3(const std::vector<Numeric> &x,
+                  const std::vector<Numeric> &y,
+                  const std::vector<Numeric> &z,
+                  const std::map<std::string, std::string> &keywords =
+                      std::map<std::string, std::string>())
+{
+  detail::_interpreter::get();
+
+  // Same as with plot_surface: We lazily load the modules here the first time 
+  // this function is called because I'm not sure that we can assume "matplotlib 
+  // installed" implies "mpl_toolkits installed" on all platforms, and we don't 
+  // want to require it for people who don't need 3d plots.
+  static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr;
+  if (!mpl_toolkitsmod) {
+    detail::_interpreter::get();
+
+    PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits");
+    PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d");
+    if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); }
+
+    mpl_toolkitsmod = PyImport_Import(mpl_toolkits);
+    Py_DECREF(mpl_toolkits);
+    if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); }
+
+    axis3dmod = PyImport_Import(axis3d);
+    Py_DECREF(axis3d);
+    if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); }
+  }
+
+  assert(x.size() == y.size());
+  assert(y.size() == z.size());
+
+  PyObject *xarray = detail::get_array(x);
+  PyObject *yarray = detail::get_array(y);
+  PyObject *zarray = detail::get_array(z);
+
+  // construct positional args
+  PyObject *args = PyTuple_New(3);
+  PyTuple_SetItem(args, 0, xarray);
+  PyTuple_SetItem(args, 1, yarray);
+  PyTuple_SetItem(args, 2, zarray);
+
+  // Build up the kw args.
+  PyObject *kwargs = PyDict_New();
+
+  for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
+       it != keywords.end(); ++it) {
+    PyDict_SetItemString(kwargs, it->first.c_str(),
+                         PyString_FromString(it->second.c_str()));
+  }
+
+  PyObject *fig =
+      PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,
+                          detail::_interpreter::get().s_python_empty_tuple);
+  if (!fig) throw std::runtime_error("Call to figure() failed.");
+
+  PyObject *gca_kwargs = PyDict_New();
+  PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d"));
+
+  PyObject *gca = PyObject_GetAttrString(fig, "gca");
+  if (!gca) throw std::runtime_error("No gca");
+  Py_INCREF(gca);
+  PyObject *axis = PyObject_Call(
+      gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs);
+
+  if (!axis) throw std::runtime_error("No axis");
+  Py_INCREF(axis);
+
+  Py_DECREF(gca);
+  Py_DECREF(gca_kwargs);
+
+  PyObject *plot3 = PyObject_GetAttrString(axis, "plot");
+  if (!plot3) throw std::runtime_error("No 3D line plot");
+  Py_INCREF(plot3);
+  PyObject *res = PyObject_Call(plot3, args, kwargs);
+  if (!res) throw std::runtime_error("Failed 3D line plot");
+  Py_DECREF(plot3);
+
+  Py_DECREF(axis);
+  Py_DECREF(args);
+  Py_DECREF(kwargs);
+  if (res) Py_DECREF(res);
+}
+
+template<typename Numeric>
+bool stem(const std::vector<Numeric> &x, const std::vector<Numeric> &y, const std::map<std::string, std::string>& keywords)
+{
+    assert(x.size() == y.size());
+
+    detail::_interpreter::get();
+
+    // using numpy arrays
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    // construct positional args
+    PyObject* args = PyTuple_New(2);
+    PyTuple_SetItem(args, 0, xarray);
+    PyTuple_SetItem(args, 1, yarray);
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for (std::map<std::string, std::string>::const_iterator it =
+            keywords.begin(); it != keywords.end(); ++it) {
+        PyDict_SetItemString(kwargs, it->first.c_str(),
+                PyString_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(
+            detail::_interpreter::get().s_python_function_stem, args, kwargs);
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    if (res)
+        Py_DECREF(res);
+
+    return res;
+}
+
+template< typename Numeric >
+bool fill(const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::map<std::string, std::string>& keywords)
+{
+    assert(x.size() == y.size());
+
+    detail::_interpreter::get();
+
+    // using numpy arrays
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    // construct positional args
+    PyObject* args = PyTuple_New(2);
+    PyTuple_SetItem(args, 0, xarray);
+    PyTuple_SetItem(args, 1, yarray);
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for (auto it = keywords.begin(); it != keywords.end(); ++it) {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs);
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+
+    if (res) Py_DECREF(res);
+
+    return res;
+}
+
+template< typename Numeric >
+bool fill_between(const std::vector<Numeric>& x, const std::vector<Numeric>& y1, const std::vector<Numeric>& y2, const std::map<std::string, std::string>& keywords)
+{
+    assert(x.size() == y1.size());
+    assert(x.size() == y2.size());
+
+    detail::_interpreter::get();
+
+    // using numpy arrays
+    PyObject* xarray = detail::get_array(x);
+    PyObject* y1array = detail::get_array(y1);
+    PyObject* y2array = detail::get_array(y2);
+
+    // construct positional args
+    PyObject* args = PyTuple_New(3);
+    PyTuple_SetItem(args, 0, xarray);
+    PyTuple_SetItem(args, 1, y1array);
+    PyTuple_SetItem(args, 2, y2array);
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it) {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs);
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+template <typename Numeric>
+bool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = "r",
+           const std::string ec = "k", Numeric head_length = 0.25, Numeric head_width = 0.1625) {
+    PyObject* obj_x = PyFloat_FromDouble(x);
+    PyObject* obj_y = PyFloat_FromDouble(y);
+    PyObject* obj_end_x = PyFloat_FromDouble(end_x);
+    PyObject* obj_end_y = PyFloat_FromDouble(end_y);
+
+    PyObject* kwargs = PyDict_New();
+    PyDict_SetItemString(kwargs, "fc", PyString_FromString(fc.c_str()));
+    PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str()));
+    PyDict_SetItemString(kwargs, "head_width", PyFloat_FromDouble(head_width));
+    PyDict_SetItemString(kwargs, "head_length", PyFloat_FromDouble(head_length));
+
+    PyObject* plot_args = PyTuple_New(4);
+    PyTuple_SetItem(plot_args, 0, obj_x);
+    PyTuple_SetItem(plot_args, 1, obj_y);
+    PyTuple_SetItem(plot_args, 2, obj_end_x);
+    PyTuple_SetItem(plot_args, 3, obj_end_y);
+
+    PyObject* res =
+            PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs);
+
+    Py_DECREF(plot_args);
+    Py_DECREF(kwargs);
+    if (res)
+        Py_DECREF(res);
+
+    return res;
+}
+
+template< typename Numeric>
+bool hist(const std::vector<Numeric>& y, long bins=10,std::string color="b",
+          double alpha=1.0, bool cumulative=false)
+{
+    detail::_interpreter::get();
+
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* kwargs = PyDict_New();
+    PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins));
+    PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str()));
+    PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha));
+    PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False);
+
+    PyObject* plot_args = PyTuple_New(1);
+
+    PyTuple_SetItem(plot_args, 0, yarray);
+
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs);
+
+
+    Py_DECREF(plot_args);
+    Py_DECREF(kwargs);
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+#ifndef WITHOUT_NUMPY
+namespace detail {
+
+inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map<std::string, std::string> &keywords, PyObject** out)
+{
+    assert(type == NPY_UINT8 || type == NPY_FLOAT);
+    assert(colors == 1 || colors == 3 || colors == 4);
+
+    detail::_interpreter::get();
+
+    // construct args
+    npy_intp dims[3] = { rows, columns, colors };
+    PyObject *args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr));
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+    {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+    }
+
+    PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs);
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    if (!res)
+        throw std::runtime_error("Call to imshow() failed");
+    if (out)
+        *out = res;
+    else
+        Py_DECREF(res);
+}
+
+} // namespace detail
+
+inline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map<std::string, std::string> &keywords = {}, PyObject** out = nullptr)
+{
+    detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out);
+}
+
+inline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map<std::string, std::string> &keywords = {}, PyObject** out = nullptr)
+{
+    detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out);
+}
+
+#ifdef WITH_OPENCV
+void imshow(const cv::Mat &image, const std::map<std::string, std::string> &keywords = {})
+{
+    // Convert underlying type of matrix, if needed
+    cv::Mat image2;
+    NPY_TYPES npy_type = NPY_UINT8;
+    switch (image.type() & CV_MAT_DEPTH_MASK) {
+    case CV_8U:
+        image2 = image;
+        break;
+    case CV_32F:
+        image2 = image;
+        npy_type = NPY_FLOAT;
+        break;
+    default:
+        image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels()));
+    }
+
+    // If color image, convert from BGR to RGB
+    switch (image2.channels()) {
+    case 3:
+        cv::cvtColor(image2, image2, CV_BGR2RGB);
+        break;
+    case 4:
+        cv::cvtColor(image2, image2, CV_BGRA2RGBA);
+    }
+
+    detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords);
+}
+#endif // WITH_OPENCV
+#endif // WITHOUT_NUMPY
+
+template<typename NumericX, typename NumericY>
+bool scatter(const std::vector<NumericX>& x,
+             const std::vector<NumericY>& y,
+             const double s=1.0, // The marker size in points**2
+             const std::map<std::string, std::string> & keywords = {})
+{
+    detail::_interpreter::get();
+
+    assert(x.size() == y.size());
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* kwargs = PyDict_New();
+    PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s));
+    for (const auto& it : keywords)
+    {
+        PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str()));
+    }
+
+    PyObject* plot_args = PyTuple_New(2);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs);
+
+    Py_DECREF(plot_args);
+    Py_DECREF(kwargs);
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+template<typename Numeric>
+bool boxplot(const std::vector<std::vector<Numeric>>& data,
+             const std::vector<std::string>& labels = {},
+             const std::map<std::string, std::string> & keywords = {})
+{
+    detail::_interpreter::get();
+
+    PyObject* listlist = detail::get_listlist(data);
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, listlist);
+
+    PyObject* kwargs = PyDict_New();
+
+    // kwargs needs the labels, if there are (the correct number of) labels
+    if (!labels.empty() && labels.size() == data.size()) {
+        PyDict_SetItemString(kwargs, "labels", detail::get_array(labels));
+    }
+
+    // take care of the remaining keywords
+    for (const auto& it : keywords)
+    {
+        PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs);
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+template<typename Numeric>
+bool boxplot(const std::vector<Numeric>& data,
+             const std::map<std::string, std::string> & keywords = {})
+{
+    detail::_interpreter::get();
+
+    PyObject* vector = detail::get_array(data);
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, vector);
+
+    PyObject* kwargs = PyDict_New();
+    for (const auto& it : keywords)
+    {
+        PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs);
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+template <typename Numeric>
+bool bar(const std::vector<Numeric> &               x,
+         const std::vector<Numeric> &               y,
+         std::string                                ec       = "black",
+         std::string                                ls       = "-",
+         double                                     lw       = 1.0,
+         const std::map<std::string, std::string> & keywords = {})
+{
+  detail::_interpreter::get();
+
+  PyObject * xarray = detail::get_array(x);
+  PyObject * yarray = detail::get_array(y);
+
+  PyObject * kwargs = PyDict_New();
+
+  PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str()));
+  PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str()));
+  PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw));
+
+  for (std::map<std::string, std::string>::const_iterator it =
+         keywords.begin();
+       it != keywords.end();
+       ++it) {
+    PyDict_SetItemString(
+      kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+  }
+
+  PyObject * plot_args = PyTuple_New(2);
+  PyTuple_SetItem(plot_args, 0, xarray);
+  PyTuple_SetItem(plot_args, 1, yarray);
+
+  PyObject * res = PyObject_Call(
+    detail::_interpreter::get().s_python_function_bar, plot_args, kwargs);
+
+  Py_DECREF(plot_args);
+  Py_DECREF(kwargs);
+  if (res) Py_DECREF(res);
+
+  return res;
+}
+
+template <typename Numeric>
+bool bar(const std::vector<Numeric> &               y,
+         std::string                                ec       = "black",
+         std::string                                ls       = "-",
+         double                                     lw       = 1.0,
+         const std::map<std::string, std::string> & keywords = {})
+{
+  using T = typename std::remove_reference<decltype(y)>::type::value_type;
+
+  detail::_interpreter::get();
+
+  std::vector<T> x;
+  for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); }
+
+  return bar(x, y, ec, ls, lw, keywords);
+}
+
+inline bool subplots_adjust(const std::map<std::string, double>& keywords = {})
+{
+    detail::_interpreter::get();
+
+    PyObject* kwargs = PyDict_New();
+    for (std::map<std::string, double>::const_iterator it =
+            keywords.begin(); it != keywords.end(); ++it) {
+        PyDict_SetItemString(kwargs, it->first.c_str(),
+                             PyFloat_FromDouble(it->second));
+    }
+
+
+    PyObject* plot_args = PyTuple_New(0);
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs);
+
+    Py_DECREF(plot_args);
+    Py_DECREF(kwargs);
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+template< typename Numeric>
+bool named_hist(std::string label,const std::vector<Numeric>& y, long bins=10, std::string color="b", double alpha=1.0)
+{
+    detail::_interpreter::get();
+
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* kwargs = PyDict_New();
+    PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str()));
+    PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins));
+    PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str()));
+    PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha));
+
+
+    PyObject* plot_args = PyTuple_New(1);
+    PyTuple_SetItem(plot_args, 0, yarray);
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs);
+
+    Py_DECREF(plot_args);
+    Py_DECREF(kwargs);
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+template<typename NumericX, typename NumericY>
+bool plot(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "")
+{
+    assert(x.size() == y.size());
+
+    detail::_interpreter::get();
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* pystring = PyString_FromString(s.c_str());
+
+    PyObject* plot_args = PyTuple_New(3);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, pystring);
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args);
+
+    Py_DECREF(plot_args);
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+template <typename NumericX, typename NumericY, typename NumericZ>
+bool contour(const std::vector<NumericX>& x, const std::vector<NumericY>& y,
+             const std::vector<NumericZ>& z,
+             const std::map<std::string, std::string>& keywords = {}) {
+    assert(x.size() == y.size() && x.size() == z.size());
+
+    PyObject* xarray = get_array(x);
+    PyObject* yarray = get_array(y);
+    PyObject* zarray = get_array(z);
+
+    PyObject* plot_args = PyTuple_New(3);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, zarray);
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
+         it != keywords.end(); ++it) {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+    }
+
+    PyObject* res =
+            PyObject_Call(detail::_interpreter::get().s_python_function_contour, plot_args, kwargs);
+
+    Py_DECREF(kwargs);
+    Py_DECREF(plot_args);
+    if (res)
+        Py_DECREF(res);
+
+    return res;
+}
+
+template<typename NumericX, typename NumericY, typename NumericU, typename NumericW>
+bool quiver(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::vector<NumericU>& u, const std::vector<NumericW>& w, const std::map<std::string, std::string>& keywords = {})
+{
+    assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size());
+
+    detail::_interpreter::get();
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+    PyObject* uarray = detail::get_array(u);
+    PyObject* warray = detail::get_array(w);
+
+    PyObject* plot_args = PyTuple_New(4);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, uarray);
+    PyTuple_SetItem(plot_args, 3, warray);
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+    {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(
+            detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs);
+
+    Py_DECREF(kwargs);
+    Py_DECREF(plot_args);
+    if (res)
+        Py_DECREF(res);
+
+    return res;
+}
+
+template<typename NumericX, typename NumericY>
+bool stem(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "")
+{
+    assert(x.size() == y.size());
+
+    detail::_interpreter::get();
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* pystring = PyString_FromString(s.c_str());
+
+    PyObject* plot_args = PyTuple_New(3);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, pystring);
+
+    PyObject* res = PyObject_CallObject(
+            detail::_interpreter::get().s_python_function_stem, plot_args);
+
+    Py_DECREF(plot_args);
+    if (res)
+        Py_DECREF(res);
+
+    return res;
+}
+
+template<typename NumericX, typename NumericY>
+bool semilogx(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "")
+{
+    assert(x.size() == y.size());
+
+    detail::_interpreter::get();
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* pystring = PyString_FromString(s.c_str());
+
+    PyObject* plot_args = PyTuple_New(3);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, pystring);
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args);
+
+    Py_DECREF(plot_args);
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+template<typename NumericX, typename NumericY>
+bool semilogy(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "")
+{
+    assert(x.size() == y.size());
+
+    detail::_interpreter::get();
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* pystring = PyString_FromString(s.c_str());
+
+    PyObject* plot_args = PyTuple_New(3);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, pystring);
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args);
+
+    Py_DECREF(plot_args);
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+template<typename NumericX, typename NumericY>
+bool loglog(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "")
+{
+    assert(x.size() == y.size());
+
+    detail::_interpreter::get();
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* pystring = PyString_FromString(s.c_str());
+
+    PyObject* plot_args = PyTuple_New(3);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, pystring);
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args);
+
+    Py_DECREF(plot_args);
+    if(res) Py_DECREF(res);
+
+    return res;
+}
+
+template<typename NumericX, typename NumericY>
+bool errorbar(const std::vector<NumericX> &x, const std::vector<NumericY> &y, const std::vector<NumericX> &yerr, const std::map<std::string, std::string> &keywords = {})
+{
+    assert(x.size() == y.size());
+
+    detail::_interpreter::get();
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+    PyObject* yerrarray = detail::get_array(yerr);
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+    {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
+    }
+
+    PyDict_SetItemString(kwargs, "yerr", yerrarray);
+
+    PyObject *plot_args = PyTuple_New(2);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+
+    PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs);
+
+    Py_DECREF(kwargs);
+    Py_DECREF(plot_args);
+
+    if (res)
+        Py_DECREF(res);
+    else
+        throw std::runtime_error("Call to errorbar() failed.");
+
+    return res;
+}
+
+template<typename Numeric>
+bool named_plot(const std::string& name, const std::vector<Numeric>& y, const std::string& format = "")
+{
+    detail::_interpreter::get();
+
+    PyObject* kwargs = PyDict_New();
+    PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
+
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* pystring = PyString_FromString(format.c_str());
+
+    PyObject* plot_args = PyTuple_New(2);
+
+    PyTuple_SetItem(plot_args, 0, yarray);
+    PyTuple_SetItem(plot_args, 1, pystring);
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs);
+
+    Py_DECREF(kwargs);
+    Py_DECREF(plot_args);
+    if (res) Py_DECREF(res);
+
+    return res;
+}
+
+template<typename Numeric>
+bool named_plot(const std::string& name, const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::string& format = "")
+{
+    detail::_interpreter::get();
+
+    PyObject* kwargs = PyDict_New();
+    PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* pystring = PyString_FromString(format.c_str());
+
+    PyObject* plot_args = PyTuple_New(3);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, pystring);
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs);
+
+    Py_DECREF(kwargs);
+    Py_DECREF(plot_args);
+    if (res) Py_DECREF(res);
+
+    return res;
+}
+
+template<typename Numeric>
+bool named_semilogx(const std::string& name, const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::string& format = "")
+{
+    detail::_interpreter::get();
+
+    PyObject* kwargs = PyDict_New();
+    PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* pystring = PyString_FromString(format.c_str());
+
+    PyObject* plot_args = PyTuple_New(3);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, pystring);
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs);
+
+    Py_DECREF(kwargs);
+    Py_DECREF(plot_args);
+    if (res) Py_DECREF(res);
+
+    return res;
+}
+
+template<typename Numeric>
+bool named_semilogy(const std::string& name, const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::string& format = "")
+{
+    detail::_interpreter::get();
+
+    PyObject* kwargs = PyDict_New();
+    PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* pystring = PyString_FromString(format.c_str());
+
+    PyObject* plot_args = PyTuple_New(3);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, pystring);
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs);
+
+    Py_DECREF(kwargs);
+    Py_DECREF(plot_args);
+    if (res) Py_DECREF(res);
+
+    return res;
+}
+
+template<typename Numeric>
+bool named_loglog(const std::string& name, const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::string& format = "")
+{
+    detail::_interpreter::get();
+
+    PyObject* kwargs = PyDict_New();
+    PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
+
+    PyObject* xarray = detail::get_array(x);
+    PyObject* yarray = detail::get_array(y);
+
+    PyObject* pystring = PyString_FromString(format.c_str());
+
+    PyObject* plot_args = PyTuple_New(3);
+    PyTuple_SetItem(plot_args, 0, xarray);
+    PyTuple_SetItem(plot_args, 1, yarray);
+    PyTuple_SetItem(plot_args, 2, pystring);
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs);
+
+    Py_DECREF(kwargs);
+    Py_DECREF(plot_args);
+    if (res) Py_DECREF(res);
+
+    return res;
+}
+
+template<typename Numeric>
+bool plot(const std::vector<Numeric>& y, const std::string& format = "")
+{
+    std::vector<Numeric> x(y.size());
+    for(size_t i=0; i<x.size(); ++i) x.at(i) = i;
+    return plot(x,y,format);
+}
+
+template<typename Numeric>
+bool plot(const std::vector<Numeric>& y, const std::map<std::string, std::string>& keywords)
+{
+    std::vector<Numeric> x(y.size());
+    for(size_t i=0; i<x.size(); ++i) x.at(i) = i;
+    return plot(x,y,keywords);
+}
+
+template<typename Numeric>
+bool stem(const std::vector<Numeric>& y, const std::string& format = "")
+{
+    std::vector<Numeric> x(y.size());
+    for (size_t i = 0; i < x.size(); ++i) x.at(i) = i;
+    return stem(x, y, format);
+}
+
+template<typename Numeric>
+void text(Numeric x, Numeric y, const std::string& s = "")
+{
+    detail::_interpreter::get();
+
+    PyObject* args = PyTuple_New(3);
+    PyTuple_SetItem(args, 0, PyFloat_FromDouble(x));
+    PyTuple_SetItem(args, 1, PyFloat_FromDouble(y));
+    PyTuple_SetItem(args, 2, PyString_FromString(s.c_str()));
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args);
+    if(!res) throw std::runtime_error("Call to text() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(res);
+}
+
+inline void colorbar(PyObject* mappable = NULL, const std::map<std::string, float>& keywords = {})
+{
+    if (mappable == NULL)
+        throw std::runtime_error("Must call colorbar with PyObject* returned from an image, contour, surface, etc.");
+
+    detail::_interpreter::get();
+
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, mappable);
+
+    PyObject* kwargs = PyDict_New();
+    for(std::map<std::string, float>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+    {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs);
+    if(!res) throw std::runtime_error("Call to colorbar() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    Py_DECREF(res);
+}
+
+
+inline long figure(long number = -1)
+{
+    detail::_interpreter::get();
+
+    PyObject *res;
+    if (number == -1)
+        res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple);
+    else {
+        assert(number > 0);
+
+        // Make sure interpreter is initialised
+        detail::_interpreter::get();
+
+        PyObject *args = PyTuple_New(1);
+        PyTuple_SetItem(args, 0, PyLong_FromLong(number));
+        res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args);
+        Py_DECREF(args);
+    }
+
+    if(!res) throw std::runtime_error("Call to figure() failed.");
+
+    PyObject* num = PyObject_GetAttrString(res, "number");
+    if (!num) throw std::runtime_error("Could not get number attribute of figure object");
+    const long figureNumber = PyLong_AsLong(num);
+
+    Py_DECREF(num);
+    Py_DECREF(res);
+
+    return figureNumber;
+}
+
+inline bool fignum_exists(long number)
+{
+    detail::_interpreter::get();
+
+    PyObject *args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, PyLong_FromLong(number));
+    PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args);
+    if(!res) throw std::runtime_error("Call to fignum_exists() failed.");
+
+    bool ret = PyObject_IsTrue(res);
+    Py_DECREF(res);
+    Py_DECREF(args);
+
+    return ret;
+}
+
+inline void figure_size(size_t w, size_t h)
+{
+    detail::_interpreter::get();
+
+    const size_t dpi = 100;
+    PyObject* size = PyTuple_New(2);
+    PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi));
+    PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi));
+
+    PyObject* kwargs = PyDict_New();
+    PyDict_SetItemString(kwargs, "figsize", size);
+    PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi));
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure,
+            detail::_interpreter::get().s_python_empty_tuple, kwargs);
+
+    Py_DECREF(kwargs);
+
+    if(!res) throw std::runtime_error("Call to figure_size() failed.");
+    Py_DECREF(res);
+}
+
+inline void legend()
+{
+    detail::_interpreter::get();
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple);
+    if(!res) throw std::runtime_error("Call to legend() failed.");
+
+    Py_DECREF(res);
+}
+
+inline void legend(const std::map<std::string, std::string>& keywords)
+{
+  detail::_interpreter::get();
+
+  // construct keyword args
+  PyObject* kwargs = PyDict_New();
+  for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+  {
+    PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
+  }
+
+  PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple, kwargs);
+  if(!res) throw std::runtime_error("Call to legend() failed.");
+
+  Py_DECREF(kwargs);
+  Py_DECREF(res);  
+}
+
+template<typename Numeric>
+void ylim(Numeric left, Numeric right)
+{
+    detail::_interpreter::get();
+
+    PyObject* list = PyList_New(2);
+    PyList_SetItem(list, 0, PyFloat_FromDouble(left));
+    PyList_SetItem(list, 1, PyFloat_FromDouble(right));
+
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, list);
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args);
+    if(!res) throw std::runtime_error("Call to ylim() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(res);
+}
+
+template<typename Numeric>
+void xlim(Numeric left, Numeric right)
+{
+    detail::_interpreter::get();
+
+    PyObject* list = PyList_New(2);
+    PyList_SetItem(list, 0, PyFloat_FromDouble(left));
+    PyList_SetItem(list, 1, PyFloat_FromDouble(right));
+
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, list);
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args);
+    if(!res) throw std::runtime_error("Call to xlim() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(res);
+}
+
+
+inline double* xlim()
+{
+    detail::_interpreter::get();
+
+    PyObject* args = PyTuple_New(0);
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args);
+    PyObject* left = PyTuple_GetItem(res,0);
+    PyObject* right = PyTuple_GetItem(res,1);
+
+    double* arr = new double[2];
+    arr[0] = PyFloat_AsDouble(left);
+    arr[1] = PyFloat_AsDouble(right);
+
+    if(!res) throw std::runtime_error("Call to xlim() failed.");
+
+    Py_DECREF(res);
+    return arr;
+}
+
+
+inline double* ylim()
+{
+    detail::_interpreter::get();
+
+    PyObject* args = PyTuple_New(0);
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args);
+    PyObject* left = PyTuple_GetItem(res,0);
+    PyObject* right = PyTuple_GetItem(res,1);
+
+    double* arr = new double[2];
+    arr[0] = PyFloat_AsDouble(left);
+    arr[1] = PyFloat_AsDouble(right);
+
+    if(!res) throw std::runtime_error("Call to ylim() failed.");
+
+    Py_DECREF(res);
+    return arr;
+}
+
+template<typename Numeric>
+inline void xticks(const std::vector<Numeric> &ticks, const std::vector<std::string> &labels = {}, const std::map<std::string, std::string>& keywords = {})
+{
+    assert(labels.size() == 0 || ticks.size() == labels.size());
+
+    detail::_interpreter::get();
+
+    // using numpy array
+    PyObject* ticksarray = detail::get_array(ticks);
+
+    PyObject* args;
+    if(labels.size() == 0) {
+        // construct positional args
+        args = PyTuple_New(1);
+        PyTuple_SetItem(args, 0, ticksarray);
+    } else {
+        // make tuple of tick labels
+        PyObject* labelstuple = PyTuple_New(labels.size());
+        for (size_t i = 0; i < labels.size(); i++)
+            PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str()));
+
+        // construct positional args
+        args = PyTuple_New(2);
+        PyTuple_SetItem(args, 0, ticksarray);
+        PyTuple_SetItem(args, 1, labelstuple);
+    }
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+    {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs);
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    if(!res) throw std::runtime_error("Call to xticks() failed");
+
+    Py_DECREF(res);
+}
+
+template<typename Numeric>
+inline void xticks(const std::vector<Numeric> &ticks, const std::map<std::string, std::string>& keywords)
+{
+    xticks(ticks, {}, keywords);
+}
+
+template<typename Numeric>
+inline void yticks(const std::vector<Numeric> &ticks, const std::vector<std::string> &labels = {}, const std::map<std::string, std::string>& keywords = {})
+{
+    assert(labels.size() == 0 || ticks.size() == labels.size());
+
+    detail::_interpreter::get();
+
+    // using numpy array
+    PyObject* ticksarray = detail::get_array(ticks);
+
+    PyObject* args;
+    if(labels.size() == 0) {
+        // construct positional args
+        args = PyTuple_New(1);
+        PyTuple_SetItem(args, 0, ticksarray);
+    } else {
+        // make tuple of tick labels
+        PyObject* labelstuple = PyTuple_New(labels.size());
+        for (size_t i = 0; i < labels.size(); i++)
+            PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str()));
+
+        // construct positional args
+        args = PyTuple_New(2);
+        PyTuple_SetItem(args, 0, ticksarray);
+        PyTuple_SetItem(args, 1, labelstuple);
+    }
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+    {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs);
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    if(!res) throw std::runtime_error("Call to yticks() failed");
+
+    Py_DECREF(res);
+}
+
+template<typename Numeric>
+inline void yticks(const std::vector<Numeric> &ticks, const std::map<std::string, std::string>& keywords)
+{
+    yticks(ticks, {}, keywords);
+}
+
+template <typename Numeric> inline void margins(Numeric margin)
+{
+    // construct positional args
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin));
+
+    PyObject* res =
+            PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args);
+    if (!res)
+        throw std::runtime_error("Call to margins() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(res);
+}
+
+template <typename Numeric> inline void margins(Numeric margin_x, Numeric margin_y)
+{
+    // construct positional args
+    PyObject* args = PyTuple_New(2);
+    PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin_x));
+    PyTuple_SetItem(args, 1, PyFloat_FromDouble(margin_y));
+
+    PyObject* res =
+            PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args);
+    if (!res)
+        throw std::runtime_error("Call to margins() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(res);
+}
+
+
+inline void tick_params(const std::map<std::string, std::string>& keywords, const std::string axis = "both")
+{
+  detail::_interpreter::get();
+
+  // construct positional args
+  PyObject* args;
+  args = PyTuple_New(1);
+  PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str()));
+
+  // construct keyword args
+  PyObject* kwargs = PyDict_New();
+  for (std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+  {
+    PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
+  }
+
+
+  PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs);
+
+  Py_DECREF(args);
+  Py_DECREF(kwargs);
+  if (!res) throw std::runtime_error("Call to tick_params() failed");
+
+  Py_DECREF(res);
+}
+
+inline void subplot(long nrows, long ncols, long plot_number)
+{
+    detail::_interpreter::get();
+    
+    // construct positional args
+    PyObject* args = PyTuple_New(3);
+    PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows));
+    PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols));
+    PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number));
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args);
+    if(!res) throw std::runtime_error("Call to subplot() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(res);
+}
+
+inline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1)
+{
+    detail::_interpreter::get();
+
+    PyObject* shape = PyTuple_New(2);
+    PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows));
+    PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols));
+
+    PyObject* loc = PyTuple_New(2);
+    PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid));
+    PyTuple_SetItem(loc, 1, PyLong_FromLong(colid));
+
+    PyObject* args = PyTuple_New(4);
+    PyTuple_SetItem(args, 0, shape);
+    PyTuple_SetItem(args, 1, loc);
+    PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan));
+    PyTuple_SetItem(args, 3, PyLong_FromLong(colspan));
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args);
+    if(!res) throw std::runtime_error("Call to subplot2grid() failed.");
+
+    Py_DECREF(shape);
+    Py_DECREF(loc);
+    Py_DECREF(args);
+    Py_DECREF(res);
+}
+
+inline void title(const std::string &titlestr, const std::map<std::string, std::string> &keywords = {})
+{
+    detail::_interpreter::get();
+
+    PyObject* pytitlestr = PyString_FromString(titlestr.c_str());
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, pytitlestr);
+
+    PyObject* kwargs = PyDict_New();
+    for (auto it = keywords.begin(); it != keywords.end(); ++it) {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs);
+    if(!res) throw std::runtime_error("Call to title() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    Py_DECREF(res);
+}
+
+inline void suptitle(const std::string &suptitlestr, const std::map<std::string, std::string> &keywords = {})
+{
+    detail::_interpreter::get();
+    
+    PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str());
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, pysuptitlestr);
+
+    PyObject* kwargs = PyDict_New();
+    for (auto it = keywords.begin(); it != keywords.end(); ++it) {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs);
+    if(!res) throw std::runtime_error("Call to suptitle() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    Py_DECREF(res);
+}
+
+inline void axis(const std::string &axisstr)
+{
+    detail::_interpreter::get();
+
+    PyObject* str = PyString_FromString(axisstr.c_str());
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, str);
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args);
+    if(!res) throw std::runtime_error("Call to title() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(res);
+}
+
+inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map<std::string, std::string>& keywords = std::map<std::string, std::string>())
+{
+    detail::_interpreter::get();
+
+    // construct positional args
+    PyObject* args = PyTuple_New(3);
+    PyTuple_SetItem(args, 0, PyFloat_FromDouble(x));
+    PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin));
+    PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax));
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+    {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs);
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+
+    if(res) Py_DECREF(res);
+}
+
+inline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1., const std::map<std::string, std::string>& keywords = std::map<std::string, std::string>())
+{
+    // construct positional args
+    PyObject* args = PyTuple_New(4);
+    PyTuple_SetItem(args, 0, PyFloat_FromDouble(xmin));
+    PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmax));
+    PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymin));
+    PyTuple_SetItem(args, 3, PyFloat_FromDouble(ymax));
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+    {
+	if (it->first == "linewidth" || it->first == "alpha")
+    	    PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(std::stod(it->second)));
+  	else
+    	    PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs);
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+
+    if(res) Py_DECREF(res);
+}
+
+inline void xlabel(const std::string &str, const std::map<std::string, std::string> &keywords = {})
+{
+    detail::_interpreter::get();
+
+    PyObject* pystr = PyString_FromString(str.c_str());
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, pystr);
+
+    PyObject* kwargs = PyDict_New();
+    for (auto it = keywords.begin(); it != keywords.end(); ++it) {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs);
+    if(!res) throw std::runtime_error("Call to xlabel() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    Py_DECREF(res);
+}
+
+inline void ylabel(const std::string &str, const std::map<std::string, std::string>& keywords = {})
+{
+    detail::_interpreter::get();
+
+    PyObject* pystr = PyString_FromString(str.c_str());
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, pystr);
+
+    PyObject* kwargs = PyDict_New();
+    for (auto it = keywords.begin(); it != keywords.end(); ++it) {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs);
+    if(!res) throw std::runtime_error("Call to ylabel() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    Py_DECREF(res);
+}
+
+inline void set_zlabel(const std::string &str, const std::map<std::string, std::string>& keywords = {})
+{
+    detail::_interpreter::get();
+
+    // Same as with plot_surface: We lazily load the modules here the first time 
+    // this function is called because I'm not sure that we can assume "matplotlib 
+    // installed" implies "mpl_toolkits installed" on all platforms, and we don't 
+    // want to require it for people who don't need 3d plots.
+    static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr;
+    if (!mpl_toolkitsmod) {
+        PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits");
+        PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d");
+        if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); }
+
+        mpl_toolkitsmod = PyImport_Import(mpl_toolkits);
+        Py_DECREF(mpl_toolkits);
+        if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); }
+
+        axis3dmod = PyImport_Import(axis3d);
+        Py_DECREF(axis3d);
+        if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); }
+    }
+
+    PyObject* pystr = PyString_FromString(str.c_str());
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, pystr);
+
+    PyObject* kwargs = PyDict_New();
+    for (auto it = keywords.begin(); it != keywords.end(); ++it) {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+    }
+
+    PyObject *ax =
+    PyObject_CallObject(detail::_interpreter::get().s_python_function_gca,
+      detail::_interpreter::get().s_python_empty_tuple);
+    if (!ax) throw std::runtime_error("Call to gca() failed.");
+    Py_INCREF(ax);
+
+    PyObject *zlabel = PyObject_GetAttrString(ax, "set_zlabel");
+    if (!zlabel) throw std::runtime_error("Attribute set_zlabel not found.");
+    Py_INCREF(zlabel);
+
+    PyObject *res = PyObject_Call(zlabel, args, kwargs);
+    if (!res) throw std::runtime_error("Call to set_zlabel() failed.");
+    Py_DECREF(zlabel);
+
+    Py_DECREF(ax);
+    Py_DECREF(args);
+    Py_DECREF(kwargs);
+    if (res) Py_DECREF(res);
+}
+
+inline void grid(bool flag)
+{
+    detail::_interpreter::get();
+
+    PyObject* pyflag = flag ? Py_True : Py_False;
+    Py_INCREF(pyflag);
+
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, pyflag);
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args);
+    if(!res) throw std::runtime_error("Call to grid() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(res);
+}
+
+inline void show(const bool block = true)
+{
+    detail::_interpreter::get();
+
+    PyObject* res;
+    if(block)
+    {
+        res = PyObject_CallObject(
+                detail::_interpreter::get().s_python_function_show,
+                detail::_interpreter::get().s_python_empty_tuple);
+    }
+    else
+    {
+        PyObject *kwargs = PyDict_New();
+        PyDict_SetItemString(kwargs, "block", Py_False);
+        res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs);
+       Py_DECREF(kwargs);
+    }
+
+
+    if (!res) throw std::runtime_error("Call to show() failed.");
+
+    Py_DECREF(res);
+}
+
+inline void close()
+{
+    detail::_interpreter::get();
+
+    PyObject* res = PyObject_CallObject(
+            detail::_interpreter::get().s_python_function_close,
+            detail::_interpreter::get().s_python_empty_tuple);
+
+    if (!res) throw std::runtime_error("Call to close() failed.");
+
+    Py_DECREF(res);
+}
+
+inline void xkcd() {
+    detail::_interpreter::get();
+
+    PyObject* res;
+    PyObject *kwargs = PyDict_New();
+
+    res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd,
+            detail::_interpreter::get().s_python_empty_tuple, kwargs);
+
+    Py_DECREF(kwargs);
+
+    if (!res)
+        throw std::runtime_error("Call to show() failed.");
+
+    Py_DECREF(res);
+}
+
+inline void draw()
+{
+    detail::_interpreter::get();
+
+    PyObject* res = PyObject_CallObject(
+        detail::_interpreter::get().s_python_function_draw,
+        detail::_interpreter::get().s_python_empty_tuple);
+
+    if (!res) throw std::runtime_error("Call to draw() failed.");
+
+    Py_DECREF(res);
+}
+
+template<typename Numeric>
+inline void pause(Numeric interval)
+{
+    detail::_interpreter::get();
+
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval));
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args);
+    if(!res) throw std::runtime_error("Call to pause() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(res);
+}
+
+inline void save(const std::string& filename)
+{
+    detail::_interpreter::get();
+
+    PyObject* pyfilename = PyString_FromString(filename.c_str());
+
+    PyObject* args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, pyfilename);
+    std::cout<<"args:"<<filename.c_str()<<std::endl;
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_save, args);
+    if (!res) throw std::runtime_error("Call to save() failed.");
+
+    Py_DECREF(args);
+    Py_DECREF(res);
+}
+
+inline void clf() {
+    detail::_interpreter::get();
+
+    PyObject *res = PyObject_CallObject(
+        detail::_interpreter::get().s_python_function_clf,
+        detail::_interpreter::get().s_python_empty_tuple);
+
+    if (!res) throw std::runtime_error("Call to clf() failed.");
+
+    Py_DECREF(res);
+}
+
+inline void cla() {
+    detail::_interpreter::get();
+
+    PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_cla,
+                                        detail::_interpreter::get().s_python_empty_tuple);
+
+    if (!res)
+        throw std::runtime_error("Call to cla() failed.");
+
+    Py_DECREF(res);
+}
+
+inline void ion() {
+    detail::_interpreter::get();
+
+    PyObject *res = PyObject_CallObject(
+        detail::_interpreter::get().s_python_function_ion,
+        detail::_interpreter::get().s_python_empty_tuple);
+
+    if (!res) throw std::runtime_error("Call to ion() failed.");
+
+    Py_DECREF(res);
+}
+
+inline std::vector<std::array<double, 2>> ginput(const int numClicks = 1, const std::map<std::string, std::string>& keywords = {})
+{
+    detail::_interpreter::get();
+
+    PyObject *args = PyTuple_New(1);
+    PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks));
+
+    // construct keyword args
+    PyObject* kwargs = PyDict_New();
+    for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it)
+    {
+        PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str()));
+    }
+
+    PyObject* res = PyObject_Call(
+        detail::_interpreter::get().s_python_function_ginput, args, kwargs);
+
+    Py_DECREF(kwargs);
+    Py_DECREF(args);
+    if (!res) throw std::runtime_error("Call to ginput() failed.");
+
+    const size_t len = PyList_Size(res);
+    std::vector<std::array<double, 2>> out;
+    out.reserve(len);
+    for (size_t i = 0; i < len; i++) {
+        PyObject *current = PyList_GetItem(res, i);
+        std::array<double, 2> position;
+        position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0));
+        position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1));
+        out.push_back(position);
+    }
+    Py_DECREF(res);
+
+    return out;
+}
+
+// Actually, is there any reason not to call this automatically for every plot?
+inline void tight_layout() {
+    detail::_interpreter::get();
+
+    PyObject *res = PyObject_CallObject(
+        detail::_interpreter::get().s_python_function_tight_layout,
+        detail::_interpreter::get().s_python_empty_tuple);
+
+    if (!res) throw std::runtime_error("Call to tight_layout() failed.");
+
+    Py_DECREF(res);
+}
+
+// Support for variadic plot() and initializer lists:
+
+namespace detail {
+
+template<typename T>
+using is_function = typename std::is_function<std::remove_pointer<std::remove_reference<T>>>::type;
+
+template<bool obj, typename T>
+struct is_callable_impl;
+
+template<typename T>
+struct is_callable_impl<false, T>
+{
+    typedef is_function<T> type;
+}; // a non-object is callable iff it is a function
+
+template<typename T>
+struct is_callable_impl<true, T>
+{
+    struct Fallback { void operator()(); };
+    struct Derived : T, Fallback { };
+
+    template<typename U, U> struct Check;
+
+    template<typename U>
+    static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match
+
+    template<typename U>
+    static std::false_type test( Check<void(Fallback::*)(), &U::operator()>* );
+
+public:
+    typedef decltype(test<Derived>(nullptr)) type;
+    typedef decltype(&Fallback::operator()) dtype;
+    static constexpr bool value = type::value;
+}; // an object is callable iff it defines operator()
+
+template<typename T>
+struct is_callable
+{
+    // dispatch to is_callable_impl<true, T> or is_callable_impl<false, T> depending on whether T is of class type or not
+    typedef typename is_callable_impl<std::is_class<T>::value, T>::type type;
+};
+
+template<typename IsYDataCallable>
+struct plot_impl { };
+
+template<>
+struct plot_impl<std::false_type>
+{
+    template<typename IterableX, typename IterableY>
+    bool operator()(const IterableX& x, const IterableY& y, const std::string& format)
+    {
+        // 2-phase lookup for distance, begin, end
+        using std::distance;
+        using std::begin;
+        using std::end;
+
+        auto xs = distance(begin(x), end(x));
+        auto ys = distance(begin(y), end(y));
+        assert(xs == ys && "x and y data must have the same number of elements!");
+
+        PyObject* xlist = PyList_New(xs);
+        PyObject* ylist = PyList_New(ys);
+        PyObject* pystring = PyString_FromString(format.c_str());
+
+        auto itx = begin(x), ity = begin(y);
+        for(size_t i = 0; i < xs; ++i) {
+            PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++));
+            PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++));
+        }
+
+        PyObject* plot_args = PyTuple_New(3);
+        PyTuple_SetItem(plot_args, 0, xlist);
+        PyTuple_SetItem(plot_args, 1, ylist);
+        PyTuple_SetItem(plot_args, 2, pystring);
+
+        PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args);
+
+        Py_DECREF(plot_args);
+        if(res) Py_DECREF(res);
+
+        return res;
+    }
+};
+
+template<>
+struct plot_impl<std::true_type>
+{
+    template<typename Iterable, typename Callable>
+    bool operator()(const Iterable& ticks, const Callable& f, const std::string& format)
+    {
+        if(begin(ticks) == end(ticks)) return true;
+
+        // We could use additional meta-programming to deduce the correct element type of y,
+        // but all values have to be convertible to double anyways
+        std::vector<double> y;
+        for(auto x : ticks) y.push_back(f(x));
+        return plot_impl<std::false_type>()(ticks,y,format);
+    }
+};
+
+} // end namespace detail
+
+// recursion stop for the above
+template<typename... Args>
+bool plot() { return true; }
+
+template<typename A, typename B, typename... Args>
+bool plot(const A& a, const B& b, const std::string& format, Args... args)
+{
+    return detail::plot_impl<typename detail::is_callable<B>::type>()(a,b,format) && plot(args...);
+}
+
+/*
+ * This group of plot() functions is needed to support initializer lists, i.e. calling
+ *    plot( {1,2,3,4} )
+ */
+inline bool plot(const std::vector<double>& x, const std::vector<double>& y, const std::string& format = "") {
+    return plot<double,double>(x,y,format);
+}
+
+inline bool plot(const std::vector<double>& y, const std::string& format = "") {
+    return plot<double>(y,format);
+}
+
+inline bool plot(const std::vector<double>& x, const std::vector<double>& y, const std::map<std::string, std::string>& keywords) {
+    return plot<double>(x,y,keywords);
+}
+
+/*
+ * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting
+ */
+class Plot
+{
+public:
+    // default initialization with plot label, some data and format
+    template<typename Numeric>
+    Plot(const std::string& name, const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::string& format = "") {
+        detail::_interpreter::get();
+
+        assert(x.size() == y.size());
+
+        PyObject* kwargs = PyDict_New();
+        if(name != "")
+            PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str()));
+
+        PyObject* xarray = detail::get_array(x);
+        PyObject* yarray = detail::get_array(y);
+
+        PyObject* pystring = PyString_FromString(format.c_str());
+
+        PyObject* plot_args = PyTuple_New(3);
+        PyTuple_SetItem(plot_args, 0, xarray);
+        PyTuple_SetItem(plot_args, 1, yarray);
+        PyTuple_SetItem(plot_args, 2, pystring);
+
+        PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs);
+
+        Py_DECREF(kwargs);
+        Py_DECREF(plot_args);
+
+        if(res)
+        {
+            line= PyList_GetItem(res, 0);
+
+            if(line)
+                set_data_fct = PyObject_GetAttrString(line,"set_data");
+            else
+                Py_DECREF(line);
+            Py_DECREF(res);
+        }
+    }
+
+    // shorter initialization with name or format only
+    // basically calls line, = plot([], [])
+    Plot(const std::string& name = "", const std::string& format = "")
+        : Plot(name, std::vector<double>(), std::vector<double>(), format) {}
+
+    template<typename Numeric>
+    bool update(const std::vector<Numeric>& x, const std::vector<Numeric>& y) {
+        assert(x.size() == y.size());
+        if(set_data_fct)
+        {
+            PyObject* xarray = detail::get_array(x);
+            PyObject* yarray = detail::get_array(y);
+
+            PyObject* plot_args = PyTuple_New(2);
+            PyTuple_SetItem(plot_args, 0, xarray);
+            PyTuple_SetItem(plot_args, 1, yarray);
+
+            PyObject* res = PyObject_CallObject(set_data_fct, plot_args);
+            if (res) Py_DECREF(res);
+            return res;
+        }
+        return false;
+    }
+
+    // clears the plot but keep it available
+    bool clear() {
+        return update(std::vector<double>(), std::vector<double>());
+    }
+
+    // definitely remove this line
+    void remove() {
+        if(line)
+        {
+            auto remove_fct = PyObject_GetAttrString(line,"remove");
+            PyObject* args = PyTuple_New(0);
+            PyObject* res = PyObject_CallObject(remove_fct, args);
+            if (res) Py_DECREF(res);
+        }
+        decref();
+    }
+
+    ~Plot() {
+        decref();
+    }
+private:
+
+    void decref() {
+        if(line)
+            Py_DECREF(line);
+        if(set_data_fct)
+            Py_DECREF(set_data_fct);
+    }
+
+
+    PyObject* line = nullptr;
+    PyObject* set_data_fct = nullptr;
+};
+
+} // end namespace matplotlibcpp
diff --git a/FAST_LIO/include/so3_math.h b/FAST_LIO/include/so3_math.h
new file mode 100644
index 0000000..8530c3c
--- /dev/null
+++ b/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
diff --git a/FAST_LIO/include/use-ikfom.hpp b/FAST_LIO/include/use-ikfom.hpp
new file mode 100644
index 0000000..fdd8f87
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/FAST_LIO/launch/gdb_debug_example.launch b/FAST_LIO/launch/gdb_debug_example.launch
new file mode 100644
index 0000000..d1f86ad
--- /dev/null
+++ b/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>
diff --git a/FAST_LIO/launch/mapping.launch.py b/FAST_LIO/launch/mapping.launch.py
new file mode 100644
index 0000000..b76313d
--- /dev/null
+++ b/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
diff --git a/FAST_LIO/launch/unilidar_l2.launch.py b/FAST_LIO/launch/unilidar_l2.launch.py
new file mode 100644
index 0000000..aeb51df
--- /dev/null
+++ b/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
diff --git a/FAST_LIO/msg/Pose6D.msg b/FAST_LIO/msg/Pose6D.msg
new file mode 100644
index 0000000..1ebd1fd
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/FAST_LIO/package.xml b/FAST_LIO/package.xml
new file mode 100644
index 0000000..8c39e18
--- /dev/null
+++ b/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>
diff --git a/FAST_LIO/rviz/fastlio.rviz b/FAST_LIO/rviz/fastlio.rviz
new file mode 100644
index 0000000..d907d25
--- /dev/null
+++ b/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
diff --git a/FAST_LIO/rviz_cfg/.gitignore b/FAST_LIO/rviz_cfg/.gitignore
new file mode 100644
index 0000000..e69de29
diff --git a/FAST_LIO/rviz_cfg/loam_livox.rviz b/FAST_LIO/rviz_cfg/loam_livox.rviz
new file mode 100644
index 0000000..ea3cdf3
--- /dev/null
+++ b/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
diff --git a/FAST_LIO/src/IMU_Processing.hpp b/FAST_LIO/src/IMU_Processing.hpp
new file mode 100644
index 0000000..f35378f
--- /dev/null
+++ b/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;
+}
diff --git a/FAST_LIO/src/laserMapping.cpp b/FAST_LIO/src/laserMapping.cpp
new file mode 100644
index 0000000..f045924
--- /dev/null
+++ b/FAST_LIO/src/laserMapping.cpp
@@ -0,0 +1,1194 @@
+// This is an advanced implementation of the algorithm 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.
+
+// Modifier: Livox               dev@livoxtech.com
+
+// Copyright 2013, Ji Zhang, Carnegie Mellon University
+// Further contributions copyright (c) 2016, Southwest Research Institute
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// 1. Redistributions of source code must retain the above copyright notice,
+//    this list of conditions and the following disclaimer.
+// 2. 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.
+// 3. Neither the name of the copyright holder 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 HOLDER 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.
+#include <omp.h>
+#include <mutex>
+#include <math.h>
+#include <thread>
+#include <fstream>
+#include <csignal>
+#include <chrono>
+#include <unistd.h>
+#include <Python.h>
+#include <so3_math.h>
+#include <rclcpp/rclcpp.hpp>
+#include <Eigen/Core>
+#include "IMU_Processing.hpp"
+#include <nav_msgs/msg/odometry.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <std_srvs/srv/trigger.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <livox_ros_driver2/msg/custom_msg.hpp>
+#include "preprocess.h"
+#include <ikd-Tree/ikd_Tree.h>
+
+#define INIT_TIME           (0.1)
+#define LASER_POINT_COV     (0.001)
+#define MAXN                (720000)
+#define PUBFRAME_PERIOD     (20)
+
+/*** Time Log Variables ***/
+double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0;
+double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN];
+double match_time = 0, solve_time = 0, solve_const_H_time = 0;
+int    kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0;
+bool   runtime_pos_log = false, pcd_save_en = false, time_sync_en = false, extrinsic_est_en = true, path_en = true;
+/**************************/
+
+float res_last[100000] = {0.0};
+float DET_RANGE = 300.0f;
+const float MOV_THRESHOLD = 1.5f;
+double time_diff_lidar_to_imu = 0.0;
+
+mutex mtx_buffer;
+condition_variable sig_buffer;
+
+string root_dir = ROOT_DIR;
+string map_file_path, lid_topic, imu_topic;
+
+double res_mean_last = 0.05, total_residual = 0.0;
+double last_timestamp_lidar = 0, last_timestamp_imu = -1.0;
+double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001;
+double filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0;
+double cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0;
+int    effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count = 0;
+int    iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0, pcd_save_interval = -1, pcd_index = 0;
+bool   point_selected_surf[100000] = {0};
+bool   lidar_pushed, flg_first_scan = true, flg_exit = false, flg_EKF_inited;
+bool   scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;
+bool    is_first_lidar = true;
+
+vector<vector<int>>  pointSearchInd_surf; 
+vector<BoxPointType> cub_needrm;
+vector<PointVector>  Nearest_Points; 
+vector<double>       extrinT(3, 0.0);
+vector<double>       extrinR(9, 0.0);
+deque<double>                     time_buffer;
+deque<PointCloudXYZI::Ptr>        lidar_buffer;
+deque<sensor_msgs::msg::Imu::ConstSharedPtr> imu_buffer;
+
+PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI());
+PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI());
+PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI());
+PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI());
+PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1));
+PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1));
+PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1));
+PointCloudXYZI::Ptr _featsArray;
+
+pcl::VoxelGrid<PointType> downSizeFilterSurf;
+pcl::VoxelGrid<PointType> downSizeFilterMap;
+
+KD_TREE<PointType> ikdtree;
+
+V3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0);
+V3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0);
+V3D euler_cur;
+V3D position_last(Zero3d);
+V3D Lidar_T_wrt_IMU(Zero3d);
+M3D Lidar_R_wrt_IMU(Eye3d);
+
+/*** EKF inputs and output ***/
+MeasureGroup Measures;
+esekfom::esekf<state_ikfom, 12, input_ikfom> kf;
+state_ikfom state_point;
+vect3 pos_lid;
+
+nav_msgs::msg::Path path;
+nav_msgs::msg::Odometry odomAftMapped;
+geometry_msgs::msg::Quaternion geoQuat;
+geometry_msgs::msg::PoseStamped msg_body_pose;
+
+shared_ptr<Preprocess> p_pre(new Preprocess());
+shared_ptr<ImuProcess> p_imu(new ImuProcess());
+
+void SigHandle(int sig)
+{
+    flg_exit = true;
+    std::cout << "catch sig %d" << sig << std::endl;
+    sig_buffer.notify_all();
+    rclcpp::shutdown();
+}
+
+inline void dump_lio_state_to_log(FILE *fp)  
+{
+    V3D rot_ang(Log(state_point.rot.toRotationMatrix()));
+    fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time);
+    fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2));                   // Angle
+    fprintf(fp, "%lf %lf %lf ", state_point.pos(0), state_point.pos(1), state_point.pos(2)); // Pos  
+    fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0);                                        // omega  
+    fprintf(fp, "%lf %lf %lf ", state_point.vel(0), state_point.vel(1), state_point.vel(2)); // Vel  
+    fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0);                                        // Acc  
+    fprintf(fp, "%lf %lf %lf ", state_point.bg(0), state_point.bg(1), state_point.bg(2));    // Bias_g  
+    fprintf(fp, "%lf %lf %lf ", state_point.ba(0), state_point.ba(1), state_point.ba(2));    // Bias_a  
+    fprintf(fp, "%lf %lf %lf ", state_point.grav[0], state_point.grav[1], state_point.grav[2]); // Bias_a  
+    fprintf(fp, "\r\n");  
+    fflush(fp);
+}
+
+void pointBodyToWorld_ikfom(PointType const * const pi, PointType * const po, state_ikfom &s)
+{
+    V3D p_body(pi->x, pi->y, pi->z);
+    V3D p_global(s.rot * (s.offset_R_L_I*p_body + s.offset_T_L_I) + s.pos);
+
+    po->x = p_global(0);
+    po->y = p_global(1);
+    po->z = p_global(2);
+    po->intensity = pi->intensity;
+}
+
+
+void pointBodyToWorld(PointType const * const pi, PointType * const po)
+{
+    V3D p_body(pi->x, pi->y, pi->z);
+    V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos);
+
+    po->x = p_global(0);
+    po->y = p_global(1);
+    po->z = p_global(2);
+    po->intensity = pi->intensity;
+}
+
+template<typename T>
+void pointBodyToWorld(const Matrix<T, 3, 1> &pi, Matrix<T, 3, 1> &po)
+{
+    V3D p_body(pi[0], pi[1], pi[2]);
+    V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos);
+
+    po[0] = p_global(0);
+    po[1] = p_global(1);
+    po[2] = p_global(2);
+}
+
+void RGBpointBodyToWorld(PointType const * const pi, PointType * const po)
+{
+    V3D p_body(pi->x, pi->y, pi->z);
+    V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos);
+
+    po->x = p_global(0);
+    po->y = p_global(1);
+    po->z = p_global(2);
+    po->intensity = pi->intensity;
+}
+
+void RGBpointBodyLidarToIMU(PointType const * const pi, PointType * const po)
+{
+    V3D p_body_lidar(pi->x, pi->y, pi->z);
+    V3D p_body_imu(state_point.offset_R_L_I*p_body_lidar + state_point.offset_T_L_I);
+
+    po->x = p_body_imu(0);
+    po->y = p_body_imu(1);
+    po->z = p_body_imu(2);
+    po->intensity = pi->intensity;
+}
+
+void points_cache_collect()
+{
+    PointVector points_history;
+    ikdtree.acquire_removed_points(points_history);
+    // for (int i = 0; i < points_history.size(); i++) _featsArray->push_back(points_history[i]);
+}
+
+BoxPointType LocalMap_Points;
+bool Localmap_Initialized = false;
+void lasermap_fov_segment()
+{
+    cub_needrm.clear();
+    kdtree_delete_counter = 0;
+    kdtree_delete_time = 0.0;    
+    pointBodyToWorld(XAxisPoint_body, XAxisPoint_world);
+    V3D pos_LiD = pos_lid;
+    if (!Localmap_Initialized){
+        for (int i = 0; i < 3; i++){
+            LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0;
+            LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0;
+        }
+        Localmap_Initialized = true;
+        return;
+    }
+    float dist_to_map_edge[3][2];
+    bool need_move = false;
+    for (int i = 0; i < 3; i++){
+        dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]);
+        dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]);
+        if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) need_move = true;
+    }
+    if (!need_move) return;
+    BoxPointType New_LocalMap_Points, tmp_boxpoints;
+    New_LocalMap_Points = LocalMap_Points;
+    float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD -1)));
+    for (int i = 0; i < 3; i++){
+        tmp_boxpoints = LocalMap_Points;
+        if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE){
+            New_LocalMap_Points.vertex_max[i] -= mov_dist;
+            New_LocalMap_Points.vertex_min[i] -= mov_dist;
+            tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist;
+            cub_needrm.push_back(tmp_boxpoints);
+        } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE){
+            New_LocalMap_Points.vertex_max[i] += mov_dist;
+            New_LocalMap_Points.vertex_min[i] += mov_dist;
+            tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist;
+            cub_needrm.push_back(tmp_boxpoints);
+        }
+    }
+    LocalMap_Points = New_LocalMap_Points;
+
+    points_cache_collect();
+    double delete_begin = omp_get_wtime();
+    if(cub_needrm.size() > 0) kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm);
+    kdtree_delete_time = omp_get_wtime() - delete_begin;
+}
+
+void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::UniquePtr msg) 
+{
+    mtx_buffer.lock();
+    scan_count ++;
+    double cur_time = get_time_sec(msg->header.stamp);
+    double preprocess_start_time = omp_get_wtime();
+    if (!is_first_lidar && cur_time < last_timestamp_lidar)
+    {
+        std::cerr << "lidar loop back, clear buffer" << std::endl;
+        lidar_buffer.clear();
+    }
+    if (is_first_lidar)
+    {
+        is_first_lidar = false;
+    }
+
+    PointCloudXYZI::Ptr  ptr(new PointCloudXYZI());
+    p_pre->process(msg, ptr);
+    lidar_buffer.push_back(ptr);
+    time_buffer.push_back(cur_time);
+    last_timestamp_lidar = cur_time;
+    s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time;
+    mtx_buffer.unlock();
+    sig_buffer.notify_all();
+}
+
+double timediff_lidar_wrt_imu = 0.0;
+bool   timediff_set_flg = false;
+void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::UniquePtr msg) 
+{
+    mtx_buffer.lock();
+    double cur_time = get_time_sec(msg->header.stamp);
+    double preprocess_start_time = omp_get_wtime();
+    scan_count ++;
+    if (!is_first_lidar && cur_time < last_timestamp_lidar)
+    {
+        std::cerr << "lidar loop back, clear buffer" << std::endl;
+        lidar_buffer.clear();
+    }
+    if(is_first_lidar)
+    {
+        is_first_lidar = false;
+    }
+    last_timestamp_lidar = cur_time;
+    
+    if (!time_sync_en && abs(last_timestamp_imu - last_timestamp_lidar) > 10.0 && !imu_buffer.empty() && !lidar_buffer.empty() )
+    {
+        printf("IMU and LiDAR not Synced, IMU time: %lf, lidar header time: %lf \n",last_timestamp_imu, last_timestamp_lidar);
+    }
+
+    if (time_sync_en && !timediff_set_flg && abs(last_timestamp_lidar - last_timestamp_imu) > 1 && !imu_buffer.empty())
+    {
+        timediff_set_flg = true;
+        timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu;
+        printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu);
+    }
+
+    PointCloudXYZI::Ptr  ptr(new PointCloudXYZI());
+    p_pre->process(msg, ptr);
+    lidar_buffer.push_back(ptr);
+    time_buffer.push_back(last_timestamp_lidar);
+    
+    s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time;
+    mtx_buffer.unlock();
+    sig_buffer.notify_all();
+}
+
+void imu_cbk(const sensor_msgs::msg::Imu::UniquePtr msg_in)
+{
+    publish_count ++;
+    // cout<<"IMU got at: "<<msg_in->header.stamp.toSec()<<endl;
+    sensor_msgs::msg::Imu::SharedPtr msg(new sensor_msgs::msg::Imu(*msg_in));
+    
+
+    msg->header.stamp = get_ros_time(get_time_sec(msg_in->header.stamp) - time_diff_lidar_to_imu);
+    if (abs(timediff_lidar_wrt_imu) > 0.1 && time_sync_en)
+    {
+        msg->header.stamp = \
+        rclcpp::Time(timediff_lidar_wrt_imu + get_time_sec(msg_in->header.stamp));
+    }
+
+    double timestamp = get_time_sec(msg->header.stamp);
+
+    mtx_buffer.lock();
+
+    if (timestamp < last_timestamp_imu)
+    {
+        std::cerr << "lidar loop back, clear buffer" << std::endl;
+        imu_buffer.clear();
+    }
+
+    last_timestamp_imu = timestamp;
+
+    imu_buffer.push_back(msg);
+    mtx_buffer.unlock();
+    sig_buffer.notify_all();
+}
+
+double lidar_mean_scantime = 0.0;
+int    scan_num = 0;
+bool sync_packages(MeasureGroup &meas)
+{
+    if (lidar_buffer.empty() || imu_buffer.empty()) {
+        return false;
+    }
+
+    /*** push a lidar scan ***/
+    if(!lidar_pushed)
+    {
+        meas.lidar = lidar_buffer.front();
+        meas.lidar_beg_time = time_buffer.front();
+        if (meas.lidar->points.size() <= 1) // time too little
+        {
+            lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
+            std::cerr << "Too few input point cloud!\n";
+        }
+        else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime)
+        {
+            lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
+        }
+        else
+        {
+            scan_num ++;
+            lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000);
+            lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num;
+        }
+
+        meas.lidar_end_time = lidar_end_time;
+
+        lidar_pushed = true;
+    }
+
+    if (last_timestamp_imu < lidar_end_time)
+    {
+        return false;
+    }
+
+    /*** push imu data, and pop from imu buffer ***/
+    double imu_time = get_time_sec(imu_buffer.front()->header.stamp);
+    meas.imu.clear();
+    while ((!imu_buffer.empty()) && (imu_time < lidar_end_time))
+    {
+        imu_time = get_time_sec(imu_buffer.front()->header.stamp);
+        if(imu_time > lidar_end_time) break;
+        meas.imu.push_back(imu_buffer.front());
+        imu_buffer.pop_front();
+    }
+
+    lidar_buffer.pop_front();
+    time_buffer.pop_front();
+    lidar_pushed = false;
+    return true;
+}
+
+int process_increments = 0;
+void map_incremental()
+{
+    PointVector PointToAdd;
+    PointVector PointNoNeedDownsample;
+    PointToAdd.reserve(feats_down_size);
+    PointNoNeedDownsample.reserve(feats_down_size);
+    for (int i = 0; i < feats_down_size; i++)
+    {
+        /* transform to world frame */
+        pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
+        /* decide if need add to map */
+        if (!Nearest_Points[i].empty() && flg_EKF_inited)
+        {
+            const PointVector &points_near = Nearest_Points[i];
+            bool need_add = true;
+            BoxPointType Box_of_Point;
+            PointType downsample_result, mid_point; 
+            mid_point.x = floor(feats_down_world->points[i].x/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min;
+            mid_point.y = floor(feats_down_world->points[i].y/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min;
+            mid_point.z = floor(feats_down_world->points[i].z/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min;
+            float dist  = calc_dist(feats_down_world->points[i],mid_point);
+            if (fabs(points_near[0].x - mid_point.x) > 0.5 * filter_size_map_min && fabs(points_near[0].y - mid_point.y) > 0.5 * filter_size_map_min && fabs(points_near[0].z - mid_point.z) > 0.5 * filter_size_map_min){
+                PointNoNeedDownsample.push_back(feats_down_world->points[i]);
+                continue;
+            }
+            for (int readd_i = 0; readd_i < NUM_MATCH_POINTS; readd_i ++)
+            {
+                if (points_near.size() < NUM_MATCH_POINTS) break;
+                if (calc_dist(points_near[readd_i], mid_point) < dist)
+                {
+                    need_add = false;
+                    break;
+                }
+            }
+            if (need_add) PointToAdd.push_back(feats_down_world->points[i]);
+        }
+        else
+        {
+            PointToAdd.push_back(feats_down_world->points[i]);
+        }
+    }
+
+    double st_time = omp_get_wtime();
+    add_point_size = ikdtree.Add_Points(PointToAdd, true);
+    ikdtree.Add_Points(PointNoNeedDownsample, false); 
+    add_point_size = PointToAdd.size() + PointNoNeedDownsample.size();
+    kdtree_incremental_time = omp_get_wtime() - st_time;
+}
+
+PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI());
+PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());
+void publish_frame_world(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFull)
+{
+    if(scan_pub_en)
+    {
+        PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body);
+        int size = laserCloudFullRes->points.size();
+        PointCloudXYZI::Ptr laserCloudWorld( \
+                        new PointCloudXYZI(size, 1));
+
+        for (int i = 0; i < size; i++)
+        {
+            RGBpointBodyToWorld(&laserCloudFullRes->points[i], \
+                                &laserCloudWorld->points[i]);
+        }
+
+        sensor_msgs::msg::PointCloud2 laserCloudmsg;
+        pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
+        // laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
+        laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
+        laserCloudmsg.header.frame_id = "camera_init";
+        pubLaserCloudFull->publish(laserCloudmsg);
+        publish_count -= PUBFRAME_PERIOD;
+    }
+
+    if (pcd_save_en)
+    {
+        int size = feats_undistort->points.size();
+        PointCloudXYZI::Ptr laserCloudWorld( \
+                        new PointCloudXYZI(size, 1));
+
+        for (int i = 0; i < size; i++)
+        {
+            RGBpointBodyToWorld(&feats_undistort->points[i], \
+                                &laserCloudWorld->points[i]);
+        }
+        *pcl_wait_save += *laserCloudWorld;
+
+        static int scan_wait_num = 0;
+        scan_wait_num ++;
+        if (pcl_wait_save->size() > 0 && pcd_save_interval > 0  && scan_wait_num >= pcd_save_interval)
+        {
+            pcd_index ++;
+            string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd"));
+            pcl::PCDWriter pcd_writer;
+            cout << "current scan saved to /PCD/" << all_points_dir << endl;
+            pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
+            pcl_wait_save->clear();
+            scan_wait_num = 0;
+        }
+    }
+    
+}
+
+void publish_frame_body(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFull_body)
+{
+    int size = feats_undistort->points.size();
+    PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1));
+
+    for (int i = 0; i < size; i++)
+    {
+        RGBpointBodyLidarToIMU(&feats_undistort->points[i], \
+                            &laserCloudIMUBody->points[i]);
+    }
+
+    sensor_msgs::msg::PointCloud2 laserCloudmsg;
+    pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg);
+    laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
+    laserCloudmsg.header.frame_id = "body";
+    pubLaserCloudFull_body->publish(laserCloudmsg);
+    publish_count -= PUBFRAME_PERIOD;
+}
+
+void publish_effect_world(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudEffect)
+{
+    PointCloudXYZI::Ptr laserCloudWorld( \
+                    new PointCloudXYZI(effct_feat_num, 1));
+    for (int i = 0; i < effct_feat_num; i++)
+    {
+        RGBpointBodyToWorld(&laserCloudOri->points[i], \
+                            &laserCloudWorld->points[i]);
+    }
+    sensor_msgs::msg::PointCloud2 laserCloudFullRes3;
+    pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3);
+    laserCloudFullRes3.header.stamp = get_ros_time(lidar_end_time);
+    laserCloudFullRes3.header.frame_id = "camera_init";
+    pubLaserCloudEffect->publish(laserCloudFullRes3);
+}
+
+void publish_map(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudMap)
+{
+    PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body);
+    int size = laserCloudFullRes->points.size();
+    PointCloudXYZI::Ptr laserCloudWorld( \
+                    new PointCloudXYZI(size, 1));
+
+    for (int i = 0; i < size; i++)
+    {
+        RGBpointBodyToWorld(&laserCloudFullRes->points[i], \
+                            &laserCloudWorld->points[i]);
+    }
+    *pcl_wait_pub += *laserCloudWorld;
+
+    sensor_msgs::msg::PointCloud2 laserCloudmsg;
+    pcl::toROSMsg(*pcl_wait_pub, laserCloudmsg);
+    // laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
+    laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
+    laserCloudmsg.header.frame_id = "camera_init";
+    pubLaserCloudMap->publish(laserCloudmsg);
+
+    // sensor_msgs::msg::PointCloud2 laserCloudMap;
+    // pcl::toROSMsg(*featsFromMap, laserCloudMap);
+    // laserCloudMap.header.stamp = get_ros_time(lidar_end_time);
+    // laserCloudMap.header.frame_id = "camera_init";
+    // pubLaserCloudMap->publish(laserCloudMap);
+}
+
+void save_to_pcd()
+{
+    pcl::PCDWriter pcd_writer;
+    pcd_writer.writeBinary(map_file_path, *pcl_wait_pub);
+}
+
+template<typename T>
+void set_posestamp(T & out)
+{
+    out.pose.position.x = state_point.pos(0);
+    out.pose.position.y = state_point.pos(1);
+    out.pose.position.z = state_point.pos(2);
+    out.pose.orientation.x = geoQuat.x;
+    out.pose.orientation.y = geoQuat.y;
+    out.pose.orientation.z = geoQuat.z;
+    out.pose.orientation.w = geoQuat.w;
+    
+}
+
+void publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped, std::unique_ptr<tf2_ros::TransformBroadcaster> & tf_br)
+{
+    odomAftMapped.header.frame_id = "camera_init";
+    odomAftMapped.child_frame_id = "body";
+    odomAftMapped.header.stamp = get_ros_time(lidar_end_time);
+    set_posestamp(odomAftMapped.pose);
+    pubOdomAftMapped->publish(odomAftMapped);
+    auto P = kf.get_P();
+    for (int i = 0; i < 6; i ++)
+    {
+        int k = i < 3 ? i + 3 : i - 3;
+        odomAftMapped.pose.covariance[i*6 + 0] = P(k, 3);
+        odomAftMapped.pose.covariance[i*6 + 1] = P(k, 4);
+        odomAftMapped.pose.covariance[i*6 + 2] = P(k, 5);
+        odomAftMapped.pose.covariance[i*6 + 3] = P(k, 0);
+        odomAftMapped.pose.covariance[i*6 + 4] = P(k, 1);
+        odomAftMapped.pose.covariance[i*6 + 5] = P(k, 2);
+    }
+
+    geometry_msgs::msg::TransformStamped trans;
+    trans.header.frame_id = "camera_init";
+    trans.header.stamp = odomAftMapped.header.stamp;
+    trans.child_frame_id = "body";
+    trans.transform.translation.x = odomAftMapped.pose.pose.position.x;
+    trans.transform.translation.y = odomAftMapped.pose.pose.position.y;
+    trans.transform.translation.z = odomAftMapped.pose.pose.position.z;
+    trans.transform.rotation.w = odomAftMapped.pose.pose.orientation.w;
+    trans.transform.rotation.x = odomAftMapped.pose.pose.orientation.x;
+    trans.transform.rotation.y = odomAftMapped.pose.pose.orientation.y;
+    trans.transform.rotation.z = odomAftMapped.pose.pose.orientation.z;
+    tf_br->sendTransform(trans);
+}
+
+void publish_path(rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath)
+{
+    set_posestamp(msg_body_pose);
+    msg_body_pose.header.stamp = get_ros_time(lidar_end_time); // ros::Time().fromSec(lidar_end_time);
+    msg_body_pose.header.frame_id = "camera_init";
+
+    /*** if path is too large, the rvis will crash ***/
+    static int jjj = 0;
+    jjj++;
+    if (jjj % 10 == 0) 
+    {
+        path.poses.push_back(msg_body_pose);
+        pubPath->publish(path);
+    }
+}
+
+void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data)
+{
+    double match_start = omp_get_wtime();
+    laserCloudOri->clear(); 
+    corr_normvect->clear(); 
+    total_residual = 0.0; 
+
+    /** closest surface search and residual computation **/
+    #ifdef MP_EN
+        omp_set_num_threads(MP_PROC_NUM);
+        #pragma omp parallel for
+    #endif
+    for (int i = 0; i < feats_down_size; i++)
+    {
+        PointType &point_body  = feats_down_body->points[i]; 
+        PointType &point_world = feats_down_world->points[i]; 
+
+        /* transform to world frame */
+        V3D p_body(point_body.x, point_body.y, point_body.z);
+        V3D p_global(s.rot * (s.offset_R_L_I*p_body + s.offset_T_L_I) + s.pos);
+        point_world.x = p_global(0);
+        point_world.y = p_global(1);
+        point_world.z = p_global(2);
+        point_world.intensity = point_body.intensity;
+
+        vector<float> pointSearchSqDis(NUM_MATCH_POINTS);
+
+        auto &points_near = Nearest_Points[i];
+
+        if (ekfom_data.converge)
+        {
+            /** Find the closest surfaces in the map **/
+            ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis);
+            point_selected_surf[i] = points_near.size() < NUM_MATCH_POINTS ? false : pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5 ? false : true;
+        }
+
+        if (!point_selected_surf[i]) continue;
+
+        VF(4) pabcd;
+        point_selected_surf[i] = false;
+        if (esti_plane(pabcd, points_near, 0.1f))
+        {
+            float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z + pabcd(3);
+            float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm());
+
+            if (s > 0.9)
+            {
+                point_selected_surf[i] = true;
+                normvec->points[i].x = pabcd(0);
+                normvec->points[i].y = pabcd(1);
+                normvec->points[i].z = pabcd(2);
+                normvec->points[i].intensity = pd2;
+                res_last[i] = abs(pd2);
+            }
+        }
+    }
+    
+    effct_feat_num = 0;
+
+    for (int i = 0; i < feats_down_size; i++)
+    {
+        if (point_selected_surf[i])
+        {
+            laserCloudOri->points[effct_feat_num] = feats_down_body->points[i];
+            corr_normvect->points[effct_feat_num] = normvec->points[i];
+            total_residual += res_last[i];
+            effct_feat_num ++;
+        }
+    }
+
+    if (effct_feat_num < 1)
+    {
+        ekfom_data.valid = false;
+        std::cerr << "No Effective Points!" << std::endl;
+        // ROS_WARN("No Effective Points! \n");
+        return;
+    }
+
+    res_mean_last = total_residual / effct_feat_num;
+    match_time  += omp_get_wtime() - match_start;
+    double solve_start_  = omp_get_wtime();
+    
+    /*** Computation of Measuremnt Jacobian matrix H and measurents vector ***/
+    ekfom_data.h_x = MatrixXd::Zero(effct_feat_num, 12); //23
+    ekfom_data.h.resize(effct_feat_num);
+
+    for (int i = 0; i < effct_feat_num; i++)
+    {
+        const PointType &laser_p  = laserCloudOri->points[i];
+        V3D point_this_be(laser_p.x, laser_p.y, laser_p.z);
+        M3D point_be_crossmat;
+        point_be_crossmat << SKEW_SYM_MATRX(point_this_be);
+        V3D point_this = s.offset_R_L_I * point_this_be + s.offset_T_L_I;
+        M3D point_crossmat;
+        point_crossmat<<SKEW_SYM_MATRX(point_this);
+
+        /*** get the normal vector of closest surface/corner ***/
+        const PointType &norm_p = corr_normvect->points[i];
+        V3D norm_vec(norm_p.x, norm_p.y, norm_p.z);
+
+        /*** calculate the Measuremnt Jacobian matrix H ***/
+        V3D C(s.rot.conjugate() *norm_vec);
+        V3D A(point_crossmat * C);
+        if (extrinsic_est_en)
+        {
+            V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C); //s.rot.conjugate()*norm_vec);
+            ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
+        }
+        else
+        {
+            ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+        }
+
+        /*** Measuremnt: distance to the closest surface/corner ***/
+        ekfom_data.h(i) = -norm_p.intensity;
+    }
+    solve_time += omp_get_wtime() - solve_start_;
+}
+
+class LaserMappingNode : public rclcpp::Node
+{
+public:
+    LaserMappingNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("laser_mapping", options)
+    {
+        this->declare_parameter<bool>("publish.path_en", true);
+        this->declare_parameter<bool>("publish.effect_map_en", false);
+        this->declare_parameter<bool>("publish.map_en", false);
+        this->declare_parameter<bool>("publish.scan_publish_en", true);
+        this->declare_parameter<bool>("publish.dense_publish_en", true);
+        this->declare_parameter<bool>("publish.scan_bodyframe_pub_en", true);
+        this->declare_parameter<int>("max_iteration", 4);
+        this->declare_parameter<string>("map_file_path", "");
+        this->declare_parameter<string>("common.lid_topic", "/livox/lidar");
+        this->declare_parameter<string>("common.imu_topic", "/livox/imu");
+        this->declare_parameter<bool>("common.time_sync_en", false);
+        this->declare_parameter<double>("common.time_offset_lidar_to_imu", 0.0);
+        this->declare_parameter<double>("filter_size_corner", 0.5);
+        this->declare_parameter<double>("filter_size_surf", 0.5);
+        this->declare_parameter<double>("filter_size_map", 0.5);
+        this->declare_parameter<double>("cube_side_length", 200.);
+        this->declare_parameter<float>("mapping.det_range", 300.);
+        this->declare_parameter<double>("mapping.fov_degree", 180.);
+        this->declare_parameter<double>("mapping.gyr_cov", 0.1);
+        this->declare_parameter<double>("mapping.acc_cov", 0.1);
+        this->declare_parameter<double>("mapping.b_gyr_cov", 0.0001);
+        this->declare_parameter<double>("mapping.b_acc_cov", 0.0001);
+        this->declare_parameter<double>("preprocess.blind", 0.01);
+        this->declare_parameter<int>("preprocess.lidar_type", AVIA);
+        this->declare_parameter<int>("preprocess.scan_line", 16);
+        this->declare_parameter<int>("preprocess.timestamp_unit", US);
+        this->declare_parameter<int>("preprocess.scan_rate", 10);
+        this->declare_parameter<int>("point_filter_num", 2);
+        this->declare_parameter<bool>("feature_extract_enable", false);
+        this->declare_parameter<bool>("runtime_pos_log_enable", false);
+        this->declare_parameter<bool>("mapping.extrinsic_est_en", true);
+        this->declare_parameter<bool>("pcd_save.pcd_save_en", false);
+        this->declare_parameter<int>("pcd_save.interval", -1);
+        this->declare_parameter<vector<double>>("mapping.extrinsic_T", vector<double>());
+        this->declare_parameter<vector<double>>("mapping.extrinsic_R", vector<double>());
+
+        this->get_parameter_or<bool>("publish.path_en", path_en, true);
+        this->get_parameter_or<bool>("publish.effect_map_en", effect_pub_en, false);
+        this->get_parameter_or<bool>("publish.map_en", map_pub_en, false);
+        this->get_parameter_or<bool>("publish.scan_publish_en", scan_pub_en, true);
+        this->get_parameter_or<bool>("publish.dense_publish_en", dense_pub_en, true);
+        this->get_parameter_or<bool>("publish.scan_bodyframe_pub_en", scan_body_pub_en, true);
+        this->get_parameter_or<int>("max_iteration", NUM_MAX_ITERATIONS, 4);
+        this->get_parameter_or<string>("map_file_path", map_file_path, "");
+        this->get_parameter_or<string>("common.lid_topic", lid_topic, "/livox/lidar");
+        this->get_parameter_or<string>("common.imu_topic", imu_topic,"/livox/imu");
+        this->get_parameter_or<bool>("common.time_sync_en", time_sync_en, false);
+        this->get_parameter_or<double>("common.time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0);
+        this->get_parameter_or<double>("filter_size_corner",filter_size_corner_min,0.5);
+        this->get_parameter_or<double>("filter_size_surf",filter_size_surf_min,0.5);
+        this->get_parameter_or<double>("filter_size_map",filter_size_map_min,0.5);
+        this->get_parameter_or<double>("cube_side_length",cube_len,200.f);
+        this->get_parameter_or<float>("mapping.det_range",DET_RANGE,300.f);
+        this->get_parameter_or<double>("mapping.fov_degree",fov_deg,180.f);
+        this->get_parameter_or<double>("mapping.gyr_cov",gyr_cov,0.1);
+        this->get_parameter_or<double>("mapping.acc_cov",acc_cov,0.1);
+        this->get_parameter_or<double>("mapping.b_gyr_cov",b_gyr_cov,0.0001);
+        this->get_parameter_or<double>("mapping.b_acc_cov",b_acc_cov,0.0001);
+        this->get_parameter_or<double>("preprocess.blind", p_pre->blind, 0.01);
+        this->get_parameter_or<int>("preprocess.lidar_type", p_pre->lidar_type, AVIA);
+        this->get_parameter_or<int>("preprocess.scan_line", p_pre->N_SCANS, 16);
+        this->get_parameter_or<int>("preprocess.timestamp_unit", p_pre->time_unit, US);
+        this->get_parameter_or<int>("preprocess.scan_rate", p_pre->SCAN_RATE, 10);
+        this->get_parameter_or<int>("point_filter_num", p_pre->point_filter_num, 2);
+        this->get_parameter_or<bool>("feature_extract_enable", p_pre->feature_enabled, false);
+        this->get_parameter_or<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
+        this->get_parameter_or<bool>("mapping.extrinsic_est_en", extrinsic_est_en, true);
+        this->get_parameter_or<bool>("pcd_save.pcd_save_en", pcd_save_en, false);
+        this->get_parameter_or<int>("pcd_save.interval", pcd_save_interval, -1);
+        this->get_parameter_or<vector<double>>("mapping.extrinsic_T", extrinT, vector<double>());
+        this->get_parameter_or<vector<double>>("mapping.extrinsic_R", extrinR, vector<double>());
+
+        RCLCPP_INFO(this->get_logger(), "p_pre->lidar_type %d", p_pre->lidar_type);
+
+        path.header.stamp = this->get_clock()->now();
+        path.header.frame_id ="camera_init";
+
+        // /*** variables definition ***/
+        // int effect_feat_num = 0, frame_num = 0;
+        // double deltaT, deltaR, aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_const_H_time = 0;
+        // bool flg_EKF_converged, EKF_stop_flg = 0;
+
+        FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0);
+        HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0);
+
+        _featsArray.reset(new PointCloudXYZI());
+
+        memset(point_selected_surf, true, sizeof(point_selected_surf));
+        memset(res_last, -1000.0f, sizeof(res_last));
+        downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
+        downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min);
+        memset(point_selected_surf, true, sizeof(point_selected_surf));
+        memset(res_last, -1000.0f, sizeof(res_last));
+
+        Lidar_T_wrt_IMU<<VEC_FROM_ARRAY(extrinT);
+        Lidar_R_wrt_IMU<<MAT_FROM_ARRAY(extrinR);
+        p_imu->set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU);
+        p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov));
+        p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov));
+        p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov));
+        p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov));
+
+        fill(epsi, epsi+23, 0.001);
+        kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi);
+
+        /*** debug record ***/
+        // FILE *fp;
+        string pos_log_dir = root_dir + "/Log/pos_log.txt";
+        fp = fopen(pos_log_dir.c_str(),"w");
+
+        // ofstream fout_pre, fout_out, fout_dbg;
+        fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),ios::out);
+        fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out);
+        fout_dbg.open(DEBUG_FILE_DIR("dbg.txt"),ios::out);
+        if (fout_pre && fout_out)
+            cout << "~~~~"<<ROOT_DIR<<" file opened" << endl;
+        else
+            cout << "~~~~"<<ROOT_DIR<<" doesn't exist" << endl;
+
+        /*** ROS subscribe initialization ***/
+        if (p_pre->lidar_type == AVIA)
+        {
+            sub_pcl_livox_ = this->create_subscription<livox_ros_driver2::msg::CustomMsg>(lid_topic, 20, livox_pcl_cbk);
+        }
+        else
+        {
+            sub_pcl_pc_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(lid_topic, rclcpp::SensorDataQoS(), standard_pcl_cbk);
+        }
+        sub_imu_ = this->create_subscription<sensor_msgs::msg::Imu>(imu_topic, 10, imu_cbk);
+        pubLaserCloudFull_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_registered", 20);
+        pubLaserCloudFull_body_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_registered_body", 20);
+        pubLaserCloudEffect_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_effected", 20);
+        pubLaserCloudMap_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/Laser_map", 20);
+        pubOdomAftMapped_ = this->create_publisher<nav_msgs::msg::Odometry>("/Odometry", 20);
+        pubPath_ = this->create_publisher<nav_msgs::msg::Path>("/path", 20);
+        tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+
+        //------------------------------------------------------------------------------------------------------
+        auto period_ms = std::chrono::milliseconds(static_cast<int64_t>(1000.0 / 100.0));
+        timer_ = rclcpp::create_timer(this, this->get_clock(), period_ms, std::bind(&LaserMappingNode::timer_callback, this));
+
+        auto map_period_ms = std::chrono::milliseconds(static_cast<int64_t>(1000.0));
+        map_pub_timer_ = rclcpp::create_timer(this, this->get_clock(), map_period_ms, std::bind(&LaserMappingNode::map_publish_callback, this));
+
+        map_save_srv_ = this->create_service<std_srvs::srv::Trigger>("map_save", std::bind(&LaserMappingNode::map_save_callback, this, std::placeholders::_1, std::placeholders::_2));
+
+        RCLCPP_INFO(this->get_logger(), "Node init finished.");
+    }
+
+    ~LaserMappingNode()
+    {
+        fout_out.close();
+        fout_pre.close();
+        fclose(fp);
+    }
+
+private:
+    void timer_callback()
+    {
+        if(sync_packages(Measures))
+        {
+            if (flg_first_scan)
+            {
+                first_lidar_time = Measures.lidar_beg_time;
+                p_imu->first_lidar_time = first_lidar_time;
+                flg_first_scan = false;
+                return;
+            }
+
+            double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time;
+
+            match_time = 0;
+            kdtree_search_time = 0.0;
+            solve_time = 0;
+            solve_const_H_time = 0;
+            svd_time   = 0;
+            t0 = omp_get_wtime();
+
+            p_imu->Process(Measures, kf, feats_undistort);
+            state_point = kf.get_x();
+            pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;
+
+            if (feats_undistort->empty() || (feats_undistort == NULL))
+            {
+                RCLCPP_WARN(this->get_logger(), "No point, skip this scan!\n");
+                return;
+            }
+
+            flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \
+                            false : true;
+            /*** Segment the map in lidar FOV ***/
+            lasermap_fov_segment();
+
+            /*** downsample the feature points in a scan ***/
+            downSizeFilterSurf.setInputCloud(feats_undistort);
+            downSizeFilterSurf.filter(*feats_down_body);
+            t1 = omp_get_wtime();
+            feats_down_size = feats_down_body->points.size();
+            /*** initialize the map kdtree ***/
+            if(ikdtree.Root_Node == nullptr)
+            {
+                RCLCPP_INFO(this->get_logger(), "Initialize the map kdtree");
+                if(feats_down_size > 5)
+                {
+                    ikdtree.set_downsample_param(filter_size_map_min);
+                    feats_down_world->resize(feats_down_size);
+                    for(int i = 0; i < feats_down_size; i++)
+                    {
+                        pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
+                    }
+                    ikdtree.Build(feats_down_world->points);
+                }
+                return;
+            }
+            int featsFromMapNum = ikdtree.validnum();
+            kdtree_size_st = ikdtree.size();
+            
+            // cout<<"[ mapping ]: In num: "<<feats_undistort->points.size()<<" downsamp "<<feats_down_size<<" Map num: "<<featsFromMapNum<<"effect num:"<<effct_feat_num<<endl;
+
+            /*** ICP and iterated Kalman filter update ***/
+            if (feats_down_size < 5)
+            {
+                RCLCPP_WARN(this->get_logger(), "No point, skip this scan!\n");
+                return;
+            }
+            
+            normvec->resize(feats_down_size);
+            feats_down_world->resize(feats_down_size);
+
+            V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I);
+            fout_pre<<setw(20)<<Measures.lidar_beg_time - first_lidar_time<<" "<<euler_cur.transpose()<<" "<< state_point.pos.transpose()<<" "<<ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<< " " << state_point.vel.transpose() \
+            <<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<< endl;
+
+            if(0) // If you need to see map point, change to "if(1)"
+            {
+                PointVector ().swap(ikdtree.PCL_Storage);
+                ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
+                featsFromMap->clear();
+                featsFromMap->points = ikdtree.PCL_Storage;
+            }
+
+            pointSearchInd_surf.resize(feats_down_size);
+            Nearest_Points.resize(feats_down_size);
+            int  rematch_num = 0;
+            bool nearest_search_en = true; //
+
+            t2 = omp_get_wtime();
+            
+            /*** iterated state estimation ***/
+            double t_update_start = omp_get_wtime();
+            double solve_H_time = 0;
+            kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);
+            state_point = kf.get_x();
+            euler_cur = SO3ToEuler(state_point.rot);
+            pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;
+            geoQuat.x = state_point.rot.coeffs()[0];
+            geoQuat.y = state_point.rot.coeffs()[1];
+            geoQuat.z = state_point.rot.coeffs()[2];
+            geoQuat.w = state_point.rot.coeffs()[3];
+
+            double t_update_end = omp_get_wtime();
+
+            /******* Publish odometry *******/
+            publish_odometry(pubOdomAftMapped_, tf_broadcaster_);
+
+            /*** add the feature points to map kdtree ***/
+            t3 = omp_get_wtime();
+            map_incremental();
+            t5 = omp_get_wtime();
+            
+            /******* Publish points *******/
+            if (path_en)                         publish_path(pubPath_);
+            if (scan_pub_en)      publish_frame_world(pubLaserCloudFull_);
+            if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body_);
+            if (effect_pub_en) publish_effect_world(pubLaserCloudEffect_);
+            // if (map_pub_en) publish_map(pubLaserCloudMap_);
+
+            /*** Debug variables ***/
+            if (runtime_pos_log)
+            {
+                frame_num ++;
+                kdtree_size_end = ikdtree.size();
+                aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num;
+                aver_time_icp = aver_time_icp * (frame_num - 1)/frame_num + (t_update_end - t_update_start) / frame_num;
+                aver_time_match = aver_time_match * (frame_num - 1)/frame_num + (match_time)/frame_num;
+                aver_time_incre = aver_time_incre * (frame_num - 1)/frame_num + (kdtree_incremental_time)/frame_num;
+                aver_time_solve = aver_time_solve * (frame_num - 1)/frame_num + (solve_time + solve_H_time)/frame_num;
+                aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1)/frame_num + solve_time / frame_num;
+                T1[time_log_counter] = Measures.lidar_beg_time;
+                s_plot[time_log_counter] = t5 - t0;
+                s_plot2[time_log_counter] = feats_undistort->points.size();
+                s_plot3[time_log_counter] = kdtree_incremental_time;
+                s_plot4[time_log_counter] = kdtree_search_time;
+                s_plot5[time_log_counter] = kdtree_delete_counter;
+                s_plot6[time_log_counter] = kdtree_delete_time;
+                s_plot7[time_log_counter] = kdtree_size_st;
+                s_plot8[time_log_counter] = kdtree_size_end;
+                s_plot9[time_log_counter] = aver_time_consu;
+                s_plot10[time_log_counter] = add_point_size;
+                time_log_counter ++;
+                printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f  ave ICP: %0.6f  map incre: %0.6f ave total: %0.6f icp: %0.6f construct H: %0.6f \n",t1-t0,aver_time_match,aver_time_solve,t3-t1,t5-t3,aver_time_consu,aver_time_icp, aver_time_const_H_time);
+                ext_euler = SO3ToEuler(state_point.offset_R_L_I);
+                fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose()<< " " << ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<<" "<< state_point.vel.transpose() \
+                <<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<<" "<<feats_undistort->points.size()<<endl;
+                dump_lio_state_to_log(fp);
+            }
+        }
+    }
+
+    void map_publish_callback()
+    {
+        if (map_pub_en) publish_map(pubLaserCloudMap_);
+    }
+
+    void map_save_callback(std_srvs::srv::Trigger::Request::ConstSharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr res)
+    {
+        RCLCPP_INFO(this->get_logger(), "Saving map to %s...", map_file_path.c_str());
+        if (pcd_save_en)
+        {
+            save_to_pcd();
+            res->success = true;
+            res->message = "Map saved.";
+        }
+        else
+        {
+            res->success = false;
+            res->message = "Map save disabled.";
+        }
+    }
+
+private:
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFull_;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFull_body_;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudEffect_;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudMap_;
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped_;
+    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath_;
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu_;
+    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pcl_pc_;
+    rclcpp::Subscription<livox_ros_driver2::msg::CustomMsg>::SharedPtr sub_pcl_livox_;
+
+    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::TimerBase::SharedPtr map_pub_timer_;
+    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr map_save_srv_;
+
+    bool effect_pub_en = false, map_pub_en = false;
+    int effect_feat_num = 0, frame_num = 0;
+    double deltaT, deltaR, aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_const_H_time = 0;
+    bool flg_EKF_converged, EKF_stop_flg = 0;
+    double epsi[23] = {0.001};
+
+    FILE *fp;
+    ofstream fout_pre, fout_out, fout_dbg;
+};
+
+int main(int argc, char** argv)
+{
+    rclcpp::init(argc, argv);
+
+    signal(SIGINT, SigHandle);
+
+    rclcpp::spin(std::make_shared<LaserMappingNode>());
+
+    if (rclcpp::ok())
+        rclcpp::shutdown();
+    /**************** save map ****************/
+    /* 1. make sure you have enough memories
+    /* 2. pcd save will largely influence the real-time performences **/
+    if (pcl_wait_save->size() > 0 && pcd_save_en)
+    {
+        string file_name = string("scans.pcd");
+        string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
+        pcl::PCDWriter pcd_writer;
+        cout << "current scan saved to /PCD/" << file_name<<endl;
+        pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
+    }
+
+    if (runtime_pos_log)
+    {
+        vector<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;    
+        FILE *fp2;
+        string log_dir = root_dir + "/Log/fast_lio_time_log.csv";
+        fp2 = fopen(log_dir.c_str(),"w");
+        fprintf(fp2,"time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time\n");
+        for (int i = 0;i<time_log_counter; i++){
+            fprintf(fp2,"%0.8f,%0.8f,%d,%0.8f,%0.8f,%d,%0.8f,%d,%d,%d,%0.8f\n",T1[i],s_plot[i],int(s_plot2[i]),s_plot3[i],s_plot4[i],int(s_plot5[i]),s_plot6[i],int(s_plot7[i]),int(s_plot8[i]), int(s_plot10[i]), s_plot11[i]);
+            t.push_back(T1[i]);
+            s_vec.push_back(s_plot9[i]);
+            s_vec2.push_back(s_plot3[i] + s_plot6[i]);
+            s_vec3.push_back(s_plot4[i]);
+            s_vec5.push_back(s_plot[i]);
+        }
+        fclose(fp2);
+    }
+
+    return 0;
+}
diff --git a/FAST_LIO/src/preprocess.cpp b/FAST_LIO/src/preprocess.cpp
new file mode 100644
index 0000000..88444f4
--- /dev/null
+++ b/FAST_LIO/src/preprocess.cpp
@@ -0,0 +1,1069 @@
+#include "preprocess.h"
+
+#include <pcl/common/common.h>
+
+#define RETURN0 0x00
+#define RETURN0AND1 0x10
+
+Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
+{
+  inf_bound = 10;
+  N_SCANS = 6;
+  SCAN_RATE = 10;
+  group_size = 8;
+  disA = 0.01;
+  disA = 0.1;  // B?
+  p2l_ratio = 225;
+  limit_maxmid = 6.25;
+  limit_midmin = 6.25;
+  limit_maxmin = 3.24;
+  jump_up_limit = 170.0;
+  jump_down_limit = 8.0;
+  cos160 = 160.0;
+  edgea = 2;
+  edgeb = 0.1;
+  smallp_intersect = 172.5;
+  smallp_ratio = 1.2;
+  given_offset_time = false;
+
+  jump_up_limit = cos(jump_up_limit / 180 * M_PI);
+  jump_down_limit = cos(jump_down_limit / 180 * M_PI);
+  cos160 = cos(cos160 / 180 * M_PI);
+  smallp_intersect = cos(smallp_intersect / 180 * M_PI);
+}
+
+Preprocess::~Preprocess()
+{
+}
+
+void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
+{
+  feature_enabled = feat_en;
+  lidar_type = lid_type;
+  blind = bld;
+  point_filter_num = pfilt_num;
+}
+
+void Preprocess::process(const livox_ros_driver2::msg::CustomMsg::UniquePtr &msg, PointCloudXYZI::Ptr& pcl_out)
+{
+  avia_handler(msg);
+  *pcl_out = pl_surf;
+}
+
+void Preprocess::process(const sensor_msgs::msg::PointCloud2::UniquePtr &msg, PointCloudXYZI::Ptr& pcl_out)
+{
+  switch (time_unit)
+  {
+    case SEC:
+      time_unit_scale = 1.e3f;
+      break;
+    case MS:
+      time_unit_scale = 1.f;
+      break;
+    case US:
+      time_unit_scale = 1.e-3f;
+      break;
+    case NS:
+      time_unit_scale = 1.e-6f;
+      break;
+    default:
+      time_unit_scale = 1.f;
+      break;
+  }
+
+  switch (lidar_type)
+  {
+    case OUST64:
+      oust64_handler(msg);
+      break;
+
+    case VELO16:
+      velodyne_handler(msg);
+      break;
+
+    case MID360:
+      mid360_handler(msg);
+      break;
+
+    default:
+      default_handler(msg);
+      break;
+  }
+  *pcl_out = pl_surf;
+}
+
+void Preprocess::avia_handler(const livox_ros_driver2::msg::CustomMsg::UniquePtr &msg)
+{
+  pl_surf.clear();
+  pl_corn.clear();
+  pl_full.clear();
+  double t1 = omp_get_wtime();
+  int plsize = msg->point_num;
+  // cout<<"plsie: "<<plsize<<endl;
+
+  pl_corn.reserve(plsize);
+  pl_surf.reserve(plsize);
+  pl_full.resize(plsize);
+
+  for (int i = 0; i < N_SCANS; i++)
+  {
+    pl_buff[i].clear();
+    pl_buff[i].reserve(plsize);
+  }
+  uint valid_num = 0;
+
+  if (feature_enabled)
+  {
+    for (uint i = 1; i < plsize; i++)
+    {
+      if ((msg->points[i].line < N_SCANS) &&
+          ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
+      {
+        pl_full[i].x = msg->points[i].x;
+        pl_full[i].y = msg->points[i].y;
+        pl_full[i].z = msg->points[i].z;
+        pl_full[i].intensity = msg->points[i].reflectivity;
+        pl_full[i].curvature =
+            msg->points[i].offset_time / float(1000000);  // use curvature as time of each laser points
+
+        bool is_new = false;
+        if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) ||
+            (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7))
+        {
+          pl_buff[msg->points[i].line].push_back(pl_full[i]);
+        }
+      }
+    }
+    static int count = 0;
+    static double time = 0.0;
+    count++;
+    double t0 = omp_get_wtime();
+    for (int j = 0; j < N_SCANS; j++)
+    {
+      if (pl_buff[j].size() <= 5)
+        continue;
+      pcl::PointCloud<PointType>& pl = pl_buff[j];
+      plsize = pl.size();
+      vector<orgtype>& types = typess[j];
+      types.clear();
+      types.resize(plsize);
+      plsize--;
+      for (uint i = 0; i < plsize; i++)
+      {
+        types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
+        vx = pl[i].x - pl[i + 1].x;
+        vy = pl[i].y - pl[i + 1].y;
+        vz = pl[i].z - pl[i + 1].z;
+        types[i].dista = sqrt(vx * vx + vy * vy + vz * vz);
+      }
+      types[plsize].range = sqrt(pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y);
+      give_feature(pl, types);
+      // pl_surf += pl;
+    }
+    time += omp_get_wtime() - t0;
+    printf("Feature extraction time: %lf \n", time / count);
+  }
+  else
+  {
+    for (uint i = 1; i < plsize; i++)
+    {
+      if ((msg->points[i].line < N_SCANS) &&
+          ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
+      {
+        valid_num++;
+        if (valid_num % point_filter_num == 0)
+        {
+          pl_full[i].x = msg->points[i].x;
+          pl_full[i].y = msg->points[i].y;
+          pl_full[i].z = msg->points[i].z;
+          pl_full[i].intensity = msg->points[i].reflectivity;
+          pl_full[i].curvature = msg->points[i].offset_time /
+                                 float(1000000);  // use curvature as time of each laser points, curvature unit: ms
+
+          if(((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7)
+              || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
+              || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7))
+              && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind)))
+          {
+            pl_surf.push_back(pl_full[i]);
+          }
+        }
+      }
+    }
+  }
+}
+
+void Preprocess::oust64_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg)
+{
+  pl_surf.clear();
+  pl_corn.clear();
+  pl_full.clear();
+  pcl::PointCloud<ouster_ros::Point> pl_orig;
+  pcl::fromROSMsg(*msg, pl_orig);
+  int plsize = pl_orig.size();
+  pl_corn.reserve(plsize);
+  pl_surf.reserve(plsize);
+  if (feature_enabled)
+  {
+    for (int i = 0; i < N_SCANS; i++)
+    {
+      pl_buff[i].clear();
+      pl_buff[i].reserve(plsize);
+    }
+
+    for (uint i = 0; i < plsize; i++)
+    {
+      double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y +
+                     pl_orig.points[i].z * pl_orig.points[i].z;
+      if (range < (blind * blind))
+        continue;
+      Eigen::Vector3d pt_vec;
+      PointType added_pt;
+      added_pt.x = pl_orig.points[i].x;
+      added_pt.y = pl_orig.points[i].y;
+      added_pt.z = pl_orig.points[i].z;
+      added_pt.intensity = pl_orig.points[i].intensity;
+      added_pt.normal_x = 0;
+      added_pt.normal_y = 0;
+      added_pt.normal_z = 0;
+      double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
+      if (yaw_angle >= 180.0)
+        yaw_angle -= 360.0;
+      if (yaw_angle <= -180.0)
+        yaw_angle += 360.0;
+
+      added_pt.curvature = pl_orig.points[i].t * time_unit_scale;
+      if (pl_orig.points[i].ring < N_SCANS)
+      {
+        pl_buff[pl_orig.points[i].ring].push_back(added_pt);
+      }
+    }
+
+    for (int j = 0; j < N_SCANS; j++)
+    {
+      PointCloudXYZI& pl = pl_buff[j];
+      int linesize = pl.size();
+      vector<orgtype>& types = typess[j];
+      types.clear();
+      types.resize(linesize);
+      linesize--;
+      for (uint i = 0; i < linesize; i++)
+      {
+        types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
+        vx = pl[i].x - pl[i + 1].x;
+        vy = pl[i].y - pl[i + 1].y;
+        vz = pl[i].z - pl[i + 1].z;
+        types[i].dista = vx * vx + vy * vy + vz * vz;
+      }
+      types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
+      give_feature(pl, types);
+    }
+  }
+  else
+  {
+    double time_stamp = rclcpp::Time(msg->header.stamp).seconds();
+    // cout << "===================================" << endl;
+    // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS);
+    for (int i = 0; i < pl_orig.points.size(); i++)
+    {
+      if (i % point_filter_num != 0)
+        continue;
+
+      double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y +
+                     pl_orig.points[i].z * pl_orig.points[i].z;
+
+      if (range < (blind * blind))
+        continue;
+
+      Eigen::Vector3d pt_vec;
+      PointType added_pt;
+      added_pt.x = pl_orig.points[i].x;
+      added_pt.y = pl_orig.points[i].y;
+      added_pt.z = pl_orig.points[i].z;
+      added_pt.intensity = pl_orig.points[i].intensity;
+      added_pt.normal_x = 0;
+      added_pt.normal_y = 0;
+      added_pt.normal_z = 0;
+      added_pt.curvature = pl_orig.points[i].t * time_unit_scale;  // curvature unit: ms
+
+      pl_surf.points.push_back(added_pt);
+    }
+  }
+  // pub_func(pl_surf, pub_full, msg->header.stamp);
+  // pub_func(pl_surf, pub_corn, msg->header.stamp);
+}
+
+void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg)
+{
+  pl_surf.clear();
+  pl_corn.clear();
+  pl_full.clear();
+
+  pcl::PointCloud<velodyne_ros::Point> pl_orig;
+  pcl::fromROSMsg(*msg, pl_orig);
+  int plsize = pl_orig.points.size();
+  if (plsize == 0)
+    return;
+  pl_surf.reserve(plsize);
+
+  /*** These variables only works when no point timestamps given ***/
+  double omega_l = 0.361 * SCAN_RATE;  // scan angular velocity
+  std::vector<bool> is_first(N_SCANS, true);
+  std::vector<double> yaw_fp(N_SCANS, 0.0);    // yaw of first scan point
+  std::vector<float> yaw_last(N_SCANS, 0.0);   // yaw of last scan point
+  std::vector<float> time_last(N_SCANS, 0.0);  // last offset time
+  /*****************************************************************/
+
+  if (pl_orig.points[plsize - 1].time > 0)
+  {
+    given_offset_time = true;
+  }
+  else
+  {
+    given_offset_time = false;
+    double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
+    double yaw_end = yaw_first;
+    int layer_first = pl_orig.points[0].ring;
+    for (uint i = plsize - 1; i > 0; i--)
+    {
+      if (pl_orig.points[i].ring == layer_first)
+      {
+        yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
+        break;
+      }
+    }
+  }
+
+  if (feature_enabled)
+  {
+    for (int i = 0; i < N_SCANS; i++)
+    {
+      pl_buff[i].clear();
+      pl_buff[i].reserve(plsize);
+    }
+
+    for (int i = 0; i < plsize; i++)
+    {
+      PointType added_pt;
+      added_pt.normal_x = 0;
+      added_pt.normal_y = 0;
+      added_pt.normal_z = 0;
+      int layer = pl_orig.points[i].ring;
+      if (layer >= N_SCANS)
+        continue;
+      added_pt.x = pl_orig.points[i].x;
+      added_pt.y = pl_orig.points[i].y;
+      added_pt.z = pl_orig.points[i].z;
+      added_pt.intensity = pl_orig.points[i].intensity;
+      added_pt.curvature = pl_orig.points[i].time * time_unit_scale;  // units: ms
+
+      if (!given_offset_time)
+      {
+        double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
+        if (is_first[layer])
+        {
+          // printf("layer: %d; is first: %d", layer, is_first[layer]);
+          yaw_fp[layer] = yaw_angle;
+          is_first[layer] = false;
+          added_pt.curvature = 0.0;
+          yaw_last[layer] = yaw_angle;
+          time_last[layer] = added_pt.curvature;
+          continue;
+        }
+
+        if (yaw_angle <= yaw_fp[layer])
+        {
+          added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
+        }
+        else
+        {
+          added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
+        }
+
+        if (added_pt.curvature < time_last[layer])
+          added_pt.curvature += 360.0 / omega_l;
+
+        yaw_last[layer] = yaw_angle;
+        time_last[layer] = added_pt.curvature;
+      }
+
+      pl_buff[layer].points.push_back(added_pt);
+    }
+
+    for (int j = 0; j < N_SCANS; j++)
+    {
+      PointCloudXYZI& pl = pl_buff[j];
+      int linesize = pl.size();
+      if (linesize < 2)
+        continue;
+      vector<orgtype>& types = typess[j];
+      types.clear();
+      types.resize(linesize);
+      linesize--;
+      for (uint i = 0; i < linesize; i++)
+      {
+        types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
+        vx = pl[i].x - pl[i + 1].x;
+        vy = pl[i].y - pl[i + 1].y;
+        vz = pl[i].z - pl[i + 1].z;
+        types[i].dista = vx * vx + vy * vy + vz * vz;
+      }
+      types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
+      give_feature(pl, types);
+    }
+  }
+  else
+  {
+    for (int i = 0; i < plsize; i++)
+    {
+      PointType added_pt;
+      // cout<<"!!!!!!"<<i<<" "<<plsize<<endl;
+
+      added_pt.normal_x = 0;
+      added_pt.normal_y = 0;
+      added_pt.normal_z = 0;
+      added_pt.x = pl_orig.points[i].x;
+      added_pt.y = pl_orig.points[i].y;
+      added_pt.z = pl_orig.points[i].z;
+      added_pt.intensity = pl_orig.points[i].intensity;
+      added_pt.curvature =
+          pl_orig.points[i].time * time_unit_scale;  // curvature unit: ms // cout<<added_pt.curvature<<endl;
+
+      if (!given_offset_time)
+      {
+        int layer = pl_orig.points[i].ring;
+        double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
+
+        if (is_first[layer])
+        {
+          // printf("layer: %d; is first: %d", layer, is_first[layer]);
+          yaw_fp[layer] = yaw_angle;
+          is_first[layer] = false;
+          added_pt.curvature = 0.0;
+          yaw_last[layer] = yaw_angle;
+          time_last[layer] = added_pt.curvature;
+          continue;
+        }
+
+        // compute offset time
+        if (yaw_angle <= yaw_fp[layer])
+        {
+          added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
+        }
+        else
+        {
+          added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
+        }
+
+        if (added_pt.curvature < time_last[layer])
+          added_pt.curvature += 360.0 / omega_l;
+
+        yaw_last[layer] = yaw_angle;
+        time_last[layer] = added_pt.curvature;
+      }
+
+      if (i % point_filter_num == 0)
+      {
+        if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind))
+        {
+          pl_surf.points.push_back(added_pt);
+        }
+      }
+    }
+  }
+}
+
+void Preprocess::mid360_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg)
+{
+  pl_surf.clear();
+  pl_corn.clear();
+  pl_full.clear();
+
+  pcl::PointCloud<livox_ros::LivoxPointXyzitl> pl_orig;
+  pcl::fromROSMsg(*msg, pl_orig);
+  int plsize = pl_orig.points.size();
+  if (plsize == 0)
+    return;
+  pl_surf.reserve(plsize);
+
+  /*** These variables only works when no point timestamps given ***/
+  double omega_l = 0.361 * SCAN_RATE;  // scan angular velocity
+  std::vector<bool> is_first(N_SCANS, true);
+  std::vector<double> yaw_fp(N_SCANS, 0.0);    // yaw of first scan point
+  std::vector<float> yaw_last(N_SCANS, 0.0);   // yaw of last scan point
+  std::vector<float> time_last(N_SCANS, 0.0);  // last offset time
+  /*****************************************************************/
+
+  given_offset_time = false;
+  double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
+  double yaw_end = yaw_first;
+  int layer_first = pl_orig.points[0].line;
+  for (uint i = plsize - 1; i > 0; i--)
+  {
+    if (pl_orig.points[i].line == layer_first)
+    {
+      yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
+      break;
+    }
+  }
+
+  for (uint i = 0; i < plsize; ++i)
+  {
+    PointType added_pt;
+    added_pt.normal_x = 0;
+    added_pt.normal_y = 0;
+    added_pt.normal_z = 0;
+    added_pt.x = pl_orig.points[i].x;
+    added_pt.y = pl_orig.points[i].y;
+    added_pt.z = pl_orig.points[i].z;
+    added_pt.intensity = pl_orig.points[i].intensity;
+    added_pt.curvature = 0.;
+
+    int layer = pl_orig.points[i].line;
+    double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
+
+    if (is_first[layer])
+    {
+      // printf("layer: %d; is first: %d", layer, is_first[layer]);
+      yaw_fp[layer] = yaw_angle;
+      is_first[layer] = false;
+      added_pt.curvature = 0.0;
+      yaw_last[layer] = yaw_angle;
+      time_last[layer] = added_pt.curvature;
+      continue;
+    }
+
+    // compute offset time
+    if (yaw_angle <= yaw_fp[layer])
+    {
+      added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
+    }
+    else
+    {
+      added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
+    }
+
+    if (added_pt.curvature < time_last[layer])
+      added_pt.curvature += 360.0 / omega_l;
+
+    yaw_last[layer] = yaw_angle;
+    time_last[layer] = added_pt.curvature;
+
+    if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind))
+    {
+      pl_surf.push_back(std::move(added_pt));
+    }
+  }
+}
+
+void Preprocess::default_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg)
+{
+  pl_surf.clear();
+  pl_corn.clear();
+  pl_full.clear();
+
+  pcl::PointCloud<pcl::PointXYZI> pl_orig;
+  pcl::fromROSMsg(*msg, pl_orig);
+  int plsize = pl_orig.points.size();
+  if (plsize == 0)
+    return;
+  pl_surf.reserve(plsize);
+
+  for(uint i = 0; i < plsize; ++i)
+  {
+    PointType added_pt;
+    added_pt.normal_x = 0;
+    added_pt.normal_y = 0;
+    added_pt.normal_z = 0;
+    added_pt.x = pl_orig.points[i].x;
+    added_pt.y = pl_orig.points[i].y;
+    added_pt.z = pl_orig.points[i].z;
+    added_pt.intensity = pl_orig.points[i].intensity;
+    added_pt.curvature = 0.;
+
+    if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind))
+    {
+      pl_surf.push_back(std::move(added_pt));
+    }
+  }
+}
+
+void Preprocess::give_feature(pcl::PointCloud<PointType>& pl, vector<orgtype>& types)
+{
+  int plsize = pl.size();
+  int plsize2;
+  if (plsize == 0)
+  {
+    printf("something wrong\n");
+    return;
+  }
+  uint head = 0;
+
+  while (types[head].range < blind)
+  {
+    head++;
+  }
+
+  // Surf
+  plsize2 = (plsize > group_size) ? (plsize - group_size) : 0;
+
+  Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero());
+  Eigen::Vector3d last_direct(Eigen::Vector3d::Zero());
+
+  uint i_nex = 0, i2;
+  uint last_i = 0;
+  uint last_i_nex = 0;
+  int last_state = 0;
+  int plane_type;
+
+  for (uint i = head; i < plsize2; i++)
+  {
+    if (types[i].range < blind)
+    {
+      continue;
+    }
+
+    i2 = i;
+
+    plane_type = plane_judge(pl, types, i, i_nex, curr_direct);
+
+    if (plane_type == 1)
+    {
+      for (uint j = i; j <= i_nex; j++)
+      {
+        if (j != i && j != i_nex)
+        {
+          types[j].ftype = Real_Plane;
+        }
+        else
+        {
+          types[j].ftype = Poss_Plane;
+        }
+      }
+
+      // if(last_state==1 && fabs(last_direct.sum())>0.5)
+      if (last_state == 1 && last_direct.norm() > 0.1)
+      {
+        double mod = last_direct.transpose() * curr_direct;
+        if (mod > -0.707 && mod < 0.707)
+        {
+          types[i].ftype = Edge_Plane;
+        }
+        else
+        {
+          types[i].ftype = Real_Plane;
+        }
+      }
+
+      i = i_nex - 1;
+      last_state = 1;
+    }
+    else  // if(plane_type == 2)
+    {
+      i = i_nex;
+      last_state = 0;
+    }
+    // else if(plane_type == 0)
+    // {
+    //   if(last_state == 1)
+    //   {
+    //     uint i_nex_tem;
+    //     uint j;
+    //     for(j=last_i+1; j<=last_i_nex; j++)
+    //     {
+    //       uint i_nex_tem2 = i_nex_tem;
+    //       Eigen::Vector3d curr_direct2;
+
+    //       uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2);
+
+    //       if(ttem != 1)
+    //       {
+    //         i_nex_tem = i_nex_tem2;
+    //         break;
+    //       }
+    //       curr_direct = curr_direct2;
+    //     }
+
+    //     if(j == last_i+1)
+    //     {
+    //       last_state = 0;
+    //     }
+    //     else
+    //     {
+    //       for(uint k=last_i_nex; k<=i_nex_tem; k++)
+    //       {
+    //         if(k != i_nex_tem)
+    //         {
+    //           types[k].ftype = Real_Plane;
+    //         }
+    //         else
+    //         {
+    //           types[k].ftype = Poss_Plane;
+    //         }
+    //       }
+    //       i = i_nex_tem-1;
+    //       i_nex = i_nex_tem;
+    //       i2 = j-1;
+    //       last_state = 1;
+    //     }
+
+    //   }
+    // }
+
+    last_i = i2;
+    last_i_nex = i_nex;
+    last_direct = curr_direct;
+  }
+
+  plsize2 = plsize > 3 ? plsize - 3 : 0;
+  for (uint i = head + 3; i < plsize2; i++)
+  {
+    if (types[i].range < blind || types[i].ftype >= Real_Plane)
+    {
+      continue;
+    }
+
+    if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16)
+    {
+      continue;
+    }
+
+    Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z);
+    Eigen::Vector3d vecs[2];
+
+    for (int j = 0; j < 2; j++)
+    {
+      int m = -1;
+      if (j == 1)
+      {
+        m = 1;
+      }
+
+      if (types[i + m].range < blind)
+      {
+        if (types[i].range > inf_bound)
+        {
+          types[i].edj[j] = Nr_inf;
+        }
+        else
+        {
+          types[i].edj[j] = Nr_blind;
+        }
+        continue;
+      }
+
+      vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z);
+      vecs[j] = vecs[j] - vec_a;
+
+      types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm();
+      if (types[i].angle[j] < jump_up_limit)
+      {
+        types[i].edj[j] = Nr_180;
+      }
+      else if (types[i].angle[j] > jump_down_limit)
+      {
+        types[i].edj[j] = Nr_zero;
+      }
+    }
+
+    types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm();
+    if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 &&
+        types[i].dista > 4 * types[i - 1].dista)
+    {
+      if (types[i].intersect > cos160)
+      {
+        if (edge_jump_judge(pl, types, i, Prev))
+        {
+          types[i].ftype = Edge_Jump;
+        }
+      }
+    }
+    else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 &&
+             types[i - 1].dista > 4 * types[i].dista)
+    {
+      if (types[i].intersect > cos160)
+      {
+        if (edge_jump_judge(pl, types, i, Next))
+        {
+          types[i].ftype = Edge_Jump;
+        }
+      }
+    }
+    else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf)
+    {
+      if (edge_jump_judge(pl, types, i, Prev))
+      {
+        types[i].ftype = Edge_Jump;
+      }
+    }
+    else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor)
+    {
+      if (edge_jump_judge(pl, types, i, Next))
+      {
+        types[i].ftype = Edge_Jump;
+      }
+    }
+    else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor)
+    {
+      if (types[i].ftype == Nor)
+      {
+        types[i].ftype = Wire;
+      }
+    }
+  }
+
+  plsize2 = plsize - 1;
+  double ratio;
+  for (uint i = head + 1; i < plsize2; i++)
+  {
+    if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind)
+    {
+      continue;
+    }
+
+    if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8)
+    {
+      continue;
+    }
+
+    if (types[i].ftype == Nor)
+    {
+      if (types[i - 1].dista > types[i].dista)
+      {
+        ratio = types[i - 1].dista / types[i].dista;
+      }
+      else
+      {
+        ratio = types[i].dista / types[i - 1].dista;
+      }
+
+      if (types[i].intersect < smallp_intersect && ratio < smallp_ratio)
+      {
+        if (types[i - 1].ftype == Nor)
+        {
+          types[i - 1].ftype = Real_Plane;
+        }
+        if (types[i + 1].ftype == Nor)
+        {
+          types[i + 1].ftype = Real_Plane;
+        }
+        types[i].ftype = Real_Plane;
+      }
+    }
+  }
+
+  int last_surface = -1;
+  for (uint j = head; j < plsize; j++)
+  {
+    if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane)
+    {
+      if (last_surface == -1)
+      {
+        last_surface = j;
+      }
+
+      if (j == uint(last_surface + point_filter_num - 1))
+      {
+        PointType ap;
+        ap.x = pl[j].x;
+        ap.y = pl[j].y;
+        ap.z = pl[j].z;
+        ap.intensity = pl[j].intensity;
+        ap.curvature = pl[j].curvature;
+        pl_surf.push_back(ap);
+
+        last_surface = -1;
+      }
+    }
+    else
+    {
+      if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane)
+      {
+        pl_corn.push_back(pl[j]);
+      }
+      if (last_surface != -1)
+      {
+        PointType ap;
+        for (uint k = last_surface; k < j; k++)
+        {
+          ap.x += pl[k].x;
+          ap.y += pl[k].y;
+          ap.z += pl[k].z;
+          ap.intensity += pl[k].intensity;
+          ap.curvature += pl[k].curvature;
+        }
+        ap.x /= (j - last_surface);
+        ap.y /= (j - last_surface);
+        ap.z /= (j - last_surface);
+        ap.intensity /= (j - last_surface);
+        ap.curvature /= (j - last_surface);
+        pl_surf.push_back(ap);
+      }
+      last_surface = -1;
+    }
+  }
+}
+
+void Preprocess::pub_func(PointCloudXYZI& pl, const rclcpp::Time& ct)
+{
+  pl.height = 1;
+  pl.width = pl.size();
+  sensor_msgs::msg::PointCloud2 output;
+  pcl::toROSMsg(pl, output);
+  output.header.frame_id = "livox";
+  output.header.stamp = ct;
+}
+
+int Preprocess::plane_judge(const PointCloudXYZI& pl, vector<orgtype>& types, uint i_cur, uint& i_nex,
+                            Eigen::Vector3d& curr_direct)
+{
+  double group_dis = disA * types[i_cur].range + disB;
+  group_dis = group_dis * group_dis;
+  // i_nex = i_cur;
+
+  double two_dis;
+  vector<double> disarr;
+  disarr.reserve(20);
+
+  for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++)
+  {
+    if (types[i_nex].range < blind)
+    {
+      curr_direct.setZero();
+      return 2;
+    }
+    disarr.push_back(types[i_nex].dista);
+  }
+
+  for (;;)
+  {
+    if ((i_cur >= pl.size()) || (i_nex >= pl.size()))
+      break;
+
+    if (types[i_nex].range < blind)
+    {
+      curr_direct.setZero();
+      return 2;
+    }
+    vx = pl[i_nex].x - pl[i_cur].x;
+    vy = pl[i_nex].y - pl[i_cur].y;
+    vz = pl[i_nex].z - pl[i_cur].z;
+    two_dis = vx * vx + vy * vy + vz * vz;
+    if (two_dis >= group_dis)
+    {
+      break;
+    }
+    disarr.push_back(types[i_nex].dista);
+    i_nex++;
+  }
+
+  double leng_wid = 0;
+  double v1[3], v2[3];
+  for (uint j = i_cur + 1; j < i_nex; j++)
+  {
+    if ((j >= pl.size()) || (i_cur >= pl.size()))
+      break;
+    v1[0] = pl[j].x - pl[i_cur].x;
+    v1[1] = pl[j].y - pl[i_cur].y;
+    v1[2] = pl[j].z - pl[i_cur].z;
+
+    v2[0] = v1[1] * vz - vy * v1[2];
+    v2[1] = v1[2] * vx - v1[0] * vz;
+    v2[2] = v1[0] * vy - vx * v1[1];
+
+    double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2];
+    if (lw > leng_wid)
+    {
+      leng_wid = lw;
+    }
+  }
+
+  if ((two_dis * two_dis / leng_wid) < p2l_ratio)
+  {
+    curr_direct.setZero();
+    return 0;
+  }
+
+  uint disarrsize = disarr.size();
+  for (uint j = 0; j < disarrsize - 1; j++)
+  {
+    for (uint k = j + 1; k < disarrsize; k++)
+    {
+      if (disarr[j] < disarr[k])
+      {
+        leng_wid = disarr[j];
+        disarr[j] = disarr[k];
+        disarr[k] = leng_wid;
+      }
+    }
+  }
+
+  if (disarr[disarr.size() - 2] < 1e-16)
+  {
+    curr_direct.setZero();
+    return 0;
+  }
+
+  if (lidar_type == AVIA)
+  {
+    double dismax_mid = disarr[0] / disarr[disarrsize / 2];
+    double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2];
+
+    if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin)
+    {
+      curr_direct.setZero();
+      return 0;
+    }
+  }
+  else
+  {
+    double dismax_min = disarr[0] / disarr[disarrsize - 2];
+    if (dismax_min >= limit_maxmin)
+    {
+      curr_direct.setZero();
+      return 0;
+    }
+  }
+
+  curr_direct << vx, vy, vz;
+  curr_direct.normalize();
+  return 1;
+}
+
+bool Preprocess::edge_jump_judge(const PointCloudXYZI& pl, vector<orgtype>& types, uint i, Surround nor_dir)
+{
+  if (nor_dir == 0)
+  {
+    if (types[i - 1].range < blind || types[i - 2].range < blind)
+    {
+      return false;
+    }
+  }
+  else if (nor_dir == 1)
+  {
+    if (types[i + 1].range < blind || types[i + 2].range < blind)
+    {
+      return false;
+    }
+  }
+  double d1 = types[i + nor_dir - 1].dista;
+  double d2 = types[i + 3 * nor_dir - 2].dista;
+  double d;
+
+  if (d1 < d2)
+  {
+    d = d1;
+    d1 = d2;
+    d2 = d;
+  }
+
+  d1 = sqrt(d1);
+  d2 = sqrt(d2);
+
+  if (d1 > edgea * d2 || (d1 - d2) > edgeb)
+  {
+    return false;
+  }
+
+  return true;
+}
diff --git a/FAST_LIO/src/preprocess.h b/FAST_LIO/src/preprocess.h
new file mode 100644
index 0000000..7f7d7a1
--- /dev/null
+++ b/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;
+};
diff --git a/agv_pro_bringup/launch/agv_pro_bringup.launch.py b/agv_pro_bringup/launch/agv_pro_bringup.launch.py
index 3791282..699f8d8 100644
--- a/agv_pro_bringup/launch/agv_pro_bringup.launch.py
+++ b/agv_pro_bringup/launch/agv_pro_bringup.launch.py
@@ -6,10 +6,26 @@ from launch.substitutions import Command,LaunchConfiguration,PythonExpression
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python.packages import get_package_share_directory
 
+def include_lidar(pkg_name, launch_file, lidar_type, expected_type):
+
+    return IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory(pkg_name),
+                'launch',
+                launch_file
+            )
+        ),
+        condition=PythonExpression(
+            ["'", lidar_type, "' == '", expected_type, "'"]
+        )
+    )
+
 def generate_launch_description():
 
     port_name_arg = LaunchConfiguration('port_name',default='/dev/agvpro_controller')
     namespace = LaunchConfiguration('namespace', default='')
+    lidar_type = LaunchConfiguration('lidar_type', default='n10p')
 
     urdf_file = os.path.join(
         get_package_share_directory('agv_pro_description'),
@@ -24,48 +40,67 @@ def generate_launch_description():
         PythonExpression(['"', namespace, '" + "/" if "', namespace, '" != "" else ""']),
     ])
 
-    return LaunchDescription([
-        DeclareLaunchArgument(
-            'port_name', 
-            default_value=port_name_arg,
-            description='port name, e.g. ttyACM0'),
+    declare_port_name_arg = DeclareLaunchArgument(
+        'port_name', 
+        default_value=port_name_arg,
+        description='port name, e.g. /dev/ttyACM0'
+    )
 
-        DeclareLaunchArgument(
-            'namespace',
-            default_value='',
-            description='Namespace for nodes'),
+    declare_namespace_arg = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Namespace for nodes'
+    )
 
-        PushRosNamespace(namespace),
+    declare_lidar_type_arg = DeclareLaunchArgument(
+        'lidar_type',
+        default_value=lidar_type,
+        description='Lidar type: n10p | mid360 | l2'
+    )
 
-        Node(
-            package='agv_pro_base',
-            executable='agv_pro_node',
-            name='agv_pro_node',
-            output='screen',
-            parameters=[{
-                'port_name': port_name_arg,
-                'namespace': namespace,             
-            }],
-            remappings=[('cmd_vel', '/cmd_vel')]
-        ),
+    ns_action = PushRosNamespace(namespace)
 
-        Node(
-            package='joint_state_publisher',
-            executable='joint_state_publisher',
-            name='joint_state_publisher'
-        ),
+    agv_pro_node = Node(
+        package='agv_pro_base',
+        executable='agv_pro_node',
+        name='agv_pro_node',
+        output='screen',
+        parameters=[{
+            'port_name': port_name_arg,
+            'namespace': namespace,             
+        }],
+        remappings=[('cmd_vel', '/cmd_vel')]
+    )
 
-        Node(
-            package='robot_state_publisher',
-            executable='robot_state_publisher',
-            name='robot_state_publisher',
-            parameters=[{'robot_description': robot_description_content}],
-            output='screen'
-        ),
+    joint_state_pub = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher'
+    )
 
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([os.path.join(
-                get_package_share_directory('lslidar_driver'),'launch'),
-                '/lsn10p_launch.py'])
-        )
-    ])
+    robot_state_pub = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        parameters=[{'robot_description': robot_description_content}],
+        output='screen'
+    )
+
+    lidar_launchs = [
+        include_lidar('lslidar_driver', 'lsn10p_launch.py', lidar_type, 'n10p'),
+        include_lidar('livox_ros_driver2', 'msg_MID360_launch.py', lidar_type, 'mid360'),
+        include_lidar('unitree_lidar_ros2', 'launch.py', lidar_type, 'l2'),
+    ]
+
+    return LaunchDescription(
+        [
+            declare_port_name_arg,
+            declare_namespace_arg,
+            declare_lidar_type_arg,
+            ns_action,
+            agv_pro_node,
+            joint_state_pub,
+            robot_state_pub,
+            *lidar_launchs,
+        ]
+    )
\ No newline at end of file
diff --git a/livox_ros_driver2/.gitignore b/livox_ros_driver2/.gitignore
new file mode 100644
index 0000000..d7c3a84
--- /dev/null
+++ b/livox_ros_driver2/.gitignore
@@ -0,0 +1,4 @@
+.vscode
+build
+package.xml
+__pycache__
\ No newline at end of file
diff --git a/livox_ros_driver2/3rdparty/rapidjson/allocators.h b/livox_ros_driver2/3rdparty/rapidjson/allocators.h
new file mode 100644
index 0000000..03010d5
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/allocators.h
@@ -0,0 +1,308 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_ALLOCATORS_H_
+#define RAPIDJSON_ALLOCATORS_H_
+
+#include "rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// Allocator
+
+/*! \class rapidjson::Allocator
+    \brief Concept for allocating, resizing and freeing memory block.
+
+    Note that Malloc() and Realloc() are non-static but Free() is static.
+
+    So if an allocator need to support Free(), it needs to put its pointer in
+    the header of memory block.
+
+\code
+concept Allocator {
+    static const bool kNeedFree;    //!< Whether this allocator needs to call
+Free().
+
+    // Allocate a memory block.
+    // \param size of the memory block in bytes.
+    // \returns pointer to the memory block.
+    void* Malloc(size_t size);
+
+    // Resize a memory block.
+    // \param originalPtr The pointer to current memory block. Null pointer is
+permitted.
+    // \param originalSize The current size in bytes. (Design issue: since some
+allocator may not book-keep this, explicitly pass to it can save memory.)
+    // \param newSize the new size in bytes.
+    void* Realloc(void* originalPtr, size_t originalSize, size_t newSize);
+
+    // Free a memory block.
+    // \param pointer to the memory block. Null pointer is permitted.
+    static void Free(void *ptr);
+};
+\endcode
+*/
+
+/*! \def RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
+    \ingroup RAPIDJSON_CONFIG
+    \brief User-defined kDefaultChunkCapacity definition.
+
+    User can define this as any \c size that is a power of 2.
+*/
+
+#ifndef RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
+#define RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY (64 * 1024)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// CrtAllocator
+
+//! C-runtime library allocator.
+/*! This class is just wrapper for standard C library memory routines.
+    \note implements Allocator concept
+*/
+class CrtAllocator {
+ public:
+  static const bool kNeedFree = true;
+  void *Malloc(size_t size) {
+    if (size)  //  behavior of malloc(0) is implementation defined.
+      return std::malloc(size);
+    else
+      return NULL;  // standardize to returning NULL.
+  }
+  void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
+    (void)originalSize;
+    if (newSize == 0) {
+      std::free(originalPtr);
+      return NULL;
+    }
+    return std::realloc(originalPtr, newSize);
+  }
+  static void Free(void *ptr) { std::free(ptr); }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// MemoryPoolAllocator
+
+//! Default memory allocator used by the parser and DOM.
+/*! This allocator allocate memory blocks from pre-allocated memory chunks.
+
+    It does not free memory blocks. And Realloc() only allocate new memory.
+
+    The memory chunks are allocated by BaseAllocator, which is CrtAllocator by
+   default.
+
+    User may also supply a buffer as the first chunk.
+
+    If the user-buffer is full then additional chunks are allocated by
+   BaseAllocator.
+
+    The user-buffer is not deallocated by this allocator.
+
+    \tparam BaseAllocator the allocator type for allocating memory chunks.
+   Default is CrtAllocator. \note implements Allocator concept
+*/
+template <typename BaseAllocator = CrtAllocator>
+class MemoryPoolAllocator {
+ public:
+  static const bool kNeedFree =
+      false;  //!< Tell users that no need to call Free() with this allocator.
+              //!< (concept Allocator)
+
+  //! Constructor with chunkSize.
+  /*! \param chunkSize The size of memory chunk. The default is
+     kDefaultChunkSize. \param baseAllocator The allocator for allocating memory
+     chunks.
+  */
+  MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity,
+                      BaseAllocator *baseAllocator = 0)
+      : chunkHead_(0),
+        chunk_capacity_(chunkSize),
+        userBuffer_(0),
+        baseAllocator_(baseAllocator),
+        ownBaseAllocator_(0) {}
+
+  //! Constructor with user-supplied buffer.
+  /*! The user buffer will be used firstly. When it is full, memory pool
+     allocates new chunk with chunk size.
+
+      The user buffer will not be deallocated when this allocator is destructed.
+
+      \param buffer User supplied buffer.
+      \param size Size of the buffer in bytes. It must at least larger than
+     sizeof(ChunkHeader). \param chunkSize The size of memory chunk. The default
+     is kDefaultChunkSize. \param baseAllocator The allocator for allocating
+     memory chunks.
+  */
+  MemoryPoolAllocator(void *buffer, size_t size,
+                      size_t chunkSize = kDefaultChunkCapacity,
+                      BaseAllocator *baseAllocator = 0)
+      : chunkHead_(0),
+        chunk_capacity_(chunkSize),
+        userBuffer_(buffer),
+        baseAllocator_(baseAllocator),
+        ownBaseAllocator_(0) {
+    RAPIDJSON_ASSERT(buffer != 0);
+    RAPIDJSON_ASSERT(size > sizeof(ChunkHeader));
+    chunkHead_ = reinterpret_cast<ChunkHeader *>(buffer);
+    chunkHead_->capacity = size - sizeof(ChunkHeader);
+    chunkHead_->size = 0;
+    chunkHead_->next = 0;
+  }
+
+  //! Destructor.
+  /*! This deallocates all memory chunks, excluding the user-supplied buffer.
+   */
+  ~MemoryPoolAllocator() {
+    Clear();
+    RAPIDJSON_DELETE(ownBaseAllocator_);
+  }
+
+  //! Deallocates all memory chunks, excluding the user-supplied buffer.
+  void Clear() {
+    while (chunkHead_ && chunkHead_ != userBuffer_) {
+      ChunkHeader *next = chunkHead_->next;
+      baseAllocator_->Free(chunkHead_);
+      chunkHead_ = next;
+    }
+    if (chunkHead_ && chunkHead_ == userBuffer_)
+      chunkHead_->size = 0;  // Clear user buffer
+  }
+
+  //! Computes the total capacity of allocated memory chunks.
+  /*! \return total capacity in bytes.
+   */
+  size_t Capacity() const {
+    size_t capacity = 0;
+    for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
+      capacity += c->capacity;
+    return capacity;
+  }
+
+  //! Computes the memory blocks allocated.
+  /*! \return total used bytes.
+   */
+  size_t Size() const {
+    size_t size = 0;
+    for (ChunkHeader *c = chunkHead_; c != 0; c = c->next) size += c->size;
+    return size;
+  }
+
+  //! Allocates a memory block. (concept Allocator)
+  void *Malloc(size_t size) {
+    if (!size) return NULL;
+
+    size = RAPIDJSON_ALIGN(size);
+    if (chunkHead_ == 0 || chunkHead_->size + size > chunkHead_->capacity)
+      if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
+        return NULL;
+
+    void *buffer = reinterpret_cast<char *>(chunkHead_) +
+                   RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
+    chunkHead_->size += size;
+    return buffer;
+  }
+
+  //! Resizes a memory block (concept Allocator)
+  void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
+    if (originalPtr == 0) return Malloc(newSize);
+
+    if (newSize == 0) return NULL;
+
+    originalSize = RAPIDJSON_ALIGN(originalSize);
+    newSize = RAPIDJSON_ALIGN(newSize);
+
+    // Do not shrink if new size is smaller than original
+    if (originalSize >= newSize) return originalPtr;
+
+    // Simply expand it if it is the last allocation and there is sufficient
+    // space
+    if (originalPtr ==
+        reinterpret_cast<char *>(chunkHead_) +
+            RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size -
+            originalSize) {
+      size_t increment = static_cast<size_t>(newSize - originalSize);
+      if (chunkHead_->size + increment <= chunkHead_->capacity) {
+        chunkHead_->size += increment;
+        return originalPtr;
+      }
+    }
+
+    // Realloc process: allocate and copy memory, do not free original buffer.
+    if (void *newBuffer = Malloc(newSize)) {
+      if (originalSize) std::memcpy(newBuffer, originalPtr, originalSize);
+      return newBuffer;
+    } else
+      return NULL;
+  }
+
+  //! Frees a memory block (concept Allocator)
+  static void Free(void *ptr) { (void)ptr; }  // Do nothing
+
+ private:
+  //! Copy constructor is not permitted.
+  MemoryPoolAllocator(const MemoryPoolAllocator &rhs) /* = delete */;
+  //! Copy assignment operator is not permitted.
+  MemoryPoolAllocator &operator=(const MemoryPoolAllocator &rhs) /* = delete */;
+
+  //! Creates a new chunk.
+  /*! \param capacity Capacity of the chunk in bytes.
+      \return true if success.
+  */
+  bool AddChunk(size_t capacity) {
+    if (!baseAllocator_)
+      ownBaseAllocator_ = baseAllocator_ = RAPIDJSON_NEW(BaseAllocator)();
+    if (ChunkHeader *chunk =
+            reinterpret_cast<ChunkHeader *>(baseAllocator_->Malloc(
+                RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) {
+      chunk->capacity = capacity;
+      chunk->size = 0;
+      chunk->next = chunkHead_;
+      chunkHead_ = chunk;
+      return true;
+    } else
+      return false;
+  }
+
+  static const int kDefaultChunkCapacity =
+      RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY;  //!< Default chunk capacity.
+
+  //! Chunk header for perpending to each chunk.
+  /*! Chunks are stored as a singly linked list.
+   */
+  struct ChunkHeader {
+    size_t capacity;  //!< Capacity of the chunk in bytes (excluding the header
+                      //!< itself).
+    size_t size;      //!< Current size of allocated memory in bytes.
+    ChunkHeader *next;  //!< Next chunk in the linked list.
+  };
+
+  ChunkHeader *chunkHead_;  //!< Head of the chunk linked-list. Only the head
+                            //!< chunk serves allocation.
+  size_t chunk_capacity_;   //!< The minimum capacity of chunk when they are
+                            //!< allocated.
+  void *userBuffer_;        //!< User supplied buffer.
+  BaseAllocator
+      *baseAllocator_;  //!< base allocator for allocating memory chunks.
+  BaseAllocator *ownBaseAllocator_;  //!< base allocator created by this object.
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_ENCODINGS_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/cursorstreamwrapper.h b/livox_ros_driver2/3rdparty/rapidjson/cursorstreamwrapper.h
new file mode 100644
index 0000000..045ddb8
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/cursorstreamwrapper.h
@@ -0,0 +1,81 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_
+#define RAPIDJSON_CURSORSTREAMWRAPPER_H_
+
+#include "stream.h"
+
+#if defined(__GNUC__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#if defined(_MSC_VER) && _MSC_VER <= 1800
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Cursor stream wrapper for counting line and column number if error exists.
+/*!
+    \tparam InputStream     Any stream that implements Stream Concept
+*/
+template <typename InputStream, typename Encoding = UTF8<>>
+class CursorStreamWrapper : public GenericStreamWrapper<InputStream, Encoding> {
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  CursorStreamWrapper(InputStream &is)
+      : GenericStreamWrapper<InputStream, Encoding>(is), line_(1), col_(0) {}
+
+  // counting line and column number
+  Ch Take() {
+    Ch ch = this->is_.Take();
+    if (ch == '\n') {
+      line_++;
+      col_ = 0;
+    } else {
+      col_++;
+    }
+    return ch;
+  }
+
+  //! Get the error line number, if error exists.
+  size_t GetLine() const { return line_; }
+  //! Get the error column number, if error exists.
+  size_t GetColumn() const { return col_; }
+
+ private:
+  size_t line_;  //!< Current Line
+  size_t col_;   //!< Current Column
+};
+
+#if defined(_MSC_VER) && _MSC_VER <= 1800
+RAPIDJSON_DIAG_POP
+#endif
+
+#if defined(__GNUC__)
+RAPIDJSON_DIAG_POP
+#endif
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_CURSORSTREAMWRAPPER_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/document.h b/livox_ros_driver2/3rdparty/rapidjson/document.h
new file mode 100644
index 0000000..27addb7
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/document.h
@@ -0,0 +1,3309 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_DOCUMENT_H_
+#define RAPIDJSON_DOCUMENT_H_
+
+/*! \file document.h */
+
+#include <limits>
+#include <new>  // placement new
+#include "encodedstream.h"
+#include "internal/meta.h"
+#include "internal/strfunc.h"
+#include "memorystream.h"
+#include "reader.h"
+
+RAPIDJSON_DIAG_PUSH
+#ifdef __clang__
+RAPIDJSON_DIAG_OFF(padded)
+RAPIDJSON_DIAG_OFF(switch - enum)
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_OFF(4127)  // conditional expression is constant
+RAPIDJSON_DIAG_OFF(
+    4244)  // conversion from kXxxFlags to 'uint16_t', possible loss of data
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_OFF(effc++)
+#endif  // __GNUC__
+
+#ifndef RAPIDJSON_NOMEMBERITERATORCLASS
+#include <iterator>  // std::random_access_iterator_tag
+#endif
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+#include <utility>  // std::move
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+// Forward declaration.
+template <typename Encoding, typename Allocator>
+class GenericValue;
+
+template <typename Encoding, typename Allocator, typename StackAllocator>
+class GenericDocument;
+
+//! Name-value pair in a JSON object value.
+/*!
+    This class was internal to GenericValue. It used to be a inner struct.
+    But a compiler (IBM XL C/C++ for AIX) have reported to have problem with
+   that so it moved as a namespace scope struct.
+    https://code.google.com/p/rapidjson/issues/detail?id=64
+*/
+template <typename Encoding, typename Allocator>
+class GenericMember {
+ public:
+  GenericValue<Encoding, Allocator>
+      name;  //!< name of member (must be a string)
+  GenericValue<Encoding, Allocator> value;  //!< value of member.
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  //! Move constructor in C++11
+  GenericMember(GenericMember &&rhs) RAPIDJSON_NOEXCEPT
+      : name(std::move(rhs.name)),
+        value(std::move(rhs.value)) {}
+
+  //! Move assignment in C++11
+  GenericMember &operator=(GenericMember &&rhs) RAPIDJSON_NOEXCEPT {
+    return *this = static_cast<GenericMember &>(rhs);
+  }
+#endif
+
+  //! Assignment with move semantics.
+  /*! \param rhs Source of the assignment. Its name and value will become a null
+   * value after assignment.
+   */
+  GenericMember &operator=(GenericMember &rhs) RAPIDJSON_NOEXCEPT {
+    if (RAPIDJSON_LIKELY(this != &rhs)) {
+      name = rhs.name;
+      value = rhs.value;
+    }
+    return *this;
+  }
+
+  // swap() for std::sort() and other potential use in STL.
+  friend inline void swap(GenericMember &a,
+                          GenericMember &b) RAPIDJSON_NOEXCEPT {
+    a.name.Swap(b.name);
+    a.value.Swap(b.value);
+  }
+
+ private:
+  //! Copy constructor is not permitted.
+  GenericMember(const GenericMember &rhs);
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericMemberIterator
+
+#ifndef RAPIDJSON_NOMEMBERITERATORCLASS
+
+//! (Constant) member iterator for a JSON object value
+/*!
+    \tparam Const Is this a constant iterator?
+    \tparam Encoding    Encoding of the value. (Even non-string values need to
+   have the same encoding in a document) \tparam Allocator   Allocator type for
+   allocating memory of object, array and string.
+
+    This class implements a Random Access Iterator for GenericMember elements
+    of a GenericValue, see ISO/IEC 14882:2003(E) C++ standard, 24.1
+   [lib.iterator.requirements].
+
+    \note This iterator implementation is mainly intended to avoid implicit
+        conversions from iterator values to \c NULL,
+        e.g. from GenericValue::FindMember.
+
+    \note Define \c RAPIDJSON_NOMEMBERITERATORCLASS to fall back to a
+        pointer-based implementation, if your platform doesn't provide
+        the C++ <iterator> header.
+
+    \see GenericMember, GenericValue::MemberIterator,
+   GenericValue::ConstMemberIterator
+ */
+template <bool Const, typename Encoding, typename Allocator>
+class GenericMemberIterator {
+  friend class GenericValue<Encoding, Allocator>;
+  template <bool, typename, typename>
+  friend class GenericMemberIterator;
+
+  typedef GenericMember<Encoding, Allocator> PlainType;
+  typedef typename internal::MaybeAddConst<Const, PlainType>::Type ValueType;
+
+ public:
+  //! Iterator type itself
+  typedef GenericMemberIterator Iterator;
+  //! Constant iterator type
+  typedef GenericMemberIterator<true, Encoding, Allocator> ConstIterator;
+  //! Non-constant iterator type
+  typedef GenericMemberIterator<false, Encoding, Allocator> NonConstIterator;
+
+  /** \name std::iterator_traits support */
+  //@{
+  typedef ValueType value_type;
+  typedef ValueType *pointer;
+  typedef ValueType &reference;
+  typedef std::ptrdiff_t difference_type;
+  typedef std::random_access_iterator_tag iterator_category;
+  //@}
+
+  //! Pointer to (const) GenericMember
+  typedef pointer Pointer;
+  //! Reference to (const) GenericMember
+  typedef reference Reference;
+  //! Signed integer type (e.g. \c ptrdiff_t)
+  typedef difference_type DifferenceType;
+
+  //! Default constructor (singular value)
+  /*! Creates an iterator pointing to no element.
+      \note All operations, except for comparisons, are undefined on such
+     values.
+   */
+  GenericMemberIterator() : ptr_() {}
+
+  //! Iterator conversions to more const
+  /*!
+      \param it (Non-const) iterator to copy from
+
+      Allows the creation of an iterator from another GenericMemberIterator
+      that is "less const".  Especially, creating a non-constant iterator
+      from a constant iterator are disabled:
+      \li const -> non-const (not ok)
+      \li const -> const (ok)
+      \li non-const -> const (ok)
+      \li non-const -> non-const (ok)
+
+      \note If the \c Const template parameter is already \c false, this
+          constructor effectively defines a regular copy-constructor.
+          Otherwise, the copy constructor is implicitly defined.
+  */
+  GenericMemberIterator(const NonConstIterator &it) : ptr_(it.ptr_) {}
+  Iterator &operator=(const NonConstIterator &it) {
+    ptr_ = it.ptr_;
+    return *this;
+  }
+
+  //! @name stepping
+  //@{
+  Iterator &operator++() {
+    ++ptr_;
+    return *this;
+  }
+  Iterator &operator--() {
+    --ptr_;
+    return *this;
+  }
+  Iterator operator++(int) {
+    Iterator old(*this);
+    ++ptr_;
+    return old;
+  }
+  Iterator operator--(int) {
+    Iterator old(*this);
+    --ptr_;
+    return old;
+  }
+  //@}
+
+  //! @name increment/decrement
+  //@{
+  Iterator operator+(DifferenceType n) const { return Iterator(ptr_ + n); }
+  Iterator operator-(DifferenceType n) const { return Iterator(ptr_ - n); }
+
+  Iterator &operator+=(DifferenceType n) {
+    ptr_ += n;
+    return *this;
+  }
+  Iterator &operator-=(DifferenceType n) {
+    ptr_ -= n;
+    return *this;
+  }
+  //@}
+
+  //! @name relations
+  //@{
+  bool operator==(ConstIterator that) const { return ptr_ == that.ptr_; }
+  bool operator!=(ConstIterator that) const { return ptr_ != that.ptr_; }
+  bool operator<=(ConstIterator that) const { return ptr_ <= that.ptr_; }
+  bool operator>=(ConstIterator that) const { return ptr_ >= that.ptr_; }
+  bool operator<(ConstIterator that) const { return ptr_ < that.ptr_; }
+  bool operator>(ConstIterator that) const { return ptr_ > that.ptr_; }
+  //@}
+
+  //! @name dereference
+  //@{
+  Reference operator*() const { return *ptr_; }
+  Pointer operator->() const { return ptr_; }
+  Reference operator[](DifferenceType n) const { return ptr_[n]; }
+  //@}
+
+  //! Distance
+  DifferenceType operator-(ConstIterator that) const {
+    return ptr_ - that.ptr_;
+  }
+
+ private:
+  //! Internal constructor from plain pointer
+  explicit GenericMemberIterator(Pointer p) : ptr_(p) {}
+
+  Pointer ptr_;  //!< raw pointer
+};
+
+#else  // RAPIDJSON_NOMEMBERITERATORCLASS
+
+// class-based member iterator implementation disabled, use plain pointers
+
+template <bool Const, typename Encoding, typename Allocator>
+class GenericMemberIterator;
+
+//! non-const GenericMemberIterator
+template <typename Encoding, typename Allocator>
+class GenericMemberIterator<false, Encoding, Allocator> {
+  //! use plain pointer as iterator type
+  typedef GenericMember<Encoding, Allocator> *Iterator;
+};
+//! const GenericMemberIterator
+template <typename Encoding, typename Allocator>
+class GenericMemberIterator<true, Encoding, Allocator> {
+  //! use plain const pointer as iterator type
+  typedef const GenericMember<Encoding, Allocator> *Iterator;
+};
+
+#endif  // RAPIDJSON_NOMEMBERITERATORCLASS
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericStringRef
+
+//! Reference to a constant string (not taking a copy)
+/*!
+    \tparam CharType character type of the string
+
+    This helper class is used to automatically infer constant string
+    references for string literals, especially from \c const \b (!)
+    character arrays.
+
+    The main use is for creating JSON string values without copying the
+    source string via an \ref Allocator.  This requires that the referenced
+    string pointers have a sufficient lifetime, which exceeds the lifetime
+    of the associated GenericValue.
+
+    \b Example
+    \code
+    Value v("foo");   // ok, no need to copy & calculate length
+    const char foo[] = "foo";
+    v.SetString(foo); // ok
+
+    const char* bar = foo;
+    // Value x(bar); // not ok, can't rely on bar's lifetime
+    Value x(StringRef(bar)); // lifetime explicitly guaranteed by user
+    Value y(StringRef(bar, 3));  // ok, explicitly pass length
+    \endcode
+
+    \see StringRef, GenericValue::SetString
+*/
+template <typename CharType>
+struct GenericStringRef {
+  typedef CharType Ch;  //!< character type of the string
+
+//! Create string reference from \c const character array
+#ifndef __clang__  // -Wdocumentation
+                   /*!
+                       This constructor implicitly creates a constant string reference from
+                       a \c const character array.  It has better performance than
+                       \ref StringRef(const CharType*) by inferring the string \ref length
+                       from the array length, and also supports strings containing null
+                       characters.
+                   
+                       \tparam N length of the string, automatically inferred
+                   
+                       \param str Constant character array, lifetime assumed to be longer
+                           than the use of the string in e.g. a GenericValue
+                   
+                       \post \ref s == str
+                   
+                       \note Constant complexity.
+                       \note There is a hidden, private overload to disallow references to
+                           non-const character arrays to be created via this constructor.
+                           By this, e.g. function-scope arrays used to be filled via
+                           \c snprintf are excluded from consideration.
+                           In such cases, the referenced string should be \b copied to the
+                           GenericValue instead.
+                    */
+#endif
+  template <SizeType N>
+  GenericStringRef(const CharType (&str)[N]) RAPIDJSON_NOEXCEPT
+      : s(str),
+        length(N - 1) {}
+
+//! Explicitly create string reference from \c const character pointer
+#ifndef __clang__  // -Wdocumentation
+                   /*!
+                       This constructor can be used to \b explicitly  create a reference to
+                       a constant string pointer.
+                   
+                       \see StringRef(const CharType*)
+                   
+                       \param str Constant character pointer, lifetime assumed to be longer
+                           than the use of the string in e.g. a GenericValue
+                   
+                       \post \ref s == str
+                   
+                       \note There is a hidden, private overload to disallow references to
+                           non-const character arrays to be created via this constructor.
+                           By this, e.g. function-scope arrays used to be filled via
+                           \c snprintf are excluded from consideration.
+                           In such cases, the referenced string should be \b copied to the
+                           GenericValue instead.
+                    */
+#endif
+  explicit GenericStringRef(const CharType *str)
+      : s(str), length(NotNullStrLen(str)) {}
+
+//! Create constant string reference from pointer and length
+#ifndef __clang__  // -Wdocumentation
+/*! \param str constant string, lifetime assumed to be longer than the use of
+   the string in e.g. a GenericValue \param len length of the string,
+   excluding the trailing NULL terminator
+
+    \post \ref s == str && \ref length == len
+    \note Constant complexity.
+ */
+#endif
+  GenericStringRef(const CharType *str, SizeType len)
+      : s(RAPIDJSON_LIKELY(str) ? str : emptyString), length(len) {
+    RAPIDJSON_ASSERT(str != 0 || len == 0u);
+  }
+
+  GenericStringRef(const GenericStringRef &rhs)
+      : s(rhs.s), length(rhs.length) {}
+
+  //! implicit conversion to plain CharType pointer
+  operator const Ch *() const { return s; }
+
+  const Ch *const s;      //!< plain CharType pointer
+  const SizeType length;  //!< length of the string (excluding the trailing NULL
+                          //!terminator)
+
+ private:
+  SizeType NotNullStrLen(const CharType *str) {
+    RAPIDJSON_ASSERT(str != 0);
+    return internal::StrLen(str);
+  }
+
+  /// Empty string - used when passing in a NULL pointer
+  static const Ch emptyString[];
+
+  //! Disallow construction from non-const array
+  template <SizeType N>
+  GenericStringRef(CharType (&str)[N]) /* = delete */;
+  //! Copy assignment operator not permitted - immutable type
+  GenericStringRef &operator=(const GenericStringRef &rhs) /* = delete */;
+};
+
+template <typename CharType>
+const CharType GenericStringRef<CharType>::emptyString[] = {CharType()};
+
+//! Mark a character pointer as constant string
+/*! Mark a plain character pointer as a "string literal".  This function
+    can be used to avoid copying a character string to be referenced as a
+    value in a JSON GenericValue object, if the string's lifetime is known
+    to be valid long enough.
+    \tparam CharType Character type of the string
+    \param str Constant string, lifetime assumed to be longer than the use of
+   the string in e.g. a GenericValue \return GenericStringRef string reference
+   object \relatesalso GenericStringRef
+
+    \see GenericValue::GenericValue(StringRefType),
+   GenericValue::operator=(StringRefType),
+   GenericValue::SetString(StringRefType), GenericValue::PushBack(StringRefType,
+   Allocator&), GenericValue::AddMember
+*/
+template <typename CharType>
+inline GenericStringRef<CharType> StringRef(const CharType *str) {
+  return GenericStringRef<CharType>(str);
+}
+
+//! Mark a character pointer as constant string
+/*! Mark a plain character pointer as a "string literal".  This function
+    can be used to avoid copying a character string to be referenced as a
+    value in a JSON GenericValue object, if the string's lifetime is known
+    to be valid long enough.
+
+    This version has better performance with supplied length, and also
+    supports string containing null characters.
+
+    \tparam CharType character type of the string
+    \param str Constant string, lifetime assumed to be longer than the use of
+   the string in e.g. a GenericValue \param length The length of source string.
+    \return GenericStringRef string reference object
+    \relatesalso GenericStringRef
+*/
+template <typename CharType>
+inline GenericStringRef<CharType> StringRef(const CharType *str,
+                                            size_t length) {
+  return GenericStringRef<CharType>(str, SizeType(length));
+}
+
+#if RAPIDJSON_HAS_STDSTRING
+//! Mark a string object as constant string
+/*! Mark a string object (e.g. \c std::string) as a "string literal".
+    This function can be used to avoid copying a string to be referenced as a
+    value in a JSON GenericValue object, if the string's lifetime is known
+    to be valid long enough.
+
+    \tparam CharType character type of the string
+    \param str Constant string, lifetime assumed to be longer than the use of
+   the string in e.g. a GenericValue \return GenericStringRef string reference
+   object \relatesalso GenericStringRef \note Requires the definition of the
+   preprocessor symbol \ref RAPIDJSON_HAS_STDSTRING.
+*/
+template <typename CharType>
+inline GenericStringRef<CharType> StringRef(
+    const std::basic_string<CharType> &str) {
+  return GenericStringRef<CharType>(str.data(), SizeType(str.size()));
+}
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericValue type traits
+namespace internal {
+
+template <typename T, typename Encoding = void, typename Allocator = void>
+struct IsGenericValueImpl : FalseType {};
+
+// select candidates according to nested encoding and allocator types
+template <typename T>
+struct IsGenericValueImpl<T, typename Void<typename T::EncodingType>::Type,
+                          typename Void<typename T::AllocatorType>::Type>
+    : IsBaseOf<
+          GenericValue<typename T::EncodingType, typename T::AllocatorType>,
+          T>::Type {};
+
+// helper to match arbitrary GenericValue instantiations, including derived
+// classes
+template <typename T>
+struct IsGenericValue : IsGenericValueImpl<T>::Type {};
+
+}  // namespace internal
+
+///////////////////////////////////////////////////////////////////////////////
+// TypeHelper
+
+namespace internal {
+
+template <typename ValueType, typename T>
+struct TypeHelper {};
+
+template <typename ValueType>
+struct TypeHelper<ValueType, bool> {
+  static bool Is(const ValueType &v) { return v.IsBool(); }
+  static bool Get(const ValueType &v) { return v.GetBool(); }
+  static ValueType &Set(ValueType &v, bool data) { return v.SetBool(data); }
+  static ValueType &Set(ValueType &v, bool data,
+                        typename ValueType::AllocatorType &) {
+    return v.SetBool(data);
+  }
+};
+
+template <typename ValueType>
+struct TypeHelper<ValueType, int> {
+  static bool Is(const ValueType &v) { return v.IsInt(); }
+  static int Get(const ValueType &v) { return v.GetInt(); }
+  static ValueType &Set(ValueType &v, int data) { return v.SetInt(data); }
+  static ValueType &Set(ValueType &v, int data,
+                        typename ValueType::AllocatorType &) {
+    return v.SetInt(data);
+  }
+};
+
+template <typename ValueType>
+struct TypeHelper<ValueType, unsigned> {
+  static bool Is(const ValueType &v) { return v.IsUint(); }
+  static unsigned Get(const ValueType &v) { return v.GetUint(); }
+  static ValueType &Set(ValueType &v, unsigned data) { return v.SetUint(data); }
+  static ValueType &Set(ValueType &v, unsigned data,
+                        typename ValueType::AllocatorType &) {
+    return v.SetUint(data);
+  }
+};
+
+#ifdef _MSC_VER
+RAPIDJSON_STATIC_ASSERT(sizeof(long) == sizeof(int));
+template <typename ValueType>
+struct TypeHelper<ValueType, long> {
+  static bool Is(const ValueType &v) { return v.IsInt(); }
+  static long Get(const ValueType &v) { return v.GetInt(); }
+  static ValueType &Set(ValueType &v, long data) { return v.SetInt(data); }
+  static ValueType &Set(ValueType &v, long data,
+                        typename ValueType::AllocatorType &) {
+    return v.SetInt(data);
+  }
+};
+
+RAPIDJSON_STATIC_ASSERT(sizeof(unsigned long) == sizeof(unsigned));
+template <typename ValueType>
+struct TypeHelper<ValueType, unsigned long> {
+  static bool Is(const ValueType &v) { return v.IsUint(); }
+  static unsigned long Get(const ValueType &v) { return v.GetUint(); }
+  static ValueType &Set(ValueType &v, unsigned long data) {
+    return v.SetUint(data);
+  }
+  static ValueType &Set(ValueType &v, unsigned long data,
+                        typename ValueType::AllocatorType &) {
+    return v.SetUint(data);
+  }
+};
+#endif
+
+template <typename ValueType>
+struct TypeHelper<ValueType, int64_t> {
+  static bool Is(const ValueType &v) { return v.IsInt64(); }
+  static int64_t Get(const ValueType &v) { return v.GetInt64(); }
+  static ValueType &Set(ValueType &v, int64_t data) { return v.SetInt64(data); }
+  static ValueType &Set(ValueType &v, int64_t data,
+                        typename ValueType::AllocatorType &) {
+    return v.SetInt64(data);
+  }
+};
+
+template <typename ValueType>
+struct TypeHelper<ValueType, uint64_t> {
+  static bool Is(const ValueType &v) { return v.IsUint64(); }
+  static uint64_t Get(const ValueType &v) { return v.GetUint64(); }
+  static ValueType &Set(ValueType &v, uint64_t data) {
+    return v.SetUint64(data);
+  }
+  static ValueType &Set(ValueType &v, uint64_t data,
+                        typename ValueType::AllocatorType &) {
+    return v.SetUint64(data);
+  }
+};
+
+template <typename ValueType>
+struct TypeHelper<ValueType, double> {
+  static bool Is(const ValueType &v) { return v.IsDouble(); }
+  static double Get(const ValueType &v) { return v.GetDouble(); }
+  static ValueType &Set(ValueType &v, double data) { return v.SetDouble(data); }
+  static ValueType &Set(ValueType &v, double data,
+                        typename ValueType::AllocatorType &) {
+    return v.SetDouble(data);
+  }
+};
+
+template <typename ValueType>
+struct TypeHelper<ValueType, float> {
+  static bool Is(const ValueType &v) { return v.IsFloat(); }
+  static float Get(const ValueType &v) { return v.GetFloat(); }
+  static ValueType &Set(ValueType &v, float data) { return v.SetFloat(data); }
+  static ValueType &Set(ValueType &v, float data,
+                        typename ValueType::AllocatorType &) {
+    return v.SetFloat(data);
+  }
+};
+
+template <typename ValueType>
+struct TypeHelper<ValueType, const typename ValueType::Ch *> {
+  typedef const typename ValueType::Ch *StringType;
+  static bool Is(const ValueType &v) { return v.IsString(); }
+  static StringType Get(const ValueType &v) { return v.GetString(); }
+  static ValueType &Set(ValueType &v, const StringType data) {
+    return v.SetString(typename ValueType::StringRefType(data));
+  }
+  static ValueType &Set(ValueType &v, const StringType data,
+                        typename ValueType::AllocatorType &a) {
+    return v.SetString(data, a);
+  }
+};
+
+#if RAPIDJSON_HAS_STDSTRING
+template <typename ValueType>
+struct TypeHelper<ValueType, std::basic_string<typename ValueType::Ch>> {
+  typedef std::basic_string<typename ValueType::Ch> StringType;
+  static bool Is(const ValueType &v) { return v.IsString(); }
+  static StringType Get(const ValueType &v) {
+    return StringType(v.GetString(), v.GetStringLength());
+  }
+  static ValueType &Set(ValueType &v, const StringType &data,
+                        typename ValueType::AllocatorType &a) {
+    return v.SetString(data, a);
+  }
+};
+#endif
+
+template <typename ValueType>
+struct TypeHelper<ValueType, typename ValueType::Array> {
+  typedef typename ValueType::Array ArrayType;
+  static bool Is(const ValueType &v) { return v.IsArray(); }
+  static ArrayType Get(ValueType &v) { return v.GetArray(); }
+  static ValueType &Set(ValueType &v, ArrayType data) { return v = data; }
+  static ValueType &Set(ValueType &v, ArrayType data,
+                        typename ValueType::AllocatorType &) {
+    return v = data;
+  }
+};
+
+template <typename ValueType>
+struct TypeHelper<ValueType, typename ValueType::ConstArray> {
+  typedef typename ValueType::ConstArray ArrayType;
+  static bool Is(const ValueType &v) { return v.IsArray(); }
+  static ArrayType Get(const ValueType &v) { return v.GetArray(); }
+};
+
+template <typename ValueType>
+struct TypeHelper<ValueType, typename ValueType::Object> {
+  typedef typename ValueType::Object ObjectType;
+  static bool Is(const ValueType &v) { return v.IsObject(); }
+  static ObjectType Get(ValueType &v) { return v.GetObject(); }
+  static ValueType &Set(ValueType &v, ObjectType data) { return v = data; }
+  static ValueType &Set(ValueType &v, ObjectType data,
+                        typename ValueType::AllocatorType &) {
+    return v = data;
+  }
+};
+
+template <typename ValueType>
+struct TypeHelper<ValueType, typename ValueType::ConstObject> {
+  typedef typename ValueType::ConstObject ObjectType;
+  static bool Is(const ValueType &v) { return v.IsObject(); }
+  static ObjectType Get(const ValueType &v) { return v.GetObject(); }
+};
+
+}  // namespace internal
+
+// Forward declarations
+template <bool, typename>
+class GenericArray;
+template <bool, typename>
+class GenericObject;
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericValue
+
+//! Represents a JSON value. Use Value for UTF8 encoding and default allocator.
+/*!
+    A JSON value can be one of 7 types. This class is a variant type supporting
+    these types.
+
+    Use the Value if UTF8 and default allocator
+
+    \tparam Encoding    Encoding of the value. (Even non-string values need to
+   have the same encoding in a document) \tparam Allocator   Allocator type for
+   allocating memory of object, array and string.
+*/
+template <typename Encoding, typename Allocator = MemoryPoolAllocator<>>
+class GenericValue {
+ public:
+  //! Name-value pair in an object.
+  typedef GenericMember<Encoding, Allocator> Member;
+  typedef Encoding EncodingType;    //!< Encoding type from template parameter.
+  typedef Allocator AllocatorType;  //!< Allocator type from template parameter.
+  typedef typename Encoding::Ch Ch;  //!< Character type derived from Encoding.
+  typedef GenericStringRef<Ch>
+      StringRefType;  //!< Reference to a constant string
+  typedef typename GenericMemberIterator<false, Encoding, Allocator>::Iterator
+      MemberIterator;  //!< Member iterator for iterating in object.
+  typedef typename GenericMemberIterator<true, Encoding, Allocator>::Iterator
+      ConstMemberIterator;  //!< Constant member iterator for iterating in
+                            //!< object.
+  typedef GenericValue
+      *ValueIterator;  //!< Value iterator for iterating in array.
+  typedef const GenericValue
+      *ConstValueIterator;  //!< Constant value iterator for iterating in array.
+  typedef GenericValue<Encoding, Allocator>
+      ValueType;  //!< Value type of itself.
+  typedef GenericArray<false, ValueType> Array;
+  typedef GenericArray<true, ValueType> ConstArray;
+  typedef GenericObject<false, ValueType> Object;
+  typedef GenericObject<true, ValueType> ConstObject;
+
+  //!@name Constructors and destructor.
+  //@{
+
+  //! Default constructor creates a null value.
+  GenericValue() RAPIDJSON_NOEXCEPT : data_() { data_.f.flags = kNullFlag; }
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  //! Move constructor in C++11
+  GenericValue(GenericValue &&rhs) RAPIDJSON_NOEXCEPT : data_(rhs.data_) {
+    rhs.data_.f.flags = kNullFlag;  // give up contents
+  }
+#endif
+
+ private:
+  //! Copy constructor is not permitted.
+  GenericValue(const GenericValue &rhs);
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  //! Moving from a GenericDocument is not permitted.
+  template <typename StackAllocator>
+  GenericValue(GenericDocument<Encoding, Allocator, StackAllocator> &&rhs);
+
+  //! Move assignment from a GenericDocument is not permitted.
+  template <typename StackAllocator>
+  GenericValue &operator=(
+      GenericDocument<Encoding, Allocator, StackAllocator> &&rhs);
+#endif
+
+ public:
+  //! Constructor with JSON value type.
+  /*! This creates a Value of specified type with default content.
+      \param type Type of the value.
+      \note Default content for number is zero.
+  */
+  explicit GenericValue(Type type) RAPIDJSON_NOEXCEPT : data_() {
+    static const uint16_t defaultFlags[] = {
+        kNullFlag,  kFalseFlag,       kTrueFlag,     kObjectFlag,
+        kArrayFlag, kShortStringFlag, kNumberAnyFlag};
+    RAPIDJSON_NOEXCEPT_ASSERT(type >= kNullType && type <= kNumberType);
+    data_.f.flags = defaultFlags[type];
+
+    // Use ShortString to store empty string.
+    if (type == kStringType) data_.ss.SetLength(0);
+  }
+
+  //! Explicit copy constructor (with allocator)
+  /*! Creates a copy of a Value by using the given Allocator
+      \tparam SourceAllocator allocator of \c rhs
+      \param rhs Value to copy from (read-only)
+      \param allocator Allocator for allocating copied elements and buffers.
+     Commonly use GenericDocument::GetAllocator(). \param copyConstStrings Force
+     copying of constant strings (e.g. referencing an in-situ buffer) \see
+     CopyFrom()
+  */
+  template <typename SourceAllocator>
+  GenericValue(const GenericValue<Encoding, SourceAllocator> &rhs,
+               Allocator &allocator, bool copyConstStrings = false) {
+    switch (rhs.GetType()) {
+      case kObjectType: {
+        SizeType count = rhs.data_.o.size;
+        Member *lm = reinterpret_cast<Member *>(
+            allocator.Malloc(count * sizeof(Member)));
+        const typename GenericValue<Encoding, SourceAllocator>::Member *rm =
+            rhs.GetMembersPointer();
+        for (SizeType i = 0; i < count; i++) {
+          new (&lm[i].name)
+              GenericValue(rm[i].name, allocator, copyConstStrings);
+          new (&lm[i].value)
+              GenericValue(rm[i].value, allocator, copyConstStrings);
+        }
+        data_.f.flags = kObjectFlag;
+        data_.o.size = data_.o.capacity = count;
+        SetMembersPointer(lm);
+      } break;
+      case kArrayType: {
+        SizeType count = rhs.data_.a.size;
+        GenericValue *le = reinterpret_cast<GenericValue *>(
+            allocator.Malloc(count * sizeof(GenericValue)));
+        const GenericValue<Encoding, SourceAllocator> *re =
+            rhs.GetElementsPointer();
+        for (SizeType i = 0; i < count; i++)
+          new (&le[i]) GenericValue(re[i], allocator, copyConstStrings);
+        data_.f.flags = kArrayFlag;
+        data_.a.size = data_.a.capacity = count;
+        SetElementsPointer(le);
+      } break;
+      case kStringType:
+        if (rhs.data_.f.flags == kConstStringFlag && !copyConstStrings) {
+          data_.f.flags = rhs.data_.f.flags;
+          data_ = *reinterpret_cast<const Data *>(&rhs.data_);
+        } else
+          SetStringRaw(StringRef(rhs.GetString(), rhs.GetStringLength()),
+                       allocator);
+        break;
+      default:
+        data_.f.flags = rhs.data_.f.flags;
+        data_ = *reinterpret_cast<const Data *>(&rhs.data_);
+        break;
+    }
+  }
+
+//! Constructor for boolean value.
+/*! \param b Boolean value
+    \note This constructor is limited to \em real boolean values and rejects
+        implicitly converted types like arbitrary pointers.  Use an explicit
+   cast to \c bool, if you want to construct a boolean JSON value in such
+   cases.
+ */
+#ifndef RAPIDJSON_DOXYGEN_RUNNING  // hide SFINAE from Doxygen
+  template <typename T>
+  explicit GenericValue(T b, RAPIDJSON_ENABLEIF((internal::IsSame<bool, T>)))
+      RAPIDJSON_NOEXCEPT  // See #472
+#else
+  explicit GenericValue(bool b) RAPIDJSON_NOEXCEPT
+#endif
+      : data_() {
+    // safe-guard against failing SFINAE
+    RAPIDJSON_STATIC_ASSERT((internal::IsSame<bool, T>::Value));
+    data_.f.flags = b ? kTrueFlag : kFalseFlag;
+  }
+
+  //! Constructor for int value.
+  explicit GenericValue(int i) RAPIDJSON_NOEXCEPT : data_() {
+    data_.n.i64 = i;
+    data_.f.flags =
+        (i >= 0) ? (kNumberIntFlag | kUintFlag | kUint64Flag) : kNumberIntFlag;
+  }
+
+  //! Constructor for unsigned value.
+  explicit GenericValue(unsigned u) RAPIDJSON_NOEXCEPT : data_() {
+    data_.n.u64 = u;
+    data_.f.flags = (u & 0x80000000)
+                        ? kNumberUintFlag
+                        : (kNumberUintFlag | kIntFlag | kInt64Flag);
+  }
+
+  //! Constructor for int64_t value.
+  explicit GenericValue(int64_t i64) RAPIDJSON_NOEXCEPT : data_() {
+    data_.n.i64 = i64;
+    data_.f.flags = kNumberInt64Flag;
+    if (i64 >= 0) {
+      data_.f.flags |= kNumberUint64Flag;
+      if (!(static_cast<uint64_t>(i64) &
+            RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x00000000)))
+        data_.f.flags |= kUintFlag;
+      if (!(static_cast<uint64_t>(i64) &
+            RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x80000000)))
+        data_.f.flags |= kIntFlag;
+    } else if (i64 >= static_cast<int64_t>(
+                          RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x80000000)))
+      data_.f.flags |= kIntFlag;
+  }
+
+  //! Constructor for uint64_t value.
+  explicit GenericValue(uint64_t u64) RAPIDJSON_NOEXCEPT : data_() {
+    data_.n.u64 = u64;
+    data_.f.flags = kNumberUint64Flag;
+    if (!(u64 & RAPIDJSON_UINT64_C2(0x80000000, 0x00000000)))
+      data_.f.flags |= kInt64Flag;
+    if (!(u64 & RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x00000000)))
+      data_.f.flags |= kUintFlag;
+    if (!(u64 & RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x80000000)))
+      data_.f.flags |= kIntFlag;
+  }
+
+  //! Constructor for double value.
+  explicit GenericValue(double d) RAPIDJSON_NOEXCEPT : data_() {
+    data_.n.d = d;
+    data_.f.flags = kNumberDoubleFlag;
+  }
+
+  //! Constructor for float value.
+  explicit GenericValue(float f) RAPIDJSON_NOEXCEPT : data_() {
+    data_.n.d = static_cast<double>(f);
+    data_.f.flags = kNumberDoubleFlag;
+  }
+
+  //! Constructor for constant string (i.e. do not make a copy of string)
+  GenericValue(const Ch *s, SizeType length) RAPIDJSON_NOEXCEPT : data_() {
+    SetStringRaw(StringRef(s, length));
+  }
+
+  //! Constructor for constant string (i.e. do not make a copy of string)
+  explicit GenericValue(StringRefType s) RAPIDJSON_NOEXCEPT : data_() {
+    SetStringRaw(s);
+  }
+
+  //! Constructor for copy-string (i.e. do make a copy of string)
+  GenericValue(const Ch *s, SizeType length, Allocator &allocator) : data_() {
+    SetStringRaw(StringRef(s, length), allocator);
+  }
+
+  //! Constructor for copy-string (i.e. do make a copy of string)
+  GenericValue(const Ch *s, Allocator &allocator) : data_() {
+    SetStringRaw(StringRef(s), allocator);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Constructor for copy-string from a string object (i.e. do make a copy of
+  //! string)
+  /*! \note Requires the definition of the preprocessor symbol \ref
+   * RAPIDJSON_HAS_STDSTRING.
+   */
+  GenericValue(const std::basic_string<Ch> &s, Allocator &allocator) : data_() {
+    SetStringRaw(StringRef(s), allocator);
+  }
+#endif
+
+  //! Constructor for Array.
+  /*!
+      \param a An array obtained by \c GetArray().
+      \note \c Array is always pass-by-value.
+      \note the source array is moved into this value and the sourec array
+     becomes empty.
+  */
+  GenericValue(Array a) RAPIDJSON_NOEXCEPT : data_(a.value_.data_) {
+    a.value_.data_ = Data();
+    a.value_.data_.f.flags = kArrayFlag;
+  }
+
+  //! Constructor for Object.
+  /*!
+      \param o An object obtained by \c GetObject().
+      \note \c Object is always pass-by-value.
+      \note the source object is moved into this value and the sourec object
+     becomes empty.
+  */
+  GenericValue(Object o) RAPIDJSON_NOEXCEPT : data_(o.value_.data_) {
+    o.value_.data_ = Data();
+    o.value_.data_.f.flags = kObjectFlag;
+  }
+
+  //! Destructor.
+  /*! Need to destruct elements of array, members of object, or copy-string.
+   */
+  ~GenericValue() {
+    if (Allocator::kNeedFree) {  // Shortcut by Allocator's trait
+      switch (data_.f.flags) {
+        case kArrayFlag: {
+          GenericValue *e = GetElementsPointer();
+          for (GenericValue *v = e; v != e + data_.a.size; ++v)
+            v->~GenericValue();
+          Allocator::Free(e);
+        } break;
+
+        case kObjectFlag:
+          for (MemberIterator m = MemberBegin(); m != MemberEnd(); ++m)
+            m->~Member();
+          Allocator::Free(GetMembersPointer());
+          break;
+
+        case kCopyStringFlag:
+          Allocator::Free(const_cast<Ch *>(GetStringPointer()));
+          break;
+
+        default:
+          break;  // Do nothing for other types.
+      }
+    }
+  }
+
+  //@}
+
+  //!@name Assignment operators
+  //@{
+
+  //! Assignment with move semantics.
+  /*! \param rhs Source of the assignment. It will become a null value after
+   * assignment.
+   */
+  GenericValue &operator=(GenericValue &rhs) RAPIDJSON_NOEXCEPT {
+    if (RAPIDJSON_LIKELY(this != &rhs)) {
+      this->~GenericValue();
+      RawAssign(rhs);
+    }
+    return *this;
+  }
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  //! Move assignment in C++11
+  GenericValue &operator=(GenericValue &&rhs) RAPIDJSON_NOEXCEPT {
+    return *this = rhs.Move();
+  }
+#endif
+
+  //! Assignment of constant string reference (no copy)
+  /*! \param str Constant string reference to be assigned
+      \note This overload is needed to avoid clashes with the generic primitive
+     type assignment overload below. \see GenericStringRef, operator=(T)
+  */
+  GenericValue &operator=(StringRefType str) RAPIDJSON_NOEXCEPT {
+    GenericValue s(str);
+    return *this = s;
+  }
+
+  //! Assignment with primitive types.
+  /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t
+      \param value The value to be assigned.
+
+      \note The source type \c T explicitly disallows all pointer types,
+          especially (\c const) \ref Ch*.  This helps avoiding implicitly
+          referencing character strings with insufficient lifetime, use
+          \ref SetString(const Ch*, Allocator&) (for copying) or
+          \ref StringRef() (to explicitly mark the pointer as constant) instead.
+          All other pointer types would implicitly convert to \c bool,
+          use \ref SetBool() instead.
+  */
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN((internal::IsPointer<T>), (GenericValue &))
+  operator=(T value) {
+    GenericValue v(value);
+    return *this = v;
+  }
+
+  //! Deep-copy assignment from Value
+  /*! Assigns a \b copy of the Value to the current Value object
+      \tparam SourceAllocator Allocator type of \c rhs
+      \param rhs Value to copy from (read-only)
+      \param allocator Allocator to use for copying
+      \param copyConstStrings Force copying of constant strings (e.g.
+     referencing an in-situ buffer)
+   */
+  template <typename SourceAllocator>
+  GenericValue &CopyFrom(const GenericValue<Encoding, SourceAllocator> &rhs,
+                         Allocator &allocator, bool copyConstStrings = false) {
+    RAPIDJSON_ASSERT(static_cast<void *>(this) !=
+                     static_cast<void const *>(&rhs));
+    this->~GenericValue();
+    new (this) GenericValue(rhs, allocator, copyConstStrings);
+    return *this;
+  }
+
+  //! Exchange the contents of this value with those of other.
+  /*!
+      \param other Another value.
+      \note Constant complexity.
+  */
+  GenericValue &Swap(GenericValue &other) RAPIDJSON_NOEXCEPT {
+    GenericValue temp;
+    temp.RawAssign(*this);
+    RawAssign(other);
+    other.RawAssign(temp);
+    return *this;
+  }
+
+  //! free-standing swap function helper
+  /*!
+      Helper function to enable support for common swap implementation pattern
+     based on \c std::swap: \code void swap(MyClass& a, MyClass& b) { using
+     std::swap; swap(a.value, b.value);
+          // ...
+      }
+      \endcode
+      \see Swap()
+   */
+  friend inline void swap(GenericValue &a, GenericValue &b) RAPIDJSON_NOEXCEPT {
+    a.Swap(b);
+  }
+
+  //! Prepare Value for move semantics
+  /*! \return *this */
+  GenericValue &Move() RAPIDJSON_NOEXCEPT { return *this; }
+  //@}
+
+  //!@name Equal-to and not-equal-to operators
+  //@{
+  //! Equal-to operator
+  /*!
+      \note If an object contains duplicated named member, comparing equality
+     with any object is always \c false. \note Complexity is quadratic in
+     Object's member number and linear for the rest (number of all values in the
+     subtree and total lengths of all strings).
+  */
+  template <typename SourceAllocator>
+  bool operator==(const GenericValue<Encoding, SourceAllocator> &rhs) const {
+    typedef GenericValue<Encoding, SourceAllocator> RhsType;
+    if (GetType() != rhs.GetType()) return false;
+
+    switch (GetType()) {
+      case kObjectType:  // Warning: O(n^2) inner-loop
+        if (data_.o.size != rhs.data_.o.size) return false;
+        for (ConstMemberIterator lhsMemberItr = MemberBegin();
+             lhsMemberItr != MemberEnd(); ++lhsMemberItr) {
+          typename RhsType::ConstMemberIterator rhsMemberItr =
+              rhs.FindMember(lhsMemberItr->name);
+          if (rhsMemberItr == rhs.MemberEnd() ||
+              lhsMemberItr->value != rhsMemberItr->value)
+            return false;
+        }
+        return true;
+
+      case kArrayType:
+        if (data_.a.size != rhs.data_.a.size) return false;
+        for (SizeType i = 0; i < data_.a.size; i++)
+          if ((*this)[i] != rhs[i]) return false;
+        return true;
+
+      case kStringType:
+        return StringEqual(rhs);
+
+      case kNumberType:
+        if (IsDouble() || rhs.IsDouble()) {
+          double a = GetDouble();      // May convert from integer to double.
+          double b = rhs.GetDouble();  // Ditto
+          return a >= b && a <= b;     // Prevent -Wfloat-equal
+        } else
+          return data_.n.u64 == rhs.data_.n.u64;
+
+      default:
+        return true;
+    }
+  }
+
+  //! Equal-to operator with const C-string pointer
+  bool operator==(const Ch *rhs) const {
+    return *this == GenericValue(StringRef(rhs));
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Equal-to operator with string object
+  /*! \note Requires the definition of the preprocessor symbol \ref
+   * RAPIDJSON_HAS_STDSTRING.
+   */
+  bool operator==(const std::basic_string<Ch> &rhs) const {
+    return *this == GenericValue(StringRef(rhs));
+  }
+#endif
+
+  //! Equal-to operator with primitive types
+  /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t,
+   * \c double, \c true, \c false
+   */
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::OrExpr<internal::IsPointer<T>, internal::IsGenericValue<T>>),
+      (bool))
+  operator==(const T &rhs) const {
+    return *this == GenericValue(rhs);
+  }
+
+  //! Not-equal-to operator
+  /*! \return !(*this == rhs)
+   */
+  template <typename SourceAllocator>
+  bool operator!=(const GenericValue<Encoding, SourceAllocator> &rhs) const {
+    return !(*this == rhs);
+  }
+
+  //! Not-equal-to operator with const C-string pointer
+  bool operator!=(const Ch *rhs) const { return !(*this == rhs); }
+
+  //! Not-equal-to operator with arbitrary types
+  /*! \return !(*this == rhs)
+   */
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN((internal::IsGenericValue<T>), (bool))
+  operator!=(const T &rhs) const {
+    return !(*this == rhs);
+  }
+
+  //! Equal-to operator with arbitrary types (symmetric version)
+  /*! \return (rhs == lhs)
+   */
+  template <typename T>
+  friend RAPIDJSON_DISABLEIF_RETURN((internal::IsGenericValue<T>), (bool))
+  operator==(const T &lhs, const GenericValue &rhs) {
+    return rhs == lhs;
+  }
+
+  //! Not-Equal-to operator with arbitrary types (symmetric version)
+  /*! \return !(rhs == lhs)
+   */
+  template <typename T>
+  friend RAPIDJSON_DISABLEIF_RETURN((internal::IsGenericValue<T>), (bool))
+  operator!=(const T &lhs, const GenericValue &rhs) {
+    return !(rhs == lhs);
+  }
+  //@}
+
+  //!@name Type
+  //@{
+
+  Type GetType() const { return static_cast<Type>(data_.f.flags & kTypeMask); }
+  bool IsNull() const { return data_.f.flags == kNullFlag; }
+  bool IsFalse() const { return data_.f.flags == kFalseFlag; }
+  bool IsTrue() const { return data_.f.flags == kTrueFlag; }
+  bool IsBool() const { return (data_.f.flags & kBoolFlag) != 0; }
+  bool IsObject() const { return data_.f.flags == kObjectFlag; }
+  bool IsArray() const { return data_.f.flags == kArrayFlag; }
+  bool IsNumber() const { return (data_.f.flags & kNumberFlag) != 0; }
+  bool IsInt() const { return (data_.f.flags & kIntFlag) != 0; }
+  bool IsUint() const { return (data_.f.flags & kUintFlag) != 0; }
+  bool IsInt64() const { return (data_.f.flags & kInt64Flag) != 0; }
+  bool IsUint64() const { return (data_.f.flags & kUint64Flag) != 0; }
+  bool IsDouble() const { return (data_.f.flags & kDoubleFlag) != 0; }
+  bool IsString() const { return (data_.f.flags & kStringFlag) != 0; }
+
+  // Checks whether a number can be losslessly converted to a double.
+  bool IsLosslessDouble() const {
+    if (!IsNumber()) return false;
+    if (IsUint64()) {
+      uint64_t u = GetUint64();
+      volatile double d = static_cast<double>(u);
+      return (d >= 0.0) &&
+             (d <
+              static_cast<double>((std::numeric_limits<uint64_t>::max)())) &&
+             (u == static_cast<uint64_t>(d));
+    }
+    if (IsInt64()) {
+      int64_t i = GetInt64();
+      volatile double d = static_cast<double>(i);
+      return (d >=
+              static_cast<double>((std::numeric_limits<int64_t>::min)())) &&
+             (d < static_cast<double>((std::numeric_limits<int64_t>::max)())) &&
+             (i == static_cast<int64_t>(d));
+    }
+    return true;  // double, int, uint are always lossless
+  }
+
+  // Checks whether a number is a float (possible lossy).
+  bool IsFloat() const {
+    if ((data_.f.flags & kDoubleFlag) == 0) return false;
+    double d = GetDouble();
+    return d >= -3.4028234e38 && d <= 3.4028234e38;
+  }
+  // Checks whether a number can be losslessly converted to a float.
+  bool IsLosslessFloat() const {
+    if (!IsNumber()) return false;
+    double a = GetDouble();
+    if (a < static_cast<double>(-(std::numeric_limits<float>::max)()) ||
+        a > static_cast<double>((std::numeric_limits<float>::max)()))
+      return false;
+    double b = static_cast<double>(static_cast<float>(a));
+    return a >= b && a <= b;  // Prevent -Wfloat-equal
+  }
+
+  //@}
+
+  //!@name Null
+  //@{
+
+  GenericValue &SetNull() {
+    this->~GenericValue();
+    new (this) GenericValue();
+    return *this;
+  }
+
+  //@}
+
+  //!@name Bool
+  //@{
+
+  bool GetBool() const {
+    RAPIDJSON_ASSERT(IsBool());
+    return data_.f.flags == kTrueFlag;
+  }
+  //!< Set boolean value
+  /*! \post IsBool() == true */
+  GenericValue &SetBool(bool b) {
+    this->~GenericValue();
+    new (this) GenericValue(b);
+    return *this;
+  }
+
+  //@}
+
+  //!@name Object
+  //@{
+
+  //! Set this value as an empty object.
+  /*! \post IsObject() == true */
+  GenericValue &SetObject() {
+    this->~GenericValue();
+    new (this) GenericValue(kObjectType);
+    return *this;
+  }
+
+  //! Get the number of members in the object.
+  SizeType MemberCount() const {
+    RAPIDJSON_ASSERT(IsObject());
+    return data_.o.size;
+  }
+
+  //! Get the capacity of object.
+  SizeType MemberCapacity() const {
+    RAPIDJSON_ASSERT(IsObject());
+    return data_.o.capacity;
+  }
+
+  //! Check whether the object is empty.
+  bool ObjectEmpty() const {
+    RAPIDJSON_ASSERT(IsObject());
+    return data_.o.size == 0;
+  }
+
+  //! Get a value from an object associated with the name.
+  /*! \pre IsObject() == true
+      \tparam T Either \c Ch or \c const \c Ch (template used for disambiguation
+     with \ref operator[](SizeType)) \note In version 0.1x, if the member is not
+     found, this function returns a null value. This makes issue 7. Since 0.2,
+     if the name is not correct, it will assert. If user is unsure whether a
+     member exists, user should use HasMember() first. A better approach is to
+     use FindMember(). \note Linear time complexity.
+  */
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::NotExpr<
+          internal::IsSame<typename internal::RemoveConst<T>::Type, Ch>>),
+      (GenericValue &))
+  operator[](T *name) {
+    GenericValue n(StringRef(name));
+    return (*this)[n];
+  }
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::NotExpr<
+          internal::IsSame<typename internal::RemoveConst<T>::Type, Ch>>),
+      (const GenericValue &))
+  operator[](T *name) const {
+    return const_cast<GenericValue &>(*this)[name];
+  }
+
+  //! Get a value from an object associated with the name.
+  /*! \pre IsObject() == true
+      \tparam SourceAllocator Allocator of the \c name value
+
+      \note Compared to \ref operator[](T*), this version is faster because it
+     does not need a StrLen(). And it can also handle strings with embedded null
+     characters.
+
+      \note Linear time complexity.
+  */
+  template <typename SourceAllocator>
+  GenericValue &operator[](
+      const GenericValue<Encoding, SourceAllocator> &name) {
+    MemberIterator member = FindMember(name);
+    if (member != MemberEnd())
+      return member->value;
+    else {
+      RAPIDJSON_ASSERT(false);  // see above note
+
+      // This will generate -Wexit-time-destructors in clang
+      // static GenericValue NullValue;
+      // return NullValue;
+
+      // Use static buffer and placement-new to prevent destruction
+      static char buffer[sizeof(GenericValue)];
+      return *new (buffer) GenericValue();
+    }
+  }
+  template <typename SourceAllocator>
+  const GenericValue &operator[](
+      const GenericValue<Encoding, SourceAllocator> &name) const {
+    return const_cast<GenericValue &>(*this)[name];
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Get a value from an object associated with name (string object).
+  GenericValue &operator[](const std::basic_string<Ch> &name) {
+    return (*this)[GenericValue(StringRef(name))];
+  }
+  const GenericValue &operator[](const std::basic_string<Ch> &name) const {
+    return (*this)[GenericValue(StringRef(name))];
+  }
+#endif
+
+  //! Const member iterator
+  /*! \pre IsObject() == true */
+  ConstMemberIterator MemberBegin() const {
+    RAPIDJSON_ASSERT(IsObject());
+    return ConstMemberIterator(GetMembersPointer());
+  }
+  //! Const \em past-the-end member iterator
+  /*! \pre IsObject() == true */
+  ConstMemberIterator MemberEnd() const {
+    RAPIDJSON_ASSERT(IsObject());
+    return ConstMemberIterator(GetMembersPointer() + data_.o.size);
+  }
+  //! Member iterator
+  /*! \pre IsObject() == true */
+  MemberIterator MemberBegin() {
+    RAPIDJSON_ASSERT(IsObject());
+    return MemberIterator(GetMembersPointer());
+  }
+  //! \em Past-the-end member iterator
+  /*! \pre IsObject() == true */
+  MemberIterator MemberEnd() {
+    RAPIDJSON_ASSERT(IsObject());
+    return MemberIterator(GetMembersPointer() + data_.o.size);
+  }
+
+  //! Request the object to have enough capacity to store members.
+  /*! \param newCapacity  The capacity that the object at least need to have.
+      \param allocator    Allocator for reallocating memory. It must be the same
+     one as used before. Commonly use GenericDocument::GetAllocator(). \return
+     The value itself for fluent API. \note Linear time complexity.
+  */
+  GenericValue &MemberReserve(SizeType newCapacity, Allocator &allocator) {
+    RAPIDJSON_ASSERT(IsObject());
+    if (newCapacity > data_.o.capacity) {
+      SetMembersPointer(reinterpret_cast<Member *>(allocator.Realloc(
+          GetMembersPointer(), data_.o.capacity * sizeof(Member),
+          newCapacity * sizeof(Member))));
+      data_.o.capacity = newCapacity;
+    }
+    return *this;
+  }
+
+  //! Check whether a member exists in the object.
+  /*!
+      \param name Member name to be searched.
+      \pre IsObject() == true
+      \return Whether a member with that name exists.
+      \note It is better to use FindMember() directly if you need the obtain the
+     value as well. \note Linear time complexity.
+  */
+  bool HasMember(const Ch *name) const {
+    return FindMember(name) != MemberEnd();
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Check whether a member exists in the object with string object.
+  /*!
+      \param name Member name to be searched.
+      \pre IsObject() == true
+      \return Whether a member with that name exists.
+      \note It is better to use FindMember() directly if you need the obtain the
+     value as well. \note Linear time complexity.
+  */
+  bool HasMember(const std::basic_string<Ch> &name) const {
+    return FindMember(name) != MemberEnd();
+  }
+#endif
+
+  //! Check whether a member exists in the object with GenericValue name.
+  /*!
+      This version is faster because it does not need a StrLen(). It can also
+     handle string with null character. \param name Member name to be searched.
+      \pre IsObject() == true
+      \return Whether a member with that name exists.
+      \note It is better to use FindMember() directly if you need the obtain the
+     value as well. \note Linear time complexity.
+  */
+  template <typename SourceAllocator>
+  bool HasMember(const GenericValue<Encoding, SourceAllocator> &name) const {
+    return FindMember(name) != MemberEnd();
+  }
+
+  //! Find member by name.
+  /*!
+      \param name Member name to be searched.
+      \pre IsObject() == true
+      \return Iterator to member, if it exists.
+          Otherwise returns \ref MemberEnd().
+
+      \note Earlier versions of Rapidjson returned a \c NULL pointer, in case
+          the requested member doesn't exist. For consistency with e.g.
+          \c std::map, this has been changed to MemberEnd() now.
+      \note Linear time complexity.
+  */
+  MemberIterator FindMember(const Ch *name) {
+    GenericValue n(StringRef(name));
+    return FindMember(n);
+  }
+
+  ConstMemberIterator FindMember(const Ch *name) const {
+    return const_cast<GenericValue &>(*this).FindMember(name);
+  }
+
+  //! Find member by name.
+  /*!
+      This version is faster because it does not need a StrLen(). It can also
+     handle string with null character. \param name Member name to be searched.
+      \pre IsObject() == true
+      \return Iterator to member, if it exists.
+          Otherwise returns \ref MemberEnd().
+
+      \note Earlier versions of Rapidjson returned a \c NULL pointer, in case
+          the requested member doesn't exist. For consistency with e.g.
+          \c std::map, this has been changed to MemberEnd() now.
+      \note Linear time complexity.
+  */
+  template <typename SourceAllocator>
+  MemberIterator FindMember(
+      const GenericValue<Encoding, SourceAllocator> &name) {
+    RAPIDJSON_ASSERT(IsObject());
+    RAPIDJSON_ASSERT(name.IsString());
+    MemberIterator member = MemberBegin();
+    for (; member != MemberEnd(); ++member)
+      if (name.StringEqual(member->name)) break;
+    return member;
+  }
+  template <typename SourceAllocator>
+  ConstMemberIterator FindMember(
+      const GenericValue<Encoding, SourceAllocator> &name) const {
+    return const_cast<GenericValue &>(*this).FindMember(name);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Find member by string object name.
+  /*!
+      \param name Member name to be searched.
+      \pre IsObject() == true
+      \return Iterator to member, if it exists.
+          Otherwise returns \ref MemberEnd().
+  */
+  MemberIterator FindMember(const std::basic_string<Ch> &name) {
+    return FindMember(GenericValue(StringRef(name)));
+  }
+  ConstMemberIterator FindMember(const std::basic_string<Ch> &name) const {
+    return FindMember(GenericValue(StringRef(name)));
+  }
+#endif
+
+  //! Add a member (name-value pair) to the object.
+  /*! \param name A string value as name of member.
+      \param value Value of any type.
+      \param allocator    Allocator for reallocating memory. It must be the same
+     one as used before. Commonly use GenericDocument::GetAllocator(). \return
+     The value itself for fluent API. \note The ownership of \c name and \c
+     value will be transferred to this object on success. \pre  IsObject() &&
+     name.IsString() \post name.IsNull() && value.IsNull() \note Amortized
+     Constant time complexity.
+  */
+  GenericValue &AddMember(GenericValue &name, GenericValue &value,
+                          Allocator &allocator) {
+    RAPIDJSON_ASSERT(IsObject());
+    RAPIDJSON_ASSERT(name.IsString());
+
+    ObjectData &o = data_.o;
+    if (o.size >= o.capacity)
+      MemberReserve(o.capacity == 0 ? kDefaultObjectCapacity
+                                    : (o.capacity + (o.capacity + 1) / 2),
+                    allocator);
+    Member *members = GetMembersPointer();
+    members[o.size].name.RawAssign(name);
+    members[o.size].value.RawAssign(value);
+    o.size++;
+    return *this;
+  }
+
+  //! Add a constant string value as member (name-value pair) to the object.
+  /*! \param name A string value as name of member.
+      \param value constant string reference as value of member.
+      \param allocator    Allocator for reallocating memory. It must be the same
+     one as used before. Commonly use GenericDocument::GetAllocator(). \return
+     The value itself for fluent API. \pre  IsObject() \note This overload is
+     needed to avoid clashes with the generic primitive type
+     AddMember(GenericValue&,T,Allocator&) overload below. \note Amortized
+     Constant time complexity.
+  */
+  GenericValue &AddMember(GenericValue &name, StringRefType value,
+                          Allocator &allocator) {
+    GenericValue v(value);
+    return AddMember(name, v, allocator);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Add a string object as member (name-value pair) to the object.
+  /*! \param name A string value as name of member.
+      \param value constant string reference as value of member.
+      \param allocator    Allocator for reallocating memory. It must be the same
+     one as used before. Commonly use GenericDocument::GetAllocator(). \return
+     The value itself for fluent API. \pre  IsObject() \note This overload is
+     needed to avoid clashes with the generic primitive type
+     AddMember(GenericValue&,T,Allocator&) overload below. \note Amortized
+     Constant time complexity.
+  */
+  GenericValue &AddMember(GenericValue &name, std::basic_string<Ch> &value,
+                          Allocator &allocator) {
+    GenericValue v(value, allocator);
+    return AddMember(name, v, allocator);
+  }
+#endif
+
+  //! Add any primitive value as member (name-value pair) to the object.
+  /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t
+      \param name A string value as name of member.
+      \param value Value of primitive type \c T as value of member
+      \param allocator Allocator for reallocating memory. Commonly use
+     GenericDocument::GetAllocator(). \return The value itself for fluent API.
+      \pre  IsObject()
+
+      \note The source type \c T explicitly disallows all pointer types,
+          especially (\c const) \ref Ch*.  This helps avoiding implicitly
+          referencing character strings with insufficient lifetime, use
+          \ref AddMember(StringRefType, GenericValue&, Allocator&) or \ref
+          AddMember(StringRefType, StringRefType, Allocator&).
+          All other pointer types would implicitly convert to \c bool,
+          use an explicit cast instead, if needed.
+      \note Amortized Constant time complexity.
+  */
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::OrExpr<internal::IsPointer<T>, internal::IsGenericValue<T>>),
+      (GenericValue &))
+  AddMember(GenericValue &name, T value, Allocator &allocator) {
+    GenericValue v(value);
+    return AddMember(name, v, allocator);
+  }
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  GenericValue &AddMember(GenericValue &&name, GenericValue &&value,
+                          Allocator &allocator) {
+    return AddMember(name, value, allocator);
+  }
+  GenericValue &AddMember(GenericValue &&name, GenericValue &value,
+                          Allocator &allocator) {
+    return AddMember(name, value, allocator);
+  }
+  GenericValue &AddMember(GenericValue &name, GenericValue &&value,
+                          Allocator &allocator) {
+    return AddMember(name, value, allocator);
+  }
+  GenericValue &AddMember(StringRefType name, GenericValue &&value,
+                          Allocator &allocator) {
+    GenericValue n(name);
+    return AddMember(n, value, allocator);
+  }
+#endif  // RAPIDJSON_HAS_CXX11_RVALUE_REFS
+
+  //! Add a member (name-value pair) to the object.
+  /*! \param name A constant string reference as name of member.
+      \param value Value of any type.
+      \param allocator    Allocator for reallocating memory. It must be the same
+     one as used before. Commonly use GenericDocument::GetAllocator(). \return
+     The value itself for fluent API. \note The ownership of \c value will be
+     transferred to this object on success. \pre  IsObject() \post
+     value.IsNull() \note Amortized Constant time complexity.
+  */
+  GenericValue &AddMember(StringRefType name, GenericValue &value,
+                          Allocator &allocator) {
+    GenericValue n(name);
+    return AddMember(n, value, allocator);
+  }
+
+  //! Add a constant string value as member (name-value pair) to the object.
+  /*! \param name A constant string reference as name of member.
+      \param value constant string reference as value of member.
+      \param allocator    Allocator for reallocating memory. It must be the same
+     one as used before. Commonly use GenericDocument::GetAllocator(). \return
+     The value itself for fluent API. \pre  IsObject() \note This overload is
+     needed to avoid clashes with the generic primitive type
+     AddMember(StringRefType,T,Allocator&) overload below. \note Amortized
+     Constant time complexity.
+  */
+  GenericValue &AddMember(StringRefType name, StringRefType value,
+                          Allocator &allocator) {
+    GenericValue v(value);
+    return AddMember(name, v, allocator);
+  }
+
+  //! Add any primitive value as member (name-value pair) to the object.
+  /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t
+      \param name A constant string reference as name of member.
+      \param value Value of primitive type \c T as value of member
+      \param allocator Allocator for reallocating memory. Commonly use
+     GenericDocument::GetAllocator(). \return The value itself for fluent API.
+      \pre  IsObject()
+
+      \note The source type \c T explicitly disallows all pointer types,
+          especially (\c const) \ref Ch*.  This helps avoiding implicitly
+          referencing character strings with insufficient lifetime, use
+          \ref AddMember(StringRefType, GenericValue&, Allocator&) or \ref
+          AddMember(StringRefType, StringRefType, Allocator&).
+          All other pointer types would implicitly convert to \c bool,
+          use an explicit cast instead, if needed.
+      \note Amortized Constant time complexity.
+  */
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::OrExpr<internal::IsPointer<T>, internal::IsGenericValue<T>>),
+      (GenericValue &))
+  AddMember(StringRefType name, T value, Allocator &allocator) {
+    GenericValue n(name);
+    return AddMember(n, value, allocator);
+  }
+
+  //! Remove all members in the object.
+  /*! This function do not deallocate memory in the object, i.e. the capacity is
+     unchanged. \note Linear time complexity.
+  */
+  void RemoveAllMembers() {
+    RAPIDJSON_ASSERT(IsObject());
+    for (MemberIterator m = MemberBegin(); m != MemberEnd(); ++m) m->~Member();
+    data_.o.size = 0;
+  }
+
+  //! Remove a member in object by its name.
+  /*! \param name Name of member to be removed.
+      \return Whether the member existed.
+      \note This function may reorder the object members. Use \ref
+          EraseMember(ConstMemberIterator) if you need to preserve the
+          relative order of the remaining members.
+      \note Linear time complexity.
+  */
+  bool RemoveMember(const Ch *name) {
+    GenericValue n(StringRef(name));
+    return RemoveMember(n);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool RemoveMember(const std::basic_string<Ch> &name) {
+    return RemoveMember(GenericValue(StringRef(name)));
+  }
+#endif
+
+  template <typename SourceAllocator>
+  bool RemoveMember(const GenericValue<Encoding, SourceAllocator> &name) {
+    MemberIterator m = FindMember(name);
+    if (m != MemberEnd()) {
+      RemoveMember(m);
+      return true;
+    } else
+      return false;
+  }
+
+  //! Remove a member in object by iterator.
+  /*! \param m member iterator (obtained by FindMember() or MemberBegin()).
+      \return the new iterator after removal.
+      \note This function may reorder the object members. Use \ref
+          EraseMember(ConstMemberIterator) if you need to preserve the
+          relative order of the remaining members.
+      \note Constant time complexity.
+  */
+  MemberIterator RemoveMember(MemberIterator m) {
+    RAPIDJSON_ASSERT(IsObject());
+    RAPIDJSON_ASSERT(data_.o.size > 0);
+    RAPIDJSON_ASSERT(GetMembersPointer() != 0);
+    RAPIDJSON_ASSERT(m >= MemberBegin() && m < MemberEnd());
+
+    MemberIterator last(GetMembersPointer() + (data_.o.size - 1));
+    if (data_.o.size > 1 && m != last)
+      *m = *last;  // Move the last one to this place
+    else
+      m->~Member();  // Only one left, just destroy
+    --data_.o.size;
+    return m;
+  }
+
+  //! Remove a member from an object by iterator.
+  /*! \param pos iterator to the member to remove
+      \pre IsObject() == true && \ref MemberBegin() <= \c pos < \ref MemberEnd()
+      \return Iterator following the removed element.
+          If the iterator \c pos refers to the last element, the \ref
+     MemberEnd() iterator is returned. \note This function preserves the
+     relative order of the remaining object members. If you do not need this,
+     use the more efficient \ref RemoveMember(MemberIterator). \note Linear time
+     complexity.
+  */
+  MemberIterator EraseMember(ConstMemberIterator pos) {
+    return EraseMember(pos, pos + 1);
+  }
+
+  //! Remove members in the range [first, last) from an object.
+  /*! \param first iterator to the first member to remove
+      \param last  iterator following the last member to remove
+      \pre IsObject() == true && \ref MemberBegin() <= \c first <= \c last <=
+     \ref MemberEnd() \return Iterator following the last removed element. \note
+     This function preserves the relative order of the remaining object members.
+      \note Linear time complexity.
+  */
+  MemberIterator EraseMember(ConstMemberIterator first,
+                             ConstMemberIterator last) {
+    RAPIDJSON_ASSERT(IsObject());
+    RAPIDJSON_ASSERT(data_.o.size > 0);
+    RAPIDJSON_ASSERT(GetMembersPointer() != 0);
+    RAPIDJSON_ASSERT(first >= MemberBegin());
+    RAPIDJSON_ASSERT(first <= last);
+    RAPIDJSON_ASSERT(last <= MemberEnd());
+
+    MemberIterator pos = MemberBegin() + (first - MemberBegin());
+    for (MemberIterator itr = pos; itr != last; ++itr) itr->~Member();
+    std::memmove(static_cast<void *>(&*pos), &*last,
+                 static_cast<size_t>(MemberEnd() - last) * sizeof(Member));
+    data_.o.size -= static_cast<SizeType>(last - first);
+    return pos;
+  }
+
+  //! Erase a member in object by its name.
+  /*! \param name Name of member to be removed.
+      \return Whether the member existed.
+      \note Linear time complexity.
+  */
+  bool EraseMember(const Ch *name) {
+    GenericValue n(StringRef(name));
+    return EraseMember(n);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool EraseMember(const std::basic_string<Ch> &name) {
+    return EraseMember(GenericValue(StringRef(name)));
+  }
+#endif
+
+  template <typename SourceAllocator>
+  bool EraseMember(const GenericValue<Encoding, SourceAllocator> &name) {
+    MemberIterator m = FindMember(name);
+    if (m != MemberEnd()) {
+      EraseMember(m);
+      return true;
+    } else
+      return false;
+  }
+
+  Object GetObject() {
+    RAPIDJSON_ASSERT(IsObject());
+    return Object(*this);
+  }
+  ConstObject GetObject() const {
+    RAPIDJSON_ASSERT(IsObject());
+    return ConstObject(*this);
+  }
+
+  //@}
+
+  //!@name Array
+  //@{
+
+  //! Set this value as an empty array.
+  /*! \post IsArray == true */
+  GenericValue &SetArray() {
+    this->~GenericValue();
+    new (this) GenericValue(kArrayType);
+    return *this;
+  }
+
+  //! Get the number of elements in array.
+  SizeType Size() const {
+    RAPIDJSON_ASSERT(IsArray());
+    return data_.a.size;
+  }
+
+  //! Get the capacity of array.
+  SizeType Capacity() const {
+    RAPIDJSON_ASSERT(IsArray());
+    return data_.a.capacity;
+  }
+
+  //! Check whether the array is empty.
+  bool Empty() const {
+    RAPIDJSON_ASSERT(IsArray());
+    return data_.a.size == 0;
+  }
+
+  //! Remove all elements in the array.
+  /*! This function do not deallocate memory in the array, i.e. the capacity is
+     unchanged. \note Linear time complexity.
+  */
+  void Clear() {
+    RAPIDJSON_ASSERT(IsArray());
+    GenericValue *e = GetElementsPointer();
+    for (GenericValue *v = e; v != e + data_.a.size; ++v) v->~GenericValue();
+    data_.a.size = 0;
+  }
+
+  //! Get an element from array by index.
+  /*! \pre IsArray() == true
+      \param index Zero-based index of element.
+      \see operator[](T*)
+  */
+  GenericValue &operator[](SizeType index) {
+    RAPIDJSON_ASSERT(IsArray());
+    RAPIDJSON_ASSERT(index < data_.a.size);
+    return GetElementsPointer()[index];
+  }
+  const GenericValue &operator[](SizeType index) const {
+    return const_cast<GenericValue &>(*this)[index];
+  }
+
+  //! Element iterator
+  /*! \pre IsArray() == true */
+  ValueIterator Begin() {
+    RAPIDJSON_ASSERT(IsArray());
+    return GetElementsPointer();
+  }
+  //! \em Past-the-end element iterator
+  /*! \pre IsArray() == true */
+  ValueIterator End() {
+    RAPIDJSON_ASSERT(IsArray());
+    return GetElementsPointer() + data_.a.size;
+  }
+  //! Constant element iterator
+  /*! \pre IsArray() == true */
+  ConstValueIterator Begin() const {
+    return const_cast<GenericValue &>(*this).Begin();
+  }
+  //! Constant \em past-the-end element iterator
+  /*! \pre IsArray() == true */
+  ConstValueIterator End() const {
+    return const_cast<GenericValue &>(*this).End();
+  }
+
+  //! Request the array to have enough capacity to store elements.
+  /*! \param newCapacity  The capacity that the array at least need to have.
+      \param allocator    Allocator for reallocating memory. It must be the same
+     one as used before. Commonly use GenericDocument::GetAllocator(). \return
+     The value itself for fluent API. \note Linear time complexity.
+  */
+  GenericValue &Reserve(SizeType newCapacity, Allocator &allocator) {
+    RAPIDJSON_ASSERT(IsArray());
+    if (newCapacity > data_.a.capacity) {
+      SetElementsPointer(reinterpret_cast<GenericValue *>(allocator.Realloc(
+          GetElementsPointer(), data_.a.capacity * sizeof(GenericValue),
+          newCapacity * sizeof(GenericValue))));
+      data_.a.capacity = newCapacity;
+    }
+    return *this;
+  }
+
+  //! Append a GenericValue at the end of the array.
+  /*! \param value        Value to be appended.
+      \param allocator    Allocator for reallocating memory. It must be the same
+     one as used before. Commonly use GenericDocument::GetAllocator(). \pre
+     IsArray() == true \post value.IsNull() == true \return The value itself for
+     fluent API. \note The ownership of \c value will be transferred to this
+     array on success. \note If the number of elements to be appended is known,
+     calls Reserve() once first may be more efficient. \note Amortized constant
+     time complexity.
+  */
+  GenericValue &PushBack(GenericValue &value, Allocator &allocator) {
+    RAPIDJSON_ASSERT(IsArray());
+    if (data_.a.size >= data_.a.capacity)
+      Reserve(data_.a.capacity == 0
+                  ? kDefaultArrayCapacity
+                  : (data_.a.capacity + (data_.a.capacity + 1) / 2),
+              allocator);
+    GetElementsPointer()[data_.a.size++].RawAssign(value);
+    return *this;
+  }
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  GenericValue &PushBack(GenericValue &&value, Allocator &allocator) {
+    return PushBack(value, allocator);
+  }
+#endif  // RAPIDJSON_HAS_CXX11_RVALUE_REFS
+
+  //! Append a constant string reference at the end of the array.
+  /*! \param value        Constant string reference to be appended.
+      \param allocator    Allocator for reallocating memory. It must be the same
+     one used previously. Commonly use GenericDocument::GetAllocator(). \pre
+     IsArray() == true \return The value itself for fluent API. \note If the
+     number of elements to be appended is known, calls Reserve() once first may
+     be more efficient. \note Amortized constant time complexity. \see
+     GenericStringRef
+  */
+  GenericValue &PushBack(StringRefType value, Allocator &allocator) {
+    return (*this).template PushBack<StringRefType>(value, allocator);
+  }
+
+  //! Append a primitive value at the end of the array.
+  /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t
+      \param value Value of primitive type T to be appended.
+      \param allocator    Allocator for reallocating memory. It must be the same
+     one as used before. Commonly use GenericDocument::GetAllocator(). \pre
+     IsArray() == true \return The value itself for fluent API. \note If the
+     number of elements to be appended is known, calls Reserve() once first may
+     be more efficient.
+
+      \note The source type \c T explicitly disallows all pointer types,
+          especially (\c const) \ref Ch*.  This helps avoiding implicitly
+          referencing character strings with insufficient lifetime, use
+          \ref PushBack(GenericValue&, Allocator&) or \ref
+          PushBack(StringRefType, Allocator&).
+          All other pointer types would implicitly convert to \c bool,
+          use an explicit cast instead, if needed.
+      \note Amortized constant time complexity.
+  */
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::OrExpr<internal::IsPointer<T>, internal::IsGenericValue<T>>),
+      (GenericValue &))
+  PushBack(T value, Allocator &allocator) {
+    GenericValue v(value);
+    return PushBack(v, allocator);
+  }
+
+  //! Remove the last element in the array.
+  /*!
+      \note Constant time complexity.
+  */
+  GenericValue &PopBack() {
+    RAPIDJSON_ASSERT(IsArray());
+    RAPIDJSON_ASSERT(!Empty());
+    GetElementsPointer()[--data_.a.size].~GenericValue();
+    return *this;
+  }
+
+  //! Remove an element of array by iterator.
+  /*!
+      \param pos iterator to the element to remove
+      \pre IsArray() == true && \ref Begin() <= \c pos < \ref End()
+      \return Iterator following the removed element. If the iterator pos refers
+     to the last element, the End() iterator is returned. \note Linear time
+     complexity.
+  */
+  ValueIterator Erase(ConstValueIterator pos) { return Erase(pos, pos + 1); }
+
+  //! Remove elements in the range [first, last) of the array.
+  /*!
+      \param first iterator to the first element to remove
+      \param last  iterator following the last element to remove
+      \pre IsArray() == true && \ref Begin() <= \c first <= \c last <= \ref
+     End() \return Iterator following the last removed element. \note Linear
+     time complexity.
+  */
+  ValueIterator Erase(ConstValueIterator first, ConstValueIterator last) {
+    RAPIDJSON_ASSERT(IsArray());
+    RAPIDJSON_ASSERT(data_.a.size > 0);
+    RAPIDJSON_ASSERT(GetElementsPointer() != 0);
+    RAPIDJSON_ASSERT(first >= Begin());
+    RAPIDJSON_ASSERT(first <= last);
+    RAPIDJSON_ASSERT(last <= End());
+    ValueIterator pos = Begin() + (first - Begin());
+    for (ValueIterator itr = pos; itr != last; ++itr) itr->~GenericValue();
+    std::memmove(static_cast<void *>(pos), last,
+                 static_cast<size_t>(End() - last) * sizeof(GenericValue));
+    data_.a.size -= static_cast<SizeType>(last - first);
+    return pos;
+  }
+
+  Array GetArray() {
+    RAPIDJSON_ASSERT(IsArray());
+    return Array(*this);
+  }
+  ConstArray GetArray() const {
+    RAPIDJSON_ASSERT(IsArray());
+    return ConstArray(*this);
+  }
+
+  //@}
+
+  //!@name Number
+  //@{
+
+  int GetInt() const {
+    RAPIDJSON_ASSERT(data_.f.flags & kIntFlag);
+    return data_.n.i.i;
+  }
+  unsigned GetUint() const {
+    RAPIDJSON_ASSERT(data_.f.flags & kUintFlag);
+    return data_.n.u.u;
+  }
+  int64_t GetInt64() const {
+    RAPIDJSON_ASSERT(data_.f.flags & kInt64Flag);
+    return data_.n.i64;
+  }
+  uint64_t GetUint64() const {
+    RAPIDJSON_ASSERT(data_.f.flags & kUint64Flag);
+    return data_.n.u64;
+  }
+
+  //! Get the value as double type.
+  /*! \note If the value is 64-bit integer type, it may lose precision. Use \c
+   * IsLosslessDouble() to check whether the converison is lossless.
+   */
+  double GetDouble() const {
+    RAPIDJSON_ASSERT(IsNumber());
+    if ((data_.f.flags & kDoubleFlag) != 0)
+      return data_.n.d;  // exact type, no conversion.
+    if ((data_.f.flags & kIntFlag) != 0) return data_.n.i.i;  // int -> double
+    if ((data_.f.flags & kUintFlag) != 0)
+      return data_.n.u.u;  // unsigned -> double
+    if ((data_.f.flags & kInt64Flag) != 0)
+      return static_cast<double>(
+          data_.n.i64);  // int64_t -> double (may lose precision)
+    RAPIDJSON_ASSERT((data_.f.flags & kUint64Flag) != 0);
+    return static_cast<double>(
+        data_.n.u64);  // uint64_t -> double (may lose precision)
+  }
+
+  //! Get the value as float type.
+  /*! \note If the value is 64-bit integer type, it may lose precision. Use \c
+   * IsLosslessFloat() to check whether the converison is lossless.
+   */
+  float GetFloat() const { return static_cast<float>(GetDouble()); }
+
+  GenericValue &SetInt(int i) {
+    this->~GenericValue();
+    new (this) GenericValue(i);
+    return *this;
+  }
+  GenericValue &SetUint(unsigned u) {
+    this->~GenericValue();
+    new (this) GenericValue(u);
+    return *this;
+  }
+  GenericValue &SetInt64(int64_t i64) {
+    this->~GenericValue();
+    new (this) GenericValue(i64);
+    return *this;
+  }
+  GenericValue &SetUint64(uint64_t u64) {
+    this->~GenericValue();
+    new (this) GenericValue(u64);
+    return *this;
+  }
+  GenericValue &SetDouble(double d) {
+    this->~GenericValue();
+    new (this) GenericValue(d);
+    return *this;
+  }
+  GenericValue &SetFloat(float f) {
+    this->~GenericValue();
+    new (this) GenericValue(static_cast<double>(f));
+    return *this;
+  }
+
+  //@}
+
+  //!@name String
+  //@{
+
+  const Ch *GetString() const {
+    RAPIDJSON_ASSERT(IsString());
+    return (data_.f.flags & kInlineStrFlag) ? data_.ss.str : GetStringPointer();
+  }
+
+  //! Get the length of string.
+  /*! Since rapidjson permits "\\u0000" in the json string,
+   * strlen(v.GetString()) may not equal to v.GetStringLength().
+   */
+  SizeType GetStringLength() const {
+    RAPIDJSON_ASSERT(IsString());
+    return ((data_.f.flags & kInlineStrFlag) ? (data_.ss.GetLength())
+                                             : data_.s.length);
+  }
+
+  //! Set this value as a string without copying source string.
+  /*! This version has better performance with supplied length, and also support
+     string containing null character. \param s source string pointer. \param
+     length The length of source string, excluding the trailing null terminator.
+      \return The value itself for fluent API.
+      \post IsString() == true && GetString() == s && GetStringLength() ==
+     length \see SetString(StringRefType)
+  */
+  GenericValue &SetString(const Ch *s, SizeType length) {
+    return SetString(StringRef(s, length));
+  }
+
+  //! Set this value as a string without copying source string.
+  /*! \param s source string reference
+      \return The value itself for fluent API.
+      \post IsString() == true && GetString() == s && GetStringLength() ==
+     s.length
+  */
+  GenericValue &SetString(StringRefType s) {
+    this->~GenericValue();
+    SetStringRaw(s);
+    return *this;
+  }
+
+  //! Set this value as a string by copying from source string.
+  /*! This version has better performance with supplied length, and also support
+     string containing null character. \param s source string. \param length The
+     length of source string, excluding the trailing null terminator. \param
+     allocator Allocator for allocating copied buffer. Commonly use
+     GenericDocument::GetAllocator(). \return The value itself for fluent API.
+      \post IsString() == true && GetString() != s && strcmp(GetString(),s) == 0
+     && GetStringLength() == length
+  */
+  GenericValue &SetString(const Ch *s, SizeType length, Allocator &allocator) {
+    return SetString(StringRef(s, length), allocator);
+  }
+
+  //! Set this value as a string by copying from source string.
+  /*! \param s source string.
+      \param allocator Allocator for allocating copied buffer. Commonly use
+     GenericDocument::GetAllocator(). \return The value itself for fluent API.
+      \post IsString() == true && GetString() != s && strcmp(GetString(),s) == 0
+     && GetStringLength() == length
+  */
+  GenericValue &SetString(const Ch *s, Allocator &allocator) {
+    return SetString(StringRef(s), allocator);
+  }
+
+  //! Set this value as a string by copying from source string.
+  /*! \param s source string reference
+      \param allocator Allocator for allocating copied buffer. Commonly use
+     GenericDocument::GetAllocator(). \return The value itself for fluent API.
+      \post IsString() == true && GetString() != s.s && strcmp(GetString(),s) ==
+     0 && GetStringLength() == length
+  */
+  GenericValue &SetString(StringRefType s, Allocator &allocator) {
+    this->~GenericValue();
+    SetStringRaw(s, allocator);
+    return *this;
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Set this value as a string by copying from source string.
+  /*! \param s source string.
+      \param allocator Allocator for allocating copied buffer. Commonly use
+     GenericDocument::GetAllocator(). \return The value itself for fluent API.
+      \post IsString() == true && GetString() != s.data() &&
+     strcmp(GetString(),s.data() == 0 && GetStringLength() == s.size() \note
+     Requires the definition of the preprocessor symbol \ref
+     RAPIDJSON_HAS_STDSTRING.
+  */
+  GenericValue &SetString(const std::basic_string<Ch> &s,
+                          Allocator &allocator) {
+    return SetString(StringRef(s), allocator);
+  }
+#endif
+
+  //@}
+
+  //!@name Array
+  //@{
+
+  //! Templated version for checking whether this value is type T.
+  /*!
+      \tparam T Either \c bool, \c int, \c unsigned, \c int64_t, \c uint64_t, \c
+     double, \c float, \c const \c char*, \c std::basic_string<Ch>
+  */
+  template <typename T>
+  bool Is() const {
+    return internal::TypeHelper<ValueType, T>::Is(*this);
+  }
+
+  template <typename T>
+  T Get() const {
+    return internal::TypeHelper<ValueType, T>::Get(*this);
+  }
+
+  template <typename T>
+  T Get() {
+    return internal::TypeHelper<ValueType, T>::Get(*this);
+  }
+
+  template <typename T>
+  ValueType &Set(const T &data) {
+    return internal::TypeHelper<ValueType, T>::Set(*this, data);
+  }
+
+  template <typename T>
+  ValueType &Set(const T &data, AllocatorType &allocator) {
+    return internal::TypeHelper<ValueType, T>::Set(*this, data, allocator);
+  }
+
+  //@}
+
+  //! Generate events of this value to a Handler.
+  /*! This function adopts the GoF visitor pattern.
+      Typical usage is to output this JSON value as JSON text via Writer, which
+     is a Handler. It can also be used to deep clone this value via
+     GenericDocument, which is also a Handler. \tparam Handler type of handler.
+      \param handler An object implementing concept Handler.
+  */
+  template <typename Handler>
+  bool Accept(Handler &handler) const {
+    switch (GetType()) {
+      case kNullType:
+        return handler.Null();
+      case kFalseType:
+        return handler.Bool(false);
+      case kTrueType:
+        return handler.Bool(true);
+
+      case kObjectType:
+        if (RAPIDJSON_UNLIKELY(!handler.StartObject())) return false;
+        for (ConstMemberIterator m = MemberBegin(); m != MemberEnd(); ++m) {
+          RAPIDJSON_ASSERT(m->name.IsString());  // User may change the type of
+                                                 // name by MemberIterator.
+          if (RAPIDJSON_UNLIKELY(
+                  !handler.Key(m->name.GetString(), m->name.GetStringLength(),
+                               (m->name.data_.f.flags & kCopyFlag) != 0)))
+            return false;
+          if (RAPIDJSON_UNLIKELY(!m->value.Accept(handler))) return false;
+        }
+        return handler.EndObject(data_.o.size);
+
+      case kArrayType:
+        if (RAPIDJSON_UNLIKELY(!handler.StartArray())) return false;
+        for (const GenericValue *v = Begin(); v != End(); ++v)
+          if (RAPIDJSON_UNLIKELY(!v->Accept(handler))) return false;
+        return handler.EndArray(data_.a.size);
+
+      case kStringType:
+        return handler.String(GetString(), GetStringLength(),
+                              (data_.f.flags & kCopyFlag) != 0);
+
+      default:
+        RAPIDJSON_ASSERT(GetType() == kNumberType);
+        if (IsDouble())
+          return handler.Double(data_.n.d);
+        else if (IsInt())
+          return handler.Int(data_.n.i.i);
+        else if (IsUint())
+          return handler.Uint(data_.n.u.u);
+        else if (IsInt64())
+          return handler.Int64(data_.n.i64);
+        else
+          return handler.Uint64(data_.n.u64);
+    }
+  }
+
+ private:
+  template <typename, typename>
+  friend class GenericValue;
+  template <typename, typename, typename>
+  friend class GenericDocument;
+
+  enum {
+    kBoolFlag = 0x0008,
+    kNumberFlag = 0x0010,
+    kIntFlag = 0x0020,
+    kUintFlag = 0x0040,
+    kInt64Flag = 0x0080,
+    kUint64Flag = 0x0100,
+    kDoubleFlag = 0x0200,
+    kStringFlag = 0x0400,
+    kCopyFlag = 0x0800,
+    kInlineStrFlag = 0x1000,
+
+    // Initial flags of different types.
+    kNullFlag = kNullType,
+    kTrueFlag = kTrueType | kBoolFlag,
+    kFalseFlag = kFalseType | kBoolFlag,
+    kNumberIntFlag = kNumberType | kNumberFlag | kIntFlag | kInt64Flag,
+    kNumberUintFlag =
+        kNumberType | kNumberFlag | kUintFlag | kUint64Flag | kInt64Flag,
+    kNumberInt64Flag = kNumberType | kNumberFlag | kInt64Flag,
+    kNumberUint64Flag = kNumberType | kNumberFlag | kUint64Flag,
+    kNumberDoubleFlag = kNumberType | kNumberFlag | kDoubleFlag,
+    kNumberAnyFlag = kNumberType | kNumberFlag | kIntFlag | kInt64Flag |
+                     kUintFlag | kUint64Flag | kDoubleFlag,
+    kConstStringFlag = kStringType | kStringFlag,
+    kCopyStringFlag = kStringType | kStringFlag | kCopyFlag,
+    kShortStringFlag = kStringType | kStringFlag | kCopyFlag | kInlineStrFlag,
+    kObjectFlag = kObjectType,
+    kArrayFlag = kArrayType,
+
+    kTypeMask = 0x07
+  };
+
+  static const SizeType kDefaultArrayCapacity = 16;
+  static const SizeType kDefaultObjectCapacity = 16;
+
+  struct Flag {
+#if RAPIDJSON_48BITPOINTER_OPTIMIZATION
+    char payload[sizeof(SizeType) * 2 +
+                 6];  // 2 x SizeType + lower 48-bit pointer
+#elif RAPIDJSON_64BIT
+    char payload[sizeof(SizeType) * 2 + sizeof(void *) + 6];  // 6 padding bytes
+#else
+        char payload[sizeof(SizeType) * 2 + sizeof(void *) +
+                     2];  // 2 padding bytes
+#endif
+    uint16_t flags;
+  };
+
+  struct String {
+    SizeType length;
+    SizeType hashcode;  //!< reserved
+    const Ch *str;
+  };  // 12 bytes in 32-bit mode, 16 bytes in 64-bit mode
+
+  // implementation detail: ShortString can represent zero-terminated strings up
+  // to MaxSize chars (excluding the terminating zero) and store a value to
+  // determine the length of the contained string in the last character
+  // str[LenPos] by storing "MaxSize - length" there. If the string to store has
+  // the maximal length of MaxSize then str[LenPos] will be 0 and therefore act
+  // as the string terminator as well. For getting the string length back from
+  // that value just use "MaxSize - str[LenPos]". This allows to store 13-chars
+  // strings in 32-bit mode, 21-chars strings in 64-bit mode, 13-chars strings
+  // for RAPIDJSON_48BITPOINTER_OPTIMIZATION=1 inline (for `UTF8`-encoded
+  // strings).
+  struct ShortString {
+    enum {
+      MaxChars = sizeof(static_cast<Flag *>(0)->payload) / sizeof(Ch),
+      MaxSize = MaxChars - 1,
+      LenPos = MaxSize
+    };
+    Ch str[MaxChars];
+
+    inline static bool Usable(SizeType len) { return (MaxSize >= len); }
+    inline void SetLength(SizeType len) {
+      str[LenPos] = static_cast<Ch>(MaxSize - len);
+    }
+    inline SizeType GetLength() const {
+      return static_cast<SizeType>(MaxSize - str[LenPos]);
+    }
+  };  // at most as many bytes as "String" above => 12 bytes in 32-bit mode, 16
+      // bytes in 64-bit mode
+
+  // By using proper binary layout, retrieval of different integer types do not
+  // need conversions.
+  union Number {
+#if RAPIDJSON_ENDIAN == RAPIDJSON_LITTLEENDIAN
+    struct I {
+      int i;
+      char padding[4];
+    } i;
+    struct U {
+      unsigned u;
+      char padding2[4];
+    } u;
+#else
+    struct I {
+      char padding[4];
+      int i;
+    } i;
+    struct U {
+      char padding2[4];
+      unsigned u;
+    } u;
+#endif
+    int64_t i64;
+    uint64_t u64;
+    double d;
+  };  // 8 bytes
+
+  struct ObjectData {
+    SizeType size;
+    SizeType capacity;
+    Member *members;
+  };  // 12 bytes in 32-bit mode, 16 bytes in 64-bit mode
+
+  struct ArrayData {
+    SizeType size;
+    SizeType capacity;
+    GenericValue *elements;
+  };  // 12 bytes in 32-bit mode, 16 bytes in 64-bit mode
+
+  union Data {
+    String s;
+    ShortString ss;
+    Number n;
+    ObjectData o;
+    ArrayData a;
+    Flag f;
+  };  // 16 bytes in 32-bit mode, 24 bytes in 64-bit mode, 16 bytes in 64-bit
+      // with RAPIDJSON_48BITPOINTER_OPTIMIZATION
+
+  RAPIDJSON_FORCEINLINE const Ch *GetStringPointer() const {
+    return RAPIDJSON_GETPOINTER(Ch, data_.s.str);
+  }
+  RAPIDJSON_FORCEINLINE const Ch *SetStringPointer(const Ch *str) {
+    return RAPIDJSON_SETPOINTER(Ch, data_.s.str, str);
+  }
+  RAPIDJSON_FORCEINLINE GenericValue *GetElementsPointer() const {
+    return RAPIDJSON_GETPOINTER(GenericValue, data_.a.elements);
+  }
+  RAPIDJSON_FORCEINLINE GenericValue *SetElementsPointer(
+      GenericValue *elements) {
+    return RAPIDJSON_SETPOINTER(GenericValue, data_.a.elements, elements);
+  }
+  RAPIDJSON_FORCEINLINE Member *GetMembersPointer() const {
+    return RAPIDJSON_GETPOINTER(Member, data_.o.members);
+  }
+  RAPIDJSON_FORCEINLINE Member *SetMembersPointer(Member *members) {
+    return RAPIDJSON_SETPOINTER(Member, data_.o.members, members);
+  }
+
+  // Initialize this value as array with initial data, without calling
+  // destructor.
+  void SetArrayRaw(GenericValue *values, SizeType count, Allocator &allocator) {
+    data_.f.flags = kArrayFlag;
+    if (count) {
+      GenericValue *e = static_cast<GenericValue *>(
+          allocator.Malloc(count * sizeof(GenericValue)));
+      SetElementsPointer(e);
+      std::memcpy(static_cast<void *>(e), values, count * sizeof(GenericValue));
+    } else
+      SetElementsPointer(0);
+    data_.a.size = data_.a.capacity = count;
+  }
+
+  //! Initialize this value as object with initial data, without calling
+  //! destructor.
+  void SetObjectRaw(Member *members, SizeType count, Allocator &allocator) {
+    data_.f.flags = kObjectFlag;
+    if (count) {
+      Member *m =
+          static_cast<Member *>(allocator.Malloc(count * sizeof(Member)));
+      SetMembersPointer(m);
+      std::memcpy(static_cast<void *>(m), members, count * sizeof(Member));
+    } else
+      SetMembersPointer(0);
+    data_.o.size = data_.o.capacity = count;
+  }
+
+  //! Initialize this value as constant string, without calling destructor.
+  void SetStringRaw(StringRefType s) RAPIDJSON_NOEXCEPT {
+    data_.f.flags = kConstStringFlag;
+    SetStringPointer(s);
+    data_.s.length = s.length;
+  }
+
+  //! Initialize this value as copy string with initial data, without calling
+  //! destructor.
+  void SetStringRaw(StringRefType s, Allocator &allocator) {
+    Ch *str = 0;
+    if (ShortString::Usable(s.length)) {
+      data_.f.flags = kShortStringFlag;
+      data_.ss.SetLength(s.length);
+      str = data_.ss.str;
+    } else {
+      data_.f.flags = kCopyStringFlag;
+      data_.s.length = s.length;
+      str = static_cast<Ch *>(allocator.Malloc((s.length + 1) * sizeof(Ch)));
+      SetStringPointer(str);
+    }
+    std::memcpy(str, s, s.length * sizeof(Ch));
+    str[s.length] = '\0';
+  }
+
+  //! Assignment without calling destructor
+  void RawAssign(GenericValue &rhs) RAPIDJSON_NOEXCEPT {
+    data_ = rhs.data_;
+    // data_.f.flags = rhs.data_.f.flags;
+    rhs.data_.f.flags = kNullFlag;
+  }
+
+  template <typename SourceAllocator>
+  bool StringEqual(const GenericValue<Encoding, SourceAllocator> &rhs) const {
+    RAPIDJSON_ASSERT(IsString());
+    RAPIDJSON_ASSERT(rhs.IsString());
+
+    const SizeType len1 = GetStringLength();
+    const SizeType len2 = rhs.GetStringLength();
+    if (len1 != len2) {
+      return false;
+    }
+
+    const Ch *const str1 = GetString();
+    const Ch *const str2 = rhs.GetString();
+    if (str1 == str2) {
+      return true;
+    }  // fast path for constant string
+
+    return (std::memcmp(str1, str2, sizeof(Ch) * len1) == 0);
+  }
+
+  Data data_;
+};
+
+//! GenericValue with UTF8 encoding
+typedef GenericValue<UTF8<>> Value;
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericDocument
+
+//! A document for parsing JSON text as DOM.
+/*!
+    \note implements Handler concept
+    \tparam Encoding Encoding for both parsing and string storage.
+    \tparam Allocator Allocator for allocating memory for the DOM
+    \tparam StackAllocator Allocator for allocating memory for stack during
+   parsing. \warning Although GenericDocument inherits from GenericValue, the
+   API does \b not provide any virtual functions, especially no virtual
+   destructor.  To avoid memory leaks, do not \c delete a GenericDocument object
+   via a pointer to a GenericValue.
+*/
+template <typename Encoding, typename Allocator = MemoryPoolAllocator<>,
+          typename StackAllocator = CrtAllocator>
+class GenericDocument : public GenericValue<Encoding, Allocator> {
+ public:
+  typedef typename Encoding::Ch Ch;  //!< Character type derived from Encoding.
+  typedef GenericValue<Encoding, Allocator>
+      ValueType;                    //!< Value type of the document.
+  typedef Allocator AllocatorType;  //!< Allocator type from template parameter.
+
+  //! Constructor
+  /*! Creates an empty document of specified type.
+      \param type             Mandatory type of object to create.
+      \param allocator        Optional allocator for allocating memory.
+      \param stackCapacity    Optional initial capacity of stack in bytes.
+      \param stackAllocator   Optional allocator for allocating memory for
+     stack.
+  */
+  explicit GenericDocument(Type type, Allocator *allocator = 0,
+                           size_t stackCapacity = kDefaultStackCapacity,
+                           StackAllocator *stackAllocator = 0)
+      : GenericValue<Encoding, Allocator>(type),
+        allocator_(allocator),
+        ownAllocator_(0),
+        stack_(stackAllocator, stackCapacity),
+        parseResult_() {
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+  }
+
+  //! Constructor
+  /*! Creates an empty document which type is Null.
+      \param allocator        Optional allocator for allocating memory.
+      \param stackCapacity    Optional initial capacity of stack in bytes.
+      \param stackAllocator   Optional allocator for allocating memory for
+     stack.
+  */
+  GenericDocument(Allocator *allocator = 0,
+                  size_t stackCapacity = kDefaultStackCapacity,
+                  StackAllocator *stackAllocator = 0)
+      : allocator_(allocator),
+        ownAllocator_(0),
+        stack_(stackAllocator, stackCapacity),
+        parseResult_() {
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+  }
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  //! Move constructor in C++11
+  GenericDocument(GenericDocument &&rhs) RAPIDJSON_NOEXCEPT
+      : ValueType(std::forward<ValueType>(
+            rhs)),  // explicit cast to avoid prohibited move from Document
+        allocator_(rhs.allocator_),
+        ownAllocator_(rhs.ownAllocator_),
+        stack_(std::move(rhs.stack_)),
+        parseResult_(rhs.parseResult_) {
+    rhs.allocator_ = 0;
+    rhs.ownAllocator_ = 0;
+    rhs.parseResult_ = ParseResult();
+  }
+#endif
+
+  ~GenericDocument() { Destroy(); }
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  //! Move assignment in C++11
+  GenericDocument &operator=(GenericDocument &&rhs) RAPIDJSON_NOEXCEPT {
+    // The cast to ValueType is necessary here, because otherwise it would
+    // attempt to call GenericValue's templated assignment operator.
+    ValueType::operator=(std::forward<ValueType>(rhs));
+
+    // Calling the destructor here would prematurely call stack_'s destructor
+    Destroy();
+
+    allocator_ = rhs.allocator_;
+    ownAllocator_ = rhs.ownAllocator_;
+    stack_ = std::move(rhs.stack_);
+    parseResult_ = rhs.parseResult_;
+
+    rhs.allocator_ = 0;
+    rhs.ownAllocator_ = 0;
+    rhs.parseResult_ = ParseResult();
+
+    return *this;
+  }
+#endif
+
+  //! Exchange the contents of this document with those of another.
+  /*!
+      \param rhs Another document.
+      \note Constant complexity.
+      \see GenericValue::Swap
+  */
+  GenericDocument &Swap(GenericDocument &rhs) RAPIDJSON_NOEXCEPT {
+    ValueType::Swap(rhs);
+    stack_.Swap(rhs.stack_);
+    internal::Swap(allocator_, rhs.allocator_);
+    internal::Swap(ownAllocator_, rhs.ownAllocator_);
+    internal::Swap(parseResult_, rhs.parseResult_);
+    return *this;
+  }
+
+  // Allow Swap with ValueType.
+  // Refer to Effective C++ 3rd Edition/Item 33: Avoid hiding inherited names.
+  using ValueType::Swap;
+
+  //! free-standing swap function helper
+  /*!
+      Helper function to enable support for common swap implementation pattern
+     based on \c std::swap: \code void swap(MyClass& a, MyClass& b) { using
+     std::swap; swap(a.doc, b.doc);
+          // ...
+      }
+      \endcode
+      \see Swap()
+   */
+  friend inline void swap(GenericDocument &a,
+                          GenericDocument &b) RAPIDJSON_NOEXCEPT {
+    a.Swap(b);
+  }
+
+  //! Populate this document by a generator which produces SAX events.
+  /*! \tparam Generator A functor with <tt>bool f(Handler)</tt> prototype.
+      \param g Generator functor which sends SAX events to the parameter.
+      \return The document itself for fluent API.
+  */
+  template <typename Generator>
+  GenericDocument &Populate(Generator &g) {
+    ClearStackOnExit scope(*this);
+    if (g(*this)) {
+      RAPIDJSON_ASSERT(stack_.GetSize() ==
+                       sizeof(ValueType));  // Got one and only one root object
+      ValueType::operator=(*stack_.template Pop<ValueType>(
+          1));  // Move value from stack to document
+    }
+    return *this;
+  }
+
+  //!@name Parse from stream
+  //!@{
+
+  //! Parse JSON text from an input stream (with Encoding conversion)
+  /*! \tparam parseFlags Combination of \ref ParseFlag.
+      \tparam SourceEncoding Encoding of input stream
+      \tparam InputStream Type of input stream, implementing Stream concept
+      \param is Input stream to be parsed.
+      \return The document itself for fluent API.
+  */
+  template <unsigned parseFlags, typename SourceEncoding, typename InputStream>
+  GenericDocument &ParseStream(InputStream &is) {
+    GenericReader<SourceEncoding, Encoding, StackAllocator> reader(
+        stack_.HasAllocator() ? &stack_.GetAllocator() : 0);
+    ClearStackOnExit scope(*this);
+    parseResult_ = reader.template Parse<parseFlags>(is, *this);
+    if (parseResult_) {
+      RAPIDJSON_ASSERT(stack_.GetSize() ==
+                       sizeof(ValueType));  // Got one and only one root object
+      ValueType::operator=(*stack_.template Pop<ValueType>(
+          1));  // Move value from stack to document
+    }
+    return *this;
+  }
+
+  //! Parse JSON text from an input stream
+  /*! \tparam parseFlags Combination of \ref ParseFlag.
+      \tparam InputStream Type of input stream, implementing Stream concept
+      \param is Input stream to be parsed.
+      \return The document itself for fluent API.
+  */
+  template <unsigned parseFlags, typename InputStream>
+  GenericDocument &ParseStream(InputStream &is) {
+    return ParseStream<parseFlags, Encoding, InputStream>(is);
+  }
+
+  //! Parse JSON text from an input stream (with \ref kParseDefaultFlags)
+  /*! \tparam InputStream Type of input stream, implementing Stream concept
+      \param is Input stream to be parsed.
+      \return The document itself for fluent API.
+  */
+  template <typename InputStream>
+  GenericDocument &ParseStream(InputStream &is) {
+    return ParseStream<kParseDefaultFlags, Encoding, InputStream>(is);
+  }
+  //!@}
+
+  //!@name Parse in-place from mutable string
+  //!@{
+
+  //! Parse JSON text from a mutable string
+  /*! \tparam parseFlags Combination of \ref ParseFlag.
+      \param str Mutable zero-terminated string to be parsed.
+      \return The document itself for fluent API.
+  */
+  template <unsigned parseFlags>
+  GenericDocument &ParseInsitu(Ch *str) {
+    GenericInsituStringStream<Encoding> s(str);
+    return ParseStream<parseFlags | kParseInsituFlag>(s);
+  }
+
+  //! Parse JSON text from a mutable string (with \ref kParseDefaultFlags)
+  /*! \param str Mutable zero-terminated string to be parsed.
+      \return The document itself for fluent API.
+  */
+  GenericDocument &ParseInsitu(Ch *str) {
+    return ParseInsitu<kParseDefaultFlags>(str);
+  }
+  //!@}
+
+  //!@name Parse from read-only string
+  //!@{
+
+  //! Parse JSON text from a read-only string (with Encoding conversion)
+  /*! \tparam parseFlags Combination of \ref ParseFlag (must not contain \ref
+     kParseInsituFlag). \tparam SourceEncoding Transcoding from input Encoding
+      \param str Read-only zero-terminated string to be parsed.
+  */
+  template <unsigned parseFlags, typename SourceEncoding>
+  GenericDocument &Parse(const typename SourceEncoding::Ch *str) {
+    RAPIDJSON_ASSERT(!(parseFlags & kParseInsituFlag));
+    GenericStringStream<SourceEncoding> s(str);
+    return ParseStream<parseFlags, SourceEncoding>(s);
+  }
+
+  //! Parse JSON text from a read-only string
+  /*! \tparam parseFlags Combination of \ref ParseFlag (must not contain \ref
+     kParseInsituFlag). \param str Read-only zero-terminated string to be
+     parsed.
+  */
+  template <unsigned parseFlags>
+  GenericDocument &Parse(const Ch *str) {
+    return Parse<parseFlags, Encoding>(str);
+  }
+
+  //! Parse JSON text from a read-only string (with \ref kParseDefaultFlags)
+  /*! \param str Read-only zero-terminated string to be parsed.
+   */
+  GenericDocument &Parse(const Ch *str) {
+    return Parse<kParseDefaultFlags>(str);
+  }
+
+  template <unsigned parseFlags, typename SourceEncoding>
+  GenericDocument &Parse(const typename SourceEncoding::Ch *str,
+                         size_t length) {
+    RAPIDJSON_ASSERT(!(parseFlags & kParseInsituFlag));
+    MemoryStream ms(reinterpret_cast<const char *>(str),
+                    length * sizeof(typename SourceEncoding::Ch));
+    EncodedInputStream<SourceEncoding, MemoryStream> is(ms);
+    ParseStream<parseFlags, SourceEncoding>(is);
+    return *this;
+  }
+
+  template <unsigned parseFlags>
+  GenericDocument &Parse(const Ch *str, size_t length) {
+    return Parse<parseFlags, Encoding>(str, length);
+  }
+
+  GenericDocument &Parse(const Ch *str, size_t length) {
+    return Parse<kParseDefaultFlags>(str, length);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  template <unsigned parseFlags, typename SourceEncoding>
+  GenericDocument &Parse(
+      const std::basic_string<typename SourceEncoding::Ch> &str) {
+    // c_str() is constant complexity according to standard. Should be faster
+    // than Parse(const char*, size_t)
+    return Parse<parseFlags, SourceEncoding>(str.c_str());
+  }
+
+  template <unsigned parseFlags>
+  GenericDocument &Parse(const std::basic_string<Ch> &str) {
+    return Parse<parseFlags, Encoding>(str.c_str());
+  }
+
+  GenericDocument &Parse(const std::basic_string<Ch> &str) {
+    return Parse<kParseDefaultFlags>(str);
+  }
+#endif  // RAPIDJSON_HAS_STDSTRING
+
+  //!@}
+
+  //!@name Handling parse errors
+  //!@{
+
+  //! Whether a parse error has occurred in the last parsing.
+  bool HasParseError() const { return parseResult_.IsError(); }
+
+  //! Get the \ref ParseErrorCode of last parsing.
+  ParseErrorCode GetParseError() const { return parseResult_.Code(); }
+
+  //! Get the position of last parsing error in input, 0 otherwise.
+  size_t GetErrorOffset() const { return parseResult_.Offset(); }
+
+//! Implicit conversion to get the last parse result
+#ifndef __clang  // -Wdocumentation
+/*! \return \ref ParseResult of the last parse operation
+
+    \code
+      Document doc;
+      ParseResult ok = doc.Parse(json);
+      if (!ok)
+        printf( "JSON parse error: %s (%u)\n", GetParseError_En(ok.Code()),
+   ok.Offset());               \endcode
+ */
+#endif
+  operator ParseResult() const { return parseResult_; }
+  //!@}
+
+  //! Get the allocator of this document.
+  Allocator &GetAllocator() {
+    RAPIDJSON_ASSERT(allocator_);
+    return *allocator_;
+  }
+
+  //! Get the capacity of stack in bytes.
+  size_t GetStackCapacity() const { return stack_.GetCapacity(); }
+
+ private:
+  // clear stack on any exit from ParseStream, e.g. due to exception
+  struct ClearStackOnExit {
+    explicit ClearStackOnExit(GenericDocument &d) : d_(d) {}
+    ~ClearStackOnExit() { d_.ClearStack(); }
+
+   private:
+    ClearStackOnExit(const ClearStackOnExit &);
+    ClearStackOnExit &operator=(const ClearStackOnExit &);
+    GenericDocument &d_;
+  };
+
+  // callers of the following private Handler functions
+  // template <typename,typename,typename> friend class GenericReader; // for
+  // parsing
+  template <typename, typename>
+  friend class GenericValue;  // for deep copying
+
+ public:
+  // Implementation of Handler
+  bool Null() {
+    new (stack_.template Push<ValueType>()) ValueType();
+    return true;
+  }
+  bool Bool(bool b) {
+    new (stack_.template Push<ValueType>()) ValueType(b);
+    return true;
+  }
+  bool Int(int i) {
+    new (stack_.template Push<ValueType>()) ValueType(i);
+    return true;
+  }
+  bool Uint(unsigned i) {
+    new (stack_.template Push<ValueType>()) ValueType(i);
+    return true;
+  }
+  bool Int64(int64_t i) {
+    new (stack_.template Push<ValueType>()) ValueType(i);
+    return true;
+  }
+  bool Uint64(uint64_t i) {
+    new (stack_.template Push<ValueType>()) ValueType(i);
+    return true;
+  }
+  bool Double(double d) {
+    new (stack_.template Push<ValueType>()) ValueType(d);
+    return true;
+  }
+
+  bool RawNumber(const Ch *str, SizeType length, bool copy) {
+    if (copy)
+      new (stack_.template Push<ValueType>())
+          ValueType(str, length, GetAllocator());
+    else
+      new (stack_.template Push<ValueType>()) ValueType(str, length);
+    return true;
+  }
+
+  bool String(const Ch *str, SizeType length, bool copy) {
+    if (copy)
+      new (stack_.template Push<ValueType>())
+          ValueType(str, length, GetAllocator());
+    else
+      new (stack_.template Push<ValueType>()) ValueType(str, length);
+    return true;
+  }
+
+  bool StartObject() {
+    new (stack_.template Push<ValueType>()) ValueType(kObjectType);
+    return true;
+  }
+
+  bool Key(const Ch *str, SizeType length, bool copy) {
+    return String(str, length, copy);
+  }
+
+  bool EndObject(SizeType memberCount) {
+    typename ValueType::Member *members =
+        stack_.template Pop<typename ValueType::Member>(memberCount);
+    stack_.template Top<ValueType>()->SetObjectRaw(members, memberCount,
+                                                   GetAllocator());
+    return true;
+  }
+
+  bool StartArray() {
+    new (stack_.template Push<ValueType>()) ValueType(kArrayType);
+    return true;
+  }
+
+  bool EndArray(SizeType elementCount) {
+    ValueType *elements = stack_.template Pop<ValueType>(elementCount);
+    stack_.template Top<ValueType>()->SetArrayRaw(elements, elementCount,
+                                                  GetAllocator());
+    return true;
+  }
+
+ private:
+  //! Prohibit copying
+  GenericDocument(const GenericDocument &);
+  //! Prohibit assignment
+  GenericDocument &operator=(const GenericDocument &);
+
+  void ClearStack() {
+    if (Allocator::kNeedFree)
+      while (stack_.GetSize() >
+             0)  // Here assumes all elements in stack array are GenericValue
+                 // (Member is actually 2 GenericValue objects)
+        (stack_.template Pop<ValueType>(1))->~ValueType();
+    else
+      stack_.Clear();
+    stack_.ShrinkToFit();
+  }
+
+  void Destroy() { RAPIDJSON_DELETE(ownAllocator_); }
+
+  static const size_t kDefaultStackCapacity = 1024;
+  Allocator *allocator_;
+  Allocator *ownAllocator_;
+  internal::Stack<StackAllocator> stack_;
+  ParseResult parseResult_;
+};
+
+//! GenericDocument with UTF8 encoding
+typedef GenericDocument<UTF8<>> Document;
+
+//! Helper class for accessing Value of array type.
+/*!
+    Instance of this helper class is obtained by \c GenericValue::GetArray().
+    In addition to all APIs for array type, it provides range-based for loop if
+   \c RAPIDJSON_HAS_CXX11_RANGE_FOR=1.
+*/
+template <bool Const, typename ValueT>
+class GenericArray {
+ public:
+  typedef GenericArray<true, ValueT> ConstArray;
+  typedef GenericArray<false, ValueT> Array;
+  typedef ValueT PlainType;
+  typedef typename internal::MaybeAddConst<Const, PlainType>::Type ValueType;
+  typedef ValueType *ValueIterator;  // This may be const or non-const iterator
+  typedef const ValueT *ConstValueIterator;
+  typedef typename ValueType::AllocatorType AllocatorType;
+  typedef typename ValueType::StringRefType StringRefType;
+
+  template <typename, typename>
+  friend class GenericValue;
+
+  GenericArray(const GenericArray &rhs) : value_(rhs.value_) {}
+  GenericArray &operator=(const GenericArray &rhs) {
+    value_ = rhs.value_;
+    return *this;
+  }
+  ~GenericArray() {}
+
+  SizeType Size() const { return value_.Size(); }
+  SizeType Capacity() const { return value_.Capacity(); }
+  bool Empty() const { return value_.Empty(); }
+  void Clear() const { value_.Clear(); }
+  ValueType &operator[](SizeType index) const { return value_[index]; }
+  ValueIterator Begin() const { return value_.Begin(); }
+  ValueIterator End() const { return value_.End(); }
+  GenericArray Reserve(SizeType newCapacity, AllocatorType &allocator) const {
+    value_.Reserve(newCapacity, allocator);
+    return *this;
+  }
+  GenericArray PushBack(ValueType &value, AllocatorType &allocator) const {
+    value_.PushBack(value, allocator);
+    return *this;
+  }
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  GenericArray PushBack(ValueType &&value, AllocatorType &allocator) const {
+    value_.PushBack(value, allocator);
+    return *this;
+  }
+#endif  // RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  GenericArray PushBack(StringRefType value, AllocatorType &allocator) const {
+    value_.PushBack(value, allocator);
+    return *this;
+  }
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::OrExpr<internal::IsPointer<T>, internal::IsGenericValue<T>>),
+      (const GenericArray &))
+  PushBack(T value, AllocatorType &allocator) const {
+    value_.PushBack(value, allocator);
+    return *this;
+  }
+  GenericArray PopBack() const {
+    value_.PopBack();
+    return *this;
+  }
+  ValueIterator Erase(ConstValueIterator pos) const {
+    return value_.Erase(pos);
+  }
+  ValueIterator Erase(ConstValueIterator first, ConstValueIterator last) const {
+    return value_.Erase(first, last);
+  }
+
+#if RAPIDJSON_HAS_CXX11_RANGE_FOR
+  ValueIterator begin() const { return value_.Begin(); }
+  ValueIterator end() const { return value_.End(); }
+#endif
+
+ private:
+  GenericArray();
+  GenericArray(ValueType &value) : value_(value) {}
+  ValueType &value_;
+};
+
+//! Helper class for accessing Value of object type.
+/*!
+    Instance of this helper class is obtained by \c GenericValue::GetObject().
+    In addition to all APIs for array type, it provides range-based for loop if
+   \c RAPIDJSON_HAS_CXX11_RANGE_FOR=1.
+*/
+template <bool Const, typename ValueT>
+class GenericObject {
+ public:
+  typedef GenericObject<true, ValueT> ConstObject;
+  typedef GenericObject<false, ValueT> Object;
+  typedef ValueT PlainType;
+  typedef typename internal::MaybeAddConst<Const, PlainType>::Type ValueType;
+  typedef GenericMemberIterator<Const, typename ValueT::EncodingType,
+                                typename ValueT::AllocatorType>
+      MemberIterator;  // This may be const or non-const iterator
+  typedef GenericMemberIterator<true, typename ValueT::EncodingType,
+                                typename ValueT::AllocatorType>
+      ConstMemberIterator;
+  typedef typename ValueType::AllocatorType AllocatorType;
+  typedef typename ValueType::StringRefType StringRefType;
+  typedef typename ValueType::EncodingType EncodingType;
+  typedef typename ValueType::Ch Ch;
+
+  template <typename, typename>
+  friend class GenericValue;
+
+  GenericObject(const GenericObject &rhs) : value_(rhs.value_) {}
+  GenericObject &operator=(const GenericObject &rhs) {
+    value_ = rhs.value_;
+    return *this;
+  }
+  ~GenericObject() {}
+
+  SizeType MemberCount() const { return value_.MemberCount(); }
+  SizeType MemberCapacity() const { return value_.MemberCapacity(); }
+  bool ObjectEmpty() const { return value_.ObjectEmpty(); }
+  template <typename T>
+  ValueType &operator[](T *name) const {
+    return value_[name];
+  }
+  template <typename SourceAllocator>
+  ValueType &operator[](
+      const GenericValue<EncodingType, SourceAllocator> &name) const {
+    return value_[name];
+  }
+#if RAPIDJSON_HAS_STDSTRING
+  ValueType &operator[](const std::basic_string<Ch> &name) const {
+    return value_[name];
+  }
+#endif
+  MemberIterator MemberBegin() const { return value_.MemberBegin(); }
+  MemberIterator MemberEnd() const { return value_.MemberEnd(); }
+  GenericObject MemberReserve(SizeType newCapacity,
+                              AllocatorType &allocator) const {
+    value_.MemberReserve(newCapacity, allocator);
+    return *this;
+  }
+  bool HasMember(const Ch *name) const { return value_.HasMember(name); }
+#if RAPIDJSON_HAS_STDSTRING
+  bool HasMember(const std::basic_string<Ch> &name) const {
+    return value_.HasMember(name);
+  }
+#endif
+  template <typename SourceAllocator>
+  bool HasMember(
+      const GenericValue<EncodingType, SourceAllocator> &name) const {
+    return value_.HasMember(name);
+  }
+  MemberIterator FindMember(const Ch *name) const {
+    return value_.FindMember(name);
+  }
+  template <typename SourceAllocator>
+  MemberIterator FindMember(
+      const GenericValue<EncodingType, SourceAllocator> &name) const {
+    return value_.FindMember(name);
+  }
+#if RAPIDJSON_HAS_STDSTRING
+  MemberIterator FindMember(const std::basic_string<Ch> &name) const {
+    return value_.FindMember(name);
+  }
+#endif
+  GenericObject AddMember(ValueType &name, ValueType &value,
+                          AllocatorType &allocator) const {
+    value_.AddMember(name, value, allocator);
+    return *this;
+  }
+  GenericObject AddMember(ValueType &name, StringRefType value,
+                          AllocatorType &allocator) const {
+    value_.AddMember(name, value, allocator);
+    return *this;
+  }
+#if RAPIDJSON_HAS_STDSTRING
+  GenericObject AddMember(ValueType &name, std::basic_string<Ch> &value,
+                          AllocatorType &allocator) const {
+    value_.AddMember(name, value, allocator);
+    return *this;
+  }
+#endif
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::OrExpr<internal::IsPointer<T>, internal::IsGenericValue<T>>),
+      (ValueType &))
+  AddMember(ValueType &name, T value, AllocatorType &allocator) const {
+    value_.AddMember(name, value, allocator);
+    return *this;
+  }
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  GenericObject AddMember(ValueType &&name, ValueType &&value,
+                          AllocatorType &allocator) const {
+    value_.AddMember(name, value, allocator);
+    return *this;
+  }
+  GenericObject AddMember(ValueType &&name, ValueType &value,
+                          AllocatorType &allocator) const {
+    value_.AddMember(name, value, allocator);
+    return *this;
+  }
+  GenericObject AddMember(ValueType &name, ValueType &&value,
+                          AllocatorType &allocator) const {
+    value_.AddMember(name, value, allocator);
+    return *this;
+  }
+  GenericObject AddMember(StringRefType name, ValueType &&value,
+                          AllocatorType &allocator) const {
+    value_.AddMember(name, value, allocator);
+    return *this;
+  }
+#endif  // RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  GenericObject AddMember(StringRefType name, ValueType &value,
+                          AllocatorType &allocator) const {
+    value_.AddMember(name, value, allocator);
+    return *this;
+  }
+  GenericObject AddMember(StringRefType name, StringRefType value,
+                          AllocatorType &allocator) const {
+    value_.AddMember(name, value, allocator);
+    return *this;
+  }
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::OrExpr<internal::IsPointer<T>, internal::IsGenericValue<T>>),
+      (GenericObject))
+  AddMember(StringRefType name, T value, AllocatorType &allocator) const {
+    value_.AddMember(name, value, allocator);
+    return *this;
+  }
+  void RemoveAllMembers() { value_.RemoveAllMembers(); }
+  bool RemoveMember(const Ch *name) const { return value_.RemoveMember(name); }
+#if RAPIDJSON_HAS_STDSTRING
+  bool RemoveMember(const std::basic_string<Ch> &name) const {
+    return value_.RemoveMember(name);
+  }
+#endif
+  template <typename SourceAllocator>
+  bool RemoveMember(
+      const GenericValue<EncodingType, SourceAllocator> &name) const {
+    return value_.RemoveMember(name);
+  }
+  MemberIterator RemoveMember(MemberIterator m) const {
+    return value_.RemoveMember(m);
+  }
+  MemberIterator EraseMember(ConstMemberIterator pos) const {
+    return value_.EraseMember(pos);
+  }
+  MemberIterator EraseMember(ConstMemberIterator first,
+                             ConstMemberIterator last) const {
+    return value_.EraseMember(first, last);
+  }
+  bool EraseMember(const Ch *name) const { return value_.EraseMember(name); }
+#if RAPIDJSON_HAS_STDSTRING
+  bool EraseMember(const std::basic_string<Ch> &name) const {
+    return EraseMember(ValueType(StringRef(name)));
+  }
+#endif
+  template <typename SourceAllocator>
+  bool EraseMember(
+      const GenericValue<EncodingType, SourceAllocator> &name) const {
+    return value_.EraseMember(name);
+  }
+
+#if RAPIDJSON_HAS_CXX11_RANGE_FOR
+  MemberIterator begin() const { return value_.MemberBegin(); }
+  MemberIterator end() const { return value_.MemberEnd(); }
+#endif
+
+ private:
+  GenericObject();
+  GenericObject(ValueType &value) : value_(value) {}
+  ValueType &value_;
+};
+
+RAPIDJSON_NAMESPACE_END
+RAPIDJSON_DIAG_POP
+
+#endif  // RAPIDJSON_DOCUMENT_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/encodedstream.h b/livox_ros_driver2/3rdparty/rapidjson/encodedstream.h
new file mode 100644
index 0000000..74ef4ca
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/encodedstream.h
@@ -0,0 +1,407 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_ENCODEDSTREAM_H_
+#define RAPIDJSON_ENCODEDSTREAM_H_
+
+#include "memorystream.h"
+#include "stream.h"
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Input byte stream wrapper with a statically bound encoding.
+/*!
+    \tparam Encoding The interpretation of encoding of the stream. Either UTF8,
+   UTF16LE, UTF16BE, UTF32LE, UTF32BE. \tparam InputByteStream Type of input
+   byte stream. For example, FileReadStream.
+*/
+template <typename Encoding, typename InputByteStream>
+class EncodedInputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  EncodedInputStream(InputByteStream &is) : is_(is) {
+    current_ = Encoding::TakeBOM(is_);
+  }
+
+  Ch Peek() const { return current_; }
+  Ch Take() {
+    Ch c = current_;
+    current_ = Encoding::Take(is_);
+    return c;
+  }
+  size_t Tell() const { return is_.Tell(); }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  EncodedInputStream(const EncodedInputStream &);
+  EncodedInputStream &operator=(const EncodedInputStream &);
+
+  InputByteStream &is_;
+  Ch current_;
+};
+
+//! Specialized for UTF8 MemoryStream.
+template <>
+class EncodedInputStream<UTF8<>, MemoryStream> {
+ public:
+  typedef UTF8<>::Ch Ch;
+
+  EncodedInputStream(MemoryStream &is) : is_(is) {
+    if (static_cast<unsigned char>(is_.Peek()) == 0xEFu) is_.Take();
+    if (static_cast<unsigned char>(is_.Peek()) == 0xBBu) is_.Take();
+    if (static_cast<unsigned char>(is_.Peek()) == 0xBFu) is_.Take();
+  }
+  Ch Peek() const { return is_.Peek(); }
+  Ch Take() { return is_.Take(); }
+  size_t Tell() const { return is_.Tell(); }
+
+  // Not implemented
+  void Put(Ch) {}
+  void Flush() {}
+  Ch *PutBegin() { return 0; }
+  size_t PutEnd(Ch *) { return 0; }
+
+  MemoryStream &is_;
+
+ private:
+  EncodedInputStream(const EncodedInputStream &);
+  EncodedInputStream &operator=(const EncodedInputStream &);
+};
+
+//! Output byte stream wrapper with statically bound encoding.
+/*!
+    \tparam Encoding The interpretation of encoding of the stream. Either UTF8,
+   UTF16LE, UTF16BE, UTF32LE, UTF32BE. \tparam OutputByteStream Type of input
+   byte stream. For example, FileWriteStream.
+*/
+template <typename Encoding, typename OutputByteStream>
+class EncodedOutputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  EncodedOutputStream(OutputByteStream &os, bool putBOM = true) : os_(os) {
+    if (putBOM) Encoding::PutBOM(os_);
+  }
+
+  void Put(Ch c) { Encoding::Put(os_, c); }
+  void Flush() { os_.Flush(); }
+
+  // Not implemented
+  Ch Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  EncodedOutputStream(const EncodedOutputStream &);
+  EncodedOutputStream &operator=(const EncodedOutputStream &);
+
+  OutputByteStream &os_;
+};
+
+#define RAPIDJSON_ENCODINGS_FUNC(x) \
+  UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
+
+//! Input stream wrapper with dynamically bound encoding and automatic encoding
+//! detection.
+/*!
+    \tparam CharType Type of character for reading.
+    \tparam InputByteStream type of input byte stream to be wrapped.
+*/
+template <typename CharType, typename InputByteStream>
+class AutoUTFInputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+
+ public:
+  typedef CharType Ch;
+
+  //! Constructor.
+  /*!
+      \param is input stream to be wrapped.
+      \param type UTF encoding type if it is not detected from the stream.
+  */
+  AutoUTFInputStream(InputByteStream &is, UTFType type = kUTF8)
+      : is_(&is), type_(type), hasBOM_(false) {
+    RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
+    DetectType();
+    static const TakeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Take)};
+    takeFunc_ = f[type_];
+    current_ = takeFunc_(*is_);
+  }
+
+  UTFType GetType() const { return type_; }
+  bool HasBOM() const { return hasBOM_; }
+
+  Ch Peek() const { return current_; }
+  Ch Take() {
+    Ch c = current_;
+    current_ = takeFunc_(*is_);
+    return c;
+  }
+  size_t Tell() const { return is_->Tell(); }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  AutoUTFInputStream(const AutoUTFInputStream &);
+  AutoUTFInputStream &operator=(const AutoUTFInputStream &);
+
+  // Detect encoding type with BOM or RFC 4627
+  void DetectType() {
+    // BOM (Byte Order Mark):
+    // 00 00 FE FF  UTF-32BE
+    // FF FE 00 00  UTF-32LE
+    // FE FF        UTF-16BE
+    // FF FE        UTF-16LE
+    // EF BB BF     UTF-8
+
+    const unsigned char *c =
+        reinterpret_cast<const unsigned char *>(is_->Peek4());
+    if (!c) return;
+
+    unsigned bom =
+        static_cast<unsigned>(c[0] | (c[1] << 8) | (c[2] << 16) | (c[3] << 24));
+    hasBOM_ = false;
+    if (bom == 0xFFFE0000) {
+      type_ = kUTF32BE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+      is_->Take();
+      is_->Take();
+    } else if (bom == 0x0000FEFF) {
+      type_ = kUTF32LE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+      is_->Take();
+      is_->Take();
+    } else if ((bom & 0xFFFF) == 0xFFFE) {
+      type_ = kUTF16BE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+    } else if ((bom & 0xFFFF) == 0xFEFF) {
+      type_ = kUTF16LE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+    } else if ((bom & 0xFFFFFF) == 0xBFBBEF) {
+      type_ = kUTF8;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+      is_->Take();
+    }
+
+    // RFC 4627: Section 3
+    // "Since the first two characters of a JSON text will always be ASCII
+    // characters [RFC0020], it is possible to determine whether an octet
+    // stream is UTF-8, UTF-16 (BE or LE), or UTF-32 (BE or LE) by looking
+    // at the pattern of nulls in the first four octets."
+    // 00 00 00 xx  UTF-32BE
+    // 00 xx 00 xx  UTF-16BE
+    // xx 00 00 00  UTF-32LE
+    // xx 00 xx 00  UTF-16LE
+    // xx xx xx xx  UTF-8
+
+    if (!hasBOM_) {
+      int pattern =
+          (c[0] ? 1 : 0) | (c[1] ? 2 : 0) | (c[2] ? 4 : 0) | (c[3] ? 8 : 0);
+      switch (pattern) {
+        case 0x08:
+          type_ = kUTF32BE;
+          break;
+        case 0x0A:
+          type_ = kUTF16BE;
+          break;
+        case 0x01:
+          type_ = kUTF32LE;
+          break;
+        case 0x05:
+          type_ = kUTF16LE;
+          break;
+        case 0x0F:
+          type_ = kUTF8;
+          break;
+        default:
+          break;  // Use type defined by user.
+      }
+    }
+
+    // Runtime check whether the size of character type is sufficient. It only
+    // perform checks with assertion.
+    if (type_ == kUTF16LE || type_ == kUTF16BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
+    if (type_ == kUTF32LE || type_ == kUTF32BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
+  }
+
+  typedef Ch (*TakeFunc)(InputByteStream &is);
+  InputByteStream *is_;
+  UTFType type_;
+  Ch current_;
+  TakeFunc takeFunc_;
+  bool hasBOM_;
+};
+
+//! Output stream wrapper with dynamically bound encoding and automatic encoding
+//! detection.
+/*!
+    \tparam CharType Type of character for writing.
+    \tparam OutputByteStream type of output byte stream to be wrapped.
+*/
+template <typename CharType, typename OutputByteStream>
+class AutoUTFOutputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+
+ public:
+  typedef CharType Ch;
+
+  //! Constructor.
+  /*!
+      \param os output stream to be wrapped.
+      \param type UTF encoding type.
+      \param putBOM Whether to write BOM at the beginning of the stream.
+  */
+  AutoUTFOutputStream(OutputByteStream &os, UTFType type, bool putBOM)
+      : os_(&os), type_(type) {
+    RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
+
+    // Runtime check whether the size of character type is sufficient. It only
+    // perform checks with assertion.
+    if (type_ == kUTF16LE || type_ == kUTF16BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
+    if (type_ == kUTF32LE || type_ == kUTF32BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
+
+    static const PutFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Put)};
+    putFunc_ = f[type_];
+
+    if (putBOM) PutBOM();
+  }
+
+  UTFType GetType() const { return type_; }
+
+  void Put(Ch c) { putFunc_(*os_, c); }
+  void Flush() { os_->Flush(); }
+
+  // Not implemented
+  Ch Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  AutoUTFOutputStream(const AutoUTFOutputStream &);
+  AutoUTFOutputStream &operator=(const AutoUTFOutputStream &);
+
+  void PutBOM() {
+    typedef void (*PutBOMFunc)(OutputByteStream &);
+    static const PutBOMFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(PutBOM)};
+    f[type_](*os_);
+  }
+
+  typedef void (*PutFunc)(OutputByteStream &, Ch);
+
+  OutputByteStream *os_;
+  UTFType type_;
+  PutFunc putFunc_;
+};
+
+#undef RAPIDJSON_ENCODINGS_FUNC
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_FILESTREAM_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/encodings.h b/livox_ros_driver2/3rdparty/rapidjson/encodings.h
new file mode 100644
index 0000000..34521cc
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/encodings.h
@@ -0,0 +1,816 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_ENCODINGS_H_
+#define RAPIDJSON_ENCODINGS_H_
+
+#include "rapidjson.h"
+
+#if defined(_MSC_VER) && !defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(
+    4244)  // conversion from 'type1' to 'type2', possible loss of data
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
+#elif defined(__GNUC__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+RAPIDJSON_DIAG_OFF(overflow)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// Encoding
+
+/*! \class rapidjson::Encoding
+    \brief Concept for encoding of Unicode characters.
+
+\code
+concept Encoding {
+    typename Ch;    //! Type of character. A "character" is actually a code unit
+in unicode's definition.
+
+    enum { supportUnicode = 1 }; // or 0 if not supporting unicode
+
+    //! \brief Encode a Unicode codepoint to an output stream.
+    //! \param os Output stream.
+    //! \param codepoint An unicode codepoint, ranging from 0x0 to 0x10FFFF
+inclusively. template<typename OutputStream> static void Encode(OutputStream&
+os, unsigned codepoint);
+
+    //! \brief Decode a Unicode codepoint from an input stream.
+    //! \param is Input stream.
+    //! \param codepoint Output of the unicode codepoint.
+    //! \return true if a valid codepoint can be decoded from the stream.
+    template <typename InputStream>
+    static bool Decode(InputStream& is, unsigned* codepoint);
+
+    //! \brief Validate one Unicode codepoint from an encoded stream.
+    //! \param is Input stream to obtain codepoint.
+    //! \param os Output for copying one codepoint.
+    //! \return true if it is valid.
+    //! \note This function just validating and copying the codepoint without
+actually decode it. template <typename InputStream, typename OutputStream>
+    static bool Validate(InputStream& is, OutputStream& os);
+
+    // The following functions are deal with byte streams.
+
+    //! Take a character from input byte stream, skip BOM if exist.
+    template <typename InputByteStream>
+    static CharType TakeBOM(InputByteStream& is);
+
+    //! Take a character from input byte stream.
+    template <typename InputByteStream>
+    static Ch Take(InputByteStream& is);
+
+    //! Put BOM to output byte stream.
+    template <typename OutputByteStream>
+    static void PutBOM(OutputByteStream& os);
+
+    //! Put a character to output byte stream.
+    template <typename OutputByteStream>
+    static void Put(OutputByteStream& os, Ch c);
+};
+\endcode
+*/
+
+///////////////////////////////////////////////////////////////////////////////
+// UTF8
+
+//! UTF-8 encoding.
+/*! http://en.wikipedia.org/wiki/UTF-8
+    http://tools.ietf.org/html/rfc3629
+    \tparam CharType Code unit for storing 8-bit UTF-8 data. Default is char.
+    \note implements Encoding concept
+*/
+template <typename CharType = char>
+struct UTF8 {
+  typedef CharType Ch;
+
+  enum { supportUnicode = 1 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    if (codepoint <= 0x7F)
+      os.Put(static_cast<Ch>(codepoint & 0xFF));
+    else if (codepoint <= 0x7FF) {
+      os.Put(static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
+    } else if (codepoint <= 0xFFFF) {
+      os.Put(static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      os.Put(static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    }
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    if (codepoint <= 0x7F)
+      PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
+    else if (codepoint <= 0x7FF) {
+      PutUnsafe(os, static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
+    } else if (codepoint <= 0xFFFF) {
+      PutUnsafe(os, static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      PutUnsafe(os, static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    }
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+#define RAPIDJSON_COPY() \
+  c = is.Take();         \
+  *codepoint = (*codepoint << 6) | (static_cast<unsigned char>(c) & 0x3Fu)
+#define RAPIDJSON_TRANS(mask) \
+  result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
+#define RAPIDJSON_TAIL() \
+  RAPIDJSON_COPY();      \
+  RAPIDJSON_TRANS(0x70)
+    typename InputStream::Ch c = is.Take();
+    if (!(c & 0x80)) {
+      *codepoint = static_cast<unsigned char>(c);
+      return true;
+    }
+
+    unsigned char type = GetRange(static_cast<unsigned char>(c));
+    if (type >= 32) {
+      *codepoint = 0;
+    } else {
+      *codepoint = (0xFFu >> type) & static_cast<unsigned char>(c);
+    }
+    bool result = true;
+    switch (type) {
+      case 2:
+        RAPIDJSON_TAIL();
+        return result;
+      case 3:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 4:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x50);
+        RAPIDJSON_TAIL();
+        return result;
+      case 5:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x10);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 6:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 10:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x20);
+        RAPIDJSON_TAIL();
+        return result;
+      case 11:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x60);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      default:
+        return false;
+    }
+#undef RAPIDJSON_COPY
+#undef RAPIDJSON_TRANS
+#undef RAPIDJSON_TAIL
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+#define RAPIDJSON_COPY() os.Put(c = is.Take())
+#define RAPIDJSON_TRANS(mask) \
+  result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
+#define RAPIDJSON_TAIL() \
+  RAPIDJSON_COPY();      \
+  RAPIDJSON_TRANS(0x70)
+    Ch c;
+    RAPIDJSON_COPY();
+    if (!(c & 0x80)) return true;
+
+    bool result = true;
+    switch (GetRange(static_cast<unsigned char>(c))) {
+      case 2:
+        RAPIDJSON_TAIL();
+        return result;
+      case 3:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 4:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x50);
+        RAPIDJSON_TAIL();
+        return result;
+      case 5:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x10);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 6:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 10:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x20);
+        RAPIDJSON_TAIL();
+        return result;
+      case 11:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x60);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      default:
+        return false;
+    }
+#undef RAPIDJSON_COPY
+#undef RAPIDJSON_TRANS
+#undef RAPIDJSON_TAIL
+  }
+
+  static unsigned char GetRange(unsigned char c) {
+    // Referring to DFA of http://bjoern.hoehrmann.de/utf-8/decoder/dfa/
+    // With new mapping 1 -> 0x10, 7 -> 0x20, 9 -> 0x40, such that AND operation
+    // can test multiple types.
+    static const unsigned char type[] = {
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0x10, 0x10, 0x10, 0x10,
+        0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10,
+        0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40,
+        0x40, 0x40, 0x40, 0x40, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+        0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+        0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+        8,    8,    2,    2,    2,    2,    2,    2,    2,    2,    2,    2,
+        2,    2,    2,    2,    2,    2,    2,    2,    2,    2,    2,    2,
+        2,    2,    2,    2,    2,    2,    2,    2,    10,   3,    3,    3,
+        3,    3,    3,    3,    3,    3,    3,    3,    3,    4,    3,    3,
+        11,   6,    6,    6,    5,    8,    8,    8,    8,    8,    8,    8,
+        8,    8,    8,    8,
+    };
+    return type[c];
+  }
+
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    typename InputByteStream::Ch c = Take(is);
+    if (static_cast<unsigned char>(c) != 0xEFu) return c;
+    c = is.Take();
+    if (static_cast<unsigned char>(c) != 0xBBu) return c;
+    c = is.Take();
+    if (static_cast<unsigned char>(c) != 0xBFu) return c;
+    c = is.Take();
+    return c;
+  }
+
+  template <typename InputByteStream>
+  static Ch Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    return static_cast<Ch>(is.Take());
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xEFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xBBu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xBFu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, Ch c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(c));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// UTF16
+
+//! UTF-16 encoding.
+/*! http://en.wikipedia.org/wiki/UTF-16
+    http://tools.ietf.org/html/rfc2781
+    \tparam CharType Type for storing 16-bit UTF-16 data. Default is wchar_t.
+   C++11 may use char16_t instead. \note implements Encoding concept
+
+    \note For in-memory access, no need to concern endianness. The code units
+   and code points are represented by CPU's endianness. For streaming, use
+   UTF16LE and UTF16BE, which handle endianness.
+*/
+template <typename CharType = wchar_t>
+struct UTF16 {
+  typedef CharType Ch;
+  RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 2);
+
+  enum { supportUnicode = 1 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
+    if (codepoint <= 0xFFFF) {
+      RAPIDJSON_ASSERT(
+          codepoint < 0xD800 ||
+          codepoint > 0xDFFF);  // Code point itself cannot be surrogate pair
+      os.Put(static_cast<typename OutputStream::Ch>(codepoint));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      unsigned v = codepoint - 0x10000;
+      os.Put(static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
+      os.Put(static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
+    }
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
+    if (codepoint <= 0xFFFF) {
+      RAPIDJSON_ASSERT(
+          codepoint < 0xD800 ||
+          codepoint > 0xDFFF);  // Code point itself cannot be surrogate pair
+      PutUnsafe(os, static_cast<typename OutputStream::Ch>(codepoint));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      unsigned v = codepoint - 0x10000;
+      PutUnsafe(os, static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
+      PutUnsafe(os,
+                static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
+    }
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
+    typename InputStream::Ch c = is.Take();
+    if (c < 0xD800 || c > 0xDFFF) {
+      *codepoint = static_cast<unsigned>(c);
+      return true;
+    } else if (c <= 0xDBFF) {
+      *codepoint = (static_cast<unsigned>(c) & 0x3FF) << 10;
+      c = is.Take();
+      *codepoint |= (static_cast<unsigned>(c) & 0x3FF);
+      *codepoint += 0x10000;
+      return c >= 0xDC00 && c <= 0xDFFF;
+    }
+    return false;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
+    typename InputStream::Ch c;
+    os.Put(static_cast<typename OutputStream::Ch>(c = is.Take()));
+    if (c < 0xD800 || c > 0xDFFF)
+      return true;
+    else if (c <= 0xDBFF) {
+      os.Put(c = is.Take());
+      return c >= 0xDC00 && c <= 0xDFFF;
+    }
+    return false;
+  }
+};
+
+//! UTF-16 little endian encoding.
+template <typename CharType = wchar_t>
+struct UTF16LE : UTF16<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<uint8_t>(is.Take());
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
+                                                      0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(
+        (static_cast<unsigned>(c) >> 8) & 0xFFu));
+  }
+};
+
+//! UTF-16 big endian encoding.
+template <typename CharType = wchar_t>
+struct UTF16BE : UTF16<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take()));
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(
+        (static_cast<unsigned>(c) >> 8) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
+                                                      0xFFu));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// UTF32
+
+//! UTF-32 encoding.
+/*! http://en.wikipedia.org/wiki/UTF-32
+    \tparam CharType Type for storing 32-bit UTF-32 data. Default is unsigned.
+   C++11 may use char32_t instead. \note implements Encoding concept
+
+    \note For in-memory access, no need to concern endianness. The code units
+   and code points are represented by CPU's endianness. For streaming, use
+   UTF32LE and UTF32BE, which handle endianness.
+*/
+template <typename CharType = unsigned>
+struct UTF32 {
+  typedef CharType Ch;
+  RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 4);
+
+  enum { supportUnicode = 1 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
+    RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+    os.Put(codepoint);
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
+    RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+    PutUnsafe(os, codepoint);
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
+    Ch c = is.Take();
+    *codepoint = c;
+    return c <= 0x10FFFF;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
+    Ch c;
+    os.Put(c = is.Take());
+    return c <= 0x10FFFF;
+  }
+};
+
+//! UTF-32 little endian enocoding.
+template <typename CharType = unsigned>
+struct UTF32LE : UTF32<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<uint8_t>(is.Take());
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24;
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu));
+  }
+};
+
+//! UTF-32 big endian encoding.
+template <typename CharType = unsigned>
+struct UTF32BE : UTF32<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take()));
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// ASCII
+
+//! ASCII encoding.
+/*! http://en.wikipedia.org/wiki/ASCII
+    \tparam CharType Code unit for storing 7-bit ASCII data. Default is char.
+    \note implements Encoding concept
+*/
+template <typename CharType = char>
+struct ASCII {
+  typedef CharType Ch;
+
+  enum { supportUnicode = 0 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_ASSERT(codepoint <= 0x7F);
+    os.Put(static_cast<Ch>(codepoint & 0xFF));
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_ASSERT(codepoint <= 0x7F);
+    PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+    uint8_t c = static_cast<uint8_t>(is.Take());
+    *codepoint = c;
+    return c <= 0X7F;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+    uint8_t c = static_cast<uint8_t>(is.Take());
+    os.Put(static_cast<typename OutputStream::Ch>(c));
+    return c <= 0x7F;
+  }
+
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    uint8_t c = static_cast<uint8_t>(Take(is));
+    return static_cast<Ch>(c);
+  }
+
+  template <typename InputByteStream>
+  static Ch Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    return static_cast<Ch>(is.Take());
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    (void)os;
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, Ch c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(c));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// AutoUTF
+
+//! Runtime-specified UTF encoding type of a stream.
+enum UTFType {
+  kUTF8 = 0,     //!< UTF-8.
+  kUTF16LE = 1,  //!< UTF-16 little endian.
+  kUTF16BE = 2,  //!< UTF-16 big endian.
+  kUTF32LE = 3,  //!< UTF-32 little endian.
+  kUTF32BE = 4   //!< UTF-32 big endian.
+};
+
+//! Dynamically select encoding according to stream's runtime-specified UTF
+//! encoding type.
+/*! \note This class can be used with AutoUTFInputtStream and
+ * AutoUTFOutputStream, which provides GetType().
+ */
+template <typename CharType>
+struct AutoUTF {
+  typedef CharType Ch;
+
+  enum { supportUnicode = 1 };
+
+#define RAPIDJSON_ENCODINGS_FUNC(x) \
+  UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
+
+  template <typename OutputStream>
+  static RAPIDJSON_FORCEINLINE void Encode(OutputStream &os,
+                                           unsigned codepoint) {
+    typedef void (*EncodeFunc)(OutputStream &, unsigned);
+    static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Encode)};
+    (*f[os.GetType()])(os, codepoint);
+  }
+
+  template <typename OutputStream>
+  static RAPIDJSON_FORCEINLINE void EncodeUnsafe(OutputStream &os,
+                                                 unsigned codepoint) {
+    typedef void (*EncodeFunc)(OutputStream &, unsigned);
+    static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(EncodeUnsafe)};
+    (*f[os.GetType()])(os, codepoint);
+  }
+
+  template <typename InputStream>
+  static RAPIDJSON_FORCEINLINE bool Decode(InputStream &is,
+                                           unsigned *codepoint) {
+    typedef bool (*DecodeFunc)(InputStream &, unsigned *);
+    static const DecodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Decode)};
+    return (*f[is.GetType()])(is, codepoint);
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
+                                             OutputStream &os) {
+    typedef bool (*ValidateFunc)(InputStream &, OutputStream &);
+    static const ValidateFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Validate)};
+    return (*f[is.GetType()])(is, os);
+  }
+
+#undef RAPIDJSON_ENCODINGS_FUNC
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Transcoder
+
+//! Encoding conversion.
+template <typename SourceEncoding, typename TargetEncoding>
+struct Transcoder {
+  //! Take one Unicode codepoint from source encoding, convert it to target
+  //! encoding and put it to the output stream.
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
+                                              OutputStream &os) {
+    unsigned codepoint;
+    if (!SourceEncoding::Decode(is, &codepoint)) return false;
+    TargetEncoding::Encode(os, codepoint);
+    return true;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
+                                                    OutputStream &os) {
+    unsigned codepoint;
+    if (!SourceEncoding::Decode(is, &codepoint)) return false;
+    TargetEncoding::EncodeUnsafe(os, codepoint);
+    return true;
+  }
+
+  //! Validate one Unicode codepoint from an encoded stream.
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
+                                             OutputStream &os) {
+    return Transcode(
+        is, os);  // Since source/target encoding is different, must transcode.
+  }
+};
+
+// Forward declaration.
+template <typename Stream>
+inline void PutUnsafe(Stream &stream, typename Stream::Ch c);
+
+//! Specialization of Transcoder with same source and target encoding.
+template <typename Encoding>
+struct Transcoder<Encoding, Encoding> {
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
+                                              OutputStream &os) {
+    os.Put(is.Take());  // Just copy one code unit. This semantic is different
+                        // from primary template class.
+    return true;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
+                                                    OutputStream &os) {
+    PutUnsafe(os, is.Take());  // Just copy one code unit. This semantic is
+                               // different from primary template class.
+    return true;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
+                                             OutputStream &os) {
+    return Encoding::Validate(is, os);  // source/target encoding are the same
+  }
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__GNUC__) || (defined(_MSC_VER) && !defined(__clang__))
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_ENCODINGS_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/error/en.h b/livox_ros_driver2/3rdparty/rapidjson/error/en.h
new file mode 100644
index 0000000..a08145d
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/error/en.h
@@ -0,0 +1,104 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_ERROR_EN_H_
+#define RAPIDJSON_ERROR_EN_H_
+
+#include "error.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(switch - enum)
+RAPIDJSON_DIAG_OFF(covered - switch - default)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Maps error code of parsing into error message.
+/*!
+    \ingroup RAPIDJSON_ERRORS
+    \param parseErrorCode Error code obtained in parsing.
+    \return the error message.
+    \note User can make a copy of this function for localization.
+        Using switch-case is safer for future modification of error codes.
+*/
+inline const RAPIDJSON_ERROR_CHARTYPE* GetParseError_En(
+    ParseErrorCode parseErrorCode) {
+  switch (parseErrorCode) {
+    case kParseErrorNone:
+      return RAPIDJSON_ERROR_STRING("No error.");
+
+    case kParseErrorDocumentEmpty:
+      return RAPIDJSON_ERROR_STRING("The document is empty.");
+    case kParseErrorDocumentRootNotSingular:
+      return RAPIDJSON_ERROR_STRING(
+          "The document root must not be followed by other values.");
+
+    case kParseErrorValueInvalid:
+      return RAPIDJSON_ERROR_STRING("Invalid value.");
+
+    case kParseErrorObjectMissName:
+      return RAPIDJSON_ERROR_STRING("Missing a name for object member.");
+    case kParseErrorObjectMissColon:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a colon after a name of object member.");
+    case kParseErrorObjectMissCommaOrCurlyBracket:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a comma or '}' after an object member.");
+
+    case kParseErrorArrayMissCommaOrSquareBracket:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a comma or ']' after an array element.");
+
+    case kParseErrorStringUnicodeEscapeInvalidHex:
+      return RAPIDJSON_ERROR_STRING(
+          "Incorrect hex digit after \\u escape in string.");
+    case kParseErrorStringUnicodeSurrogateInvalid:
+      return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid.");
+    case kParseErrorStringEscapeInvalid:
+      return RAPIDJSON_ERROR_STRING("Invalid escape character in string.");
+    case kParseErrorStringMissQuotationMark:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a closing quotation mark in string.");
+    case kParseErrorStringInvalidEncoding:
+      return RAPIDJSON_ERROR_STRING("Invalid encoding in string.");
+
+    case kParseErrorNumberTooBig:
+      return RAPIDJSON_ERROR_STRING("Number too big to be stored in double.");
+    case kParseErrorNumberMissFraction:
+      return RAPIDJSON_ERROR_STRING("Miss fraction part in number.");
+    case kParseErrorNumberMissExponent:
+      return RAPIDJSON_ERROR_STRING("Miss exponent in number.");
+
+    case kParseErrorTermination:
+      return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error.");
+    case kParseErrorUnspecificSyntaxError:
+      return RAPIDJSON_ERROR_STRING("Unspecific syntax error.");
+
+    default:
+      return RAPIDJSON_ERROR_STRING("Unknown error.");
+  }
+}
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_ERROR_EN_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/error/error.h b/livox_ros_driver2/3rdparty/rapidjson/error/error.h
new file mode 100644
index 0000000..4814b69
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/error/error.h
@@ -0,0 +1,186 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_ERROR_ERROR_H_
+#define RAPIDJSON_ERROR_ERROR_H_
+
+#include "../rapidjson.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+/*! \file error.h */
+
+/*! \defgroup RAPIDJSON_ERRORS RapidJSON error handling */
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ERROR_CHARTYPE
+
+//! Character type of error messages.
+/*! \ingroup RAPIDJSON_ERRORS
+    The default character type is \c char.
+    On Windows, user can define this macro as \c TCHAR for supporting both
+    unicode/non-unicode settings.
+*/
+#ifndef RAPIDJSON_ERROR_CHARTYPE
+#define RAPIDJSON_ERROR_CHARTYPE char
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ERROR_STRING
+
+//! Macro for converting string literial to \ref RAPIDJSON_ERROR_CHARTYPE[].
+/*! \ingroup RAPIDJSON_ERRORS
+    By default this conversion macro does nothing.
+    On Windows, user can define this macro as \c _T(x) for supporting both
+    unicode/non-unicode settings.
+*/
+#ifndef RAPIDJSON_ERROR_STRING
+#define RAPIDJSON_ERROR_STRING(x) x
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// ParseErrorCode
+
+//! Error code of parsing.
+/*! \ingroup RAPIDJSON_ERRORS
+    \see GenericReader::Parse, GenericReader::GetParseErrorCode
+*/
+enum ParseErrorCode {
+  kParseErrorNone = 0,  //!< No error.
+
+  kParseErrorDocumentEmpty,            //!< The document is empty.
+  kParseErrorDocumentRootNotSingular,  //!< The document root must not follow by
+                                       //!< other values.
+
+  kParseErrorValueInvalid,  //!< Invalid value.
+
+  kParseErrorObjectMissName,   //!< Missing a name for object member.
+  kParseErrorObjectMissColon,  //!< Missing a colon after a name of object
+                               //!< member.
+  kParseErrorObjectMissCommaOrCurlyBracket,  //!< Missing a comma or '}' after
+                                             //!an
+                                             //!< object member.
+
+  kParseErrorArrayMissCommaOrSquareBracket,  //!< Missing a comma or ']' after
+                                             //!an
+                                             //!< array element.
+
+  kParseErrorStringUnicodeEscapeInvalidHex,  //!< Incorrect hex digit after \\u
+                                             //!< escape in string.
+  kParseErrorStringUnicodeSurrogateInvalid,  //!< The surrogate pair in string
+                                             //!is
+                                             //!< invalid.
+  kParseErrorStringEscapeInvalid,      //!< Invalid escape character in string.
+  kParseErrorStringMissQuotationMark,  //!< Missing a closing quotation mark in
+                                       //!< string.
+  kParseErrorStringInvalidEncoding,    //!< Invalid encoding in string.
+
+  kParseErrorNumberTooBig,        //!< Number too big to be stored in double.
+  kParseErrorNumberMissFraction,  //!< Miss fraction part in number.
+  kParseErrorNumberMissExponent,  //!< Miss exponent in number.
+
+  kParseErrorTermination,           //!< Parsing was terminated.
+  kParseErrorUnspecificSyntaxError  //!< Unspecific syntax error.
+};
+
+//! Result of parsing (wraps ParseErrorCode)
+/*!
+    \ingroup RAPIDJSON_ERRORS
+    \code
+        Document doc;
+        ParseResult ok = doc.Parse("[42]");
+        if (!ok) {
+            fprintf(stderr, "JSON parse error: %s (%u)",
+                    GetParseError_En(ok.Code()), ok.Offset());
+            exit(EXIT_FAILURE);
+        }
+    \endcode
+    \see GenericReader::Parse, GenericDocument::Parse
+*/
+struct ParseResult {
+  //!! Unspecified boolean type
+  typedef bool (ParseResult::*BooleanType)() const;
+
+ public:
+  //! Default constructor, no error.
+  ParseResult() : code_(kParseErrorNone), offset_(0) {}
+  //! Constructor to set an error.
+  ParseResult(ParseErrorCode code, size_t offset)
+      : code_(code), offset_(offset) {}
+
+  //! Get the error code.
+  ParseErrorCode Code() const { return code_; }
+  //! Get the error offset, if \ref IsError(), 0 otherwise.
+  size_t Offset() const { return offset_; }
+
+  //! Explicit conversion to \c bool, returns \c true, iff !\ref IsError().
+  operator BooleanType() const {
+    return !IsError() ? &ParseResult::IsError : NULL;
+  }
+  //! Whether the result is an error.
+  bool IsError() const { return code_ != kParseErrorNone; }
+
+  bool operator==(const ParseResult &that) const { return code_ == that.code_; }
+  bool operator==(ParseErrorCode code) const { return code_ == code; }
+  friend bool operator==(ParseErrorCode code, const ParseResult &err) {
+    return code == err.code_;
+  }
+
+  bool operator!=(const ParseResult &that) const { return !(*this == that); }
+  bool operator!=(ParseErrorCode code) const { return !(*this == code); }
+  friend bool operator!=(ParseErrorCode code, const ParseResult &err) {
+    return err != code;
+  }
+
+  //! Reset error code.
+  void Clear() { Set(kParseErrorNone); }
+  //! Update error code and offset.
+  void Set(ParseErrorCode code, size_t offset = 0) {
+    code_ = code;
+    offset_ = offset;
+  }
+
+ private:
+  ParseErrorCode code_;
+  size_t offset_;
+};
+
+//! Function pointer type of GetParseError().
+/*! \ingroup RAPIDJSON_ERRORS
+
+    This is the prototype for \c GetParseError_X(), where \c X is a locale.
+    User can dynamically change locale in runtime, e.g.:
+\code
+    GetParseErrorFunc GetParseError = GetParseError_En; // or whatever
+    const RAPIDJSON_ERROR_CHARTYPE* s =
+GetParseError(document.GetParseErrorCode()); \endcode
+*/
+typedef const RAPIDJSON_ERROR_CHARTYPE *(*GetParseErrorFunc)(ParseErrorCode);
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_ERROR_ERROR_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/filereadstream.h b/livox_ros_driver2/3rdparty/rapidjson/filereadstream.h
new file mode 100644
index 0000000..250aabb
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/filereadstream.h
@@ -0,0 +1,123 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_FILEREADSTREAM_H_
+#define RAPIDJSON_FILEREADSTREAM_H_
+
+#include <cstdio>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+RAPIDJSON_DIAG_OFF(unreachable - code)
+RAPIDJSON_DIAG_OFF(missing - noreturn)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! File byte stream for input using fread().
+/*!
+    \note implements Stream concept
+*/
+class FileReadStream {
+ public:
+  typedef char Ch;  //!< Character type (byte).
+
+  //! Constructor.
+  /*!
+      \param fp File pointer opened for read.
+      \param buffer user-supplied buffer.
+      \param bufferSize size of buffer in bytes. Must >=4 bytes.
+  */
+  FileReadStream(std::FILE *fp, char *buffer, size_t bufferSize)
+      : fp_(fp),
+        buffer_(buffer),
+        bufferSize_(bufferSize),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
+    RAPIDJSON_ASSERT(fp_ != 0);
+    RAPIDJSON_ASSERT(bufferSize >= 4);
+    Read();
+  }
+
+  Ch Peek() const { return *current_; }
+  Ch Take() {
+    Ch c = *current_;
+    Read();
+    return c;
+  }
+  size_t Tell() const {
+    return count_ + static_cast<size_t>(current_ - buffer_);
+  }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+  // For encoding detection only.
+  const Ch *Peek4() const {
+    return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
+  }
+
+ private:
+  void Read() {
+    if (current_ < bufferLast_)
+      ++current_;
+    else if (!eof_) {
+      count_ += readCount_;
+      readCount_ = std::fread(buffer_, 1, bufferSize_, fp_);
+      bufferLast_ = buffer_ + readCount_ - 1;
+      current_ = buffer_;
+
+      if (readCount_ < bufferSize_) {
+        buffer_[readCount_] = '\0';
+        ++bufferLast_;
+        eof_ = true;
+      }
+    }
+  }
+
+  std::FILE *fp_;
+  Ch *buffer_;
+  size_t bufferSize_;
+  Ch *bufferLast_;
+  Ch *current_;
+  size_t readCount_;
+  size_t count_;  //!< Number of characters read
+  bool eof_;
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_FILESTREAM_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/filewritestream.h b/livox_ros_driver2/3rdparty/rapidjson/filewritestream.h
new file mode 100644
index 0000000..f6defc1
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/filewritestream.h
@@ -0,0 +1,128 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_FILEWRITESTREAM_H_
+#define RAPIDJSON_FILEWRITESTREAM_H_
+
+#include <cstdio>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(unreachable - code)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Wrapper of C file stream for output using fwrite().
+/*!
+    \note implements Stream concept
+*/
+class FileWriteStream {
+ public:
+  typedef char Ch;  //!< Character type. Only support char.
+
+  FileWriteStream(std::FILE *fp, char *buffer, size_t bufferSize)
+      : fp_(fp),
+        buffer_(buffer),
+        bufferEnd_(buffer + bufferSize),
+        current_(buffer_) {
+    RAPIDJSON_ASSERT(fp_ != 0);
+  }
+
+  void Put(char c) {
+    if (current_ >= bufferEnd_) Flush();
+
+    *current_++ = c;
+  }
+
+  void PutN(char c, size_t n) {
+    size_t avail = static_cast<size_t>(bufferEnd_ - current_);
+    while (n > avail) {
+      std::memset(current_, c, avail);
+      current_ += avail;
+      Flush();
+      n -= avail;
+      avail = static_cast<size_t>(bufferEnd_ - current_);
+    }
+
+    if (n > 0) {
+      std::memset(current_, c, n);
+      current_ += n;
+    }
+  }
+
+  void Flush() {
+    if (current_ != buffer_) {
+      size_t result =
+          std::fwrite(buffer_, 1, static_cast<size_t>(current_ - buffer_), fp_);
+      if (result < static_cast<size_t>(current_ - buffer_)) {
+        // failure deliberately ignored at this time
+        // added to avoid warn_unused_result build errors
+      }
+      current_ = buffer_;
+    }
+  }
+
+  // Not implemented
+  char Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(char *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  // Prohibit copy constructor & assignment operator.
+  FileWriteStream(const FileWriteStream &);
+  FileWriteStream &operator=(const FileWriteStream &);
+
+  std::FILE *fp_;
+  char *buffer_;
+  char *bufferEnd_;
+  char *current_;
+};
+
+//! Implement specialized version of PutN() with memset() for better
+//! performance.
+template <>
+inline void PutN(FileWriteStream &stream, char c, size_t n) {
+  stream.PutN(c, n);
+}
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_FILESTREAM_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/fwd.h b/livox_ros_driver2/3rdparty/rapidjson/fwd.h
new file mode 100644
index 0000000..cafb48e
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/fwd.h
@@ -0,0 +1,170 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_FWD_H_
+#define RAPIDJSON_FWD_H_
+
+#include "rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+// encodings.h
+
+template <typename CharType>
+struct UTF8;
+template <typename CharType>
+struct UTF16;
+template <typename CharType>
+struct UTF16BE;
+template <typename CharType>
+struct UTF16LE;
+template <typename CharType>
+struct UTF32;
+template <typename CharType>
+struct UTF32BE;
+template <typename CharType>
+struct UTF32LE;
+template <typename CharType>
+struct ASCII;
+template <typename CharType>
+struct AutoUTF;
+
+template <typename SourceEncoding, typename TargetEncoding>
+struct Transcoder;
+
+// allocators.h
+
+class CrtAllocator;
+
+template <typename BaseAllocator>
+class MemoryPoolAllocator;
+
+// stream.h
+
+template <typename Encoding>
+struct GenericStringStream;
+
+typedef GenericStringStream<UTF8<char>> StringStream;
+
+template <typename Encoding>
+struct GenericInsituStringStream;
+
+typedef GenericInsituStringStream<UTF8<char>> InsituStringStream;
+
+// stringbuffer.h
+
+template <typename Encoding, typename Allocator>
+class GenericStringBuffer;
+
+typedef GenericStringBuffer<UTF8<char>, CrtAllocator> StringBuffer;
+
+// filereadstream.h
+
+class FileReadStream;
+
+// filewritestream.h
+
+class FileWriteStream;
+
+// memorybuffer.h
+
+template <typename Allocator>
+struct GenericMemoryBuffer;
+
+typedef GenericMemoryBuffer<CrtAllocator> MemoryBuffer;
+
+// memorystream.h
+
+struct MemoryStream;
+
+// reader.h
+
+template <typename Encoding, typename Derived>
+struct BaseReaderHandler;
+
+template <typename SourceEncoding, typename TargetEncoding,
+          typename StackAllocator>
+class GenericReader;
+
+typedef GenericReader<UTF8<char>, UTF8<char>, CrtAllocator> Reader;
+
+// writer.h
+
+template <typename OutputStream, typename SourceEncoding,
+          typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
+class Writer;
+
+// prettywriter.h
+
+template <typename OutputStream, typename SourceEncoding,
+          typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
+class PrettyWriter;
+
+// document.h
+
+template <typename Encoding, typename Allocator>
+class GenericMember;
+
+template <bool Const, typename Encoding, typename Allocator>
+class GenericMemberIterator;
+
+template <typename CharType>
+struct GenericStringRef;
+
+template <typename Encoding, typename Allocator>
+class GenericValue;
+
+typedef GenericValue<UTF8<char>, MemoryPoolAllocator<CrtAllocator>> Value;
+
+template <typename Encoding, typename Allocator, typename StackAllocator>
+class GenericDocument;
+
+typedef GenericDocument<UTF8<char>, MemoryPoolAllocator<CrtAllocator>,
+                        CrtAllocator>
+    Document;
+
+// pointer.h
+
+template <typename ValueType, typename Allocator>
+class GenericPointer;
+
+typedef GenericPointer<Value, CrtAllocator> Pointer;
+
+// schema.h
+
+template <typename SchemaDocumentType>
+class IGenericRemoteSchemaDocumentProvider;
+
+template <typename ValueT, typename Allocator>
+class GenericSchemaDocument;
+
+typedef GenericSchemaDocument<Value, CrtAllocator> SchemaDocument;
+typedef IGenericRemoteSchemaDocumentProvider<SchemaDocument>
+    IRemoteSchemaDocumentProvider;
+
+template <typename SchemaDocumentType, typename OutputHandler,
+          typename StateAllocator>
+class GenericSchemaValidator;
+
+typedef GenericSchemaValidator<
+    SchemaDocument, BaseReaderHandler<UTF8<char>, void>, CrtAllocator>
+    SchemaValidator;
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_RAPIDJSONFWD_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/biginteger.h b/livox_ros_driver2/3rdparty/rapidjson/internal/biginteger.h
new file mode 100644
index 0000000..b60e006
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/biginteger.h
@@ -0,0 +1,295 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_BIGINTEGER_H_
+#define RAPIDJSON_BIGINTEGER_H_
+
+#include "../rapidjson.h"
+
+#if defined(_MSC_VER) && !__INTEL_COMPILER && defined(_M_AMD64)
+#include <intrin.h>  // for _umul128
+#pragma intrinsic(_umul128)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+class BigInteger {
+ public:
+  typedef uint64_t Type;
+
+  BigInteger(const BigInteger &rhs) : count_(rhs.count_) {
+    std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
+  }
+
+  explicit BigInteger(uint64_t u) : count_(1) { digits_[0] = u; }
+
+  BigInteger(const char *decimals, size_t length) : count_(1) {
+    RAPIDJSON_ASSERT(length > 0);
+    digits_[0] = 0;
+    size_t i = 0;
+    const size_t kMaxDigitPerIteration =
+        19;  // 2^64 = 18446744073709551616 > 10^19
+    while (length >= kMaxDigitPerIteration) {
+      AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
+      length -= kMaxDigitPerIteration;
+      i += kMaxDigitPerIteration;
+    }
+
+    if (length > 0) AppendDecimal64(decimals + i, decimals + i + length);
+  }
+
+  BigInteger &operator=(const BigInteger &rhs) {
+    if (this != &rhs) {
+      count_ = rhs.count_;
+      std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
+    }
+    return *this;
+  }
+
+  BigInteger &operator=(uint64_t u) {
+    digits_[0] = u;
+    count_ = 1;
+    return *this;
+  }
+
+  BigInteger &operator+=(uint64_t u) {
+    Type backup = digits_[0];
+    digits_[0] += u;
+    for (size_t i = 0; i < count_ - 1; i++) {
+      if (digits_[i] >= backup) return *this;  // no carry
+      backup = digits_[i + 1];
+      digits_[i + 1] += 1;
+    }
+
+    // Last carry
+    if (digits_[count_ - 1] < backup) PushBack(1);
+
+    return *this;
+  }
+
+  BigInteger &operator*=(uint64_t u) {
+    if (u == 0) return *this = 0;
+    if (u == 1) return *this;
+    if (*this == 1) return *this = u;
+
+    uint64_t k = 0;
+    for (size_t i = 0; i < count_; i++) {
+      uint64_t hi;
+      digits_[i] = MulAdd64(digits_[i], u, k, &hi);
+      k = hi;
+    }
+
+    if (k > 0) PushBack(k);
+
+    return *this;
+  }
+
+  BigInteger &operator*=(uint32_t u) {
+    if (u == 0) return *this = 0;
+    if (u == 1) return *this;
+    if (*this == 1) return *this = u;
+
+    uint64_t k = 0;
+    for (size_t i = 0; i < count_; i++) {
+      const uint64_t c = digits_[i] >> 32;
+      const uint64_t d = digits_[i] & 0xFFFFFFFF;
+      const uint64_t uc = u * c;
+      const uint64_t ud = u * d;
+      const uint64_t p0 = ud + k;
+      const uint64_t p1 = uc + (p0 >> 32);
+      digits_[i] = (p0 & 0xFFFFFFFF) | (p1 << 32);
+      k = p1 >> 32;
+    }
+
+    if (k > 0) PushBack(k);
+
+    return *this;
+  }
+
+  BigInteger &operator<<=(size_t shift) {
+    if (IsZero() || shift == 0) return *this;
+
+    size_t offset = shift / kTypeBit;
+    size_t interShift = shift % kTypeBit;
+    RAPIDJSON_ASSERT(count_ + offset <= kCapacity);
+
+    if (interShift == 0) {
+      std::memmove(digits_ + offset, digits_, count_ * sizeof(Type));
+      count_ += offset;
+    } else {
+      digits_[count_] = 0;
+      for (size_t i = count_; i > 0; i--)
+        digits_[i + offset] = (digits_[i] << interShift) |
+                              (digits_[i - 1] >> (kTypeBit - interShift));
+      digits_[offset] = digits_[0] << interShift;
+      count_ += offset;
+      if (digits_[count_]) count_++;
+    }
+
+    std::memset(digits_, 0, offset * sizeof(Type));
+
+    return *this;
+  }
+
+  bool operator==(const BigInteger &rhs) const {
+    return count_ == rhs.count_ &&
+           std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0;
+  }
+
+  bool operator==(const Type rhs) const {
+    return count_ == 1 && digits_[0] == rhs;
+  }
+
+  BigInteger &MultiplyPow5(unsigned exp) {
+    static const uint32_t kPow5[12] = {
+        5,
+        5 * 5,
+        5 * 5 * 5,
+        5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5};
+    if (exp == 0) return *this;
+    for (; exp >= 27; exp -= 27)
+      *this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D);  // 5^27
+    for (; exp >= 13; exp -= 13)
+      *this *= static_cast<uint32_t>(1220703125u);  // 5^13
+    if (exp > 0) *this *= kPow5[exp - 1];
+    return *this;
+  }
+
+  // Compute absolute difference of this and rhs.
+  // Assume this != rhs
+  bool Difference(const BigInteger &rhs, BigInteger *out) const {
+    int cmp = Compare(rhs);
+    RAPIDJSON_ASSERT(cmp != 0);
+    const BigInteger *a, *b;  // Makes a > b
+    bool ret;
+    if (cmp < 0) {
+      a = &rhs;
+      b = this;
+      ret = true;
+    } else {
+      a = this;
+      b = &rhs;
+      ret = false;
+    }
+
+    Type borrow = 0;
+    for (size_t i = 0; i < a->count_; i++) {
+      Type d = a->digits_[i] - borrow;
+      if (i < b->count_) d -= b->digits_[i];
+      borrow = (d > a->digits_[i]) ? 1 : 0;
+      out->digits_[i] = d;
+      if (d != 0) out->count_ = i + 1;
+    }
+
+    return ret;
+  }
+
+  int Compare(const BigInteger &rhs) const {
+    if (count_ != rhs.count_) return count_ < rhs.count_ ? -1 : 1;
+
+    for (size_t i = count_; i-- > 0;)
+      if (digits_[i] != rhs.digits_[i])
+        return digits_[i] < rhs.digits_[i] ? -1 : 1;
+
+    return 0;
+  }
+
+  size_t GetCount() const { return count_; }
+  Type GetDigit(size_t index) const {
+    RAPIDJSON_ASSERT(index < count_);
+    return digits_[index];
+  }
+  bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
+
+ private:
+  void AppendDecimal64(const char *begin, const char *end) {
+    uint64_t u = ParseUint64(begin, end);
+    if (IsZero())
+      *this = u;
+    else {
+      unsigned exp = static_cast<unsigned>(end - begin);
+      (MultiplyPow5(exp) <<= exp) += u;  // *this = *this * 10^exp + u
+    }
+  }
+
+  void PushBack(Type digit) {
+    RAPIDJSON_ASSERT(count_ < kCapacity);
+    digits_[count_++] = digit;
+  }
+
+  static uint64_t ParseUint64(const char *begin, const char *end) {
+    uint64_t r = 0;
+    for (const char *p = begin; p != end; ++p) {
+      RAPIDJSON_ASSERT(*p >= '0' && *p <= '9');
+      r = r * 10u + static_cast<unsigned>(*p - '0');
+    }
+    return r;
+  }
+
+  // Assume a * b + k < 2^128
+  static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k,
+                           uint64_t *outHigh) {
+#if defined(_MSC_VER) && defined(_M_AMD64)
+    uint64_t low = _umul128(a, b, outHigh) + k;
+    if (low < k) (*outHigh)++;
+    return low;
+#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
+    defined(__x86_64__)
+    __extension__ typedef unsigned __int128 uint128;
+    uint128 p = static_cast<uint128>(a) * static_cast<uint128>(b);
+    p += k;
+    *outHigh = static_cast<uint64_t>(p >> 64);
+    return static_cast<uint64_t>(p);
+#else
+    const uint64_t a0 = a & 0xFFFFFFFF, a1 = a >> 32, b0 = b & 0xFFFFFFFF,
+                   b1 = b >> 32;
+    uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1;
+    x1 += (x0 >> 32);  // can't give carry
+    x1 += x2;
+    if (x1 < x2) x3 += (static_cast<uint64_t>(1) << 32);
+    uint64_t lo = (x1 << 32) + (x0 & 0xFFFFFFFF);
+    uint64_t hi = x3 + (x1 >> 32);
+
+    lo += k;
+    if (lo < k) hi++;
+    *outHigh = hi;
+    return lo;
+#endif
+  }
+
+  static const size_t kBitCount = 3328;  // 64bit * 54 > 10^1000
+  static const size_t kCapacity = kBitCount / sizeof(Type);
+  static const size_t kTypeBit = sizeof(Type) * 8;
+
+  Type digits_[kCapacity];
+  size_t count_;
+};
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_BIGINTEGER_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/clzll.h b/livox_ros_driver2/3rdparty/rapidjson/internal/clzll.h
new file mode 100644
index 0000000..8cb8b8a
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/clzll.h
@@ -0,0 +1,77 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_CLZLL_H_
+#define RAPIDJSON_CLZLL_H_
+
+#include "../rapidjson.h"
+
+#if defined(_MSC_VER)
+#include <intrin.h>
+#if defined(_WIN64)
+#pragma intrinsic(_BitScanReverse64)
+#else
+#pragma intrinsic(_BitScanReverse)
+#endif
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+#if (defined(__GNUC__) && __GNUC__ >= 4) || \
+    RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
+#define RAPIDJSON_CLZLL __builtin_clzll
+#else
+
+inline uint32_t clzll(uint64_t x) {
+  // Passing 0 to __builtin_clzll is UB in GCC and results in an
+  // infinite loop in the software implementation.
+  RAPIDJSON_ASSERT(x != 0);
+
+#if defined(_MSC_VER)
+  unsigned long r = 0;
+#if defined(_WIN64)
+  _BitScanReverse64(&r, x);
+#else
+  // Scan the high 32 bits.
+  if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32))) return 63 - (r + 32);
+
+  // Scan the low 32 bits.
+  _BitScanReverse(&r, static_cast<uint32_t>(x & 0xFFFFFFFF));
+#endif  // _WIN64
+
+  return 63 - r;
+#else
+  uint32_t r;
+  while (!(x & (static_cast<uint64_t>(1) << 63))) {
+    x <<= 1;
+    ++r;
+  }
+
+  return r;
+#endif  // _MSC_VER
+}
+
+#define RAPIDJSON_CLZLL RAPIDJSON_NAMESPACE::internal::clzll
+#endif  // (defined(__GNUC__) && __GNUC__ >= 4) ||
+        // RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_CLZLL_H_
\ No newline at end of file
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/diyfp.h b/livox_ros_driver2/3rdparty/rapidjson/internal/diyfp.h
new file mode 100644
index 0000000..bed2989
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/diyfp.h
@@ -0,0 +1,305 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+// This is a C++ header-only implementation of Grisu2 algorithm from the
+// publication: Loitsch, Florian. "Printing floating-point numbers quickly and
+// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243.
+
+#ifndef RAPIDJSON_DIYFP_H_
+#define RAPIDJSON_DIYFP_H_
+
+#include <limits>
+#include "../rapidjson.h"
+#include "clzll.h"
+
+#if defined(_MSC_VER) && defined(_M_AMD64) && !defined(__INTEL_COMPILER)
+#include <intrin.h>
+#pragma intrinsic(_umul128)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+struct DiyFp {
+  DiyFp() : f(), e() {}
+
+  DiyFp(uint64_t fp, int exp) : f(fp), e(exp) {}
+
+  explicit DiyFp(double d) {
+    union {
+      double d;
+      uint64_t u64;
+    } u = {d};
+
+    int biased_e =
+        static_cast<int>((u.u64 & kDpExponentMask) >> kDpSignificandSize);
+    uint64_t significand = (u.u64 & kDpSignificandMask);
+    if (biased_e != 0) {
+      f = significand + kDpHiddenBit;
+      e = biased_e - kDpExponentBias;
+    } else {
+      f = significand;
+      e = kDpMinExponent + 1;
+    }
+  }
+
+  DiyFp operator-(const DiyFp &rhs) const { return DiyFp(f - rhs.f, e); }
+
+  DiyFp operator*(const DiyFp &rhs) const {
+#if defined(_MSC_VER) && defined(_M_AMD64)
+    uint64_t h;
+    uint64_t l = _umul128(f, rhs.f, &h);
+    if (l & (uint64_t(1) << 63))  // rounding
+      h++;
+    return DiyFp(h, e + rhs.e + 64);
+#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
+    defined(__x86_64__)
+    __extension__ typedef unsigned __int128 uint128;
+    uint128 p = static_cast<uint128>(f) * static_cast<uint128>(rhs.f);
+    uint64_t h = static_cast<uint64_t>(p >> 64);
+    uint64_t l = static_cast<uint64_t>(p);
+    if (l & (uint64_t(1) << 63))  // rounding
+      h++;
+    return DiyFp(h, e + rhs.e + 64);
+#else
+    const uint64_t M32 = 0xFFFFFFFF;
+    const uint64_t a = f >> 32;
+    const uint64_t b = f & M32;
+    const uint64_t c = rhs.f >> 32;
+    const uint64_t d = rhs.f & M32;
+    const uint64_t ac = a * c;
+    const uint64_t bc = b * c;
+    const uint64_t ad = a * d;
+    const uint64_t bd = b * d;
+    uint64_t tmp = (bd >> 32) + (ad & M32) + (bc & M32);
+    tmp += 1U << 31;  /// mult_round
+    return DiyFp(ac + (ad >> 32) + (bc >> 32) + (tmp >> 32), e + rhs.e + 64);
+#endif
+  }
+
+  DiyFp Normalize() const {
+    int s = static_cast<int>(RAPIDJSON_CLZLL(f));
+    return DiyFp(f << s, e - s);
+  }
+
+  DiyFp NormalizeBoundary() const {
+    DiyFp res = *this;
+    while (!(res.f & (kDpHiddenBit << 1))) {
+      res.f <<= 1;
+      res.e--;
+    }
+    res.f <<= (kDiySignificandSize - kDpSignificandSize - 2);
+    res.e = res.e - (kDiySignificandSize - kDpSignificandSize - 2);
+    return res;
+  }
+
+  void NormalizedBoundaries(DiyFp *minus, DiyFp *plus) const {
+    DiyFp pl = DiyFp((f << 1) + 1, e - 1).NormalizeBoundary();
+    DiyFp mi = (f == kDpHiddenBit) ? DiyFp((f << 2) - 1, e - 2)
+                                   : DiyFp((f << 1) - 1, e - 1);
+    mi.f <<= mi.e - pl.e;
+    mi.e = pl.e;
+    *plus = pl;
+    *minus = mi;
+  }
+
+  double ToDouble() const {
+    union {
+      double d;
+      uint64_t u64;
+    } u;
+    RAPIDJSON_ASSERT(f <= kDpHiddenBit + kDpSignificandMask);
+    if (e < kDpDenormalExponent) {
+      // Underflow.
+      return 0.0;
+    }
+    if (e >= kDpMaxExponent) {
+      // Overflow.
+      return std::numeric_limits<double>::infinity();
+    }
+    const uint64_t be = (e == kDpDenormalExponent && (f & kDpHiddenBit) == 0)
+                            ? 0
+                            : static_cast<uint64_t>(e + kDpExponentBias);
+    u.u64 = (f & kDpSignificandMask) | (be << kDpSignificandSize);
+    return u.d;
+  }
+
+  static const int kDiySignificandSize = 64;
+  static const int kDpSignificandSize = 52;
+  static const int kDpExponentBias = 0x3FF + kDpSignificandSize;
+  static const int kDpMaxExponent = 0x7FF - kDpExponentBias;
+  static const int kDpMinExponent = -kDpExponentBias;
+  static const int kDpDenormalExponent = -kDpExponentBias + 1;
+  static const uint64_t kDpExponentMask =
+      RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
+  static const uint64_t kDpSignificandMask =
+      RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
+  static const uint64_t kDpHiddenBit =
+      RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
+
+  uint64_t f;
+  int e;
+};
+
+inline DiyFp GetCachedPowerByIndex(size_t index) {
+  // 10^-348, 10^-340, ..., 10^340
+  static const uint64_t kCachedPowers_F[] = {
+      RAPIDJSON_UINT64_C2(0xfa8fd5a0, 0x081c0288),
+      RAPIDJSON_UINT64_C2(0xbaaee17f, 0xa23ebf76),
+      RAPIDJSON_UINT64_C2(0x8b16fb20, 0x3055ac76),
+      RAPIDJSON_UINT64_C2(0xcf42894a, 0x5dce35ea),
+      RAPIDJSON_UINT64_C2(0x9a6bb0aa, 0x55653b2d),
+      RAPIDJSON_UINT64_C2(0xe61acf03, 0x3d1a45df),
+      RAPIDJSON_UINT64_C2(0xab70fe17, 0xc79ac6ca),
+      RAPIDJSON_UINT64_C2(0xff77b1fc, 0xbebcdc4f),
+      RAPIDJSON_UINT64_C2(0xbe5691ef, 0x416bd60c),
+      RAPIDJSON_UINT64_C2(0x8dd01fad, 0x907ffc3c),
+      RAPIDJSON_UINT64_C2(0xd3515c28, 0x31559a83),
+      RAPIDJSON_UINT64_C2(0x9d71ac8f, 0xada6c9b5),
+      RAPIDJSON_UINT64_C2(0xea9c2277, 0x23ee8bcb),
+      RAPIDJSON_UINT64_C2(0xaecc4991, 0x4078536d),
+      RAPIDJSON_UINT64_C2(0x823c1279, 0x5db6ce57),
+      RAPIDJSON_UINT64_C2(0xc2109436, 0x4dfb5637),
+      RAPIDJSON_UINT64_C2(0x9096ea6f, 0x3848984f),
+      RAPIDJSON_UINT64_C2(0xd77485cb, 0x25823ac7),
+      RAPIDJSON_UINT64_C2(0xa086cfcd, 0x97bf97f4),
+      RAPIDJSON_UINT64_C2(0xef340a98, 0x172aace5),
+      RAPIDJSON_UINT64_C2(0xb23867fb, 0x2a35b28e),
+      RAPIDJSON_UINT64_C2(0x84c8d4df, 0xd2c63f3b),
+      RAPIDJSON_UINT64_C2(0xc5dd4427, 0x1ad3cdba),
+      RAPIDJSON_UINT64_C2(0x936b9fce, 0xbb25c996),
+      RAPIDJSON_UINT64_C2(0xdbac6c24, 0x7d62a584),
+      RAPIDJSON_UINT64_C2(0xa3ab6658, 0x0d5fdaf6),
+      RAPIDJSON_UINT64_C2(0xf3e2f893, 0xdec3f126),
+      RAPIDJSON_UINT64_C2(0xb5b5ada8, 0xaaff80b8),
+      RAPIDJSON_UINT64_C2(0x87625f05, 0x6c7c4a8b),
+      RAPIDJSON_UINT64_C2(0xc9bcff60, 0x34c13053),
+      RAPIDJSON_UINT64_C2(0x964e858c, 0x91ba2655),
+      RAPIDJSON_UINT64_C2(0xdff97724, 0x70297ebd),
+      RAPIDJSON_UINT64_C2(0xa6dfbd9f, 0xb8e5b88f),
+      RAPIDJSON_UINT64_C2(0xf8a95fcf, 0x88747d94),
+      RAPIDJSON_UINT64_C2(0xb9447093, 0x8fa89bcf),
+      RAPIDJSON_UINT64_C2(0x8a08f0f8, 0xbf0f156b),
+      RAPIDJSON_UINT64_C2(0xcdb02555, 0x653131b6),
+      RAPIDJSON_UINT64_C2(0x993fe2c6, 0xd07b7fac),
+      RAPIDJSON_UINT64_C2(0xe45c10c4, 0x2a2b3b06),
+      RAPIDJSON_UINT64_C2(0xaa242499, 0x697392d3),
+      RAPIDJSON_UINT64_C2(0xfd87b5f2, 0x8300ca0e),
+      RAPIDJSON_UINT64_C2(0xbce50864, 0x92111aeb),
+      RAPIDJSON_UINT64_C2(0x8cbccc09, 0x6f5088cc),
+      RAPIDJSON_UINT64_C2(0xd1b71758, 0xe219652c),
+      RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000),
+      RAPIDJSON_UINT64_C2(0xe8d4a510, 0x00000000),
+      RAPIDJSON_UINT64_C2(0xad78ebc5, 0xac620000),
+      RAPIDJSON_UINT64_C2(0x813f3978, 0xf8940984),
+      RAPIDJSON_UINT64_C2(0xc097ce7b, 0xc90715b3),
+      RAPIDJSON_UINT64_C2(0x8f7e32ce, 0x7bea5c70),
+      RAPIDJSON_UINT64_C2(0xd5d238a4, 0xabe98068),
+      RAPIDJSON_UINT64_C2(0x9f4f2726, 0x179a2245),
+      RAPIDJSON_UINT64_C2(0xed63a231, 0xd4c4fb27),
+      RAPIDJSON_UINT64_C2(0xb0de6538, 0x8cc8ada8),
+      RAPIDJSON_UINT64_C2(0x83c7088e, 0x1aab65db),
+      RAPIDJSON_UINT64_C2(0xc45d1df9, 0x42711d9a),
+      RAPIDJSON_UINT64_C2(0x924d692c, 0xa61be758),
+      RAPIDJSON_UINT64_C2(0xda01ee64, 0x1a708dea),
+      RAPIDJSON_UINT64_C2(0xa26da399, 0x9aef774a),
+      RAPIDJSON_UINT64_C2(0xf209787b, 0xb47d6b85),
+      RAPIDJSON_UINT64_C2(0xb454e4a1, 0x79dd1877),
+      RAPIDJSON_UINT64_C2(0x865b8692, 0x5b9bc5c2),
+      RAPIDJSON_UINT64_C2(0xc83553c5, 0xc8965d3d),
+      RAPIDJSON_UINT64_C2(0x952ab45c, 0xfa97a0b3),
+      RAPIDJSON_UINT64_C2(0xde469fbd, 0x99a05fe3),
+      RAPIDJSON_UINT64_C2(0xa59bc234, 0xdb398c25),
+      RAPIDJSON_UINT64_C2(0xf6c69a72, 0xa3989f5c),
+      RAPIDJSON_UINT64_C2(0xb7dcbf53, 0x54e9bece),
+      RAPIDJSON_UINT64_C2(0x88fcf317, 0xf22241e2),
+      RAPIDJSON_UINT64_C2(0xcc20ce9b, 0xd35c78a5),
+      RAPIDJSON_UINT64_C2(0x98165af3, 0x7b2153df),
+      RAPIDJSON_UINT64_C2(0xe2a0b5dc, 0x971f303a),
+      RAPIDJSON_UINT64_C2(0xa8d9d153, 0x5ce3b396),
+      RAPIDJSON_UINT64_C2(0xfb9b7cd9, 0xa4a7443c),
+      RAPIDJSON_UINT64_C2(0xbb764c4c, 0xa7a44410),
+      RAPIDJSON_UINT64_C2(0x8bab8eef, 0xb6409c1a),
+      RAPIDJSON_UINT64_C2(0xd01fef10, 0xa657842c),
+      RAPIDJSON_UINT64_C2(0x9b10a4e5, 0xe9913129),
+      RAPIDJSON_UINT64_C2(0xe7109bfb, 0xa19c0c9d),
+      RAPIDJSON_UINT64_C2(0xac2820d9, 0x623bf429),
+      RAPIDJSON_UINT64_C2(0x80444b5e, 0x7aa7cf85),
+      RAPIDJSON_UINT64_C2(0xbf21e440, 0x03acdd2d),
+      RAPIDJSON_UINT64_C2(0x8e679c2f, 0x5e44ff8f),
+      RAPIDJSON_UINT64_C2(0xd433179d, 0x9c8cb841),
+      RAPIDJSON_UINT64_C2(0x9e19db92, 0xb4e31ba9),
+      RAPIDJSON_UINT64_C2(0xeb96bf6e, 0xbadf77d9),
+      RAPIDJSON_UINT64_C2(0xaf87023b, 0x9bf0ee6b)};
+  static const int16_t kCachedPowers_E[] = {
+      -1220, -1193, -1166, -1140, -1113, -1087, -1060, -1034, -1007, -980, -954,
+      -927,  -901,  -874,  -847,  -821,  -794,  -768,  -741,  -715,  -688, -661,
+      -635,  -608,  -582,  -555,  -529,  -502,  -475,  -449,  -422,  -396, -369,
+      -343,  -316,  -289,  -263,  -236,  -210,  -183,  -157,  -130,  -103, -77,
+      -50,   -24,   3,     30,    56,    83,    109,   136,   162,   189,  216,
+      242,   269,   295,   322,   348,   375,   402,   428,   455,   481,  508,
+      534,   561,   588,   614,   641,   667,   694,   720,   747,   774,  800,
+      827,   853,   880,   907,   933,   960,   986,   1013,  1039,  1066};
+  RAPIDJSON_ASSERT(index < 87);
+  return DiyFp(kCachedPowers_F[index], kCachedPowers_E[index]);
+}
+
+inline DiyFp GetCachedPower(int e, int *K) {
+  // int k = static_cast<int>(ceil((-61 - e) * 0.30102999566398114)) + 374;
+  double dk = (-61 - e) * 0.30102999566398114 +
+              347;  // dk must be positive, so can do ceiling in positive
+  int k = static_cast<int>(dk);
+  if (dk - k > 0.0) k++;
+
+  unsigned index = static_cast<unsigned>((k >> 3) + 1);
+  *K = -(-348 + static_cast<int>(
+                    index << 3));  // decimal exponent no need lookup table
+
+  return GetCachedPowerByIndex(index);
+}
+
+inline DiyFp GetCachedPower10(int exp, int *outExp) {
+  RAPIDJSON_ASSERT(exp >= -348);
+  unsigned index = static_cast<unsigned>(exp + 348) / 8u;
+  *outExp = -348 + static_cast<int>(index) * 8;
+  return GetCachedPowerByIndex(index);
+}
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_DIYFP_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/dtoa.h b/livox_ros_driver2/3rdparty/rapidjson/internal/dtoa.h
new file mode 100644
index 0000000..b64a79d
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/dtoa.h
@@ -0,0 +1,269 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+// This is a C++ header-only implementation of Grisu2 algorithm from the
+// publication: Loitsch, Florian. "Printing floating-point numbers quickly and
+// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243.
+
+#ifndef RAPIDJSON_DTOA_
+#define RAPIDJSON_DTOA_
+
+#include "diyfp.h"
+#include "ieee754.h"
+#include "itoa.h"  // GetDigitsLut()
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+RAPIDJSON_DIAG_OFF(array - bounds)  // some gcc versions generate wrong warnings
+// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59124
+#endif
+
+inline void GrisuRound(char *buffer, int len, uint64_t delta, uint64_t rest,
+                       uint64_t ten_kappa, uint64_t wp_w) {
+  while (rest < wp_w && delta - rest >= ten_kappa &&
+         (rest + ten_kappa < wp_w ||  /// closer
+          wp_w - rest > rest + ten_kappa - wp_w)) {
+    buffer[len - 1]--;
+    rest += ten_kappa;
+  }
+}
+
+inline int CountDecimalDigit32(uint32_t n) {
+  // Simple pure C++ implementation was faster than __builtin_clz version in
+  // this situation.
+  if (n < 10) return 1;
+  if (n < 100) return 2;
+  if (n < 1000) return 3;
+  if (n < 10000) return 4;
+  if (n < 100000) return 5;
+  if (n < 1000000) return 6;
+  if (n < 10000000) return 7;
+  if (n < 100000000) return 8;
+  // Will not reach 10 digits in DigitGen()
+  // if (n < 1000000000) return 9;
+  // return 10;
+  return 9;
+}
+
+inline void DigitGen(const DiyFp &W, const DiyFp &Mp, uint64_t delta,
+                     char *buffer, int *len, int *K) {
+  static const uint32_t kPow10[] = {1,         10,        100,     1000,
+                                    10000,     100000,    1000000, 10000000,
+                                    100000000, 1000000000};
+  const DiyFp one(uint64_t(1) << -Mp.e, Mp.e);
+  const DiyFp wp_w = Mp - W;
+  uint32_t p1 = static_cast<uint32_t>(Mp.f >> -one.e);
+  uint64_t p2 = Mp.f & (one.f - 1);
+  int kappa = CountDecimalDigit32(p1);  // kappa in [0, 9]
+  *len = 0;
+
+  while (kappa > 0) {
+    uint32_t d = 0;
+    switch (kappa) {
+      case 9:
+        d = p1 / 100000000;
+        p1 %= 100000000;
+        break;
+      case 8:
+        d = p1 / 10000000;
+        p1 %= 10000000;
+        break;
+      case 7:
+        d = p1 / 1000000;
+        p1 %= 1000000;
+        break;
+      case 6:
+        d = p1 / 100000;
+        p1 %= 100000;
+        break;
+      case 5:
+        d = p1 / 10000;
+        p1 %= 10000;
+        break;
+      case 4:
+        d = p1 / 1000;
+        p1 %= 1000;
+        break;
+      case 3:
+        d = p1 / 100;
+        p1 %= 100;
+        break;
+      case 2:
+        d = p1 / 10;
+        p1 %= 10;
+        break;
+      case 1:
+        d = p1;
+        p1 = 0;
+        break;
+      default:;
+    }
+    if (d || *len)
+      buffer[(*len)++] = static_cast<char>('0' + static_cast<char>(d));
+    kappa--;
+    uint64_t tmp = (static_cast<uint64_t>(p1) << -one.e) + p2;
+    if (tmp <= delta) {
+      *K += kappa;
+      GrisuRound(buffer, *len, delta, tmp,
+                 static_cast<uint64_t>(kPow10[kappa]) << -one.e, wp_w.f);
+      return;
+    }
+  }
+
+  // kappa = 0
+  for (;;) {
+    p2 *= 10;
+    delta *= 10;
+    char d = static_cast<char>(p2 >> -one.e);
+    if (d || *len) buffer[(*len)++] = static_cast<char>('0' + d);
+    p2 &= one.f - 1;
+    kappa--;
+    if (p2 < delta) {
+      *K += kappa;
+      int index = -kappa;
+      GrisuRound(buffer, *len, delta, p2, one.f,
+                 wp_w.f * (index < 9 ? kPow10[index] : 0));
+      return;
+    }
+  }
+}
+
+inline void Grisu2(double value, char *buffer, int *length, int *K) {
+  const DiyFp v(value);
+  DiyFp w_m, w_p;
+  v.NormalizedBoundaries(&w_m, &w_p);
+
+  const DiyFp c_mk = GetCachedPower(w_p.e, K);
+  const DiyFp W = v.Normalize() * c_mk;
+  DiyFp Wp = w_p * c_mk;
+  DiyFp Wm = w_m * c_mk;
+  Wm.f++;
+  Wp.f--;
+  DigitGen(W, Wp, Wp.f - Wm.f, buffer, length, K);
+}
+
+inline char *WriteExponent(int K, char *buffer) {
+  if (K < 0) {
+    *buffer++ = '-';
+    K = -K;
+  }
+
+  if (K >= 100) {
+    *buffer++ = static_cast<char>('0' + static_cast<char>(K / 100));
+    K %= 100;
+    const char *d = GetDigitsLut() + K * 2;
+    *buffer++ = d[0];
+    *buffer++ = d[1];
+  } else if (K >= 10) {
+    const char *d = GetDigitsLut() + K * 2;
+    *buffer++ = d[0];
+    *buffer++ = d[1];
+  } else
+    *buffer++ = static_cast<char>('0' + static_cast<char>(K));
+
+  return buffer;
+}
+
+inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
+  const int kk = length + k;  // 10^(kk-1) <= v < 10^kk
+
+  if (0 <= k && kk <= 21) {
+    // 1234e7 -> 12340000000
+    for (int i = length; i < kk; i++) buffer[i] = '0';
+    buffer[kk] = '.';
+    buffer[kk + 1] = '0';
+    return &buffer[kk + 2];
+  } else if (0 < kk && kk <= 21) {
+    // 1234e-2 -> 12.34
+    std::memmove(&buffer[kk + 1], &buffer[kk],
+                 static_cast<size_t>(length - kk));
+    buffer[kk] = '.';
+    if (0 > k + maxDecimalPlaces) {
+      // When maxDecimalPlaces = 2, 1.2345 -> 1.23, 1.102 -> 1.1
+      // Remove extra trailing zeros (at least one) after truncation.
+      for (int i = kk + maxDecimalPlaces; i > kk + 1; i--)
+        if (buffer[i] != '0') return &buffer[i + 1];
+      return &buffer[kk + 2];  // Reserve one zero
+    } else
+      return &buffer[length + 1];
+  } else if (-6 < kk && kk <= 0) {
+    // 1234e-6 -> 0.001234
+    const int offset = 2 - kk;
+    std::memmove(&buffer[offset], &buffer[0], static_cast<size_t>(length));
+    buffer[0] = '0';
+    buffer[1] = '.';
+    for (int i = 2; i < offset; i++) buffer[i] = '0';
+    if (length - kk > maxDecimalPlaces) {
+      // When maxDecimalPlaces = 2, 0.123 -> 0.12, 0.102 -> 0.1
+      // Remove extra trailing zeros (at least one) after truncation.
+      for (int i = maxDecimalPlaces + 1; i > 2; i--)
+        if (buffer[i] != '0') return &buffer[i + 1];
+      return &buffer[3];  // Reserve one zero
+    } else
+      return &buffer[length + offset];
+  } else if (kk < -maxDecimalPlaces) {
+    // Truncate to zero
+    buffer[0] = '0';
+    buffer[1] = '.';
+    buffer[2] = '0';
+    return &buffer[3];
+  } else if (length == 1) {
+    // 1e30
+    buffer[1] = 'e';
+    return WriteExponent(kk - 1, &buffer[2]);
+  } else {
+    // 1234e30 -> 1.234e33
+    std::memmove(&buffer[2], &buffer[1], static_cast<size_t>(length - 1));
+    buffer[1] = '.';
+    buffer[length + 1] = 'e';
+    return WriteExponent(kk - 1, &buffer[0 + length + 2]);
+  }
+}
+
+inline char *dtoa(double value, char *buffer, int maxDecimalPlaces = 324) {
+  RAPIDJSON_ASSERT(maxDecimalPlaces >= 1);
+  Double d(value);
+  if (d.IsZero()) {
+    if (d.Sign()) *buffer++ = '-';  // -0.0, Issue #289
+    buffer[0] = '0';
+    buffer[1] = '.';
+    buffer[2] = '0';
+    return &buffer[3];
+  } else {
+    if (value < 0) {
+      *buffer++ = '-';
+      value = -value;
+    }
+    int length, K;
+    Grisu2(value, buffer, &length, &K);
+    return Prettify(buffer, length, K, maxDecimalPlaces);
+  }
+}
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_DTOA_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/ieee754.h b/livox_ros_driver2/3rdparty/rapidjson/internal/ieee754.h
new file mode 100644
index 0000000..246c4ac
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/ieee754.h
@@ -0,0 +1,100 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_IEEE754_
+#define RAPIDJSON_IEEE754_
+
+#include "../rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+class Double {
+ public:
+  Double() {}
+  Double(double d) : d_(d) {}
+  Double(uint64_t u) : u_(u) {}
+
+  double Value() const { return d_; }
+  uint64_t Uint64Value() const { return u_; }
+
+  double NextPositiveDouble() const {
+    RAPIDJSON_ASSERT(!Sign());
+    return Double(u_ + 1).Value();
+  }
+
+  bool Sign() const { return (u_ & kSignMask) != 0; }
+  uint64_t Significand() const { return u_ & kSignificandMask; }
+  int Exponent() const {
+    return static_cast<int>(((u_ & kExponentMask) >> kSignificandSize) -
+                            kExponentBias);
+  }
+
+  bool IsNan() const {
+    return (u_ & kExponentMask) == kExponentMask && Significand() != 0;
+  }
+  bool IsInf() const {
+    return (u_ & kExponentMask) == kExponentMask && Significand() == 0;
+  }
+  bool IsNanOrInf() const { return (u_ & kExponentMask) == kExponentMask; }
+  bool IsNormal() const {
+    return (u_ & kExponentMask) != 0 || Significand() == 0;
+  }
+  bool IsZero() const { return (u_ & (kExponentMask | kSignificandMask)) == 0; }
+
+  uint64_t IntegerSignificand() const {
+    return IsNormal() ? Significand() | kHiddenBit : Significand();
+  }
+  int IntegerExponent() const {
+    return (IsNormal() ? Exponent() : kDenormalExponent) - kSignificandSize;
+  }
+  uint64_t ToBias() const {
+    return (u_ & kSignMask) ? ~u_ + 1 : u_ | kSignMask;
+  }
+
+  static int EffectiveSignificandSize(int order) {
+    if (order >= -1021)
+      return 53;
+    else if (order <= -1074)
+      return 0;
+    else
+      return order + 1074;
+  }
+
+ private:
+  static const int kSignificandSize = 52;
+  static const int kExponentBias = 0x3FF;
+  static const int kDenormalExponent = 1 - kExponentBias;
+  static const uint64_t kSignMask = RAPIDJSON_UINT64_C2(0x80000000, 0x00000000);
+  static const uint64_t kExponentMask =
+      RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
+  static const uint64_t kSignificandMask =
+      RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
+  static const uint64_t kHiddenBit =
+      RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
+
+  union {
+    double d_;
+    uint64_t u_;
+  };
+};
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_IEEE754_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/itoa.h b/livox_ros_driver2/3rdparty/rapidjson/internal/itoa.h
new file mode 100644
index 0000000..ec1174c
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/itoa.h
@@ -0,0 +1,288 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_ITOA_
+#define RAPIDJSON_ITOA_
+
+#include "../rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+inline const char *GetDigitsLut() {
+  static const char cDigitsLut[200] = {
+      '0', '0', '0', '1', '0', '2', '0', '3', '0', '4', '0', '5', '0', '6', '0',
+      '7', '0', '8', '0', '9', '1', '0', '1', '1', '1', '2', '1', '3', '1', '4',
+      '1', '5', '1', '6', '1', '7', '1', '8', '1', '9', '2', '0', '2', '1', '2',
+      '2', '2', '3', '2', '4', '2', '5', '2', '6', '2', '7', '2', '8', '2', '9',
+      '3', '0', '3', '1', '3', '2', '3', '3', '3', '4', '3', '5', '3', '6', '3',
+      '7', '3', '8', '3', '9', '4', '0', '4', '1', '4', '2', '4', '3', '4', '4',
+      '4', '5', '4', '6', '4', '7', '4', '8', '4', '9', '5', '0', '5', '1', '5',
+      '2', '5', '3', '5', '4', '5', '5', '5', '6', '5', '7', '5', '8', '5', '9',
+      '6', '0', '6', '1', '6', '2', '6', '3', '6', '4', '6', '5', '6', '6', '6',
+      '7', '6', '8', '6', '9', '7', '0', '7', '1', '7', '2', '7', '3', '7', '4',
+      '7', '5', '7', '6', '7', '7', '7', '8', '7', '9', '8', '0', '8', '1', '8',
+      '2', '8', '3', '8', '4', '8', '5', '8', '6', '8', '7', '8', '8', '8', '9',
+      '9', '0', '9', '1', '9', '2', '9', '3', '9', '4', '9', '5', '9', '6', '9',
+      '7', '9', '8', '9', '9'};
+  return cDigitsLut;
+}
+
+inline char *u32toa(uint32_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+
+  const char *cDigitsLut = GetDigitsLut();
+
+  if (value < 10000) {
+    const uint32_t d1 = (value / 100) << 1;
+    const uint32_t d2 = (value % 100) << 1;
+
+    if (value >= 1000) *buffer++ = cDigitsLut[d1];
+    if (value >= 100) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= 10) *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+  } else if (value < 100000000) {
+    // value = bbbbcccc
+    const uint32_t b = value / 10000;
+    const uint32_t c = value % 10000;
+
+    const uint32_t d1 = (b / 100) << 1;
+    const uint32_t d2 = (b % 100) << 1;
+
+    const uint32_t d3 = (c / 100) << 1;
+    const uint32_t d4 = (c % 100) << 1;
+
+    if (value >= 10000000) *buffer++ = cDigitsLut[d1];
+    if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= 100000) *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+
+    *buffer++ = cDigitsLut[d3];
+    *buffer++ = cDigitsLut[d3 + 1];
+    *buffer++ = cDigitsLut[d4];
+    *buffer++ = cDigitsLut[d4 + 1];
+  } else {
+    // value = aabbbbcccc in decimal
+
+    const uint32_t a = value / 100000000;  // 1 to 42
+    value %= 100000000;
+
+    if (a >= 10) {
+      const unsigned i = a << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+    } else
+      *buffer++ = static_cast<char>('0' + static_cast<char>(a));
+
+    const uint32_t b = value / 10000;  // 0 to 9999
+    const uint32_t c = value % 10000;  // 0 to 9999
+
+    const uint32_t d1 = (b / 100) << 1;
+    const uint32_t d2 = (b % 100) << 1;
+
+    const uint32_t d3 = (c / 100) << 1;
+    const uint32_t d4 = (c % 100) << 1;
+
+    *buffer++ = cDigitsLut[d1];
+    *buffer++ = cDigitsLut[d1 + 1];
+    *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+    *buffer++ = cDigitsLut[d3];
+    *buffer++ = cDigitsLut[d3 + 1];
+    *buffer++ = cDigitsLut[d4];
+    *buffer++ = cDigitsLut[d4 + 1];
+  }
+  return buffer;
+}
+
+inline char *i32toa(int32_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+  uint32_t u = static_cast<uint32_t>(value);
+  if (value < 0) {
+    *buffer++ = '-';
+    u = ~u + 1;
+  }
+
+  return u32toa(u, buffer);
+}
+
+inline char *u64toa(uint64_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+  const char *cDigitsLut = GetDigitsLut();
+  const uint64_t kTen8 = 100000000;
+  const uint64_t kTen9 = kTen8 * 10;
+  const uint64_t kTen10 = kTen8 * 100;
+  const uint64_t kTen11 = kTen8 * 1000;
+  const uint64_t kTen12 = kTen8 * 10000;
+  const uint64_t kTen13 = kTen8 * 100000;
+  const uint64_t kTen14 = kTen8 * 1000000;
+  const uint64_t kTen15 = kTen8 * 10000000;
+  const uint64_t kTen16 = kTen8 * kTen8;
+
+  if (value < kTen8) {
+    uint32_t v = static_cast<uint32_t>(value);
+    if (v < 10000) {
+      const uint32_t d1 = (v / 100) << 1;
+      const uint32_t d2 = (v % 100) << 1;
+
+      if (v >= 1000) *buffer++ = cDigitsLut[d1];
+      if (v >= 100) *buffer++ = cDigitsLut[d1 + 1];
+      if (v >= 10) *buffer++ = cDigitsLut[d2];
+      *buffer++ = cDigitsLut[d2 + 1];
+    } else {
+      // value = bbbbcccc
+      const uint32_t b = v / 10000;
+      const uint32_t c = v % 10000;
+
+      const uint32_t d1 = (b / 100) << 1;
+      const uint32_t d2 = (b % 100) << 1;
+
+      const uint32_t d3 = (c / 100) << 1;
+      const uint32_t d4 = (c % 100) << 1;
+
+      if (value >= 10000000) *buffer++ = cDigitsLut[d1];
+      if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
+      if (value >= 100000) *buffer++ = cDigitsLut[d2];
+      *buffer++ = cDigitsLut[d2 + 1];
+
+      *buffer++ = cDigitsLut[d3];
+      *buffer++ = cDigitsLut[d3 + 1];
+      *buffer++ = cDigitsLut[d4];
+      *buffer++ = cDigitsLut[d4 + 1];
+    }
+  } else if (value < kTen16) {
+    const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
+    const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
+
+    const uint32_t b0 = v0 / 10000;
+    const uint32_t c0 = v0 % 10000;
+
+    const uint32_t d1 = (b0 / 100) << 1;
+    const uint32_t d2 = (b0 % 100) << 1;
+
+    const uint32_t d3 = (c0 / 100) << 1;
+    const uint32_t d4 = (c0 % 100) << 1;
+
+    const uint32_t b1 = v1 / 10000;
+    const uint32_t c1 = v1 % 10000;
+
+    const uint32_t d5 = (b1 / 100) << 1;
+    const uint32_t d6 = (b1 % 100) << 1;
+
+    const uint32_t d7 = (c1 / 100) << 1;
+    const uint32_t d8 = (c1 % 100) << 1;
+
+    if (value >= kTen15) *buffer++ = cDigitsLut[d1];
+    if (value >= kTen14) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= kTen13) *buffer++ = cDigitsLut[d2];
+    if (value >= kTen12) *buffer++ = cDigitsLut[d2 + 1];
+    if (value >= kTen11) *buffer++ = cDigitsLut[d3];
+    if (value >= kTen10) *buffer++ = cDigitsLut[d3 + 1];
+    if (value >= kTen9) *buffer++ = cDigitsLut[d4];
+
+    *buffer++ = cDigitsLut[d4 + 1];
+    *buffer++ = cDigitsLut[d5];
+    *buffer++ = cDigitsLut[d5 + 1];
+    *buffer++ = cDigitsLut[d6];
+    *buffer++ = cDigitsLut[d6 + 1];
+    *buffer++ = cDigitsLut[d7];
+    *buffer++ = cDigitsLut[d7 + 1];
+    *buffer++ = cDigitsLut[d8];
+    *buffer++ = cDigitsLut[d8 + 1];
+  } else {
+    const uint32_t a = static_cast<uint32_t>(value / kTen16);  // 1 to 1844
+    value %= kTen16;
+
+    if (a < 10)
+      *buffer++ = static_cast<char>('0' + static_cast<char>(a));
+    else if (a < 100) {
+      const uint32_t i = a << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+    } else if (a < 1000) {
+      *buffer++ = static_cast<char>('0' + static_cast<char>(a / 100));
+
+      const uint32_t i = (a % 100) << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+    } else {
+      const uint32_t i = (a / 100) << 1;
+      const uint32_t j = (a % 100) << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+      *buffer++ = cDigitsLut[j];
+      *buffer++ = cDigitsLut[j + 1];
+    }
+
+    const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
+    const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
+
+    const uint32_t b0 = v0 / 10000;
+    const uint32_t c0 = v0 % 10000;
+
+    const uint32_t d1 = (b0 / 100) << 1;
+    const uint32_t d2 = (b0 % 100) << 1;
+
+    const uint32_t d3 = (c0 / 100) << 1;
+    const uint32_t d4 = (c0 % 100) << 1;
+
+    const uint32_t b1 = v1 / 10000;
+    const uint32_t c1 = v1 % 10000;
+
+    const uint32_t d5 = (b1 / 100) << 1;
+    const uint32_t d6 = (b1 % 100) << 1;
+
+    const uint32_t d7 = (c1 / 100) << 1;
+    const uint32_t d8 = (c1 % 100) << 1;
+
+    *buffer++ = cDigitsLut[d1];
+    *buffer++ = cDigitsLut[d1 + 1];
+    *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+    *buffer++ = cDigitsLut[d3];
+    *buffer++ = cDigitsLut[d3 + 1];
+    *buffer++ = cDigitsLut[d4];
+    *buffer++ = cDigitsLut[d4 + 1];
+    *buffer++ = cDigitsLut[d5];
+    *buffer++ = cDigitsLut[d5 + 1];
+    *buffer++ = cDigitsLut[d6];
+    *buffer++ = cDigitsLut[d6 + 1];
+    *buffer++ = cDigitsLut[d7];
+    *buffer++ = cDigitsLut[d7 + 1];
+    *buffer++ = cDigitsLut[d8];
+    *buffer++ = cDigitsLut[d8 + 1];
+  }
+
+  return buffer;
+}
+
+inline char *i64toa(int64_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+  uint64_t u = static_cast<uint64_t>(value);
+  if (value < 0) {
+    *buffer++ = '-';
+    u = ~u + 1;
+  }
+
+  return u64toa(u, buffer);
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_ITOA_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/meta.h b/livox_ros_driver2/3rdparty/rapidjson/internal/meta.h
new file mode 100644
index 0000000..598f576
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/meta.h
@@ -0,0 +1,243 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_INTERNAL_META_H_
+#define RAPIDJSON_INTERNAL_META_H_
+
+#include "../rapidjson.h"
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#if defined(_MSC_VER) && !defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(6334)
+#endif
+
+#if RAPIDJSON_HAS_CXX11_TYPETRAITS
+#include <type_traits>
+#endif
+
+//@cond RAPIDJSON_INTERNAL
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+// Helper to wrap/convert arbitrary types to void, useful for arbitrary type
+// matching
+template <typename T>
+struct Void {
+  typedef void Type;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// BoolType, TrueType, FalseType
+//
+template <bool Cond>
+struct BoolType {
+  static const bool Value = Cond;
+  typedef BoolType Type;
+};
+typedef BoolType<true> TrueType;
+typedef BoolType<false> FalseType;
+
+///////////////////////////////////////////////////////////////////////////////
+// SelectIf, BoolExpr, NotExpr, AndExpr, OrExpr
+//
+
+template <bool C>
+struct SelectIfImpl {
+  template <typename T1, typename T2>
+  struct Apply {
+    typedef T1 Type;
+  };
+};
+template <>
+struct SelectIfImpl<false> {
+  template <typename T1, typename T2>
+  struct Apply {
+    typedef T2 Type;
+  };
+};
+template <bool C, typename T1, typename T2>
+struct SelectIfCond : SelectIfImpl<C>::template Apply<T1, T2> {};
+template <typename C, typename T1, typename T2>
+struct SelectIf : SelectIfCond<C::Value, T1, T2> {};
+
+template <bool Cond1, bool Cond2>
+struct AndExprCond : FalseType {};
+template <>
+struct AndExprCond<true, true> : TrueType {};
+template <bool Cond1, bool Cond2>
+struct OrExprCond : TrueType {};
+template <>
+struct OrExprCond<false, false> : FalseType {};
+
+template <typename C>
+struct BoolExpr : SelectIf<C, TrueType, FalseType>::Type {};
+template <typename C>
+struct NotExpr : SelectIf<C, FalseType, TrueType>::Type {};
+template <typename C1, typename C2>
+struct AndExpr : AndExprCond<C1::Value, C2::Value>::Type {};
+template <typename C1, typename C2>
+struct OrExpr : OrExprCond<C1::Value, C2::Value>::Type {};
+
+///////////////////////////////////////////////////////////////////////////////
+// AddConst, MaybeAddConst, RemoveConst
+template <typename T>
+struct AddConst {
+  typedef const T Type;
+};
+template <bool Constify, typename T>
+struct MaybeAddConst : SelectIfCond<Constify, const T, T> {};
+template <typename T>
+struct RemoveConst {
+  typedef T Type;
+};
+template <typename T>
+struct RemoveConst<const T> {
+  typedef T Type;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// IsSame, IsConst, IsMoreConst, IsPointer
+//
+template <typename T, typename U>
+struct IsSame : FalseType {};
+template <typename T>
+struct IsSame<T, T> : TrueType {};
+
+template <typename T>
+struct IsConst : FalseType {};
+template <typename T>
+struct IsConst<const T> : TrueType {};
+
+template <typename CT, typename T>
+struct IsMoreConst
+    : AndExpr<
+          IsSame<typename RemoveConst<CT>::Type, typename RemoveConst<T>::Type>,
+          BoolType<IsConst<CT>::Value >= IsConst<T>::Value>>::Type {};
+
+template <typename T>
+struct IsPointer : FalseType {};
+template <typename T>
+struct IsPointer<T *> : TrueType {};
+
+///////////////////////////////////////////////////////////////////////////////
+// IsBaseOf
+//
+#if RAPIDJSON_HAS_CXX11_TYPETRAITS
+
+template <typename B, typename D>
+struct IsBaseOf : BoolType<::std::is_base_of<B, D>::value> {};
+
+#else  // simplified version adopted from Boost
+
+template <typename B, typename D>
+struct IsBaseOfImpl {
+  RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0);
+  RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0);
+
+  typedef char (&Yes)[1];
+  typedef char (&No)[2];
+
+  template <typename T>
+  static Yes Check(const D *, T);
+  static No Check(const B *, int);
+
+  struct Host {
+    operator const B *() const;
+    operator const D *();
+  };
+
+  enum { Value = (sizeof(Check(Host(), 0)) == sizeof(Yes)) };
+};
+
+template <typename B, typename D>
+struct IsBaseOf : OrExpr<IsSame<B, D>, BoolExpr<IsBaseOfImpl<B, D>>>::Type {};
+
+#endif  // RAPIDJSON_HAS_CXX11_TYPETRAITS
+
+//////////////////////////////////////////////////////////////////////////
+// EnableIf / DisableIf
+//
+template <bool Condition, typename T = void>
+struct EnableIfCond {
+  typedef T Type;
+};
+template <typename T>
+struct EnableIfCond<false, T> { /* empty */
+};
+
+template <bool Condition, typename T = void>
+struct DisableIfCond {
+  typedef T Type;
+};
+template <typename T>
+struct DisableIfCond<true, T> { /* empty */
+};
+
+template <typename Condition, typename T = void>
+struct EnableIf : EnableIfCond<Condition::Value, T> {};
+
+template <typename Condition, typename T = void>
+struct DisableIf : DisableIfCond<Condition::Value, T> {};
+
+// SFINAE helpers
+struct SfinaeTag {};
+template <typename T>
+struct RemoveSfinaeTag;
+template <typename T>
+struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
+  typedef T Type;
+};
+
+#define RAPIDJSON_REMOVEFPTR_(type)                          \
+  typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag< \
+      ::RAPIDJSON_NAMESPACE::internal::SfinaeTag &(*)type>::Type
+
+#define RAPIDJSON_ENABLEIF(cond)                                            \
+  typename ::RAPIDJSON_NAMESPACE::internal::EnableIf<RAPIDJSON_REMOVEFPTR_( \
+      cond)>::Type * = NULL
+
+#define RAPIDJSON_DISABLEIF(cond)                                            \
+  typename ::RAPIDJSON_NAMESPACE::internal::DisableIf<RAPIDJSON_REMOVEFPTR_( \
+      cond)>::Type * = NULL
+
+#define RAPIDJSON_ENABLEIF_RETURN(cond, returntype)   \
+  typename ::RAPIDJSON_NAMESPACE::internal::EnableIf< \
+      RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
+
+#define RAPIDJSON_DISABLEIF_RETURN(cond, returntype)   \
+  typename ::RAPIDJSON_NAMESPACE::internal::DisableIf< \
+      RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+//@endcond
+
+#if defined(_MSC_VER) && !defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_INTERNAL_META_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/pow10.h b/livox_ros_driver2/3rdparty/rapidjson/internal/pow10.h
new file mode 100644
index 0000000..6f15f74
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/pow10.h
@@ -0,0 +1,77 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_POW10_
+#define RAPIDJSON_POW10_
+
+#include "../rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+//! Computes integer powers of 10 in double (10.0^n).
+/*! This function uses lookup table for fast and accurate results.
+    \param n non-negative exponent. Must <= 308.
+    \return 10.0^n
+*/
+inline double Pow10(int n) {
+  static const double e[] = {
+      // 1e-0...1e308: 309 * 8 bytes = 2472 bytes
+      1e+0,   1e+1,   1e+2,   1e+3,   1e+4,   1e+5,   1e+6,   1e+7,   1e+8,
+      1e+9,   1e+10,  1e+11,  1e+12,  1e+13,  1e+14,  1e+15,  1e+16,  1e+17,
+      1e+18,  1e+19,  1e+20,  1e+21,  1e+22,  1e+23,  1e+24,  1e+25,  1e+26,
+      1e+27,  1e+28,  1e+29,  1e+30,  1e+31,  1e+32,  1e+33,  1e+34,  1e+35,
+      1e+36,  1e+37,  1e+38,  1e+39,  1e+40,  1e+41,  1e+42,  1e+43,  1e+44,
+      1e+45,  1e+46,  1e+47,  1e+48,  1e+49,  1e+50,  1e+51,  1e+52,  1e+53,
+      1e+54,  1e+55,  1e+56,  1e+57,  1e+58,  1e+59,  1e+60,  1e+61,  1e+62,
+      1e+63,  1e+64,  1e+65,  1e+66,  1e+67,  1e+68,  1e+69,  1e+70,  1e+71,
+      1e+72,  1e+73,  1e+74,  1e+75,  1e+76,  1e+77,  1e+78,  1e+79,  1e+80,
+      1e+81,  1e+82,  1e+83,  1e+84,  1e+85,  1e+86,  1e+87,  1e+88,  1e+89,
+      1e+90,  1e+91,  1e+92,  1e+93,  1e+94,  1e+95,  1e+96,  1e+97,  1e+98,
+      1e+99,  1e+100, 1e+101, 1e+102, 1e+103, 1e+104, 1e+105, 1e+106, 1e+107,
+      1e+108, 1e+109, 1e+110, 1e+111, 1e+112, 1e+113, 1e+114, 1e+115, 1e+116,
+      1e+117, 1e+118, 1e+119, 1e+120, 1e+121, 1e+122, 1e+123, 1e+124, 1e+125,
+      1e+126, 1e+127, 1e+128, 1e+129, 1e+130, 1e+131, 1e+132, 1e+133, 1e+134,
+      1e+135, 1e+136, 1e+137, 1e+138, 1e+139, 1e+140, 1e+141, 1e+142, 1e+143,
+      1e+144, 1e+145, 1e+146, 1e+147, 1e+148, 1e+149, 1e+150, 1e+151, 1e+152,
+      1e+153, 1e+154, 1e+155, 1e+156, 1e+157, 1e+158, 1e+159, 1e+160, 1e+161,
+      1e+162, 1e+163, 1e+164, 1e+165, 1e+166, 1e+167, 1e+168, 1e+169, 1e+170,
+      1e+171, 1e+172, 1e+173, 1e+174, 1e+175, 1e+176, 1e+177, 1e+178, 1e+179,
+      1e+180, 1e+181, 1e+182, 1e+183, 1e+184, 1e+185, 1e+186, 1e+187, 1e+188,
+      1e+189, 1e+190, 1e+191, 1e+192, 1e+193, 1e+194, 1e+195, 1e+196, 1e+197,
+      1e+198, 1e+199, 1e+200, 1e+201, 1e+202, 1e+203, 1e+204, 1e+205, 1e+206,
+      1e+207, 1e+208, 1e+209, 1e+210, 1e+211, 1e+212, 1e+213, 1e+214, 1e+215,
+      1e+216, 1e+217, 1e+218, 1e+219, 1e+220, 1e+221, 1e+222, 1e+223, 1e+224,
+      1e+225, 1e+226, 1e+227, 1e+228, 1e+229, 1e+230, 1e+231, 1e+232, 1e+233,
+      1e+234, 1e+235, 1e+236, 1e+237, 1e+238, 1e+239, 1e+240, 1e+241, 1e+242,
+      1e+243, 1e+244, 1e+245, 1e+246, 1e+247, 1e+248, 1e+249, 1e+250, 1e+251,
+      1e+252, 1e+253, 1e+254, 1e+255, 1e+256, 1e+257, 1e+258, 1e+259, 1e+260,
+      1e+261, 1e+262, 1e+263, 1e+264, 1e+265, 1e+266, 1e+267, 1e+268, 1e+269,
+      1e+270, 1e+271, 1e+272, 1e+273, 1e+274, 1e+275, 1e+276, 1e+277, 1e+278,
+      1e+279, 1e+280, 1e+281, 1e+282, 1e+283, 1e+284, 1e+285, 1e+286, 1e+287,
+      1e+288, 1e+289, 1e+290, 1e+291, 1e+292, 1e+293, 1e+294, 1e+295, 1e+296,
+      1e+297, 1e+298, 1e+299, 1e+300, 1e+301, 1e+302, 1e+303, 1e+304, 1e+305,
+      1e+306, 1e+307, 1e+308};
+  RAPIDJSON_ASSERT(n >= 0 && n <= 308);
+  return e[n];
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_POW10_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/regex.h b/livox_ros_driver2/3rdparty/rapidjson/internal/regex.h
new file mode 100644
index 0000000..19f49da
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/regex.h
@@ -0,0 +1,753 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_INTERNAL_REGEX_H_
+#define RAPIDJSON_INTERNAL_REGEX_H_
+
+#include "../allocators.h"
+#include "../stream.h"
+#include "stack.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+RAPIDJSON_DIAG_OFF(switch - enum)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#ifndef RAPIDJSON_REGEX_VERBOSE
+#define RAPIDJSON_REGEX_VERBOSE 0
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+///////////////////////////////////////////////////////////////////////////////
+// DecodedStream
+
+template <typename SourceStream, typename Encoding>
+class DecodedStream {
+ public:
+  DecodedStream(SourceStream &ss) : ss_(ss), codepoint_() { Decode(); }
+  unsigned Peek() { return codepoint_; }
+  unsigned Take() {
+    unsigned c = codepoint_;
+    if (c)  // No further decoding when '\0'
+      Decode();
+    return c;
+  }
+
+ private:
+  void Decode() {
+    if (!Encoding::Decode(ss_, &codepoint_)) codepoint_ = 0;
+  }
+
+  SourceStream &ss_;
+  unsigned codepoint_;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericRegex
+
+static const SizeType kRegexInvalidState = ~SizeType(
+    0);  //!< Represents an invalid index in GenericRegex::State::out, out1
+static const SizeType kRegexInvalidRange = ~SizeType(0);
+
+template <typename Encoding, typename Allocator>
+class GenericRegexSearch;
+
+//! Regular expression engine with subset of ECMAscript grammar.
+/*!
+    Supported regular expression syntax:
+    - \c ab     Concatenation
+    - \c a|b    Alternation
+    - \c a?     Zero or one
+    - \c a*     Zero or more
+    - \c a+     One or more
+    - \c a{3}   Exactly 3 times
+    - \c a{3,}  At least 3 times
+    - \c a{3,5} 3 to 5 times
+    - \c (ab)   Grouping
+    - \c ^a     At the beginning
+    - \c a$     At the end
+    - \c .      Any character
+    - \c [abc]  Character classes
+    - \c [a-c]  Character class range
+    - \c [a-z0-9_] Character class combination
+    - \c [^abc] Negated character classes
+    - \c [^a-c] Negated character class range
+    - \c [\b]   Backspace (U+0008)
+    - \c \\| \\\\ ...  Escape characters
+    - \c \\f Form feed (U+000C)
+    - \c \\n Line feed (U+000A)
+    - \c \\r Carriage return (U+000D)
+    - \c \\t Tab (U+0009)
+    - \c \\v Vertical tab (U+000B)
+
+    \note This is a Thompson NFA engine, implemented with reference to
+        Cox, Russ. "Regular Expression Matching Can Be Simple And Fast (but is
+   slow in Java, Perl, PHP, Python, Ruby,...).",
+        https://swtch.com/~rsc/regexp/regexp1.html
+*/
+template <typename Encoding, typename Allocator = CrtAllocator>
+class GenericRegex {
+ public:
+  typedef Encoding EncodingType;
+  typedef typename Encoding::Ch Ch;
+  template <typename, typename>
+  friend class GenericRegexSearch;
+
+  GenericRegex(const Ch *source, Allocator *allocator = 0)
+      : ownAllocator_(allocator ? 0 : RAPIDJSON_NEW(Allocator)()),
+        allocator_(allocator ? allocator : ownAllocator_),
+        states_(allocator_, 256),
+        ranges_(allocator_, 256),
+        root_(kRegexInvalidState),
+        stateCount_(),
+        rangeCount_(),
+        anchorBegin_(),
+        anchorEnd_() {
+    GenericStringStream<Encoding> ss(source);
+    DecodedStream<GenericStringStream<Encoding>, Encoding> ds(ss);
+    Parse(ds);
+  }
+
+  ~GenericRegex() { RAPIDJSON_DELETE(ownAllocator_); }
+
+  bool IsValid() const { return root_ != kRegexInvalidState; }
+
+ private:
+  enum Operator {
+    kZeroOrOne,
+    kZeroOrMore,
+    kOneOrMore,
+    kConcatenation,
+    kAlternation,
+    kLeftParenthesis
+  };
+
+  static const unsigned kAnyCharacterClass = 0xFFFFFFFF;  //!< For '.'
+  static const unsigned kRangeCharacterClass = 0xFFFFFFFE;
+  static const unsigned kRangeNegationFlag = 0x80000000;
+
+  struct Range {
+    unsigned start;  //
+    unsigned end;
+    SizeType next;
+  };
+
+  struct State {
+    SizeType out;   //!< Equals to kInvalid for matching state
+    SizeType out1;  //!< Equals to non-kInvalid for split
+    SizeType rangeStart;
+    unsigned codepoint;
+  };
+
+  struct Frag {
+    Frag(SizeType s, SizeType o, SizeType m) : start(s), out(o), minIndex(m) {}
+    SizeType start;
+    SizeType out;  //!< link-list of all output states
+    SizeType minIndex;
+  };
+
+  State &GetState(SizeType index) {
+    RAPIDJSON_ASSERT(index < stateCount_);
+    return states_.template Bottom<State>()[index];
+  }
+
+  const State &GetState(SizeType index) const {
+    RAPIDJSON_ASSERT(index < stateCount_);
+    return states_.template Bottom<State>()[index];
+  }
+
+  Range &GetRange(SizeType index) {
+    RAPIDJSON_ASSERT(index < rangeCount_);
+    return ranges_.template Bottom<Range>()[index];
+  }
+
+  const Range &GetRange(SizeType index) const {
+    RAPIDJSON_ASSERT(index < rangeCount_);
+    return ranges_.template Bottom<Range>()[index];
+  }
+
+  template <typename InputStream>
+  void Parse(DecodedStream<InputStream, Encoding> &ds) {
+    Stack<Allocator> operandStack(allocator_, 256);   // Frag
+    Stack<Allocator> operatorStack(allocator_, 256);  // Operator
+    Stack<Allocator> atomCountStack(allocator_,
+                                    256);  // unsigned (Atom per parenthesis)
+
+    *atomCountStack.template Push<unsigned>() = 0;
+
+    unsigned codepoint;
+    while (ds.Peek() != 0) {
+      switch (codepoint = ds.Take()) {
+        case '^':
+          anchorBegin_ = true;
+          break;
+
+        case '$':
+          anchorEnd_ = true;
+          break;
+
+        case '|':
+          while (!operatorStack.Empty() &&
+                 *operatorStack.template Top<Operator>() < kAlternation)
+            if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
+              return;
+          *operatorStack.template Push<Operator>() = kAlternation;
+          *atomCountStack.template Top<unsigned>() = 0;
+          break;
+
+        case '(':
+          *operatorStack.template Push<Operator>() = kLeftParenthesis;
+          *atomCountStack.template Push<unsigned>() = 0;
+          break;
+
+        case ')':
+          while (!operatorStack.Empty() &&
+                 *operatorStack.template Top<Operator>() != kLeftParenthesis)
+            if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
+              return;
+          if (operatorStack.Empty()) return;
+          operatorStack.template Pop<Operator>(1);
+          atomCountStack.template Pop<unsigned>(1);
+          ImplicitConcatenation(atomCountStack, operatorStack);
+          break;
+
+        case '?':
+          if (!Eval(operandStack, kZeroOrOne)) return;
+          break;
+
+        case '*':
+          if (!Eval(operandStack, kZeroOrMore)) return;
+          break;
+
+        case '+':
+          if (!Eval(operandStack, kOneOrMore)) return;
+          break;
+
+        case '{': {
+          unsigned n, m;
+          if (!ParseUnsigned(ds, &n)) return;
+
+          if (ds.Peek() == ',') {
+            ds.Take();
+            if (ds.Peek() == '}')
+              m = kInfinityQuantifier;
+            else if (!ParseUnsigned(ds, &m) || m < n)
+              return;
+          } else
+            m = n;
+
+          if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}') return;
+          ds.Take();
+        } break;
+
+        case '.':
+          PushOperand(operandStack, kAnyCharacterClass);
+          ImplicitConcatenation(atomCountStack, operatorStack);
+          break;
+
+        case '[': {
+          SizeType range;
+          if (!ParseRange(ds, &range)) return;
+          SizeType s = NewState(kRegexInvalidState, kRegexInvalidState,
+                                kRangeCharacterClass);
+          GetState(s).rangeStart = range;
+          *operandStack.template Push<Frag>() = Frag(s, s, s);
+        }
+          ImplicitConcatenation(atomCountStack, operatorStack);
+          break;
+
+        case '\\':  // Escape character
+          if (!CharacterEscape(ds, &codepoint))
+            return;  // Unsupported escape character
+          // fall through to default
+          RAPIDJSON_DELIBERATE_FALLTHROUGH;
+
+        default:  // Pattern character
+          PushOperand(operandStack, codepoint);
+          ImplicitConcatenation(atomCountStack, operatorStack);
+      }
+    }
+
+    while (!operatorStack.Empty())
+      if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1))) return;
+
+    // Link the operand to matching state.
+    if (operandStack.GetSize() == sizeof(Frag)) {
+      Frag *e = operandStack.template Pop<Frag>(1);
+      Patch(e->out, NewState(kRegexInvalidState, kRegexInvalidState, 0));
+      root_ = e->start;
+
+#if RAPIDJSON_REGEX_VERBOSE
+      printf("root: %d\n", root_);
+      for (SizeType i = 0; i < stateCount_; i++) {
+        State &s = GetState(i);
+        printf("[%2d] out: %2d out1: %2d c: '%c'\n", i, s.out, s.out1,
+               (char)s.codepoint);
+      }
+      printf("\n");
+#endif
+    }
+  }
+
+  SizeType NewState(SizeType out, SizeType out1, unsigned codepoint) {
+    State *s = states_.template Push<State>();
+    s->out = out;
+    s->out1 = out1;
+    s->codepoint = codepoint;
+    s->rangeStart = kRegexInvalidRange;
+    return stateCount_++;
+  }
+
+  void PushOperand(Stack<Allocator> &operandStack, unsigned codepoint) {
+    SizeType s = NewState(kRegexInvalidState, kRegexInvalidState, codepoint);
+    *operandStack.template Push<Frag>() = Frag(s, s, s);
+  }
+
+  void ImplicitConcatenation(Stack<Allocator> &atomCountStack,
+                             Stack<Allocator> &operatorStack) {
+    if (*atomCountStack.template Top<unsigned>())
+      *operatorStack.template Push<Operator>() = kConcatenation;
+    (*atomCountStack.template Top<unsigned>())++;
+  }
+
+  SizeType Append(SizeType l1, SizeType l2) {
+    SizeType old = l1;
+    while (GetState(l1).out != kRegexInvalidState) l1 = GetState(l1).out;
+    GetState(l1).out = l2;
+    return old;
+  }
+
+  void Patch(SizeType l, SizeType s) {
+    for (SizeType next; l != kRegexInvalidState; l = next) {
+      next = GetState(l).out;
+      GetState(l).out = s;
+    }
+  }
+
+  bool Eval(Stack<Allocator> &operandStack, Operator op) {
+    switch (op) {
+      case kConcatenation:
+        RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag) * 2);
+        {
+          Frag e2 = *operandStack.template Pop<Frag>(1);
+          Frag e1 = *operandStack.template Pop<Frag>(1);
+          Patch(e1.out, e2.start);
+          *operandStack.template Push<Frag>() =
+              Frag(e1.start, e2.out, Min(e1.minIndex, e2.minIndex));
+        }
+        return true;
+
+      case kAlternation:
+        if (operandStack.GetSize() >= sizeof(Frag) * 2) {
+          Frag e2 = *operandStack.template Pop<Frag>(1);
+          Frag e1 = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(e1.start, e2.start, 0);
+          *operandStack.template Push<Frag>() =
+              Frag(s, Append(e1.out, e2.out), Min(e1.minIndex, e2.minIndex));
+          return true;
+        }
+        return false;
+
+      case kZeroOrOne:
+        if (operandStack.GetSize() >= sizeof(Frag)) {
+          Frag e = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          *operandStack.template Push<Frag>() =
+              Frag(s, Append(e.out, s), e.minIndex);
+          return true;
+        }
+        return false;
+
+      case kZeroOrMore:
+        if (operandStack.GetSize() >= sizeof(Frag)) {
+          Frag e = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          Patch(e.out, s);
+          *operandStack.template Push<Frag>() = Frag(s, s, e.minIndex);
+          return true;
+        }
+        return false;
+
+      case kOneOrMore:
+        if (operandStack.GetSize() >= sizeof(Frag)) {
+          Frag e = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          Patch(e.out, s);
+          *operandStack.template Push<Frag>() = Frag(e.start, s, e.minIndex);
+          return true;
+        }
+        return false;
+
+      default:
+        // syntax error (e.g. unclosed kLeftParenthesis)
+        return false;
+    }
+  }
+
+  bool EvalQuantifier(Stack<Allocator> &operandStack, unsigned n, unsigned m) {
+    RAPIDJSON_ASSERT(n <= m);
+    RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag));
+
+    if (n == 0) {
+      if (m == 0)  // a{0} not support
+        return false;
+      else if (m == kInfinityQuantifier)
+        Eval(operandStack, kZeroOrMore);  // a{0,} -> a*
+      else {
+        Eval(operandStack, kZeroOrOne);  // a{0,5} -> a?
+        for (unsigned i = 0; i < m - 1; i++)
+          CloneTopOperand(operandStack);  // a{0,5} -> a? a? a? a? a?
+        for (unsigned i = 0; i < m - 1; i++)
+          Eval(operandStack, kConcatenation);  // a{0,5} -> a?a?a?a?a?
+      }
+      return true;
+    }
+
+    for (unsigned i = 0; i < n - 1; i++)  // a{3} -> a a a
+      CloneTopOperand(operandStack);
+
+    if (m == kInfinityQuantifier)
+      Eval(operandStack, kOneOrMore);  // a{3,} -> a a a+
+    else if (m > n) {
+      CloneTopOperand(operandStack);   // a{3,5} -> a a a a
+      Eval(operandStack, kZeroOrOne);  // a{3,5} -> a a a a?
+      for (unsigned i = n; i < m - 1; i++)
+        CloneTopOperand(operandStack);  // a{3,5} -> a a a a? a?
+      for (unsigned i = n; i < m; i++)
+        Eval(operandStack, kConcatenation);  // a{3,5} -> a a aa?a?
+    }
+
+    for (unsigned i = 0; i < n - 1; i++)
+      Eval(operandStack,
+           kConcatenation);  // a{3} -> aaa, a{3,} -> aaa+, a{3.5} -> aaaa?a?
+
+    return true;
+  }
+
+  static SizeType Min(SizeType a, SizeType b) { return a < b ? a : b; }
+
+  void CloneTopOperand(Stack<Allocator> &operandStack) {
+    const Frag src =
+        *operandStack
+             .template Top<Frag>();  // Copy constructor to prevent invalidation
+    SizeType count =
+        stateCount_ - src.minIndex;  // Assumes top operand contains states in
+                                     // [src->minIndex, stateCount_)
+    State *s = states_.template Push<State>(count);
+    memcpy(s, &GetState(src.minIndex), count * sizeof(State));
+    for (SizeType j = 0; j < count; j++) {
+      if (s[j].out != kRegexInvalidState) s[j].out += count;
+      if (s[j].out1 != kRegexInvalidState) s[j].out1 += count;
+    }
+    *operandStack.template Push<Frag>() =
+        Frag(src.start + count, src.out + count, src.minIndex + count);
+    stateCount_ += count;
+  }
+
+  template <typename InputStream>
+  bool ParseUnsigned(DecodedStream<InputStream, Encoding> &ds, unsigned *u) {
+    unsigned r = 0;
+    if (ds.Peek() < '0' || ds.Peek() > '9') return false;
+    while (ds.Peek() >= '0' && ds.Peek() <= '9') {
+      if (r >= 429496729 && ds.Peek() > '5')  // 2^32 - 1 = 4294967295
+        return false;                         // overflow
+      r = r * 10 + (ds.Take() - '0');
+    }
+    *u = r;
+    return true;
+  }
+
+  template <typename InputStream>
+  bool ParseRange(DecodedStream<InputStream, Encoding> &ds, SizeType *range) {
+    bool isBegin = true;
+    bool negate = false;
+    int step = 0;
+    SizeType start = kRegexInvalidRange;
+    SizeType current = kRegexInvalidRange;
+    unsigned codepoint;
+    while ((codepoint = ds.Take()) != 0) {
+      if (isBegin) {
+        isBegin = false;
+        if (codepoint == '^') {
+          negate = true;
+          continue;
+        }
+      }
+
+      switch (codepoint) {
+        case ']':
+          if (start == kRegexInvalidRange)
+            return false;   // Error: nothing inside []
+          if (step == 2) {  // Add trailing '-'
+            SizeType r = NewRange('-');
+            RAPIDJSON_ASSERT(current != kRegexInvalidRange);
+            GetRange(current).next = r;
+          }
+          if (negate) GetRange(start).start |= kRangeNegationFlag;
+          *range = start;
+          return true;
+
+        case '\\':
+          if (ds.Peek() == 'b') {
+            ds.Take();
+            codepoint = 0x0008;  // Escape backspace character
+          } else if (!CharacterEscape(ds, &codepoint))
+            return false;
+          // fall through to default
+          RAPIDJSON_DELIBERATE_FALLTHROUGH;
+
+        default:
+          switch (step) {
+            case 1:
+              if (codepoint == '-') {
+                step++;
+                break;
+              }
+              // fall through to step 0 for other characters
+              RAPIDJSON_DELIBERATE_FALLTHROUGH;
+
+            case 0: {
+              SizeType r = NewRange(codepoint);
+              if (current != kRegexInvalidRange) GetRange(current).next = r;
+              if (start == kRegexInvalidRange) start = r;
+              current = r;
+            }
+              step = 1;
+              break;
+
+            default:
+              RAPIDJSON_ASSERT(step == 2);
+              GetRange(current).end = codepoint;
+              step = 0;
+          }
+      }
+    }
+    return false;
+  }
+
+  SizeType NewRange(unsigned codepoint) {
+    Range *r = ranges_.template Push<Range>();
+    r->start = r->end = codepoint;
+    r->next = kRegexInvalidRange;
+    return rangeCount_++;
+  }
+
+  template <typename InputStream>
+  bool CharacterEscape(DecodedStream<InputStream, Encoding> &ds,
+                       unsigned *escapedCodepoint) {
+    unsigned codepoint;
+    switch (codepoint = ds.Take()) {
+      case '^':
+      case '$':
+      case '|':
+      case '(':
+      case ')':
+      case '?':
+      case '*':
+      case '+':
+      case '.':
+      case '[':
+      case ']':
+      case '{':
+      case '}':
+      case '\\':
+        *escapedCodepoint = codepoint;
+        return true;
+      case 'f':
+        *escapedCodepoint = 0x000C;
+        return true;
+      case 'n':
+        *escapedCodepoint = 0x000A;
+        return true;
+      case 'r':
+        *escapedCodepoint = 0x000D;
+        return true;
+      case 't':
+        *escapedCodepoint = 0x0009;
+        return true;
+      case 'v':
+        *escapedCodepoint = 0x000B;
+        return true;
+      default:
+        return false;  // Unsupported escape character
+    }
+  }
+
+  Allocator *ownAllocator_;
+  Allocator *allocator_;
+  Stack<Allocator> states_;
+  Stack<Allocator> ranges_;
+  SizeType root_;
+  SizeType stateCount_;
+  SizeType rangeCount_;
+
+  static const unsigned kInfinityQuantifier = ~0u;
+
+  // For SearchWithAnchoring()
+  bool anchorBegin_;
+  bool anchorEnd_;
+};
+
+template <typename RegexType, typename Allocator = CrtAllocator>
+class GenericRegexSearch {
+ public:
+  typedef typename RegexType::EncodingType Encoding;
+  typedef typename Encoding::Ch Ch;
+
+  GenericRegexSearch(const RegexType &regex, Allocator *allocator = 0)
+      : regex_(regex),
+        allocator_(allocator),
+        ownAllocator_(0),
+        state0_(allocator, 0),
+        state1_(allocator, 0),
+        stateSet_() {
+    RAPIDJSON_ASSERT(regex_.IsValid());
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+    stateSet_ = static_cast<unsigned *>(allocator_->Malloc(GetStateSetSize()));
+    state0_.template Reserve<SizeType>(regex_.stateCount_);
+    state1_.template Reserve<SizeType>(regex_.stateCount_);
+  }
+
+  ~GenericRegexSearch() {
+    Allocator::Free(stateSet_);
+    RAPIDJSON_DELETE(ownAllocator_);
+  }
+
+  template <typename InputStream>
+  bool Match(InputStream &is) {
+    return SearchWithAnchoring(is, true, true);
+  }
+
+  bool Match(const Ch *s) {
+    GenericStringStream<Encoding> is(s);
+    return Match(is);
+  }
+
+  template <typename InputStream>
+  bool Search(InputStream &is) {
+    return SearchWithAnchoring(is, regex_.anchorBegin_, regex_.anchorEnd_);
+  }
+
+  bool Search(const Ch *s) {
+    GenericStringStream<Encoding> is(s);
+    return Search(is);
+  }
+
+ private:
+  typedef typename RegexType::State State;
+  typedef typename RegexType::Range Range;
+
+  template <typename InputStream>
+  bool SearchWithAnchoring(InputStream &is, bool anchorBegin, bool anchorEnd) {
+    DecodedStream<InputStream, Encoding> ds(is);
+
+    state0_.Clear();
+    Stack<Allocator> *current = &state0_, *next = &state1_;
+    const size_t stateSetSize = GetStateSetSize();
+    std::memset(stateSet_, 0, stateSetSize);
+
+    bool matched = AddState(*current, regex_.root_);
+    unsigned codepoint;
+    while (!current->Empty() && (codepoint = ds.Take()) != 0) {
+      std::memset(stateSet_, 0, stateSetSize);
+      next->Clear();
+      matched = false;
+      for (const SizeType *s = current->template Bottom<SizeType>();
+           s != current->template End<SizeType>(); ++s) {
+        const State &sr = regex_.GetState(*s);
+        if (sr.codepoint == codepoint ||
+            sr.codepoint == RegexType::kAnyCharacterClass ||
+            (sr.codepoint == RegexType::kRangeCharacterClass &&
+             MatchRange(sr.rangeStart, codepoint))) {
+          matched = AddState(*next, sr.out) || matched;
+          if (!anchorEnd && matched) return true;
+        }
+        if (!anchorBegin) AddState(*next, regex_.root_);
+      }
+      internal::Swap(current, next);
+    }
+
+    return matched;
+  }
+
+  size_t GetStateSetSize() const { return (regex_.stateCount_ + 31) / 32 * 4; }
+
+  // Return whether the added states is a match state
+  bool AddState(Stack<Allocator> &l, SizeType index) {
+    RAPIDJSON_ASSERT(index != kRegexInvalidState);
+
+    const State &s = regex_.GetState(index);
+    if (s.out1 != kRegexInvalidState) {  // Split
+      bool matched = AddState(l, s.out);
+      return AddState(l, s.out1) || matched;
+    } else if (!(stateSet_[index >> 5] & (1u << (index & 31)))) {
+      stateSet_[index >> 5] |= (1u << (index & 31));
+      *l.template PushUnsafe<SizeType>() = index;
+    }
+    return s.out ==
+           kRegexInvalidState;  // by using PushUnsafe() above, we can ensure s
+                                // is not validated due to reallocation.
+  }
+
+  bool MatchRange(SizeType rangeIndex, unsigned codepoint) const {
+    bool yes = (regex_.GetRange(rangeIndex).start &
+                RegexType::kRangeNegationFlag) == 0;
+    while (rangeIndex != kRegexInvalidRange) {
+      const Range &r = regex_.GetRange(rangeIndex);
+      if (codepoint >= (r.start & ~RegexType::kRangeNegationFlag) &&
+          codepoint <= r.end)
+        return yes;
+      rangeIndex = r.next;
+    }
+    return !yes;
+  }
+
+  const RegexType &regex_;
+  Allocator *allocator_;
+  Allocator *ownAllocator_;
+  Stack<Allocator> state0_;
+  Stack<Allocator> state1_;
+  uint32_t *stateSet_;
+};
+
+typedef GenericRegex<UTF8<>> Regex;
+typedef GenericRegexSearch<Regex> RegexSearch;
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#if defined(__clang__) || defined(_MSC_VER)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_INTERNAL_REGEX_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/stack.h b/livox_ros_driver2/3rdparty/rapidjson/internal/stack.h
new file mode 100644
index 0000000..bf80503
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/stack.h
@@ -0,0 +1,245 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_INTERNAL_STACK_H_
+#define RAPIDJSON_INTERNAL_STACK_H_
+
+#include <cstddef>
+#include "../allocators.h"
+#include "swap.h"
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+///////////////////////////////////////////////////////////////////////////////
+// Stack
+
+//! A type-unsafe stack for storing different types of data.
+/*! \tparam Allocator Allocator for allocating stack memory.
+ */
+template <typename Allocator>
+class Stack {
+ public:
+  // Optimization note: Do not allocate memory for stack_ in constructor.
+  // Do it lazily when first Push() -> Expand() -> Resize().
+  Stack(Allocator *allocator, size_t stackCapacity)
+      : allocator_(allocator),
+        ownAllocator_(0),
+        stack_(0),
+        stackTop_(0),
+        stackEnd_(0),
+        initialCapacity_(stackCapacity) {}
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  Stack(Stack &&rhs)
+      : allocator_(rhs.allocator_),
+        ownAllocator_(rhs.ownAllocator_),
+        stack_(rhs.stack_),
+        stackTop_(rhs.stackTop_),
+        stackEnd_(rhs.stackEnd_),
+        initialCapacity_(rhs.initialCapacity_) {
+    rhs.allocator_ = 0;
+    rhs.ownAllocator_ = 0;
+    rhs.stack_ = 0;
+    rhs.stackTop_ = 0;
+    rhs.stackEnd_ = 0;
+    rhs.initialCapacity_ = 0;
+  }
+#endif
+
+  ~Stack() { Destroy(); }
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  Stack &operator=(Stack &&rhs) {
+    if (&rhs != this) {
+      Destroy();
+
+      allocator_ = rhs.allocator_;
+      ownAllocator_ = rhs.ownAllocator_;
+      stack_ = rhs.stack_;
+      stackTop_ = rhs.stackTop_;
+      stackEnd_ = rhs.stackEnd_;
+      initialCapacity_ = rhs.initialCapacity_;
+
+      rhs.allocator_ = 0;
+      rhs.ownAllocator_ = 0;
+      rhs.stack_ = 0;
+      rhs.stackTop_ = 0;
+      rhs.stackEnd_ = 0;
+      rhs.initialCapacity_ = 0;
+    }
+    return *this;
+  }
+#endif
+
+  void Swap(Stack &rhs) RAPIDJSON_NOEXCEPT {
+    internal::Swap(allocator_, rhs.allocator_);
+    internal::Swap(ownAllocator_, rhs.ownAllocator_);
+    internal::Swap(stack_, rhs.stack_);
+    internal::Swap(stackTop_, rhs.stackTop_);
+    internal::Swap(stackEnd_, rhs.stackEnd_);
+    internal::Swap(initialCapacity_, rhs.initialCapacity_);
+  }
+
+  void Clear() { stackTop_ = stack_; }
+
+  void ShrinkToFit() {
+    if (Empty()) {
+      // If the stack is empty, completely deallocate the memory.
+      Allocator::Free(stack_);  // NOLINT (+clang-analyzer-unix.Malloc)
+      stack_ = 0;
+      stackTop_ = 0;
+      stackEnd_ = 0;
+    } else
+      Resize(GetSize());
+  }
+
+  // Optimization note: try to minimize the size of this function for force
+  // inline. Expansion is run very infrequently, so it is moved to another
+  // (probably non-inline) function.
+  template <typename T>
+  RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) {
+    // Expand the stack if needed
+    if (RAPIDJSON_UNLIKELY(static_cast<std::ptrdiff_t>(sizeof(T) * count) >
+                           (stackEnd_ - stackTop_)))
+      Expand<T>(count);
+  }
+
+  template <typename T>
+  RAPIDJSON_FORCEINLINE T *Push(size_t count = 1) {
+    Reserve<T>(count);
+    return PushUnsafe<T>(count);
+  }
+
+  template <typename T>
+  RAPIDJSON_FORCEINLINE T *PushUnsafe(size_t count = 1) {
+    RAPIDJSON_ASSERT(stackTop_);
+    RAPIDJSON_ASSERT(static_cast<std::ptrdiff_t>(sizeof(T) * count) <=
+                     (stackEnd_ - stackTop_));
+    T *ret = reinterpret_cast<T *>(stackTop_);
+    stackTop_ += sizeof(T) * count;
+    return ret;
+  }
+
+  template <typename T>
+  T *Pop(size_t count) {
+    RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T));
+    stackTop_ -= count * sizeof(T);
+    return reinterpret_cast<T *>(stackTop_);
+  }
+
+  template <typename T>
+  T *Top() {
+    RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
+    return reinterpret_cast<T *>(stackTop_ - sizeof(T));
+  }
+
+  template <typename T>
+  const T *Top() const {
+    RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
+    return reinterpret_cast<T *>(stackTop_ - sizeof(T));
+  }
+
+  template <typename T>
+  T *End() {
+    return reinterpret_cast<T *>(stackTop_);
+  }
+
+  template <typename T>
+  const T *End() const {
+    return reinterpret_cast<T *>(stackTop_);
+  }
+
+  template <typename T>
+  T *Bottom() {
+    return reinterpret_cast<T *>(stack_);
+  }
+
+  template <typename T>
+  const T *Bottom() const {
+    return reinterpret_cast<T *>(stack_);
+  }
+
+  bool HasAllocator() const { return allocator_ != 0; }
+
+  Allocator &GetAllocator() {
+    RAPIDJSON_ASSERT(allocator_);
+    return *allocator_;
+  }
+
+  bool Empty() const { return stackTop_ == stack_; }
+  size_t GetSize() const { return static_cast<size_t>(stackTop_ - stack_); }
+  size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); }
+
+ private:
+  template <typename T>
+  void Expand(size_t count) {
+    // Only expand the capacity if the current stack exists. Otherwise just
+    // create a stack with initial capacity.
+    size_t newCapacity;
+    if (stack_ == 0) {
+      if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+      newCapacity = initialCapacity_;
+    } else {
+      newCapacity = GetCapacity();
+      newCapacity += (newCapacity + 1) / 2;
+    }
+    size_t newSize = GetSize() + sizeof(T) * count;
+    if (newCapacity < newSize) newCapacity = newSize;
+
+    Resize(newCapacity);
+  }
+
+  void Resize(size_t newCapacity) {
+    const size_t size = GetSize();  // Backup the current size
+    stack_ = static_cast<char *>(
+        allocator_->Realloc(stack_, GetCapacity(), newCapacity));
+    stackTop_ = stack_ + size;
+    stackEnd_ = stack_ + newCapacity;
+  }
+
+  void Destroy() {
+    Allocator::Free(stack_);
+    RAPIDJSON_DELETE(ownAllocator_);  // Only delete if it is owned by the stack
+  }
+
+  // Prohibit copy constructor & assignment operator.
+  Stack(const Stack &);
+  Stack &operator=(const Stack &);
+
+  Allocator *allocator_;
+  Allocator *ownAllocator_;
+  char *stack_;
+  char *stackTop_;
+  char *stackEnd_;
+  size_t initialCapacity_;
+};
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_STACK_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/strfunc.h b/livox_ros_driver2/3rdparty/rapidjson/internal/strfunc.h
new file mode 100644
index 0000000..9024b2f
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/strfunc.h
@@ -0,0 +1,74 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_INTERNAL_STRFUNC_H_
+#define RAPIDJSON_INTERNAL_STRFUNC_H_
+
+#include <cwchar>
+#include "../stream.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+//! Custom strlen() which works on different character types.
+/*! \tparam Ch Character type (e.g. char, wchar_t, short)
+    \param s Null-terminated input string.
+    \return Number of characters in the string.
+    \note This has the same semantics as strlen(), the return value is not
+   number of Unicode codepoints.
+*/
+template <typename Ch>
+inline SizeType StrLen(const Ch *s) {
+  RAPIDJSON_ASSERT(s != 0);
+  const Ch *p = s;
+  while (*p) ++p;
+  return SizeType(p - s);
+}
+
+template <>
+inline SizeType StrLen(const char *s) {
+  return SizeType(std::strlen(s));
+}
+
+template <>
+inline SizeType StrLen(const wchar_t *s) {
+  return SizeType(std::wcslen(s));
+}
+
+//! Returns number of code points in a encoded string.
+template <typename Encoding>
+bool CountStringCodePoint(const typename Encoding::Ch *s, SizeType length,
+                          SizeType *outCount) {
+  RAPIDJSON_ASSERT(s != 0);
+  RAPIDJSON_ASSERT(outCount != 0);
+  GenericStringStream<Encoding> is(s);
+  const typename Encoding::Ch *end = s + length;
+  SizeType count = 0;
+  while (is.src_ < end) {
+    unsigned codepoint;
+    if (!Encoding::Decode(is, &codepoint)) return false;
+    count++;
+  }
+  *outCount = count;
+  return true;
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_INTERNAL_STRFUNC_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/strtod.h b/livox_ros_driver2/3rdparty/rapidjson/internal/strtod.h
new file mode 100644
index 0000000..ea7ae43
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/strtod.h
@@ -0,0 +1,303 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_STRTOD_
+#define RAPIDJSON_STRTOD_
+
+#include <climits>
+#include <limits>
+#include "biginteger.h"
+#include "diyfp.h"
+#include "ieee754.h"
+#include "pow10.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+inline double FastPath(double significand, int exp) {
+  if (exp < -308)
+    return 0.0;
+  else if (exp >= 0)
+    return significand * internal::Pow10(exp);
+  else
+    return significand / internal::Pow10(-exp);
+}
+
+inline double StrtodNormalPrecision(double d, int p) {
+  if (p < -308) {
+    // Prevent expSum < -308, making Pow10(p) = 0
+    d = FastPath(d, -308);
+    d = FastPath(d, p + 308);
+  } else
+    d = FastPath(d, p);
+  return d;
+}
+
+template <typename T>
+inline T Min3(T a, T b, T c) {
+  T m = a;
+  if (m > b) m = b;
+  if (m > c) m = c;
+  return m;
+}
+
+inline int CheckWithinHalfULP(double b, const BigInteger &d, int dExp) {
+  const Double db(b);
+  const uint64_t bInt = db.IntegerSignificand();
+  const int bExp = db.IntegerExponent();
+  const int hExp = bExp - 1;
+
+  int dS_Exp2 = 0, dS_Exp5 = 0, bS_Exp2 = 0, bS_Exp5 = 0, hS_Exp2 = 0,
+      hS_Exp5 = 0;
+
+  // Adjust for decimal exponent
+  if (dExp >= 0) {
+    dS_Exp2 += dExp;
+    dS_Exp5 += dExp;
+  } else {
+    bS_Exp2 -= dExp;
+    bS_Exp5 -= dExp;
+    hS_Exp2 -= dExp;
+    hS_Exp5 -= dExp;
+  }
+
+  // Adjust for binary exponent
+  if (bExp >= 0)
+    bS_Exp2 += bExp;
+  else {
+    dS_Exp2 -= bExp;
+    hS_Exp2 -= bExp;
+  }
+
+  // Adjust for half ulp exponent
+  if (hExp >= 0)
+    hS_Exp2 += hExp;
+  else {
+    dS_Exp2 -= hExp;
+    bS_Exp2 -= hExp;
+  }
+
+  // Remove common power of two factor from all three scaled values
+  int common_Exp2 = Min3(dS_Exp2, bS_Exp2, hS_Exp2);
+  dS_Exp2 -= common_Exp2;
+  bS_Exp2 -= common_Exp2;
+  hS_Exp2 -= common_Exp2;
+
+  BigInteger dS = d;
+  dS.MultiplyPow5(static_cast<unsigned>(dS_Exp5)) <<=
+      static_cast<unsigned>(dS_Exp2);
+
+  BigInteger bS(bInt);
+  bS.MultiplyPow5(static_cast<unsigned>(bS_Exp5)) <<=
+      static_cast<unsigned>(bS_Exp2);
+
+  BigInteger hS(1);
+  hS.MultiplyPow5(static_cast<unsigned>(hS_Exp5)) <<=
+      static_cast<unsigned>(hS_Exp2);
+
+  BigInteger delta(0);
+  dS.Difference(bS, &delta);
+
+  return delta.Compare(hS);
+}
+
+inline bool StrtodFast(double d, int p, double *result) {
+  // Use fast path for string-to-double conversion if possible
+  // see
+  // http://www.exploringbinary.com/fast-path-decimal-to-floating-point-conversion/
+  if (p > 22 && p < 22 + 16) {
+    // Fast Path Cases In Disguise
+    d *= internal::Pow10(p - 22);
+    p = 22;
+  }
+
+  if (p >= -22 && p <= 22 && d <= 9007199254740991.0) {  // 2^53 - 1
+    *result = FastPath(d, p);
+    return true;
+  } else
+    return false;
+}
+
+// Compute an approximation and see if it is within 1/2 ULP
+inline bool StrtodDiyFp(const char *decimals, int dLen, int dExp,
+                        double *result) {
+  uint64_t significand = 0;
+  int i = 0;  // 2^64 - 1 = 18446744073709551615, 1844674407370955161 =
+              // 0x1999999999999999
+  for (; i < dLen; i++) {
+    if (significand > RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) ||
+        (significand == RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) &&
+         decimals[i] > '5'))
+      break;
+    significand = significand * 10u + static_cast<unsigned>(decimals[i] - '0');
+  }
+
+  if (i < dLen && decimals[i] >= '5')  // Rounding
+    significand++;
+
+  int remaining = dLen - i;
+  const int kUlpShift = 3;
+  const int kUlp = 1 << kUlpShift;
+  int64_t error = (remaining == 0) ? 0 : kUlp / 2;
+
+  DiyFp v(significand, 0);
+  v = v.Normalize();
+  error <<= -v.e;
+
+  dExp += remaining;
+
+  int actualExp;
+  DiyFp cachedPower = GetCachedPower10(dExp, &actualExp);
+  if (actualExp != dExp) {
+    static const DiyFp kPow10[] = {
+        DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60),  // 10^1
+        DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57),  // 10^2
+        DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54),  // 10^3
+        DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50),  // 10^4
+        DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47),  // 10^5
+        DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44),  // 10^6
+        DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40)   // 10^7
+    };
+    int adjustment = dExp - actualExp;
+    RAPIDJSON_ASSERT(adjustment >= 1 && adjustment < 8);
+    v = v * kPow10[adjustment - 1];
+    if (dLen + adjustment >
+        19)  // has more digits than decimal digits in 64-bit
+      error += kUlp / 2;
+  }
+
+  v = v * cachedPower;
+
+  error += kUlp + (error == 0 ? 0 : 1);
+
+  const int oldExp = v.e;
+  v = v.Normalize();
+  error <<= oldExp - v.e;
+
+  const int effectiveSignificandSize =
+      Double::EffectiveSignificandSize(64 + v.e);
+  int precisionSize = 64 - effectiveSignificandSize;
+  if (precisionSize + kUlpShift >= 64) {
+    int scaleExp = (precisionSize + kUlpShift) - 63;
+    v.f >>= scaleExp;
+    v.e += scaleExp;
+    error = (error >> scaleExp) + 1 + kUlp;
+    precisionSize -= scaleExp;
+  }
+
+  DiyFp rounded(v.f >> precisionSize, v.e + precisionSize);
+  const uint64_t precisionBits =
+      (v.f & ((uint64_t(1) << precisionSize) - 1)) * kUlp;
+  const uint64_t halfWay = (uint64_t(1) << (precisionSize - 1)) * kUlp;
+  if (precisionBits >= halfWay + static_cast<unsigned>(error)) {
+    rounded.f++;
+    if (rounded.f & (DiyFp::kDpHiddenBit
+                     << 1)) {  // rounding overflows mantissa (issue #340)
+      rounded.f >>= 1;
+      rounded.e++;
+    }
+  }
+
+  *result = rounded.ToDouble();
+
+  return halfWay - static_cast<unsigned>(error) >= precisionBits ||
+         precisionBits >= halfWay + static_cast<unsigned>(error);
+}
+
+inline double StrtodBigInteger(double approx, const char *decimals, int dLen,
+                               int dExp) {
+  RAPIDJSON_ASSERT(dLen >= 0);
+  const BigInteger dInt(decimals, static_cast<unsigned>(dLen));
+  Double a(approx);
+  int cmp = CheckWithinHalfULP(a.Value(), dInt, dExp);
+  if (cmp < 0)
+    return a.Value();  // within half ULP
+  else if (cmp == 0) {
+    // Round towards even
+    if (a.Significand() & 1)
+      return a.NextPositiveDouble();
+    else
+      return a.Value();
+  } else  // adjustment
+    return a.NextPositiveDouble();
+}
+
+inline double StrtodFullPrecision(double d, int p, const char *decimals,
+                                  size_t length, size_t decimalPosition,
+                                  int exp) {
+  RAPIDJSON_ASSERT(d >= 0.0);
+  RAPIDJSON_ASSERT(length >= 1);
+
+  double result = 0.0;
+  if (StrtodFast(d, p, &result)) return result;
+
+  RAPIDJSON_ASSERT(length <= INT_MAX);
+  int dLen = static_cast<int>(length);
+
+  RAPIDJSON_ASSERT(length >= decimalPosition);
+  RAPIDJSON_ASSERT(length - decimalPosition <= INT_MAX);
+  int dExpAdjust = static_cast<int>(length - decimalPosition);
+
+  RAPIDJSON_ASSERT(exp >= INT_MIN + dExpAdjust);
+  int dExp = exp - dExpAdjust;
+
+  // Make sure length+dExp does not overflow
+  RAPIDJSON_ASSERT(dExp <= INT_MAX - dLen);
+
+  // Trim leading zeros
+  while (dLen > 0 && *decimals == '0') {
+    dLen--;
+    decimals++;
+  }
+
+  // Trim trailing zeros
+  while (dLen > 0 && decimals[dLen - 1] == '0') {
+    dLen--;
+    dExp++;
+  }
+
+  if (dLen == 0) {  // Buffer only contains zeros.
+    return 0.0;
+  }
+
+  // Trim right-most digits
+  const int kMaxDecimalDigit = 767 + 1;
+  if (dLen > kMaxDecimalDigit) {
+    dExp += dLen - kMaxDecimalDigit;
+    dLen = kMaxDecimalDigit;
+  }
+
+  // If too small, underflow to zero.
+  // Any x <= 10^-324 is interpreted as zero.
+  if (dLen + dExp <= -324) return 0.0;
+
+  // If too large, overflow to infinity.
+  // Any x >= 10^309 is interpreted as +infinity.
+  if (dLen + dExp > 309) return std::numeric_limits<double>::infinity();
+
+  if (StrtodDiyFp(decimals, dLen, dExp, &result)) return result;
+
+  // Use approximation from StrtodDiyFp and make adjustment with BigInteger
+  // comparison
+  return StrtodBigInteger(result, decimals, dLen, dExp);
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_STRTOD_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/internal/swap.h b/livox_ros_driver2/3rdparty/rapidjson/internal/swap.h
new file mode 100644
index 0000000..db41b18
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/internal/swap.h
@@ -0,0 +1,50 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_INTERNAL_SWAP_H_
+#define RAPIDJSON_INTERNAL_SWAP_H_
+
+#include "../rapidjson.h"
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+//! Custom swap() to avoid dependency on C++ <algorithm> header
+/*! \tparam T Type of the arguments to swap, should be instantiated with
+   primitive C++ types only. \note This has the same semantics as std::swap().
+*/
+template <typename T>
+inline void Swap(T &a, T &b) RAPIDJSON_NOEXCEPT {
+  T tmp = a;
+  a = b;
+  b = tmp;
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_INTERNAL_SWAP_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/istreamwrapper.h b/livox_ros_driver2/3rdparty/rapidjson/istreamwrapper.h
new file mode 100644
index 0000000..b9663b0
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/istreamwrapper.h
@@ -0,0 +1,161 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_ISTREAMWRAPPER_H_
+#define RAPIDJSON_ISTREAMWRAPPER_H_
+
+#include <ios>
+#include <iosfwd>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4351)  // new behavior: elements of array 'array' will be
+                          // default initialized
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Wrapper of \c std::basic_istream into RapidJSON's Stream concept.
+/*!
+    The classes can be wrapped including but not limited to:
+
+    - \c std::istringstream
+    - \c std::stringstream
+    - \c std::wistringstream
+    - \c std::wstringstream
+    - \c std::ifstream
+    - \c std::fstream
+    - \c std::wifstream
+    - \c std::wfstream
+
+    \tparam StreamType Class derived from \c std::basic_istream.
+*/
+
+template <typename StreamType>
+class BasicIStreamWrapper {
+ public:
+  typedef typename StreamType::char_type Ch;
+
+  //! Constructor.
+  /*!
+      \param stream stream opened for read.
+  */
+  BasicIStreamWrapper(StreamType &stream)
+      : stream_(stream),
+        buffer_(peekBuffer_),
+        bufferSize_(4),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
+    Read();
+  }
+
+  //! Constructor.
+  /*!
+      \param stream stream opened for read.
+      \param buffer user-supplied buffer.
+      \param bufferSize size of buffer in bytes. Must >=4 bytes.
+  */
+  BasicIStreamWrapper(StreamType &stream, char *buffer, size_t bufferSize)
+      : stream_(stream),
+        buffer_(buffer),
+        bufferSize_(bufferSize),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
+    RAPIDJSON_ASSERT(bufferSize >= 4);
+    Read();
+  }
+
+  Ch Peek() const { return *current_; }
+  Ch Take() {
+    Ch c = *current_;
+    Read();
+    return c;
+  }
+  size_t Tell() const {
+    return count_ + static_cast<size_t>(current_ - buffer_);
+  }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+  // For encoding detection only.
+  const Ch *Peek4() const {
+    return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
+  }
+
+ private:
+  BasicIStreamWrapper();
+  BasicIStreamWrapper(const BasicIStreamWrapper &);
+  BasicIStreamWrapper &operator=(const BasicIStreamWrapper &);
+
+  void Read() {
+    if (current_ < bufferLast_)
+      ++current_;
+    else if (!eof_) {
+      count_ += readCount_;
+      readCount_ = bufferSize_;
+      bufferLast_ = buffer_ + readCount_ - 1;
+      current_ = buffer_;
+
+      if (!stream_.read(buffer_, static_cast<std::streamsize>(bufferSize_))) {
+        readCount_ = static_cast<size_t>(stream_.gcount());
+        *(bufferLast_ = buffer_ + readCount_) = '\0';
+        eof_ = true;
+      }
+    }
+  }
+
+  StreamType &stream_;
+  Ch peekBuffer_[4], *buffer_;
+  size_t bufferSize_;
+  Ch *bufferLast_;
+  Ch *current_;
+  size_t readCount_;
+  size_t count_;  //!< Number of characters read
+  bool eof_;
+};
+
+typedef BasicIStreamWrapper<std::istream> IStreamWrapper;
+typedef BasicIStreamWrapper<std::wistream> WIStreamWrapper;
+
+#if defined(__clang__) || defined(_MSC_VER)
+RAPIDJSON_DIAG_POP
+#endif
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_ISTREAMWRAPPER_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/license.txt b/livox_ros_driver2/3rdparty/rapidjson/license.txt
new file mode 100644
index 0000000..7ccc161
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/license.txt
@@ -0,0 +1,57 @@
+Tencent is pleased to support the open source community by making RapidJSON available. 
+ 
+Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip.  All rights reserved.
+
+If you have downloaded a copy of the RapidJSON binary from Tencent, please note that the RapidJSON binary is licensed under the MIT License.
+If you have downloaded a copy of the RapidJSON source code from Tencent, please note that RapidJSON source code is licensed under the MIT License, except for the third-party components listed below which are subject to different license terms.  Your integration of RapidJSON into your own projects may require compliance with the MIT License, as well as the other licenses applicable to the third-party components included within RapidJSON. To avoid the problematic JSON license in your own projects, it's sufficient to exclude the bin/jsonchecker/ directory, as it's the only code under the JSON license.
+A copy of the MIT License is included in this file.
+
+Other dependencies and licenses:
+
+Open Source Software Licensed Under the BSD License:
+--------------------------------------------------------------------
+
+The msinttypes r29 
+Copyright (c) 2006-2013 Alexander Chemeris 
+All rights reserved.
+
+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  copyright holder 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 REGENTS 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 REGENTS AND 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.
+
+Open Source Software Licensed Under the JSON License:
+--------------------------------------------------------------------
+
+json.org 
+Copyright (c) 2002 JSON.org
+All Rights Reserved.
+
+JSON_checker
+Copyright (c) 2002 JSON.org
+All Rights Reserved.
+
+	
+Terms of the JSON License:
+---------------------------------------------------
+
+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 shall be used for Good, not Evil.
+
+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.
+
+
+Terms of the MIT License:
+--------------------------------------------------------------------
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
diff --git a/livox_ros_driver2/3rdparty/rapidjson/memorybuffer.h b/livox_ros_driver2/3rdparty/rapidjson/memorybuffer.h
new file mode 100644
index 0000000..a827d6a
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/memorybuffer.h
@@ -0,0 +1,78 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_MEMORYBUFFER_H_
+#define RAPIDJSON_MEMORYBUFFER_H_
+
+#include "internal/stack.h"
+#include "stream.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Represents an in-memory output byte stream.
+/*!
+    This class is mainly for being wrapped by EncodedOutputStream or
+   AutoUTFOutputStream.
+
+    It is similar to FileWriteBuffer but the destination is an in-memory buffer
+   instead of a file.
+
+    Differences between MemoryBuffer and StringBuffer:
+    1. StringBuffer has Encoding but MemoryBuffer is only a byte buffer.
+    2. StringBuffer::GetString() returns a null-terminated string.
+   MemoryBuffer::GetBuffer() returns a buffer without terminator.
+
+    \tparam Allocator type for allocating memory buffer.
+    \note implements Stream concept
+*/
+template <typename Allocator = CrtAllocator>
+struct GenericMemoryBuffer {
+  typedef char Ch;  // byte
+
+  GenericMemoryBuffer(Allocator *allocator = 0,
+                      size_t capacity = kDefaultCapacity)
+      : stack_(allocator, capacity) {}
+
+  void Put(Ch c) { *stack_.template Push<Ch>() = c; }
+  void Flush() {}
+
+  void Clear() { stack_.Clear(); }
+  void ShrinkToFit() { stack_.ShrinkToFit(); }
+  Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
+  void Pop(size_t count) { stack_.template Pop<Ch>(count); }
+
+  const Ch *GetBuffer() const { return stack_.template Bottom<Ch>(); }
+
+  size_t GetSize() const { return stack_.GetSize(); }
+
+  static const size_t kDefaultCapacity = 256;
+  mutable internal::Stack<Allocator> stack_;
+};
+
+typedef GenericMemoryBuffer<> MemoryBuffer;
+
+//! Implement specialized version of PutN() with memset() for better
+//! performance.
+template <>
+inline void PutN(MemoryBuffer &memoryBuffer, char c, size_t n) {
+  std::memset(memoryBuffer.stack_.Push<char>(n), c, n * sizeof(c));
+}
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_MEMORYBUFFER_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/memorystream.h b/livox_ros_driver2/3rdparty/rapidjson/memorystream.h
new file mode 100644
index 0000000..f542ece
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/memorystream.h
@@ -0,0 +1,84 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_MEMORYSTREAM_H_
+#define RAPIDJSON_MEMORYSTREAM_H_
+
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(unreachable - code)
+RAPIDJSON_DIAG_OFF(missing - noreturn)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Represents an in-memory input byte stream.
+/*!
+    This class is mainly for being wrapped by EncodedInputStream or
+   AutoUTFInputStream.
+
+    It is similar to FileReadBuffer but the source is an in-memory buffer
+   instead of a file.
+
+    Differences between MemoryStream and StringStream:
+    1. StringStream has encoding but MemoryStream is a byte stream.
+    2. MemoryStream needs size of the source buffer and the buffer don't need to
+   be null terminated. StringStream assume null-terminated string as source.
+    3. MemoryStream supports Peek4() for encoding detection. StringStream is
+   specified with an encoding so it should not have Peek4(). \note implements
+   Stream concept
+*/
+struct MemoryStream {
+  typedef char Ch;  // byte
+
+  MemoryStream(const Ch *src, size_t size)
+      : src_(src), begin_(src), end_(src + size), size_(size) {}
+
+  Ch Peek() const { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_; }
+  Ch Take() { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_++; }
+  size_t Tell() const { return static_cast<size_t>(src_ - begin_); }
+
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+  // For encoding detection only.
+  const Ch *Peek4() const { return Tell() + 4 <= size_ ? src_ : 0; }
+
+  const Ch *src_;    //!< Current read position.
+  const Ch *begin_;  //!< Original head of the string.
+  const Ch *end_;    //!< End of stream.
+  size_t size_;      //!< Size of the stream.
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_MEMORYBUFFER_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/msinttypes/inttypes.h b/livox_ros_driver2/3rdparty/rapidjson/msinttypes/inttypes.h
new file mode 100644
index 0000000..bc32dad
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/msinttypes/inttypes.h
@@ -0,0 +1,316 @@
+// ISO C9x  compliant inttypes.h for Microsoft Visual Studio
+// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
+//
+//  Copyright (c) 2006-2013 Alexander Chemeris
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//   1. Redistributions of source code must retain the above copyright notice,
+//      this list of conditions and the following disclaimer.
+//
+//   2. 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.
+//
+//   3. Neither the name of the product 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 AUTHOR ``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 AUTHOR 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.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+// The above software in this distribution may have been modified by
+// THL A29 Limited ("Tencent Modifications").
+// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
+
+#ifndef _MSC_VER  // [
+#error "Use this header only with Microsoft Visual C++ compilers!"
+#endif  // _MSC_VER ]
+
+#ifndef _MSC_INTTYPES_H_  // [
+#define _MSC_INTTYPES_H_
+
+#if _MSC_VER > 1000
+#pragma once
+#endif
+
+#include "stdint.h"
+
+// miloyip: VC supports inttypes.h since VC2013
+#if _MSC_VER >= 1800
+#include <inttypes.h>
+#else
+
+// 7.8 Format conversion of integer types
+
+typedef struct {
+  intmax_t quot;
+  intmax_t rem;
+} imaxdiv_t;
+
+// 7.8.1 Macros for format specifiers
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_FORMAT_MACROS)  // [   See footnote 185 at page 198
+
+// The fprintf macros for signed integers are:
+#define PRId8 "d"
+#define PRIi8 "i"
+#define PRIdLEAST8 "d"
+#define PRIiLEAST8 "i"
+#define PRIdFAST8 "d"
+#define PRIiFAST8 "i"
+
+#define PRId16 "hd"
+#define PRIi16 "hi"
+#define PRIdLEAST16 "hd"
+#define PRIiLEAST16 "hi"
+#define PRIdFAST16 "hd"
+#define PRIiFAST16 "hi"
+
+#define PRId32 "I32d"
+#define PRIi32 "I32i"
+#define PRIdLEAST32 "I32d"
+#define PRIiLEAST32 "I32i"
+#define PRIdFAST32 "I32d"
+#define PRIiFAST32 "I32i"
+
+#define PRId64 "I64d"
+#define PRIi64 "I64i"
+#define PRIdLEAST64 "I64d"
+#define PRIiLEAST64 "I64i"
+#define PRIdFAST64 "I64d"
+#define PRIiFAST64 "I64i"
+
+#define PRIdMAX "I64d"
+#define PRIiMAX "I64i"
+
+#define PRIdPTR "Id"
+#define PRIiPTR "Ii"
+
+// The fprintf macros for unsigned integers are:
+#define PRIo8 "o"
+#define PRIu8 "u"
+#define PRIx8 "x"
+#define PRIX8 "X"
+#define PRIoLEAST8 "o"
+#define PRIuLEAST8 "u"
+#define PRIxLEAST8 "x"
+#define PRIXLEAST8 "X"
+#define PRIoFAST8 "o"
+#define PRIuFAST8 "u"
+#define PRIxFAST8 "x"
+#define PRIXFAST8 "X"
+
+#define PRIo16 "ho"
+#define PRIu16 "hu"
+#define PRIx16 "hx"
+#define PRIX16 "hX"
+#define PRIoLEAST16 "ho"
+#define PRIuLEAST16 "hu"
+#define PRIxLEAST16 "hx"
+#define PRIXLEAST16 "hX"
+#define PRIoFAST16 "ho"
+#define PRIuFAST16 "hu"
+#define PRIxFAST16 "hx"
+#define PRIXFAST16 "hX"
+
+#define PRIo32 "I32o"
+#define PRIu32 "I32u"
+#define PRIx32 "I32x"
+#define PRIX32 "I32X"
+#define PRIoLEAST32 "I32o"
+#define PRIuLEAST32 "I32u"
+#define PRIxLEAST32 "I32x"
+#define PRIXLEAST32 "I32X"
+#define PRIoFAST32 "I32o"
+#define PRIuFAST32 "I32u"
+#define PRIxFAST32 "I32x"
+#define PRIXFAST32 "I32X"
+
+#define PRIo64 "I64o"
+#define PRIu64 "I64u"
+#define PRIx64 "I64x"
+#define PRIX64 "I64X"
+#define PRIoLEAST64 "I64o"
+#define PRIuLEAST64 "I64u"
+#define PRIxLEAST64 "I64x"
+#define PRIXLEAST64 "I64X"
+#define PRIoFAST64 "I64o"
+#define PRIuFAST64 "I64u"
+#define PRIxFAST64 "I64x"
+#define PRIXFAST64 "I64X"
+
+#define PRIoMAX "I64o"
+#define PRIuMAX "I64u"
+#define PRIxMAX "I64x"
+#define PRIXMAX "I64X"
+
+#define PRIoPTR "Io"
+#define PRIuPTR "Iu"
+#define PRIxPTR "Ix"
+#define PRIXPTR "IX"
+
+// The fscanf macros for signed integers are:
+#define SCNd8 "d"
+#define SCNi8 "i"
+#define SCNdLEAST8 "d"
+#define SCNiLEAST8 "i"
+#define SCNdFAST8 "d"
+#define SCNiFAST8 "i"
+
+#define SCNd16 "hd"
+#define SCNi16 "hi"
+#define SCNdLEAST16 "hd"
+#define SCNiLEAST16 "hi"
+#define SCNdFAST16 "hd"
+#define SCNiFAST16 "hi"
+
+#define SCNd32 "ld"
+#define SCNi32 "li"
+#define SCNdLEAST32 "ld"
+#define SCNiLEAST32 "li"
+#define SCNdFAST32 "ld"
+#define SCNiFAST32 "li"
+
+#define SCNd64 "I64d"
+#define SCNi64 "I64i"
+#define SCNdLEAST64 "I64d"
+#define SCNiLEAST64 "I64i"
+#define SCNdFAST64 "I64d"
+#define SCNiFAST64 "I64i"
+
+#define SCNdMAX "I64d"
+#define SCNiMAX "I64i"
+
+#ifdef _WIN64  // [
+#define SCNdPTR "I64d"
+#define SCNiPTR "I64i"
+#else  // _WIN64 ][
+#define SCNdPTR "ld"
+#define SCNiPTR "li"
+#endif  // _WIN64 ]
+
+// The fscanf macros for unsigned integers are:
+#define SCNo8 "o"
+#define SCNu8 "u"
+#define SCNx8 "x"
+#define SCNX8 "X"
+#define SCNoLEAST8 "o"
+#define SCNuLEAST8 "u"
+#define SCNxLEAST8 "x"
+#define SCNXLEAST8 "X"
+#define SCNoFAST8 "o"
+#define SCNuFAST8 "u"
+#define SCNxFAST8 "x"
+#define SCNXFAST8 "X"
+
+#define SCNo16 "ho"
+#define SCNu16 "hu"
+#define SCNx16 "hx"
+#define SCNX16 "hX"
+#define SCNoLEAST16 "ho"
+#define SCNuLEAST16 "hu"
+#define SCNxLEAST16 "hx"
+#define SCNXLEAST16 "hX"
+#define SCNoFAST16 "ho"
+#define SCNuFAST16 "hu"
+#define SCNxFAST16 "hx"
+#define SCNXFAST16 "hX"
+
+#define SCNo32 "lo"
+#define SCNu32 "lu"
+#define SCNx32 "lx"
+#define SCNX32 "lX"
+#define SCNoLEAST32 "lo"
+#define SCNuLEAST32 "lu"
+#define SCNxLEAST32 "lx"
+#define SCNXLEAST32 "lX"
+#define SCNoFAST32 "lo"
+#define SCNuFAST32 "lu"
+#define SCNxFAST32 "lx"
+#define SCNXFAST32 "lX"
+
+#define SCNo64 "I64o"
+#define SCNu64 "I64u"
+#define SCNx64 "I64x"
+#define SCNX64 "I64X"
+#define SCNoLEAST64 "I64o"
+#define SCNuLEAST64 "I64u"
+#define SCNxLEAST64 "I64x"
+#define SCNXLEAST64 "I64X"
+#define SCNoFAST64 "I64o"
+#define SCNuFAST64 "I64u"
+#define SCNxFAST64 "I64x"
+#define SCNXFAST64 "I64X"
+
+#define SCNoMAX "I64o"
+#define SCNuMAX "I64u"
+#define SCNxMAX "I64x"
+#define SCNXMAX "I64X"
+
+#ifdef _WIN64  // [
+#define SCNoPTR "I64o"
+#define SCNuPTR "I64u"
+#define SCNxPTR "I64x"
+#define SCNXPTR "I64X"
+#else  // _WIN64 ][
+#define SCNoPTR "lo"
+#define SCNuPTR "lu"
+#define SCNxPTR "lx"
+#define SCNXPTR "lX"
+#endif  // _WIN64 ]
+
+#endif  // __STDC_FORMAT_MACROS ]
+
+// 7.8.2 Functions for greatest-width integer types
+
+// 7.8.2.1 The imaxabs function
+#define imaxabs _abs64
+
+// 7.8.2.2 The imaxdiv function
+
+// This is modified version of div() function from Microsoft's div.c found
+// in %MSVC.NET%\crt\src\div.c
+#ifdef STATIC_IMAXDIV  // [
+static
+#else                  // STATIC_IMAXDIV ][
+_inline
+#endif                 // STATIC_IMAXDIV ]
+    imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) {
+  imaxdiv_t result;
+
+  result.quot = numer / denom;
+  result.rem = numer % denom;
+
+  if (numer < 0 && result.rem > 0) {
+    // did division wrong; must fix up
+    ++result.quot;
+    result.rem -= denom;
+  }
+
+  return result;
+}
+
+// 7.8.2.3 The strtoimax and strtoumax functions
+#define strtoimax _strtoi64
+#define strtoumax _strtoui64
+
+// 7.8.2.4 The wcstoimax and wcstoumax functions
+#define wcstoimax _wcstoi64
+#define wcstoumax _wcstoui64
+
+#endif  // _MSC_VER >= 1800
+
+#endif  // _MSC_INTTYPES_H_ ]
diff --git a/livox_ros_driver2/3rdparty/rapidjson/msinttypes/stdint.h b/livox_ros_driver2/3rdparty/rapidjson/msinttypes/stdint.h
new file mode 100644
index 0000000..8fd0655
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/msinttypes/stdint.h
@@ -0,0 +1,301 @@
+// ISO C9x  compliant stdint.h for Microsoft Visual Studio
+// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
+//
+//  Copyright (c) 2006-2013 Alexander Chemeris
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//   1. Redistributions of source code must retain the above copyright notice,
+//      this list of conditions and the following disclaimer.
+//
+//   2. 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.
+//
+//   3. Neither the name of the product 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 AUTHOR ``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 AUTHOR 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.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+// The above software in this distribution may have been modified by
+// THL A29 Limited ("Tencent Modifications").
+// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
+
+#ifndef _MSC_VER  // [
+#error "Use this header only with Microsoft Visual C++ compilers!"
+#endif  // _MSC_VER ]
+
+#ifndef _MSC_STDINT_H_  // [
+#define _MSC_STDINT_H_
+
+#if _MSC_VER > 1000
+#pragma once
+#endif
+
+// miloyip: Originally Visual Studio 2010 uses its own stdint.h. However it
+// generates warning with INT64_C(), so change to use this file for vs2010.
+#if _MSC_VER >= 1600  // [
+#include <stdint.h>
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_CONSTANT_MACROS)  // [   See footnote 224 at page 260
+
+#undef INT8_C
+#undef INT16_C
+#undef INT32_C
+#undef INT64_C
+#undef UINT8_C
+#undef UINT16_C
+#undef UINT32_C
+#undef UINT64_C
+
+// 7.18.4.1 Macros for minimum-width integer constants
+
+#define INT8_C(val) val##i8
+#define INT16_C(val) val##i16
+#define INT32_C(val) val##i32
+#define INT64_C(val) val##i64
+
+#define UINT8_C(val) val##ui8
+#define UINT16_C(val) val##ui16
+#define UINT32_C(val) val##ui32
+#define UINT64_C(val) val##ui64
+
+// 7.18.4.2 Macros for greatest-width integer constants
+// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
+// Check out Issue 9 for the details.
+#ifndef INTMAX_C  //   [
+#define INTMAX_C INT64_C
+#endif             // INTMAX_C    ]
+#ifndef UINTMAX_C  //  [
+#define UINTMAX_C UINT64_C
+#endif  // UINTMAX_C   ]
+
+#endif  // __STDC_CONSTANT_MACROS ]
+
+#else  // ] _MSC_VER >= 1700 [
+
+#include <limits.h>
+
+// For Visual Studio 6 in C++ mode and for many Visual Studio versions when
+// compiling for ARM we have to wrap <wchar.h> include with 'extern "C++" {}'
+// or compiler would give many errors like this:
+//   error C2733: second C linkage of overloaded function 'wmemchr' not allowed
+#if defined(__cplusplus) && !defined(_M_ARM)
+extern "C" {
+#endif
+#include <wchar.h>
+#if defined(__cplusplus) && !defined(_M_ARM)
+}
+#endif
+
+// Define _W64 macros to mark types changing their size, like intptr_t.
+#ifndef _W64
+#if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300
+#define _W64 __w64
+#else
+#define _W64
+#endif
+#endif
+
+// 7.18.1 Integer types
+
+// 7.18.1.1 Exact-width integer types
+
+// Visual Studio 6 and Embedded Visual C++ 4 doesn't
+// realize that, e.g. char has the same size as __int8
+// so we give up on __intX for them.
+#if (_MSC_VER < 1300)
+typedef signed char int8_t;
+typedef signed short int16_t;
+typedef signed int int32_t;
+typedef unsigned char uint8_t;
+typedef unsigned short uint16_t;
+typedef unsigned int uint32_t;
+#else
+typedef signed __int8 int8_t;
+typedef signed __int16 int16_t;
+typedef signed __int32 int32_t;
+typedef unsigned __int8 uint8_t;
+typedef unsigned __int16 uint16_t;
+typedef unsigned __int32 uint32_t;
+#endif
+typedef signed __int64 int64_t;
+typedef unsigned __int64 uint64_t;
+
+// 7.18.1.2 Minimum-width integer types
+typedef int8_t int_least8_t;
+typedef int16_t int_least16_t;
+typedef int32_t int_least32_t;
+typedef int64_t int_least64_t;
+typedef uint8_t uint_least8_t;
+typedef uint16_t uint_least16_t;
+typedef uint32_t uint_least32_t;
+typedef uint64_t uint_least64_t;
+
+// 7.18.1.3 Fastest minimum-width integer types
+typedef int8_t int_fast8_t;
+typedef int16_t int_fast16_t;
+typedef int32_t int_fast32_t;
+typedef int64_t int_fast64_t;
+typedef uint8_t uint_fast8_t;
+typedef uint16_t uint_fast16_t;
+typedef uint32_t uint_fast32_t;
+typedef uint64_t uint_fast64_t;
+
+// 7.18.1.4 Integer types capable of holding object pointers
+#ifdef _WIN64  // [
+typedef signed __int64 intptr_t;
+typedef unsigned __int64 uintptr_t;
+#else          // _WIN64 ][
+typedef _W64 signed int intptr_t;
+typedef _W64 unsigned int uintptr_t;
+#endif  // _WIN64 ]
+
+// 7.18.1.5 Greatest-width integer types
+typedef int64_t intmax_t;
+typedef uint64_t uintmax_t;
+
+// 7.18.2 Limits of specified-width integer types
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_LIMIT_MACROS)  // [   See footnote 220 at page 257 and
+// footnote 221 at page 259
+
+// 7.18.2.1 Limits of exact-width integer types
+#define INT8_MIN ((int8_t)_I8_MIN)
+#define INT8_MAX _I8_MAX
+#define INT16_MIN ((int16_t)_I16_MIN)
+#define INT16_MAX _I16_MAX
+#define INT32_MIN ((int32_t)_I32_MIN)
+#define INT32_MAX _I32_MAX
+#define INT64_MIN ((int64_t)_I64_MIN)
+#define INT64_MAX _I64_MAX
+#define UINT8_MAX _UI8_MAX
+#define UINT16_MAX _UI16_MAX
+#define UINT32_MAX _UI32_MAX
+#define UINT64_MAX _UI64_MAX
+
+// 7.18.2.2 Limits of minimum-width integer types
+#define INT_LEAST8_MIN INT8_MIN
+#define INT_LEAST8_MAX INT8_MAX
+#define INT_LEAST16_MIN INT16_MIN
+#define INT_LEAST16_MAX INT16_MAX
+#define INT_LEAST32_MIN INT32_MIN
+#define INT_LEAST32_MAX INT32_MAX
+#define INT_LEAST64_MIN INT64_MIN
+#define INT_LEAST64_MAX INT64_MAX
+#define UINT_LEAST8_MAX UINT8_MAX
+#define UINT_LEAST16_MAX UINT16_MAX
+#define UINT_LEAST32_MAX UINT32_MAX
+#define UINT_LEAST64_MAX UINT64_MAX
+
+// 7.18.2.3 Limits of fastest minimum-width integer types
+#define INT_FAST8_MIN INT8_MIN
+#define INT_FAST8_MAX INT8_MAX
+#define INT_FAST16_MIN INT16_MIN
+#define INT_FAST16_MAX INT16_MAX
+#define INT_FAST32_MIN INT32_MIN
+#define INT_FAST32_MAX INT32_MAX
+#define INT_FAST64_MIN INT64_MIN
+#define INT_FAST64_MAX INT64_MAX
+#define UINT_FAST8_MAX UINT8_MAX
+#define UINT_FAST16_MAX UINT16_MAX
+#define UINT_FAST32_MAX UINT32_MAX
+#define UINT_FAST64_MAX UINT64_MAX
+
+// 7.18.2.4 Limits of integer types capable of holding object pointers
+#ifdef _WIN64  // [
+#define INTPTR_MIN INT64_MIN
+#define INTPTR_MAX INT64_MAX
+#define UINTPTR_MAX UINT64_MAX
+#else  // _WIN64 ][
+#define INTPTR_MIN INT32_MIN
+#define INTPTR_MAX INT32_MAX
+#define UINTPTR_MAX UINT32_MAX
+#endif  // _WIN64 ]
+
+// 7.18.2.5 Limits of greatest-width integer types
+#define INTMAX_MIN INT64_MIN
+#define INTMAX_MAX INT64_MAX
+#define UINTMAX_MAX UINT64_MAX
+
+// 7.18.3 Limits of other integer types
+
+#ifdef _WIN64  // [
+#define PTRDIFF_MIN _I64_MIN
+#define PTRDIFF_MAX _I64_MAX
+#else  // _WIN64 ][
+#define PTRDIFF_MIN _I32_MIN
+#define PTRDIFF_MAX _I32_MAX
+#endif  // _WIN64 ]
+
+#define SIG_ATOMIC_MIN INT_MIN
+#define SIG_ATOMIC_MAX INT_MAX
+
+#ifndef SIZE_MAX  // [
+#ifdef _WIN64     // [
+#define SIZE_MAX _UI64_MAX
+#else  // _WIN64 ][
+#define SIZE_MAX _UI32_MAX
+#endif  // _WIN64 ]
+#endif  // SIZE_MAX ]
+
+// WCHAR_MIN and WCHAR_MAX are also defined in <wchar.h>
+#ifndef WCHAR_MIN  // [
+#define WCHAR_MIN 0
+#endif             // WCHAR_MIN ]
+#ifndef WCHAR_MAX  // [
+#define WCHAR_MAX _UI16_MAX
+#endif  // WCHAR_MAX ]
+
+#define WINT_MIN 0
+#define WINT_MAX _UI16_MAX
+
+#endif  // __STDC_LIMIT_MACROS ]
+
+// 7.18.4 Limits of other integer types
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_CONSTANT_MACROS)  // [   See footnote 224 at page 260
+
+// 7.18.4.1 Macros for minimum-width integer constants
+
+#define INT8_C(val) val##i8
+#define INT16_C(val) val##i16
+#define INT32_C(val) val##i32
+#define INT64_C(val) val##i64
+
+#define UINT8_C(val) val##ui8
+#define UINT16_C(val) val##ui16
+#define UINT32_C(val) val##ui32
+#define UINT64_C(val) val##ui64
+
+// 7.18.4.2 Macros for greatest-width integer constants
+// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
+// Check out Issue 9 for the details.
+#ifndef INTMAX_C  //   [
+#define INTMAX_C INT64_C
+#endif             // INTMAX_C    ]
+#ifndef UINTMAX_C  //  [
+#define UINTMAX_C UINT64_C
+#endif  // UINTMAX_C   ]
+
+#endif  // __STDC_CONSTANT_MACROS ]
+
+#endif  // _MSC_VER >= 1600 ]
+
+#endif  // _MSC_STDINT_H_ ]
diff --git a/livox_ros_driver2/3rdparty/rapidjson/ostreamwrapper.h b/livox_ros_driver2/3rdparty/rapidjson/ostreamwrapper.h
new file mode 100644
index 0000000..56c50a7
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/ostreamwrapper.h
@@ -0,0 +1,96 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_OSTREAMWRAPPER_H_
+#define RAPIDJSON_OSTREAMWRAPPER_H_
+
+#include <iosfwd>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Wrapper of \c std::basic_ostream into RapidJSON's Stream concept.
+/*!
+    The classes can be wrapped including but not limited to:
+
+    - \c std::ostringstream
+    - \c std::stringstream
+    - \c std::wpstringstream
+    - \c std::wstringstream
+    - \c std::ifstream
+    - \c std::fstream
+    - \c std::wofstream
+    - \c std::wfstream
+
+    \tparam StreamType Class derived from \c std::basic_ostream.
+*/
+
+template <typename StreamType>
+class BasicOStreamWrapper {
+ public:
+  typedef typename StreamType::char_type Ch;
+  BasicOStreamWrapper(StreamType &stream) : stream_(stream) {}
+
+  void Put(Ch c) { stream_.put(c); }
+
+  void Flush() { stream_.flush(); }
+
+  // Not implemented
+  char Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(char *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  BasicOStreamWrapper(const BasicOStreamWrapper &);
+  BasicOStreamWrapper &operator=(const BasicOStreamWrapper &);
+
+  StreamType &stream_;
+};
+
+typedef BasicOStreamWrapper<std::ostream> OStreamWrapper;
+typedef BasicOStreamWrapper<std::wostream> WOStreamWrapper;
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_OSTREAMWRAPPER_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/pointer.h b/livox_ros_driver2/3rdparty/rapidjson/pointer.h
new file mode 100644
index 0000000..218dd6c
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/pointer.h
@@ -0,0 +1,1712 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_POINTER_H_
+#define RAPIDJSON_POINTER_H_
+
+#include "document.h"
+#include "internal/itoa.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(switch - enum)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+static const SizeType kPointerInvalidIndex =
+    ~SizeType(0);  //!< Represents an invalid index in GenericPointer::Token
+
+//! Error code of parsing.
+/*! \ingroup RAPIDJSON_ERRORS
+    \see GenericPointer::GenericPointer, GenericPointer::GetParseErrorCode
+*/
+enum PointerParseErrorCode {
+  kPointerParseErrorNone = 0,  //!< The parse is successful
+
+  kPointerParseErrorTokenMustBeginWithSolidus,  //!< A token must begin with a
+                                                //!< '/'
+  kPointerParseErrorInvalidEscape,              //!< Invalid escape
+  kPointerParseErrorInvalidPercentEncoding,     //!< Invalid percent encoding in
+                                                //!URI
+                                                //!< fragment
+  kPointerParseErrorCharacterMustPercentEncode  //!< A character must percent
+                                                //!< encoded in URI fragment
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericPointer
+
+//! Represents a JSON Pointer. Use Pointer for UTF8 encoding and default
+//! allocator.
+/*!
+    This class implements RFC 6901 "JavaScript Object Notation (JSON) Pointer"
+    (https://tools.ietf.org/html/rfc6901).
+
+    A JSON pointer is for identifying a specific value in a JSON document
+    (GenericDocument). It can simplify coding of DOM tree manipulation, because
+   it can access multiple-level depth of DOM tree with single API call.
+
+    After it parses a string representation (e.g. "/foo/0" or URI fragment
+    representation (e.g. "#/foo/0") into its internal representation (tokens),
+    it can be used to resolve a specific value in multiple documents, or
+   sub-tree of documents.
+
+    Contrary to GenericValue, Pointer can be copy constructed and copy assigned.
+    Apart from assignment, a Pointer cannot be modified after construction.
+
+    Although Pointer is very convenient, please aware that constructing Pointer
+    involves parsing and dynamic memory allocation. A special constructor with
+   user- supplied tokens eliminates these.
+
+    GenericPointer depends on GenericDocument and GenericValue.
+
+    \tparam ValueType The value type of the DOM tree. E.g. GenericValue<UTF8<> >
+    \tparam Allocator The allocator type for allocating memory for internal
+   representation.
+
+    \note GenericPointer uses same encoding of ValueType.
+    However, Allocator of GenericPointer is independent of Allocator of Value.
+*/
+template <typename ValueType, typename Allocator = CrtAllocator>
+class GenericPointer {
+ public:
+  typedef typename ValueType::EncodingType
+      EncodingType;                   //!< Encoding type from Value
+  typedef typename ValueType::Ch Ch;  //!< Character type from Value
+
+  //! A token is the basic units of internal representation.
+  /*!
+      A JSON pointer string representation "/foo/123" is parsed to two tokens:
+      "foo" and 123. 123 will be represented in both numeric form and string
+     form. They are resolved according to the actual value type (object or
+     array).
+
+      For token that are not numbers, or the numeric value is out of bound
+      (greater than limits of SizeType), they are only treated as string form
+      (i.e. the token's index will be equal to kPointerInvalidIndex).
+
+      This struct is public so that user can create a Pointer without parsing
+     and allocation, using a special constructor.
+  */
+  struct Token {
+    const Ch
+        *name;  //!< Name of the token. It has null character at the end but
+                //!< it can contain null character.
+    SizeType length;  //!< Length of the name.
+    SizeType index;   //!< A valid array index, if it is not equal to
+                      //!< kPointerInvalidIndex.
+  };
+
+  //!@name Constructors and destructor.
+  //@{
+
+  //! Default constructor.
+  GenericPointer(Allocator *allocator = 0)
+      : allocator_(allocator),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
+        parseErrorCode_(kPointerParseErrorNone) {}
+
+  //! Constructor that parses a string or URI fragment representation.
+  /*!
+      \param source A null-terminated, string or URI fragment representation of
+     JSON pointer. \param allocator User supplied allocator for this pointer. If
+     no allocator is provided, it creates a self-owned one.
+  */
+  explicit GenericPointer(const Ch *source, Allocator *allocator = 0)
+      : allocator_(allocator),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
+        parseErrorCode_(kPointerParseErrorNone) {
+    Parse(source, internal::StrLen(source));
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Constructor that parses a string or URI fragment representation.
+  /*!
+      \param source A string or URI fragment representation of JSON pointer.
+      \param allocator User supplied allocator for this pointer. If no allocator
+     is provided, it creates a self-owned one. \note Requires the definition of
+     the preprocessor symbol \ref RAPIDJSON_HAS_STDSTRING.
+  */
+  explicit GenericPointer(const std::basic_string<Ch> &source,
+                          Allocator *allocator = 0)
+      : allocator_(allocator),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
+        parseErrorCode_(kPointerParseErrorNone) {
+    Parse(source.c_str(), source.size());
+  }
+#endif
+
+  //! Constructor that parses a string or URI fragment representation, with
+  //! length of the source string.
+  /*!
+      \param source A string or URI fragment representation of JSON pointer.
+      \param length Length of source.
+      \param allocator User supplied allocator for this pointer. If no allocator
+     is provided, it creates a self-owned one. \note Slightly faster than the
+     overload without length.
+  */
+  GenericPointer(const Ch *source, size_t length, Allocator *allocator = 0)
+      : allocator_(allocator),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
+        parseErrorCode_(kPointerParseErrorNone) {
+    Parse(source, length);
+  }
+
+  //! Constructor with user-supplied tokens.
+  /*!
+      This constructor let user supplies const array of tokens.
+      This prevents the parsing process and eliminates allocation.
+      This is preferred for memory constrained environments.
+
+      \param tokens An constant array of tokens representing the JSON pointer.
+      \param tokenCount Number of tokens.
+
+      \b Example
+      \code
+      #define NAME(s) { s, sizeof(s) / sizeof(s[0]) - 1, kPointerInvalidIndex }
+      #define INDEX(i) { #i, sizeof(#i) - 1, i }
+
+      static const Pointer::Token kTokens[] = { NAME("foo"), INDEX(123) };
+      static const Pointer p(kTokens, sizeof(kTokens) / sizeof(kTokens[0]));
+      // Equivalent to static const Pointer p("/foo/123");
+
+      #undef NAME
+      #undef INDEX
+      \endcode
+  */
+  GenericPointer(const Token *tokens, size_t tokenCount)
+      : allocator_(),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(const_cast<Token *>(tokens)),
+        tokenCount_(tokenCount),
+        parseErrorOffset_(),
+        parseErrorCode_(kPointerParseErrorNone) {}
+
+  //! Copy constructor.
+  GenericPointer(const GenericPointer &rhs)
+      : allocator_(rhs.allocator_),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
+        parseErrorCode_(kPointerParseErrorNone) {
+    *this = rhs;
+  }
+
+  //! Copy constructor.
+  GenericPointer(const GenericPointer &rhs, Allocator *allocator)
+      : allocator_(allocator),
+        ownAllocator_(),
+        nameBuffer_(),
+        tokens_(),
+        tokenCount_(),
+        parseErrorOffset_(),
+        parseErrorCode_(kPointerParseErrorNone) {
+    *this = rhs;
+  }
+
+  //! Destructor.
+  ~GenericPointer() {
+    if (nameBuffer_)  // If user-supplied tokens constructor is used,
+                      // nameBuffer_
+                      // is nullptr and tokens_ are not deallocated.
+      Allocator::Free(tokens_);
+    RAPIDJSON_DELETE(ownAllocator_);
+  }
+
+  //! Assignment operator.
+  GenericPointer &operator=(const GenericPointer &rhs) {
+    if (this != &rhs) {
+      // Do not delete ownAllcator
+      if (nameBuffer_) Allocator::Free(tokens_);
+
+      tokenCount_ = rhs.tokenCount_;
+      parseErrorOffset_ = rhs.parseErrorOffset_;
+      parseErrorCode_ = rhs.parseErrorCode_;
+
+      if (rhs.nameBuffer_)
+        CopyFromRaw(rhs);  // Normally parsed tokens.
+      else {
+        tokens_ = rhs.tokens_;  // User supplied const tokens.
+        nameBuffer_ = 0;
+      }
+    }
+    return *this;
+  }
+
+  //! Swap the content of this pointer with an other.
+  /*!
+      \param other The pointer to swap with.
+      \note Constant complexity.
+  */
+  GenericPointer &Swap(GenericPointer &other) RAPIDJSON_NOEXCEPT {
+    internal::Swap(allocator_, other.allocator_);
+    internal::Swap(ownAllocator_, other.ownAllocator_);
+    internal::Swap(nameBuffer_, other.nameBuffer_);
+    internal::Swap(tokens_, other.tokens_);
+    internal::Swap(tokenCount_, other.tokenCount_);
+    internal::Swap(parseErrorOffset_, other.parseErrorOffset_);
+    internal::Swap(parseErrorCode_, other.parseErrorCode_);
+    return *this;
+  }
+
+  //! free-standing swap function helper
+  /*!
+      Helper function to enable support for common swap implementation pattern
+     based on \c std::swap: \code void swap(MyClass& a, MyClass& b) { using
+     std::swap; swap(a.pointer, b.pointer);
+          // ...
+      }
+      \endcode
+      \see Swap()
+   */
+  friend inline void swap(GenericPointer &a,
+                          GenericPointer &b) RAPIDJSON_NOEXCEPT {
+    a.Swap(b);
+  }
+
+  //@}
+
+  //!@name Append token
+  //@{
+
+  //! Append a token and return a new Pointer
+  /*!
+      \param token Token to be appended.
+      \param allocator Allocator for the newly return Pointer.
+      \return A new Pointer with appended token.
+  */
+  GenericPointer Append(const Token &token, Allocator *allocator = 0) const {
+    GenericPointer r;
+    r.allocator_ = allocator;
+    Ch *p = r.CopyFromRaw(*this, 1, token.length + 1);
+    std::memcpy(p, token.name, (token.length + 1) * sizeof(Ch));
+    r.tokens_[tokenCount_].name = p;
+    r.tokens_[tokenCount_].length = token.length;
+    r.tokens_[tokenCount_].index = token.index;
+    return r;
+  }
+
+  //! Append a name token with length, and return a new Pointer
+  /*!
+      \param name Name to be appended.
+      \param length Length of name.
+      \param allocator Allocator for the newly return Pointer.
+      \return A new Pointer with appended token.
+  */
+  GenericPointer Append(const Ch *name, SizeType length,
+                        Allocator *allocator = 0) const {
+    Token token = {name, length, kPointerInvalidIndex};
+    return Append(token, allocator);
+  }
+
+  //! Append a name token without length, and return a new Pointer
+  /*!
+      \param name Name (const Ch*) to be appended.
+      \param allocator Allocator for the newly return Pointer.
+      \return A new Pointer with appended token.
+  */
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::NotExpr<
+          internal::IsSame<typename internal::RemoveConst<T>::Type, Ch>>),
+      (GenericPointer))
+  Append(T *name, Allocator *allocator = 0) const {
+    return Append(name, internal::StrLen(name), allocator);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Append a name token, and return a new Pointer
+  /*!
+      \param name Name to be appended.
+      \param allocator Allocator for the newly return Pointer.
+      \return A new Pointer with appended token.
+  */
+  GenericPointer Append(const std::basic_string<Ch> &name,
+                        Allocator *allocator = 0) const {
+    return Append(name.c_str(), static_cast<SizeType>(name.size()), allocator);
+  }
+#endif
+
+  //! Append a index token, and return a new Pointer
+  /*!
+      \param index Index to be appended.
+      \param allocator Allocator for the newly return Pointer.
+      \return A new Pointer with appended token.
+  */
+  GenericPointer Append(SizeType index, Allocator *allocator = 0) const {
+    char buffer[21];
+    char *end = sizeof(SizeType) == 4 ? internal::u32toa(index, buffer)
+                                      : internal::u64toa(index, buffer);
+    SizeType length = static_cast<SizeType>(end - buffer);
+    buffer[length] = '\0';
+
+    if (sizeof(Ch) == 1) {
+      Token token = {reinterpret_cast<Ch *>(buffer), length, index};
+      return Append(token, allocator);
+    } else {
+      Ch name[21];
+      for (size_t i = 0; i <= length; i++) name[i] = static_cast<Ch>(buffer[i]);
+      Token token = {name, length, index};
+      return Append(token, allocator);
+    }
+  }
+
+  //! Append a token by value, and return a new Pointer
+  /*!
+      \param token token to be appended.
+      \param allocator Allocator for the newly return Pointer.
+      \return A new Pointer with appended token.
+  */
+  GenericPointer Append(const ValueType &token,
+                        Allocator *allocator = 0) const {
+    if (token.IsString())
+      return Append(token.GetString(), token.GetStringLength(), allocator);
+    else {
+      RAPIDJSON_ASSERT(token.IsUint64());
+      RAPIDJSON_ASSERT(token.GetUint64() <= SizeType(~0));
+      return Append(static_cast<SizeType>(token.GetUint64()), allocator);
+    }
+  }
+
+  //!@name Handling Parse Error
+  //@{
+
+  //! Check whether this is a valid pointer.
+  bool IsValid() const { return parseErrorCode_ == kPointerParseErrorNone; }
+
+  //! Get the parsing error offset in code unit.
+  size_t GetParseErrorOffset() const { return parseErrorOffset_; }
+
+  //! Get the parsing error code.
+  PointerParseErrorCode GetParseErrorCode() const { return parseErrorCode_; }
+
+  //@}
+
+  //! Get the allocator of this pointer.
+  Allocator &GetAllocator() { return *allocator_; }
+
+  //!@name Tokens
+  //@{
+
+  //! Get the token array (const version only).
+  const Token *GetTokens() const { return tokens_; }
+
+  //! Get the number of tokens.
+  size_t GetTokenCount() const { return tokenCount_; }
+
+  //@}
+
+  //!@name Equality/inequality operators
+  //@{
+
+  //! Equality operator.
+  /*!
+      \note When any pointers are invalid, always returns false.
+  */
+  bool operator==(const GenericPointer &rhs) const {
+    if (!IsValid() || !rhs.IsValid() || tokenCount_ != rhs.tokenCount_)
+      return false;
+
+    for (size_t i = 0; i < tokenCount_; i++) {
+      if (tokens_[i].index != rhs.tokens_[i].index ||
+          tokens_[i].length != rhs.tokens_[i].length ||
+          (tokens_[i].length != 0 &&
+           std::memcmp(tokens_[i].name, rhs.tokens_[i].name,
+                       sizeof(Ch) * tokens_[i].length) != 0)) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  //! Inequality operator.
+  /*!
+      \note When any pointers are invalid, always returns true.
+  */
+  bool operator!=(const GenericPointer &rhs) const { return !(*this == rhs); }
+
+  //! Less than operator.
+  /*!
+      \note Invalid pointers are always greater than valid ones.
+  */
+  bool operator<(const GenericPointer &rhs) const {
+    if (!IsValid()) return false;
+    if (!rhs.IsValid()) return true;
+
+    if (tokenCount_ != rhs.tokenCount_) return tokenCount_ < rhs.tokenCount_;
+
+    for (size_t i = 0; i < tokenCount_; i++) {
+      if (tokens_[i].index != rhs.tokens_[i].index)
+        return tokens_[i].index < rhs.tokens_[i].index;
+
+      if (tokens_[i].length != rhs.tokens_[i].length)
+        return tokens_[i].length < rhs.tokens_[i].length;
+
+      if (int cmp = std::memcmp(tokens_[i].name, rhs.tokens_[i].name,
+                                sizeof(Ch) * tokens_[i].length))
+        return cmp < 0;
+    }
+
+    return false;
+  }
+
+  //@}
+
+  //!@name Stringify
+  //@{
+
+  //! Stringify the pointer into string representation.
+  /*!
+      \tparam OutputStream Type of output stream.
+      \param os The output stream.
+  */
+  template <typename OutputStream>
+  bool Stringify(OutputStream &os) const {
+    return Stringify<false, OutputStream>(os);
+  }
+
+  //! Stringify the pointer into URI fragment representation.
+  /*!
+      \tparam OutputStream Type of output stream.
+      \param os The output stream.
+  */
+  template <typename OutputStream>
+  bool StringifyUriFragment(OutputStream &os) const {
+    return Stringify<true, OutputStream>(os);
+  }
+
+  //@}
+
+  //!@name Create value
+  //@{
+
+  //! Create a value in a subtree.
+  /*!
+      If the value is not exist, it creates all parent values and a JSON Null
+     value. So it always succeed and return the newly created or existing value.
+
+      Remind that it may change types of parents according to tokens, so it
+      potentially removes previously stored values. For example, if a document
+      was an array, and "/foo" is used to create a value, then the document
+      will be changed to an object, and all existing array elements are lost.
+
+      \param root Root value of a DOM subtree to be resolved. It can be any
+     value other than document root. \param allocator Allocator for creating the
+     values if the specified value or its parents are not exist. \param
+     alreadyExist If non-null, it stores whether the resolved value is already
+     exist. \return The resolved newly created (a JSON Null value), or already
+     exists value.
+  */
+  ValueType &Create(ValueType &root,
+                    typename ValueType::AllocatorType &allocator,
+                    bool *alreadyExist = 0) const {
+    RAPIDJSON_ASSERT(IsValid());
+    ValueType *v = &root;
+    bool exist = true;
+    for (const Token *t = tokens_; t != tokens_ + tokenCount_; ++t) {
+      if (v->IsArray() && t->name[0] == '-' && t->length == 1) {
+        v->PushBack(ValueType().Move(), allocator);
+        v = &((*v)[v->Size() - 1]);
+        exist = false;
+      } else {
+        if (t->index == kPointerInvalidIndex) {  // must be object name
+          if (!v->IsObject()) v->SetObject();    // Change to Object
+        } else {                                 // object name or array index
+          if (!v->IsArray() && !v->IsObject())
+            v->SetArray();  // Change to Array
+        }
+
+        if (v->IsArray()) {
+          if (t->index >= v->Size()) {
+            v->Reserve(t->index + 1, allocator);
+            while (t->index >= v->Size())
+              v->PushBack(ValueType().Move(), allocator);
+            exist = false;
+          }
+          v = &((*v)[t->index]);
+        } else {
+          typename ValueType::MemberIterator m =
+              v->FindMember(GenericValue<EncodingType>(
+                  GenericStringRef<Ch>(t->name, t->length)));
+          if (m == v->MemberEnd()) {
+            v->AddMember(ValueType(t->name, t->length, allocator).Move(),
+                         ValueType().Move(), allocator);
+            m = v->MemberEnd();
+            v = &(--m)->value;  // Assumes AddMember() appends at the end
+            exist = false;
+          } else
+            v = &m->value;
+        }
+      }
+    }
+
+    if (alreadyExist) *alreadyExist = exist;
+
+    return *v;
+  }
+
+  //! Creates a value in a document.
+  /*!
+      \param document A document to be resolved.
+      \param alreadyExist If non-null, it stores whether the resolved value is
+     already exist. \return The resolved newly created, or already exists value.
+  */
+  template <typename stackAllocator>
+  ValueType &Create(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
+                      stackAllocator> &document,
+      bool *alreadyExist = 0) const {
+    return Create(document, document.GetAllocator(), alreadyExist);
+  }
+
+  //@}
+
+  //!@name Query value
+  //@{
+
+  //! Query a value in a subtree.
+  /*!
+      \param root Root value of a DOM sub-tree to be resolved. It can be any
+     value other than document root. \param unresolvedTokenIndex If the pointer
+     cannot resolve a token in the pointer, this parameter can obtain the index
+     of unresolved token. \return Pointer to the value if it can be resolved.
+     Otherwise null.
+
+      \note
+      There are only 3 situations when a value cannot be resolved:
+      1. A value in the path is not an array nor object.
+      2. An object value does not contain the token.
+      3. A token is out of range of an array value.
+
+      Use unresolvedTokenIndex to retrieve the token index.
+  */
+  ValueType *Get(ValueType &root, size_t *unresolvedTokenIndex = 0) const {
+    RAPIDJSON_ASSERT(IsValid());
+    ValueType *v = &root;
+    for (const Token *t = tokens_; t != tokens_ + tokenCount_; ++t) {
+      switch (v->GetType()) {
+        case kObjectType: {
+          typename ValueType::MemberIterator m =
+              v->FindMember(GenericValue<EncodingType>(
+                  GenericStringRef<Ch>(t->name, t->length)));
+          if (m == v->MemberEnd()) break;
+          v = &m->value;
+        }
+          continue;
+        case kArrayType:
+          if (t->index == kPointerInvalidIndex || t->index >= v->Size()) break;
+          v = &((*v)[t->index]);
+          continue;
+        default:
+          break;
+      }
+
+      // Error: unresolved token
+      if (unresolvedTokenIndex)
+        *unresolvedTokenIndex = static_cast<size_t>(t - tokens_);
+      return 0;
+    }
+    return v;
+  }
+
+  //! Query a const value in a const subtree.
+  /*!
+      \param root Root value of a DOM sub-tree to be resolved. It can be any
+     value other than document root. \return Pointer to the value if it can be
+     resolved. Otherwise null.
+  */
+  const ValueType *Get(const ValueType &root,
+                       size_t *unresolvedTokenIndex = 0) const {
+    return Get(const_cast<ValueType &>(root), unresolvedTokenIndex);
+  }
+
+  //@}
+
+  //!@name Query a value with default
+  //@{
+
+  //! Query a value in a subtree with default value.
+  /*!
+      Similar to Get(), but if the specified value do not exists, it creates all
+     parents and clone the default value. So that this function always succeed.
+
+      \param root Root value of a DOM sub-tree to be resolved. It can be any
+     value other than document root. \param defaultValue Default value to be
+     cloned if the value was not exists. \param allocator Allocator for creating
+     the values if the specified value or its parents are not exist. \see
+     Create()
+  */
+  ValueType &GetWithDefault(
+      ValueType &root, const ValueType &defaultValue,
+      typename ValueType::AllocatorType &allocator) const {
+    bool alreadyExist;
+    ValueType &v = Create(root, allocator, &alreadyExist);
+    return alreadyExist ? v : v.CopyFrom(defaultValue, allocator);
+  }
+
+  //! Query a value in a subtree with default null-terminated string.
+  ValueType &GetWithDefault(
+      ValueType &root, const Ch *defaultValue,
+      typename ValueType::AllocatorType &allocator) const {
+    bool alreadyExist;
+    ValueType &v = Create(root, allocator, &alreadyExist);
+    return alreadyExist ? v : v.SetString(defaultValue, allocator);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Query a value in a subtree with default std::basic_string.
+  ValueType &GetWithDefault(
+      ValueType &root, const std::basic_string<Ch> &defaultValue,
+      typename ValueType::AllocatorType &allocator) const {
+    bool alreadyExist;
+    ValueType &v = Create(root, allocator, &alreadyExist);
+    return alreadyExist ? v : v.SetString(defaultValue, allocator);
+  }
+#endif
+
+  //! Query a value in a subtree with default primitive value.
+  /*!
+      \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t,
+     \c bool
+  */
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::OrExpr<internal::IsPointer<T>, internal::IsGenericValue<T>>),
+      (ValueType &))
+  GetWithDefault(ValueType &root, T defaultValue,
+                 typename ValueType::AllocatorType &allocator) const {
+    return GetWithDefault(root, ValueType(defaultValue).Move(), allocator);
+  }
+
+  //! Query a value in a document with default value.
+  template <typename stackAllocator>
+  ValueType &GetWithDefault(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
+                      stackAllocator> &document,
+      const ValueType &defaultValue) const {
+    return GetWithDefault(document, defaultValue, document.GetAllocator());
+  }
+
+  //! Query a value in a document with default null-terminated string.
+  template <typename stackAllocator>
+  ValueType &GetWithDefault(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
+                      stackAllocator> &document,
+      const Ch *defaultValue) const {
+    return GetWithDefault(document, defaultValue, document.GetAllocator());
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Query a value in a document with default std::basic_string.
+  template <typename stackAllocator>
+  ValueType &GetWithDefault(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
+                      stackAllocator> &document,
+      const std::basic_string<Ch> &defaultValue) const {
+    return GetWithDefault(document, defaultValue, document.GetAllocator());
+  }
+#endif
+
+  //! Query a value in a document with default primitive value.
+  /*!
+      \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t,
+     \c bool
+  */
+  template <typename T, typename stackAllocator>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::OrExpr<internal::IsPointer<T>, internal::IsGenericValue<T>>),
+      (ValueType &))
+  GetWithDefault(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
+                      stackAllocator> &document,
+      T defaultValue) const {
+    return GetWithDefault(document, defaultValue, document.GetAllocator());
+  }
+
+  //@}
+
+  //!@name Set a value
+  //@{
+
+  //! Set a value in a subtree, with move semantics.
+  /*!
+      It creates all parents if they are not exist or types are different to the
+     tokens. So this function always succeeds but potentially remove existing
+     values.
+
+      \param root Root value of a DOM sub-tree to be resolved. It can be any
+     value other than document root. \param value Value to be set. \param
+     allocator Allocator for creating the values if the specified value or its
+     parents are not exist. \see Create()
+  */
+  ValueType &Set(ValueType &root, ValueType &value,
+                 typename ValueType::AllocatorType &allocator) const {
+    return Create(root, allocator) = value;
+  }
+
+  //! Set a value in a subtree, with copy semantics.
+  ValueType &Set(ValueType &root, const ValueType &value,
+                 typename ValueType::AllocatorType &allocator) const {
+    return Create(root, allocator).CopyFrom(value, allocator);
+  }
+
+  //! Set a null-terminated string in a subtree.
+  ValueType &Set(ValueType &root, const Ch *value,
+                 typename ValueType::AllocatorType &allocator) const {
+    return Create(root, allocator) = ValueType(value, allocator).Move();
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Set a std::basic_string in a subtree.
+  ValueType &Set(ValueType &root, const std::basic_string<Ch> &value,
+                 typename ValueType::AllocatorType &allocator) const {
+    return Create(root, allocator) = ValueType(value, allocator).Move();
+  }
+#endif
+
+  //! Set a primitive value in a subtree.
+  /*!
+      \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t,
+     \c bool
+  */
+  template <typename T>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::OrExpr<internal::IsPointer<T>, internal::IsGenericValue<T>>),
+      (ValueType &))
+  Set(ValueType &root, T value,
+      typename ValueType::AllocatorType &allocator) const {
+    return Create(root, allocator) = ValueType(value).Move();
+  }
+
+  //! Set a value in a document, with move semantics.
+  template <typename stackAllocator>
+  ValueType &Set(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
+                      stackAllocator> &document,
+      ValueType &value) const {
+    return Create(document) = value;
+  }
+
+  //! Set a value in a document, with copy semantics.
+  template <typename stackAllocator>
+  ValueType &Set(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
+                      stackAllocator> &document,
+      const ValueType &value) const {
+    return Create(document).CopyFrom(value, document.GetAllocator());
+  }
+
+  //! Set a null-terminated string in a document.
+  template <typename stackAllocator>
+  ValueType &Set(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
+                      stackAllocator> &document,
+      const Ch *value) const {
+    return Create(document) = ValueType(value, document.GetAllocator()).Move();
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  //! Sets a std::basic_string in a document.
+  template <typename stackAllocator>
+  ValueType &Set(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
+                      stackAllocator> &document,
+      const std::basic_string<Ch> &value) const {
+    return Create(document) = ValueType(value, document.GetAllocator()).Move();
+  }
+#endif
+
+  //! Set a primitive value in a document.
+  /*!
+  \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t, \c
+  bool
+  */
+  template <typename T, typename stackAllocator>
+  RAPIDJSON_DISABLEIF_RETURN(
+      (internal::OrExpr<internal::IsPointer<T>, internal::IsGenericValue<T>>),
+      (ValueType &))
+  Set(GenericDocument<EncodingType, typename ValueType::AllocatorType,
+                      stackAllocator> &document,
+      T value) const {
+    return Create(document) = value;
+  }
+
+  //@}
+
+  //!@name Swap a value
+  //@{
+
+  //! Swap a value with a value in a subtree.
+  /*!
+      It creates all parents if they are not exist or types are different to the
+     tokens. So this function always succeeds but potentially remove existing
+     values.
+
+      \param root Root value of a DOM sub-tree to be resolved. It can be any
+     value other than document root. \param value Value to be swapped. \param
+     allocator Allocator for creating the values if the specified value or its
+     parents are not exist. \see Create()
+  */
+  ValueType &Swap(ValueType &root, ValueType &value,
+                  typename ValueType::AllocatorType &allocator) const {
+    return Create(root, allocator).Swap(value);
+  }
+
+  //! Swap a value with a value in a document.
+  template <typename stackAllocator>
+  ValueType &Swap(
+      GenericDocument<EncodingType, typename ValueType::AllocatorType,
+                      stackAllocator> &document,
+      ValueType &value) const {
+    return Create(document).Swap(value);
+  }
+
+  //@}
+
+  //! Erase a value in a subtree.
+  /*!
+      \param root Root value of a DOM sub-tree to be resolved. It can be any
+     value other than document root. \return Whether the resolved value is found
+     and erased.
+
+      \note Erasing with an empty pointer \c Pointer(""), i.e. the root, always
+     fail and return false.
+  */
+  bool Erase(ValueType &root) const {
+    RAPIDJSON_ASSERT(IsValid());
+    if (tokenCount_ == 0)  // Cannot erase the root
+      return false;
+
+    ValueType *v = &root;
+    const Token *last = tokens_ + (tokenCount_ - 1);
+    for (const Token *t = tokens_; t != last; ++t) {
+      switch (v->GetType()) {
+        case kObjectType: {
+          typename ValueType::MemberIterator m =
+              v->FindMember(GenericValue<EncodingType>(
+                  GenericStringRef<Ch>(t->name, t->length)));
+          if (m == v->MemberEnd()) return false;
+          v = &m->value;
+        } break;
+        case kArrayType:
+          if (t->index == kPointerInvalidIndex || t->index >= v->Size())
+            return false;
+          v = &((*v)[t->index]);
+          break;
+        default:
+          return false;
+      }
+    }
+
+    switch (v->GetType()) {
+      case kObjectType:
+        return v->EraseMember(GenericStringRef<Ch>(last->name, last->length));
+      case kArrayType:
+        if (last->index == kPointerInvalidIndex || last->index >= v->Size())
+          return false;
+        v->Erase(v->Begin() + last->index);
+        return true;
+      default:
+        return false;
+    }
+  }
+
+ private:
+  //! Clone the content from rhs to this.
+  /*!
+      \param rhs Source pointer.
+      \param extraToken Extra tokens to be allocated.
+      \param extraNameBufferSize Extra name buffer size (in number of Ch) to be
+     allocated. \return Start of non-occupied name buffer, for storing extra
+     names.
+  */
+  Ch *CopyFromRaw(const GenericPointer &rhs, size_t extraToken = 0,
+                  size_t extraNameBufferSize = 0) {
+    if (!allocator_)  // allocator is independently owned.
+      ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+
+    size_t nameBufferSize = rhs.tokenCount_;  // null terminators for tokens
+    for (Token *t = rhs.tokens_; t != rhs.tokens_ + rhs.tokenCount_; ++t)
+      nameBufferSize += t->length;
+
+    tokenCount_ = rhs.tokenCount_ + extraToken;
+    tokens_ = static_cast<Token *>(allocator_->Malloc(
+        tokenCount_ * sizeof(Token) +
+        (nameBufferSize + extraNameBufferSize) * sizeof(Ch)));
+    nameBuffer_ = reinterpret_cast<Ch *>(tokens_ + tokenCount_);
+    if (rhs.tokenCount_ > 0) {
+      std::memcpy(tokens_, rhs.tokens_, rhs.tokenCount_ * sizeof(Token));
+    }
+    if (nameBufferSize > 0) {
+      std::memcpy(nameBuffer_, rhs.nameBuffer_, nameBufferSize * sizeof(Ch));
+    }
+
+    // Adjust pointers to name buffer
+    std::ptrdiff_t diff = nameBuffer_ - rhs.nameBuffer_;
+    for (Token *t = tokens_; t != tokens_ + rhs.tokenCount_; ++t)
+      t->name += diff;
+
+    return nameBuffer_ + nameBufferSize;
+  }
+
+  //! Check whether a character should be percent-encoded.
+  /*!
+      According to RFC 3986 2.3 Unreserved Characters.
+      \param c The character (code unit) to be tested.
+  */
+  bool NeedPercentEncode(Ch c) const {
+    return !((c >= '0' && c <= '9') || (c >= 'A' && c <= 'Z') ||
+             (c >= 'a' && c <= 'z') || c == '-' || c == '.' || c == '_' ||
+             c == '~');
+  }
+
+//! Parse a JSON String or its URI fragment representation into tokens.
+#ifndef __clang__  // -Wdocumentation
+                   /*!
+                       \param source Either a JSON Pointer string, or its URI fragment
+                      representation. Not need to be null terminated.                 \param length
+                      Length of the
+                      source string.                 \note Source cannot be JSON String
+                      Representation of JSON
+                      Pointer, e.g. In "/\u0000", \u0000 will not be unescaped.
+                   */
+#endif
+  void Parse(const Ch *source, size_t length) {
+    RAPIDJSON_ASSERT(source != NULL);
+    RAPIDJSON_ASSERT(nameBuffer_ == 0);
+    RAPIDJSON_ASSERT(tokens_ == 0);
+
+    // Create own allocator if user did not supply.
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+
+    // Count number of '/' as tokenCount
+    tokenCount_ = 0;
+    for (const Ch *s = source; s != source + length; s++)
+      if (*s == '/') tokenCount_++;
+
+    Token *token = tokens_ = static_cast<Token *>(
+        allocator_->Malloc(tokenCount_ * sizeof(Token) + length * sizeof(Ch)));
+    Ch *name = nameBuffer_ = reinterpret_cast<Ch *>(tokens_ + tokenCount_);
+    size_t i = 0;
+
+    // Detect if it is a URI fragment
+    bool uriFragment = false;
+    if (source[i] == '#') {
+      uriFragment = true;
+      i++;
+    }
+
+    if (i != length && source[i] != '/') {
+      parseErrorCode_ = kPointerParseErrorTokenMustBeginWithSolidus;
+      goto error;
+    }
+
+    while (i < length) {
+      RAPIDJSON_ASSERT(source[i] == '/');
+      i++;  // consumes '/'
+
+      token->name = name;
+      bool isNumber = true;
+
+      while (i < length && source[i] != '/') {
+        Ch c = source[i];
+        if (uriFragment) {
+          // Decoding percent-encoding for URI fragment
+          if (c == '%') {
+            PercentDecodeStream is(&source[i], source + length);
+            GenericInsituStringStream<EncodingType> os(name);
+            Ch *begin = os.PutBegin();
+            if (!Transcoder<UTF8<>, EncodingType>().Validate(is, os) ||
+                !is.IsValid()) {
+              parseErrorCode_ = kPointerParseErrorInvalidPercentEncoding;
+              goto error;
+            }
+            size_t len = os.PutEnd(begin);
+            i += is.Tell() - 1;
+            if (len == 1)
+              c = *name;
+            else {
+              name += len;
+              isNumber = false;
+              i++;
+              continue;
+            }
+          } else if (NeedPercentEncode(c)) {
+            parseErrorCode_ = kPointerParseErrorCharacterMustPercentEncode;
+            goto error;
+          }
+        }
+
+        i++;
+
+        // Escaping "~0" -> '~', "~1" -> '/'
+        if (c == '~') {
+          if (i < length) {
+            c = source[i];
+            if (c == '0')
+              c = '~';
+            else if (c == '1')
+              c = '/';
+            else {
+              parseErrorCode_ = kPointerParseErrorInvalidEscape;
+              goto error;
+            }
+            i++;
+          } else {
+            parseErrorCode_ = kPointerParseErrorInvalidEscape;
+            goto error;
+          }
+        }
+
+        // First check for index: all of characters are digit
+        if (c < '0' || c > '9') isNumber = false;
+
+        *name++ = c;
+      }
+      token->length = static_cast<SizeType>(name - token->name);
+      if (token->length == 0) isNumber = false;
+      *name++ = '\0';  // Null terminator
+
+      // Second check for index: more than one digit cannot have leading zero
+      if (isNumber && token->length > 1 && token->name[0] == '0')
+        isNumber = false;
+
+      // String to SizeType conversion
+      SizeType n = 0;
+      if (isNumber) {
+        for (size_t j = 0; j < token->length; j++) {
+          SizeType m = n * 10 + static_cast<SizeType>(token->name[j] - '0');
+          if (m < n) {  // overflow detection
+            isNumber = false;
+            break;
+          }
+          n = m;
+        }
+      }
+
+      token->index = isNumber ? n : kPointerInvalidIndex;
+      token++;
+    }
+
+    RAPIDJSON_ASSERT(name <=
+                     nameBuffer_ + length);  // Should not overflow buffer
+    parseErrorCode_ = kPointerParseErrorNone;
+    return;
+
+  error:
+    Allocator::Free(tokens_);
+    nameBuffer_ = 0;
+    tokens_ = 0;
+    tokenCount_ = 0;
+    parseErrorOffset_ = i;
+    return;
+  }
+
+  //! Stringify to string or URI fragment representation.
+  /*!
+      \tparam uriFragment True for stringifying to URI fragment representation.
+     False for string representation. \tparam OutputStream type of output
+     stream. \param os The output stream.
+  */
+  template <bool uriFragment, typename OutputStream>
+  bool Stringify(OutputStream &os) const {
+    RAPIDJSON_ASSERT(IsValid());
+
+    if (uriFragment) os.Put('#');
+
+    for (Token *t = tokens_; t != tokens_ + tokenCount_; ++t) {
+      os.Put('/');
+      for (size_t j = 0; j < t->length; j++) {
+        Ch c = t->name[j];
+        if (c == '~') {
+          os.Put('~');
+          os.Put('0');
+        } else if (c == '/') {
+          os.Put('~');
+          os.Put('1');
+        } else if (uriFragment && NeedPercentEncode(c)) {
+          // Transcode to UTF8 sequence
+          GenericStringStream<typename ValueType::EncodingType> source(
+              &t->name[j]);
+          PercentEncodeStream<OutputStream> target(os);
+          if (!Transcoder<EncodingType, UTF8<>>().Validate(source, target))
+            return false;
+          j += source.Tell() - 1;
+        } else
+          os.Put(c);
+      }
+    }
+    return true;
+  }
+
+  //! A helper stream for decoding a percent-encoded sequence into code unit.
+  /*!
+      This stream decodes %XY triplet into code unit (0-255).
+      If it encounters invalid characters, it sets output code unit as 0 and
+      mark invalid, and to be checked by IsValid().
+  */
+  class PercentDecodeStream {
+   public:
+    typedef typename ValueType::Ch Ch;
+
+    //! Constructor
+    /*!
+        \param source Start of the stream
+        \param end Past-the-end of the stream.
+    */
+    PercentDecodeStream(const Ch *source, const Ch *end)
+        : src_(source), head_(source), end_(end), valid_(true) {}
+
+    Ch Take() {
+      if (*src_ != '%' || src_ + 3 > end_) {  // %XY triplet
+        valid_ = false;
+        return 0;
+      }
+      src_++;
+      Ch c = 0;
+      for (int j = 0; j < 2; j++) {
+        c = static_cast<Ch>(c << 4);
+        Ch h = *src_;
+        if (h >= '0' && h <= '9')
+          c = static_cast<Ch>(c + h - '0');
+        else if (h >= 'A' && h <= 'F')
+          c = static_cast<Ch>(c + h - 'A' + 10);
+        else if (h >= 'a' && h <= 'f')
+          c = static_cast<Ch>(c + h - 'a' + 10);
+        else {
+          valid_ = false;
+          return 0;
+        }
+        src_++;
+      }
+      return c;
+    }
+
+    size_t Tell() const { return static_cast<size_t>(src_ - head_); }
+    bool IsValid() const { return valid_; }
+
+   private:
+    const Ch *src_;   //!< Current read position.
+    const Ch *head_;  //!< Original head of the string.
+    const Ch *end_;   //!< Past-the-end position.
+    bool valid_;      //!< Whether the parsing is valid.
+  };
+
+  //! A helper stream to encode character (UTF-8 code unit) into percent-encoded
+  //! sequence.
+  template <typename OutputStream>
+  class PercentEncodeStream {
+   public:
+    PercentEncodeStream(OutputStream &os) : os_(os) {}
+    void Put(char c) {  // UTF-8 must be byte
+      unsigned char u = static_cast<unsigned char>(c);
+      static const char hexDigits[16] = {'0', '1', '2', '3', '4', '5',
+                                         '6', '7', '8', '9', 'A', 'B',
+                                         'C', 'D', 'E', 'F'};
+      os_.Put('%');
+      os_.Put(static_cast<typename OutputStream::Ch>(hexDigits[u >> 4]));
+      os_.Put(static_cast<typename OutputStream::Ch>(hexDigits[u & 15]));
+    }
+
+   private:
+    OutputStream &os_;
+  };
+
+  Allocator *allocator_;  //!< The current allocator. It is either user-supplied
+                          //!< or equal to ownAllocator_.
+  Allocator *ownAllocator_;  //!< Allocator owned by this Pointer.
+  Ch *nameBuffer_;           //!< A buffer containing all names in tokens.
+  Token *tokens_;            //!< A list of tokens.
+  size_t tokenCount_;        //!< Number of tokens in tokens_.
+  size_t parseErrorOffset_;  //!< Offset in code unit when parsing fail.
+  PointerParseErrorCode parseErrorCode_;  //!< Parsing error code.
+};
+
+//! GenericPointer for Value (UTF-8, default allocator).
+typedef GenericPointer<Value> Pointer;
+
+//!@name Helper functions for GenericPointer
+//@{
+
+//////////////////////////////////////////////////////////////////////////////
+
+template <typename T>
+typename T::ValueType &CreateValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    typename T::AllocatorType &a) {
+  return pointer.Create(root, a);
+}
+
+template <typename T, typename CharType, size_t N>
+typename T::ValueType &CreateValueByPointer(T &root,
+                                            const CharType (&source)[N],
+                                            typename T::AllocatorType &a) {
+  return GenericPointer<typename T::ValueType>(source, N - 1).Create(root, a);
+}
+
+// No allocator parameter
+
+template <typename DocumentType>
+typename DocumentType::ValueType &CreateValueByPointer(
+    DocumentType &document,
+    const GenericPointer<typename DocumentType::ValueType> &pointer) {
+  return pointer.Create(document);
+}
+
+template <typename DocumentType, typename CharType, size_t N>
+typename DocumentType::ValueType &CreateValueByPointer(
+    DocumentType &document, const CharType (&source)[N]) {
+  return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
+      .Create(document);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+template <typename T>
+typename T::ValueType *GetValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    size_t *unresolvedTokenIndex = 0) {
+  return pointer.Get(root, unresolvedTokenIndex);
+}
+
+template <typename T>
+const typename T::ValueType *GetValueByPointer(
+    const T &root, const GenericPointer<typename T::ValueType> &pointer,
+    size_t *unresolvedTokenIndex = 0) {
+  return pointer.Get(root, unresolvedTokenIndex);
+}
+
+template <typename T, typename CharType, size_t N>
+typename T::ValueType *GetValueByPointer(T &root, const CharType (&source)[N],
+                                         size_t *unresolvedTokenIndex = 0) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .Get(root, unresolvedTokenIndex);
+}
+
+template <typename T, typename CharType, size_t N>
+const typename T::ValueType *GetValueByPointer(
+    const T &root, const CharType (&source)[N],
+    size_t *unresolvedTokenIndex = 0) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .Get(root, unresolvedTokenIndex);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+template <typename T>
+typename T::ValueType &GetValueByPointerWithDefault(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    const typename T::ValueType &defaultValue, typename T::AllocatorType &a) {
+  return pointer.GetWithDefault(root, defaultValue, a);
+}
+
+template <typename T>
+typename T::ValueType &GetValueByPointerWithDefault(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    const typename T::Ch *defaultValue, typename T::AllocatorType &a) {
+  return pointer.GetWithDefault(root, defaultValue, a);
+}
+
+#if RAPIDJSON_HAS_STDSTRING
+template <typename T>
+typename T::ValueType &GetValueByPointerWithDefault(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    const std::basic_string<typename T::Ch> &defaultValue,
+    typename T::AllocatorType &a) {
+  return pointer.GetWithDefault(root, defaultValue, a);
+}
+#endif
+
+template <typename T, typename T2>
+RAPIDJSON_DISABLEIF_RETURN(
+    (internal::OrExpr<internal::IsPointer<T2>, internal::IsGenericValue<T2>>),
+    (typename T::ValueType &))
+GetValueByPointerWithDefault(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    T2 defaultValue, typename T::AllocatorType &a) {
+  return pointer.GetWithDefault(root, defaultValue, a);
+}
+
+template <typename T, typename CharType, size_t N>
+typename T::ValueType &GetValueByPointerWithDefault(
+    T &root, const CharType (&source)[N],
+    const typename T::ValueType &defaultValue, typename T::AllocatorType &a) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .GetWithDefault(root, defaultValue, a);
+}
+
+template <typename T, typename CharType, size_t N>
+typename T::ValueType &GetValueByPointerWithDefault(
+    T &root, const CharType (&source)[N], const typename T::Ch *defaultValue,
+    typename T::AllocatorType &a) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .GetWithDefault(root, defaultValue, a);
+}
+
+#if RAPIDJSON_HAS_STDSTRING
+template <typename T, typename CharType, size_t N>
+typename T::ValueType &GetValueByPointerWithDefault(
+    T &root, const CharType (&source)[N],
+    const std::basic_string<typename T::Ch> &defaultValue,
+    typename T::AllocatorType &a) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .GetWithDefault(root, defaultValue, a);
+}
+#endif
+
+template <typename T, typename CharType, size_t N, typename T2>
+RAPIDJSON_DISABLEIF_RETURN(
+    (internal::OrExpr<internal::IsPointer<T2>, internal::IsGenericValue<T2>>),
+    (typename T::ValueType &))
+GetValueByPointerWithDefault(T &root, const CharType (&source)[N],
+                             T2 defaultValue, typename T::AllocatorType &a) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .GetWithDefault(root, defaultValue, a);
+}
+
+// No allocator parameter
+
+template <typename DocumentType>
+typename DocumentType::ValueType &GetValueByPointerWithDefault(
+    DocumentType &document,
+    const GenericPointer<typename DocumentType::ValueType> &pointer,
+    const typename DocumentType::ValueType &defaultValue) {
+  return pointer.GetWithDefault(document, defaultValue);
+}
+
+template <typename DocumentType>
+typename DocumentType::ValueType &GetValueByPointerWithDefault(
+    DocumentType &document,
+    const GenericPointer<typename DocumentType::ValueType> &pointer,
+    const typename DocumentType::Ch *defaultValue) {
+  return pointer.GetWithDefault(document, defaultValue);
+}
+
+#if RAPIDJSON_HAS_STDSTRING
+template <typename DocumentType>
+typename DocumentType::ValueType &GetValueByPointerWithDefault(
+    DocumentType &document,
+    const GenericPointer<typename DocumentType::ValueType> &pointer,
+    const std::basic_string<typename DocumentType::Ch> &defaultValue) {
+  return pointer.GetWithDefault(document, defaultValue);
+}
+#endif
+
+template <typename DocumentType, typename T2>
+RAPIDJSON_DISABLEIF_RETURN(
+    (internal::OrExpr<internal::IsPointer<T2>, internal::IsGenericValue<T2>>),
+    (typename DocumentType::ValueType &))
+GetValueByPointerWithDefault(
+    DocumentType &document,
+    const GenericPointer<typename DocumentType::ValueType> &pointer,
+    T2 defaultValue) {
+  return pointer.GetWithDefault(document, defaultValue);
+}
+
+template <typename DocumentType, typename CharType, size_t N>
+typename DocumentType::ValueType &GetValueByPointerWithDefault(
+    DocumentType &document, const CharType (&source)[N],
+    const typename DocumentType::ValueType &defaultValue) {
+  return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
+      .GetWithDefault(document, defaultValue);
+}
+
+template <typename DocumentType, typename CharType, size_t N>
+typename DocumentType::ValueType &GetValueByPointerWithDefault(
+    DocumentType &document, const CharType (&source)[N],
+    const typename DocumentType::Ch *defaultValue) {
+  return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
+      .GetWithDefault(document, defaultValue);
+}
+
+#if RAPIDJSON_HAS_STDSTRING
+template <typename DocumentType, typename CharType, size_t N>
+typename DocumentType::ValueType &GetValueByPointerWithDefault(
+    DocumentType &document, const CharType (&source)[N],
+    const std::basic_string<typename DocumentType::Ch> &defaultValue) {
+  return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
+      .GetWithDefault(document, defaultValue);
+}
+#endif
+
+template <typename DocumentType, typename CharType, size_t N, typename T2>
+RAPIDJSON_DISABLEIF_RETURN(
+    (internal::OrExpr<internal::IsPointer<T2>, internal::IsGenericValue<T2>>),
+    (typename DocumentType::ValueType &))
+GetValueByPointerWithDefault(DocumentType &document,
+                             const CharType (&source)[N], T2 defaultValue) {
+  return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
+      .GetWithDefault(document, defaultValue);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+template <typename T>
+typename T::ValueType &SetValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    typename T::ValueType &value, typename T::AllocatorType &a) {
+  return pointer.Set(root, value, a);
+}
+
+template <typename T>
+typename T::ValueType &SetValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    const typename T::ValueType &value, typename T::AllocatorType &a) {
+  return pointer.Set(root, value, a);
+}
+
+template <typename T>
+typename T::ValueType &SetValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    const typename T::Ch *value, typename T::AllocatorType &a) {
+  return pointer.Set(root, value, a);
+}
+
+#if RAPIDJSON_HAS_STDSTRING
+template <typename T>
+typename T::ValueType &SetValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    const std::basic_string<typename T::Ch> &value,
+    typename T::AllocatorType &a) {
+  return pointer.Set(root, value, a);
+}
+#endif
+
+template <typename T, typename T2>
+RAPIDJSON_DISABLEIF_RETURN(
+    (internal::OrExpr<internal::IsPointer<T2>, internal::IsGenericValue<T2>>),
+    (typename T::ValueType &))
+SetValueByPointer(T &root, const GenericPointer<typename T::ValueType> &pointer,
+                  T2 value, typename T::AllocatorType &a) {
+  return pointer.Set(root, value, a);
+}
+
+template <typename T, typename CharType, size_t N>
+typename T::ValueType &SetValueByPointer(T &root, const CharType (&source)[N],
+                                         typename T::ValueType &value,
+                                         typename T::AllocatorType &a) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .Set(root, value, a);
+}
+
+template <typename T, typename CharType, size_t N>
+typename T::ValueType &SetValueByPointer(T &root, const CharType (&source)[N],
+                                         const typename T::ValueType &value,
+                                         typename T::AllocatorType &a) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .Set(root, value, a);
+}
+
+template <typename T, typename CharType, size_t N>
+typename T::ValueType &SetValueByPointer(T &root, const CharType (&source)[N],
+                                         const typename T::Ch *value,
+                                         typename T::AllocatorType &a) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .Set(root, value, a);
+}
+
+#if RAPIDJSON_HAS_STDSTRING
+template <typename T, typename CharType, size_t N>
+typename T::ValueType &SetValueByPointer(
+    T &root, const CharType (&source)[N],
+    const std::basic_string<typename T::Ch> &value,
+    typename T::AllocatorType &a) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .Set(root, value, a);
+}
+#endif
+
+template <typename T, typename CharType, size_t N, typename T2>
+RAPIDJSON_DISABLEIF_RETURN(
+    (internal::OrExpr<internal::IsPointer<T2>, internal::IsGenericValue<T2>>),
+    (typename T::ValueType &))
+SetValueByPointer(T &root, const CharType (&source)[N], T2 value,
+                  typename T::AllocatorType &a) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .Set(root, value, a);
+}
+
+// No allocator parameter
+
+template <typename DocumentType>
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document,
+    const GenericPointer<typename DocumentType::ValueType> &pointer,
+    typename DocumentType::ValueType &value) {
+  return pointer.Set(document, value);
+}
+
+template <typename DocumentType>
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document,
+    const GenericPointer<typename DocumentType::ValueType> &pointer,
+    const typename DocumentType::ValueType &value) {
+  return pointer.Set(document, value);
+}
+
+template <typename DocumentType>
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document,
+    const GenericPointer<typename DocumentType::ValueType> &pointer,
+    const typename DocumentType::Ch *value) {
+  return pointer.Set(document, value);
+}
+
+#if RAPIDJSON_HAS_STDSTRING
+template <typename DocumentType>
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document,
+    const GenericPointer<typename DocumentType::ValueType> &pointer,
+    const std::basic_string<typename DocumentType::Ch> &value) {
+  return pointer.Set(document, value);
+}
+#endif
+
+template <typename DocumentType, typename T2>
+RAPIDJSON_DISABLEIF_RETURN(
+    (internal::OrExpr<internal::IsPointer<T2>, internal::IsGenericValue<T2>>),
+    (typename DocumentType::ValueType &))
+SetValueByPointer(
+    DocumentType &document,
+    const GenericPointer<typename DocumentType::ValueType> &pointer, T2 value) {
+  return pointer.Set(document, value);
+}
+
+template <typename DocumentType, typename CharType, size_t N>
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document, const CharType (&source)[N],
+    typename DocumentType::ValueType &value) {
+  return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
+      .Set(document, value);
+}
+
+template <typename DocumentType, typename CharType, size_t N>
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document, const CharType (&source)[N],
+    const typename DocumentType::ValueType &value) {
+  return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
+      .Set(document, value);
+}
+
+template <typename DocumentType, typename CharType, size_t N>
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document, const CharType (&source)[N],
+    const typename DocumentType::Ch *value) {
+  return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
+      .Set(document, value);
+}
+
+#if RAPIDJSON_HAS_STDSTRING
+template <typename DocumentType, typename CharType, size_t N>
+typename DocumentType::ValueType &SetValueByPointer(
+    DocumentType &document, const CharType (&source)[N],
+    const std::basic_string<typename DocumentType::Ch> &value) {
+  return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
+      .Set(document, value);
+}
+#endif
+
+template <typename DocumentType, typename CharType, size_t N, typename T2>
+RAPIDJSON_DISABLEIF_RETURN(
+    (internal::OrExpr<internal::IsPointer<T2>, internal::IsGenericValue<T2>>),
+    (typename DocumentType::ValueType &))
+SetValueByPointer(DocumentType &document, const CharType (&source)[N],
+                  T2 value) {
+  return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
+      .Set(document, value);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+template <typename T>
+typename T::ValueType &SwapValueByPointer(
+    T &root, const GenericPointer<typename T::ValueType> &pointer,
+    typename T::ValueType &value, typename T::AllocatorType &a) {
+  return pointer.Swap(root, value, a);
+}
+
+template <typename T, typename CharType, size_t N>
+typename T::ValueType &SwapValueByPointer(T &root, const CharType (&source)[N],
+                                          typename T::ValueType &value,
+                                          typename T::AllocatorType &a) {
+  return GenericPointer<typename T::ValueType>(source, N - 1)
+      .Swap(root, value, a);
+}
+
+template <typename DocumentType>
+typename DocumentType::ValueType &SwapValueByPointer(
+    DocumentType &document,
+    const GenericPointer<typename DocumentType::ValueType> &pointer,
+    typename DocumentType::ValueType &value) {
+  return pointer.Swap(document, value);
+}
+
+template <typename DocumentType, typename CharType, size_t N>
+typename DocumentType::ValueType &SwapValueByPointer(
+    DocumentType &document, const CharType (&source)[N],
+    typename DocumentType::ValueType &value) {
+  return GenericPointer<typename DocumentType::ValueType>(source, N - 1)
+      .Swap(document, value);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+
+template <typename T>
+bool EraseValueByPointer(T &root,
+                         const GenericPointer<typename T::ValueType> &pointer) {
+  return pointer.Erase(root);
+}
+
+template <typename T, typename CharType, size_t N>
+bool EraseValueByPointer(T &root, const CharType (&source)[N]) {
+  return GenericPointer<typename T::ValueType>(source, N - 1).Erase(root);
+}
+
+//@}
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__) || defined(_MSC_VER)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_POINTER_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/prettywriter.h b/livox_ros_driver2/3rdparty/rapidjson/prettywriter.h
new file mode 100644
index 0000000..f24bd0f
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/prettywriter.h
@@ -0,0 +1,333 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_PRETTYWRITER_H_
+#define RAPIDJSON_PRETTYWRITER_H_
+
+#include "writer.h"
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Combination of PrettyWriter format flags.
+/*! \see PrettyWriter::SetFormatOptions
+ */
+enum PrettyFormatOptions {
+  kFormatDefault = 0,         //!< Default pretty formatting.
+  kFormatSingleLineArray = 1  //!< Format arrays on a single line.
+};
+
+//! Writer with indentation and spacing.
+/*!
+    \tparam OutputStream Type of output os.
+    \tparam SourceEncoding Encoding of source string.
+    \tparam TargetEncoding Encoding of output stream.
+    \tparam StackAllocator Type of allocator for allocating memory of stack.
+*/
+template <typename OutputStream, typename SourceEncoding = UTF8<>,
+          typename TargetEncoding = UTF8<>,
+          typename StackAllocator = CrtAllocator,
+          unsigned writeFlags = kWriteDefaultFlags>
+class PrettyWriter : public Writer<OutputStream, SourceEncoding, TargetEncoding,
+                                   StackAllocator, writeFlags> {
+ public:
+  typedef Writer<OutputStream, SourceEncoding, TargetEncoding, StackAllocator,
+                 writeFlags>
+      Base;
+  typedef typename Base::Ch Ch;
+
+  //! Constructor
+  /*! \param os Output stream.
+      \param allocator User supplied allocator. If it is null, it will create a
+     private one. \param levelDepth Initial capacity of stack.
+  */
+  explicit PrettyWriter(OutputStream &os, StackAllocator *allocator = 0,
+                        size_t levelDepth = Base::kDefaultLevelDepth)
+      : Base(os, allocator, levelDepth),
+        indentChar_(' '),
+        indentCharCount_(4),
+        formatOptions_(kFormatDefault) {}
+
+  explicit PrettyWriter(StackAllocator *allocator = 0,
+                        size_t levelDepth = Base::kDefaultLevelDepth)
+      : Base(allocator, levelDepth), indentChar_(' '), indentCharCount_(4) {}
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  PrettyWriter(PrettyWriter &&rhs)
+      : Base(std::forward<PrettyWriter>(rhs)),
+        indentChar_(rhs.indentChar_),
+        indentCharCount_(rhs.indentCharCount_),
+        formatOptions_(rhs.formatOptions_) {}
+#endif
+
+  //! Set custom indentation.
+  /*! \param indentChar       Character for indentation. Must be whitespace
+     character (' ', '\\t', '\\n', '\\r'). \param indentCharCount  Number of
+     indent characters for each indentation level. \note The default indentation
+     is 4 spaces.
+  */
+  PrettyWriter &SetIndent(Ch indentChar, unsigned indentCharCount) {
+    RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' ||
+                     indentChar == '\n' || indentChar == '\r');
+    indentChar_ = indentChar;
+    indentCharCount_ = indentCharCount;
+    return *this;
+  }
+
+  //! Set pretty writer formatting options.
+  /*! \param options Formatting options.
+   */
+  PrettyWriter &SetFormatOptions(PrettyFormatOptions options) {
+    formatOptions_ = options;
+    return *this;
+  }
+
+  /*! @name Implementation of Handler
+      \see Handler
+  */
+  //@{
+
+  bool Null() {
+    PrettyPrefix(kNullType);
+    return Base::EndValue(Base::WriteNull());
+  }
+  bool Bool(bool b) {
+    PrettyPrefix(b ? kTrueType : kFalseType);
+    return Base::EndValue(Base::WriteBool(b));
+  }
+  bool Int(int i) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteInt(i));
+  }
+  bool Uint(unsigned u) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteUint(u));
+  }
+  bool Int64(int64_t i64) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteInt64(i64));
+  }
+  bool Uint64(uint64_t u64) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteUint64(u64));
+  }
+  bool Double(double d) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteDouble(d));
+  }
+
+  bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
+    RAPIDJSON_ASSERT(str != 0);
+    (void)copy;
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteString(str, length));
+  }
+
+  bool String(const Ch *str, SizeType length, bool copy = false) {
+    RAPIDJSON_ASSERT(str != 0);
+    (void)copy;
+    PrettyPrefix(kStringType);
+    return Base::EndValue(Base::WriteString(str, length));
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool String(const std::basic_string<Ch> &str) {
+    return String(str.data(), SizeType(str.size()));
+  }
+#endif
+
+  bool StartObject() {
+    PrettyPrefix(kObjectType);
+    new (Base::level_stack_.template Push<typename Base::Level>())
+        typename Base::Level(false);
+    return Base::WriteStartObject();
+  }
+
+  bool Key(const Ch *str, SizeType length, bool copy = false) {
+    return String(str, length, copy);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool Key(const std::basic_string<Ch> &str) {
+    return Key(str.data(), SizeType(str.size()));
+  }
+#endif
+
+  bool EndObject(SizeType memberCount = 0) {
+    (void)memberCount;
+    RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
+                     sizeof(typename Base::Level));  // not inside an Object
+    RAPIDJSON_ASSERT(!Base::level_stack_.template Top<typename Base::Level>()
+                          ->inArray);  // currently inside an Array, not Object
+    RAPIDJSON_ASSERT(
+        0 ==
+        Base::level_stack_.template Top<typename Base::Level>()->valueCount %
+            2);  // Object has a Key without a Value
+
+    bool empty =
+        Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
+        0;
+
+    if (!empty) {
+      Base::os_->Put('\n');
+      WriteIndent();
+    }
+    bool ret = Base::EndValue(Base::WriteEndObject());
+    (void)ret;
+    RAPIDJSON_ASSERT(ret == true);
+    if (Base::level_stack_.Empty())  // end of json text
+      Base::Flush();
+    return true;
+  }
+
+  bool StartArray() {
+    PrettyPrefix(kArrayType);
+    new (Base::level_stack_.template Push<typename Base::Level>())
+        typename Base::Level(true);
+    return Base::WriteStartArray();
+  }
+
+  bool EndArray(SizeType memberCount = 0) {
+    (void)memberCount;
+    RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
+                     sizeof(typename Base::Level));
+    RAPIDJSON_ASSERT(
+        Base::level_stack_.template Top<typename Base::Level>()->inArray);
+    bool empty =
+        Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
+        0;
+
+    if (!empty && !(formatOptions_ & kFormatSingleLineArray)) {
+      Base::os_->Put('\n');
+      WriteIndent();
+    }
+    bool ret = Base::EndValue(Base::WriteEndArray());
+    (void)ret;
+    RAPIDJSON_ASSERT(ret == true);
+    if (Base::level_stack_.Empty())  // end of json text
+      Base::Flush();
+    return true;
+  }
+
+  //@}
+
+  /*! @name Convenience extensions */
+  //@{
+
+  //! Simpler but slower overload.
+  bool String(const Ch *str) { return String(str, internal::StrLen(str)); }
+  bool Key(const Ch *str) { return Key(str, internal::StrLen(str)); }
+
+  //@}
+
+  //! Write a raw JSON value.
+  /*!
+      For user to write a stringified JSON as a value.
+
+      \param json A well-formed JSON value. It should not contain null character
+     within [0, length - 1] range. \param length Length of the json. \param type
+     Type of the root of json. \note When using PrettyWriter::RawValue(), the
+     result json may not be indented correctly.
+  */
+  bool RawValue(const Ch *json, size_t length, Type type) {
+    RAPIDJSON_ASSERT(json != 0);
+    PrettyPrefix(type);
+    return Base::EndValue(Base::WriteRawValue(json, length));
+  }
+
+ protected:
+  void PrettyPrefix(Type type) {
+    (void)type;
+    if (Base::level_stack_.GetSize() != 0) {  // this value is not at root
+      typename Base::Level *level =
+          Base::level_stack_.template Top<typename Base::Level>();
+
+      if (level->inArray) {
+        if (level->valueCount > 0) {
+          Base::os_->Put(
+              ',');  // add comma if it is not the first element in array
+          if (formatOptions_ & kFormatSingleLineArray) Base::os_->Put(' ');
+        }
+
+        if (!(formatOptions_ & kFormatSingleLineArray)) {
+          Base::os_->Put('\n');
+          WriteIndent();
+        }
+      } else {  // in object
+        if (level->valueCount > 0) {
+          if (level->valueCount % 2 == 0) {
+            Base::os_->Put(',');
+            Base::os_->Put('\n');
+          } else {
+            Base::os_->Put(':');
+            Base::os_->Put(' ');
+          }
+        } else
+          Base::os_->Put('\n');
+
+        if (level->valueCount % 2 == 0) WriteIndent();
+      }
+      if (!level->inArray && level->valueCount % 2 == 0)
+        RAPIDJSON_ASSERT(type == kStringType);  // if it's in object, then even
+                                                // number should be a name
+      level->valueCount++;
+    } else {
+      RAPIDJSON_ASSERT(
+          !Base::hasRoot_);  // Should only has one and only one root.
+      Base::hasRoot_ = true;
+    }
+  }
+
+  void WriteIndent() {
+    size_t count =
+        (Base::level_stack_.GetSize() / sizeof(typename Base::Level)) *
+        indentCharCount_;
+    PutN(*Base::os_, static_cast<typename OutputStream::Ch>(indentChar_),
+         count);
+  }
+
+  Ch indentChar_;
+  unsigned indentCharCount_;
+  PrettyFormatOptions formatOptions_;
+
+ private:
+  // Prohibit copy constructor & assignment operator.
+  PrettyWriter(const PrettyWriter &);
+  PrettyWriter &operator=(const PrettyWriter &);
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_RAPIDJSON_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/rapidjson.h b/livox_ros_driver2/3rdparty/rapidjson/rapidjson.h
new file mode 100644
index 0000000..7af85e8
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/rapidjson.h
@@ -0,0 +1,719 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_RAPIDJSON_H_
+#define RAPIDJSON_RAPIDJSON_H_
+
+/*!\file rapidjson.h
+    \brief common definitions and configuration
+
+    \see RAPIDJSON_CONFIG
+ */
+
+/*! \defgroup RAPIDJSON_CONFIG RapidJSON configuration
+    \brief Configuration macros for library features
+
+    Some RapidJSON features are configurable to adapt the library to a wide
+    variety of platforms, environments and usage scenarios.  Most of the
+    features can be configured in terms of overridden or predefined
+    preprocessor macros at compile-time.
+
+    Some additional customization is available in the \ref RAPIDJSON_ERRORS
+   APIs.
+
+    \note These macros should be given on the compiler command-line
+          (where applicable)  to avoid inconsistent values when compiling
+          different translation units of a single application.
+ */
+
+#include <cstdlib>  // malloc(), realloc(), free(), size_t
+#include <cstring>  // memset(), memcpy(), memmove(), memcmp()
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_VERSION_STRING
+//
+// ALWAYS synchronize the following 3 macros with corresponding variables in
+// /CMakeLists.txt.
+//
+
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+// token stringification
+#define RAPIDJSON_STRINGIFY(x) RAPIDJSON_DO_STRINGIFY(x)
+#define RAPIDJSON_DO_STRINGIFY(x) #x
+
+// token concatenation
+#define RAPIDJSON_JOIN(X, Y) RAPIDJSON_DO_JOIN(X, Y)
+#define RAPIDJSON_DO_JOIN(X, Y) RAPIDJSON_DO_JOIN2(X, Y)
+#define RAPIDJSON_DO_JOIN2(X, Y) X##Y
+//!@endcond
+
+/*! \def RAPIDJSON_MAJOR_VERSION
+    \ingroup RAPIDJSON_CONFIG
+    \brief Major version of RapidJSON in integer.
+*/
+/*! \def RAPIDJSON_MINOR_VERSION
+    \ingroup RAPIDJSON_CONFIG
+    \brief Minor version of RapidJSON in integer.
+*/
+/*! \def RAPIDJSON_PATCH_VERSION
+    \ingroup RAPIDJSON_CONFIG
+    \brief Patch version of RapidJSON in integer.
+*/
+/*! \def RAPIDJSON_VERSION_STRING
+    \ingroup RAPIDJSON_CONFIG
+    \brief Version of RapidJSON in "<major>.<minor>.<patch>" string format.
+*/
+#define RAPIDJSON_MAJOR_VERSION 1
+#define RAPIDJSON_MINOR_VERSION 1
+#define RAPIDJSON_PATCH_VERSION 0
+#define RAPIDJSON_VERSION_STRING \
+  RAPIDJSON_STRINGIFY(           \
+      RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION)
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NAMESPACE_(BEGIN|END)
+/*! \def RAPIDJSON_NAMESPACE
+    \ingroup RAPIDJSON_CONFIG
+    \brief   provide custom rapidjson namespace
+
+    In order to avoid symbol clashes and/or "One Definition Rule" errors
+    between multiple inclusions of (different versions of) RapidJSON in
+    a single binary, users can customize the name of the main RapidJSON
+    namespace.
+
+    In case of a single nesting level, defining \c RAPIDJSON_NAMESPACE
+    to a custom name (e.g. \c MyRapidJSON) is sufficient.  If multiple
+    levels are needed, both \ref RAPIDJSON_NAMESPACE_BEGIN and \ref
+    RAPIDJSON_NAMESPACE_END need to be defined as well:
+
+    \code
+    // in some .cpp file
+    #define RAPIDJSON_NAMESPACE my::rapidjson
+    #define RAPIDJSON_NAMESPACE_BEGIN namespace my { namespace rapidjson {
+    #define RAPIDJSON_NAMESPACE_END   } }
+    #include "rapidjson/..."
+    \endcode
+
+    \see rapidjson
+ */
+/*! \def RAPIDJSON_NAMESPACE_BEGIN
+    \ingroup RAPIDJSON_CONFIG
+    \brief   provide custom rapidjson namespace (opening expression)
+    \see RAPIDJSON_NAMESPACE
+*/
+/*! \def RAPIDJSON_NAMESPACE_END
+    \ingroup RAPIDJSON_CONFIG
+    \brief   provide custom rapidjson namespace (closing expression)
+    \see RAPIDJSON_NAMESPACE
+*/
+#ifndef RAPIDJSON_NAMESPACE
+#define RAPIDJSON_NAMESPACE rapidjson
+#endif
+#ifndef RAPIDJSON_NAMESPACE_BEGIN
+#define RAPIDJSON_NAMESPACE_BEGIN namespace RAPIDJSON_NAMESPACE {
+#endif
+#ifndef RAPIDJSON_NAMESPACE_END
+#define RAPIDJSON_NAMESPACE_END }
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_HAS_STDSTRING
+
+#ifndef RAPIDJSON_HAS_STDSTRING
+#ifdef RAPIDJSON_DOXYGEN_RUNNING
+#define RAPIDJSON_HAS_STDSTRING 1  // force generation of documentation
+#else
+#define RAPIDJSON_HAS_STDSTRING 0  // no std::string support by default
+#endif
+/*! \def RAPIDJSON_HAS_STDSTRING
+    \ingroup RAPIDJSON_CONFIG
+    \brief Enable RapidJSON support for \c std::string
+
+    By defining this preprocessor symbol to \c 1, several convenience functions
+   for using \ref rapidjson::GenericValue with \c std::string are enabled,
+   especially for construction and comparison.
+
+    \hideinitializer
+*/
+#endif  // !defined(RAPIDJSON_HAS_STDSTRING)
+
+#if RAPIDJSON_HAS_STDSTRING
+#include <string>
+#endif  // RAPIDJSON_HAS_STDSTRING
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NO_INT64DEFINE
+
+/*! \def RAPIDJSON_NO_INT64DEFINE
+    \ingroup RAPIDJSON_CONFIG
+    \brief Use external 64-bit integer types.
+
+    RapidJSON requires the 64-bit integer types \c int64_t and  \c uint64_t
+   types to be available at global scope.
+
+    If users have their own definition, define RAPIDJSON_NO_INT64DEFINE to
+    prevent RapidJSON from defining its own types.
+*/
+#ifndef RAPIDJSON_NO_INT64DEFINE
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#if defined(_MSC_VER) && (_MSC_VER < 1800)  // Visual Studio 2013
+#include "msinttypes/inttypes.h"
+#include "msinttypes/stdint.h"
+#else
+// Other compilers should have this.
+#include <inttypes.h>
+#include <stdint.h>
+#endif
+//!@endcond
+#ifdef RAPIDJSON_DOXYGEN_RUNNING
+#define RAPIDJSON_NO_INT64DEFINE
+#endif
+#endif  // RAPIDJSON_NO_INT64TYPEDEF
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_FORCEINLINE
+
+#ifndef RAPIDJSON_FORCEINLINE
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#if defined(_MSC_VER) && defined(NDEBUG)
+#define RAPIDJSON_FORCEINLINE __forceinline
+#elif defined(__GNUC__) && __GNUC__ >= 4 && defined(NDEBUG)
+#define RAPIDJSON_FORCEINLINE __attribute__((always_inline))
+#else
+#define RAPIDJSON_FORCEINLINE
+#endif
+//!@endcond
+#endif  // RAPIDJSON_FORCEINLINE
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ENDIAN
+#define RAPIDJSON_LITTLEENDIAN 0  //!< Little endian machine
+#define RAPIDJSON_BIGENDIAN 1     //!< Big endian machine
+
+//! Endianness of the machine.
+/*!
+    \def RAPIDJSON_ENDIAN
+    \ingroup RAPIDJSON_CONFIG
+
+    GCC 4.6 provided macro for detecting endianness of the target machine. But
+   other compilers may not have this. User can define RAPIDJSON_ENDIAN to either
+    \ref RAPIDJSON_LITTLEENDIAN or \ref RAPIDJSON_BIGENDIAN.
+
+    Default detection implemented with reference to
+    \li
+   https://gcc.gnu.org/onlinedocs/gcc-4.6.0/cpp/Common-Predefined-Macros.html
+    \li http://www.boost.org/doc/libs/1_42_0/boost/detail/endian.hpp
+*/
+#ifndef RAPIDJSON_ENDIAN
+// Detect with GCC 4.6's macro
+#ifdef __BYTE_ORDER__
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+#else
+#      error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
+#endif  // __BYTE_ORDER__
+// Detect with GLIBC's endian.h
+#elif defined(__GLIBC__)
+#include <endian.h>
+#if (__BYTE_ORDER == __LITTLE_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif (__BYTE_ORDER == __BIG_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+#else
+#      error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
+#endif  // __GLIBC__
+// Detect with _LITTLE_ENDIAN and _BIG_ENDIAN macro
+#elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+// Detect with architecture macros
+#elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || \
+    defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || \
+    defined(__hppa) || defined(_MIPSEB) || defined(_POWER) ||      \
+    defined(__s390__)
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+#elif defined(__i386__) || defined(__alpha__) || defined(__ia64) ||  \
+    defined(__ia64__) || defined(_M_IX86) || defined(_M_IA64) ||     \
+    defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) ||   \
+    defined(_M_AMD64) || defined(__x86_64) || defined(__x86_64__) || \
+    defined(_M_X64) || defined(__bfin__)
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64))
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif defined(RAPIDJSON_DOXYGEN_RUNNING)
+#define RAPIDJSON_ENDIAN
+#else
+#    error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
+#endif
+#endif  // RAPIDJSON_ENDIAN
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_64BIT
+
+//! Whether using 64-bit architecture
+#ifndef RAPIDJSON_64BIT
+#if defined(__LP64__) || (defined(__x86_64__) && defined(__ILP32__)) || \
+    defined(_WIN64) || defined(__EMSCRIPTEN__)
+#define RAPIDJSON_64BIT 1
+#else
+#define RAPIDJSON_64BIT 0
+#endif
+#endif  // RAPIDJSON_64BIT
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ALIGN
+
+//! Data alignment of the machine.
+/*! \ingroup RAPIDJSON_CONFIG
+    \param x pointer to align
+
+    Some machines require strict data alignment. The default is 8 bytes.
+    User can customize by defining the RAPIDJSON_ALIGN function macro.
+*/
+#ifndef RAPIDJSON_ALIGN
+#define RAPIDJSON_ALIGN(x) \
+  (((x) + static_cast<size_t>(7u)) & ~static_cast<size_t>(7u))
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_UINT64_C2
+
+//! Construct a 64-bit literal by a pair of 32-bit integer.
+/*!
+    64-bit literal with or without ULL suffix is prone to compiler warnings.
+    UINT64_C() is C macro which cause compilation problems.
+    Use this macro to define 64-bit constants by a pair of 32-bit integer.
+*/
+#ifndef RAPIDJSON_UINT64_C2
+#define RAPIDJSON_UINT64_C2(high32, low32) \
+  ((static_cast<uint64_t>(high32) << 32) | static_cast<uint64_t>(low32))
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_48BITPOINTER_OPTIMIZATION
+
+//! Use only lower 48-bit address for some pointers.
+/*!
+    \ingroup RAPIDJSON_CONFIG
+
+    This optimization uses the fact that current X86-64 architecture only
+   implement lower 48-bit virtual address. The higher 16-bit can be used for
+   storing other data. \c GenericValue uses this optimization to reduce its size
+   form 24 bytes to 16 bytes in 64-bit architecture.
+*/
+#ifndef RAPIDJSON_48BITPOINTER_OPTIMIZATION
+#if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || \
+    defined(__x86_64) || defined(_M_X64) || defined(_M_AMD64)
+#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 1
+#else
+#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 0
+#endif
+#endif  // RAPIDJSON_48BITPOINTER_OPTIMIZATION
+
+#if RAPIDJSON_48BITPOINTER_OPTIMIZATION == 1
+#if RAPIDJSON_64BIT != 1
+#error RAPIDJSON_48BITPOINTER_OPTIMIZATION can only be set to 1 when RAPIDJSON_64BIT=1
+#endif
+#define RAPIDJSON_SETPOINTER(type, p, x)                                       \
+  (p = reinterpret_cast<type *>(                                               \
+       (reinterpret_cast<uintptr_t>(p) &                                       \
+        static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0xFFFF0000, 0x00000000))) | \
+       reinterpret_cast<uintptr_t>(reinterpret_cast<const void *>(x))))
+#define RAPIDJSON_GETPOINTER(type, p)  \
+  (reinterpret_cast<type *>(           \
+      reinterpret_cast<uintptr_t>(p) & \
+      static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0x0000FFFF, 0xFFFFFFFF))))
+#else
+#define RAPIDJSON_SETPOINTER(type, p, x) (p = (x))
+#define RAPIDJSON_GETPOINTER(type, p) (p)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_SSE2/RAPIDJSON_SSE42/RAPIDJSON_NEON/RAPIDJSON_SIMD
+
+/*! \def RAPIDJSON_SIMD
+    \ingroup RAPIDJSON_CONFIG
+    \brief Enable SSE2/SSE4.2/Neon optimization.
+
+    RapidJSON supports optimized implementations for some parsing operations
+    based on the SSE2, SSE4.2 or NEon SIMD extensions on modern Intel
+    or ARM compatible processors.
+
+    To enable these optimizations, three different symbols can be defined;
+    \code
+    // Enable SSE2 optimization.
+    #define RAPIDJSON_SSE2
+
+    // Enable SSE4.2 optimization.
+    #define RAPIDJSON_SSE42
+    \endcode
+
+    // Enable ARM Neon optimization.
+    #define RAPIDJSON_NEON
+    \endcode
+
+    \c RAPIDJSON_SSE42 takes precedence over SSE2, if both are defined.
+
+    If any of these symbols is defined, RapidJSON defines the macro
+    \c RAPIDJSON_SIMD to indicate the availability of the optimized code.
+*/
+#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) || \
+    defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING)
+#define RAPIDJSON_SIMD
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NO_SIZETYPEDEFINE
+
+#ifndef RAPIDJSON_NO_SIZETYPEDEFINE
+/*! \def RAPIDJSON_NO_SIZETYPEDEFINE
+    \ingroup RAPIDJSON_CONFIG
+    \brief User-provided \c SizeType definition.
+
+    In order to avoid using 32-bit size types for indexing strings and arrays,
+    define this preprocessor symbol and provide the type rapidjson::SizeType
+    before including RapidJSON:
+    \code
+    #define RAPIDJSON_NO_SIZETYPEDEFINE
+    namespace rapidjson { typedef ::std::size_t SizeType; }
+    #include "rapidjson/..."
+    \endcode
+
+    \see rapidjson::SizeType
+*/
+#ifdef RAPIDJSON_DOXYGEN_RUNNING
+#define RAPIDJSON_NO_SIZETYPEDEFINE
+#endif
+RAPIDJSON_NAMESPACE_BEGIN
+//! Size type (for string lengths, array sizes, etc.)
+/*! RapidJSON uses 32-bit array/string indices even on 64-bit platforms,
+    instead of using \c size_t. Users may override the SizeType by defining
+    \ref RAPIDJSON_NO_SIZETYPEDEFINE.
+*/
+typedef unsigned SizeType;
+RAPIDJSON_NAMESPACE_END
+#endif
+
+// always import std::size_t to rapidjson namespace
+RAPIDJSON_NAMESPACE_BEGIN
+using std::size_t;
+RAPIDJSON_NAMESPACE_END
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ASSERT
+
+//! Assertion.
+/*! \ingroup RAPIDJSON_CONFIG
+    By default, rapidjson uses C \c assert() for internal assertions.
+    User can override it by defining RAPIDJSON_ASSERT(x) macro.
+
+    \note Parsing errors are handled and can be customized by the
+          \ref RAPIDJSON_ERRORS APIs.
+*/
+#ifndef RAPIDJSON_ASSERT
+#include <cassert>
+#define RAPIDJSON_ASSERT(x) assert(x)
+#endif  // RAPIDJSON_ASSERT
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_STATIC_ASSERT
+
+// Prefer C++11 static_assert, if available
+#ifndef RAPIDJSON_STATIC_ASSERT
+#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800)
+#define RAPIDJSON_STATIC_ASSERT(x) static_assert(x, RAPIDJSON_STRINGIFY(x))
+#endif  // C++11
+#endif  // RAPIDJSON_STATIC_ASSERT
+
+// Adopt C++03 implementation from boost
+#ifndef RAPIDJSON_STATIC_ASSERT
+#ifndef __clang__
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#endif
+RAPIDJSON_NAMESPACE_BEGIN
+template <bool x>
+struct STATIC_ASSERTION_FAILURE;
+template <>
+struct STATIC_ASSERTION_FAILURE<true> {
+  enum { value = 1 };
+};
+template <size_t x>
+struct StaticAssertTest {};
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__GNUC__) || defined(__clang__)
+#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE __attribute__((unused))
+#else
+#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
+#endif
+#ifndef __clang__
+//!@endcond
+#endif
+
+/*! \def RAPIDJSON_STATIC_ASSERT
+    \brief (Internal) macro to check for conditions at compile-time
+    \param x compile-time condition
+    \hideinitializer
+ */
+#define RAPIDJSON_STATIC_ASSERT(x)                               \
+  typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest<sizeof(        \
+      ::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x)>)> \
+      RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__)              \
+          RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
+#endif  // RAPIDJSON_STATIC_ASSERT
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_LIKELY, RAPIDJSON_UNLIKELY
+
+//! Compiler branching hint for expression with high probability to be true.
+/*!
+    \ingroup RAPIDJSON_CONFIG
+    \param x Boolean expression likely to be true.
+*/
+#ifndef RAPIDJSON_LIKELY
+#if defined(__GNUC__) || defined(__clang__)
+#define RAPIDJSON_LIKELY(x) __builtin_expect(!!(x), 1)
+#else
+#define RAPIDJSON_LIKELY(x) (x)
+#endif
+#endif
+
+//! Compiler branching hint for expression with low probability to be true.
+/*!
+    \ingroup RAPIDJSON_CONFIG
+    \param x Boolean expression unlikely to be true.
+*/
+#ifndef RAPIDJSON_UNLIKELY
+#if defined(__GNUC__) || defined(__clang__)
+#define RAPIDJSON_UNLIKELY(x) __builtin_expect(!!(x), 0)
+#else
+#define RAPIDJSON_UNLIKELY(x) (x)
+#endif
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// Helpers
+
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+
+#define RAPIDJSON_MULTILINEMACRO_BEGIN do {
+#define RAPIDJSON_MULTILINEMACRO_END \
+  }                                  \
+  while ((void)0, 0)
+
+// adopted from Boost
+#define RAPIDJSON_VERSION_CODE(x, y, z) (((x)*100000) + ((y)*100) + (z))
+
+#if defined(__has_builtin)
+#define RAPIDJSON_HAS_BUILTIN(x) __has_builtin(x)
+#else
+#define RAPIDJSON_HAS_BUILTIN(x) 0
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_DIAG_PUSH/POP, RAPIDJSON_DIAG_OFF
+
+#if defined(__GNUC__)
+#define RAPIDJSON_GNUC \
+  RAPIDJSON_VERSION_CODE(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__)
+#endif
+
+#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
+                           RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 2, 0))
+
+#define RAPIDJSON_PRAGMA(x) _Pragma(RAPIDJSON_STRINGIFY(x))
+#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(GCC diagnostic x)
+#define RAPIDJSON_DIAG_OFF(x) \
+  RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W, x)))
+
+// push/pop support in Clang and GCC>=4.6
+#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
+                           RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0))
+#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
+#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
+#else                       // GCC >= 4.2, < 4.6
+#define RAPIDJSON_DIAG_PUSH /* ignored */
+#define RAPIDJSON_DIAG_POP  /* ignored */
+#endif
+
+#elif defined(_MSC_VER)
+
+// pragma (MSVC specific)
+#define RAPIDJSON_PRAGMA(x) __pragma(x)
+#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(warning(x))
+
+#define RAPIDJSON_DIAG_OFF(x) RAPIDJSON_DIAG_PRAGMA(disable : x)
+#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
+#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
+
+#else
+
+#define RAPIDJSON_DIAG_OFF(x) /* ignored */
+#define RAPIDJSON_DIAG_PUSH   /* ignored */
+#define RAPIDJSON_DIAG_POP    /* ignored */
+
+#endif  // RAPIDJSON_DIAG_*
+
+///////////////////////////////////////////////////////////////////////////////
+// C++11 features
+
+#ifndef RAPIDJSON_HAS_CXX11_RVALUE_REFS
+#if defined(__clang__)
+#if __has_feature(cxx_rvalue_references) &&           \
+    (defined(_MSC_VER) || defined(_LIBCPP_VERSION) || \
+     defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306)
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
+#else
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
+#endif
+#elif (defined(RAPIDJSON_GNUC) &&                             \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 3, 0)) && \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
+    (defined(_MSC_VER) && _MSC_VER >= 1600) ||                \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
+     defined(__GXX_EXPERIMENTAL_CXX0X__))
+
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
+#else
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
+#endif
+#endif  // RAPIDJSON_HAS_CXX11_RVALUE_REFS
+
+#ifndef RAPIDJSON_HAS_CXX11_NOEXCEPT
+#if defined(__clang__)
+#define RAPIDJSON_HAS_CXX11_NOEXCEPT __has_feature(cxx_noexcept)
+#elif (defined(RAPIDJSON_GNUC) &&                             \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
+    (defined(_MSC_VER) && _MSC_VER >= 1900) ||                \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
+     defined(__GXX_EXPERIMENTAL_CXX0X__))
+#define RAPIDJSON_HAS_CXX11_NOEXCEPT 1
+#else
+#define RAPIDJSON_HAS_CXX11_NOEXCEPT 0
+#endif
+#endif
+#if RAPIDJSON_HAS_CXX11_NOEXCEPT
+#define RAPIDJSON_NOEXCEPT noexcept
+#else
+#define RAPIDJSON_NOEXCEPT /* noexcept */
+#endif                     // RAPIDJSON_HAS_CXX11_NOEXCEPT
+
+// no automatic detection, yet
+#ifndef RAPIDJSON_HAS_CXX11_TYPETRAITS
+#if (defined(_MSC_VER) && _MSC_VER >= 1700)
+#define RAPIDJSON_HAS_CXX11_TYPETRAITS 1
+#else
+#define RAPIDJSON_HAS_CXX11_TYPETRAITS 0
+#endif
+#endif
+
+#ifndef RAPIDJSON_HAS_CXX11_RANGE_FOR
+#if defined(__clang__)
+#define RAPIDJSON_HAS_CXX11_RANGE_FOR __has_feature(cxx_range_for)
+#elif (defined(RAPIDJSON_GNUC) &&                             \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
+    (defined(_MSC_VER) && _MSC_VER >= 1700) ||                \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
+     defined(__GXX_EXPERIMENTAL_CXX0X__))
+#define RAPIDJSON_HAS_CXX11_RANGE_FOR 1
+#else
+#define RAPIDJSON_HAS_CXX11_RANGE_FOR 0
+#endif
+#endif  // RAPIDJSON_HAS_CXX11_RANGE_FOR
+
+///////////////////////////////////////////////////////////////////////////////
+// C++17 features
+
+#if defined(__has_cpp_attribute)
+#if __has_cpp_attribute(fallthrough)
+#define RAPIDJSON_DELIBERATE_FALLTHROUGH [[fallthrough]]
+#else
+#define RAPIDJSON_DELIBERATE_FALLTHROUGH
+#endif
+#else
+#define RAPIDJSON_DELIBERATE_FALLTHROUGH
+#endif
+
+//!@endcond
+
+//! Assertion (in non-throwing contexts).
+/*! \ingroup RAPIDJSON_CONFIG
+   Some functions provide a \c noexcept guarantee, if the compiler supports it.
+   In these cases, the \ref RAPIDJSON_ASSERT macro cannot be overridden to
+   throw an exception.  This macro adds a separate customization point for
+   such cases.
+
+   Defaults to C \c assert() (as \ref RAPIDJSON_ASSERT), if \c noexcept is
+   supported, and to \ref RAPIDJSON_ASSERT otherwise.
+*/
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NOEXCEPT_ASSERT
+
+#ifndef RAPIDJSON_NOEXCEPT_ASSERT
+#ifdef RAPIDJSON_ASSERT_THROWS
+#if RAPIDJSON_HAS_CXX11_NOEXCEPT
+#define RAPIDJSON_NOEXCEPT_ASSERT(x)
+#else
+#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
+#endif  // RAPIDJSON_HAS_CXX11_NOEXCEPT
+#else
+#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
+#endif  // RAPIDJSON_ASSERT_THROWS
+#endif  // RAPIDJSON_NOEXCEPT_ASSERT
+
+///////////////////////////////////////////////////////////////////////////////
+// new/delete
+
+#ifndef RAPIDJSON_NEW
+///! customization point for global \c new
+#define RAPIDJSON_NEW(TypeName) new TypeName
+#endif
+#ifndef RAPIDJSON_DELETE
+///! customization point for global \c delete
+#define RAPIDJSON_DELETE(x) delete x
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// Type
+
+/*! \namespace rapidjson
+    \brief main RapidJSON namespace
+    \see RAPIDJSON_NAMESPACE
+*/
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Type of JSON value
+enum Type {
+  kNullType = 0,    //!< null
+  kFalseType = 1,   //!< false
+  kTrueType = 2,    //!< true
+  kObjectType = 3,  //!< object
+  kArrayType = 4,   //!< array
+  kStringType = 5,  //!< string
+  kNumberType = 6   //!< number
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_RAPIDJSON_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/reader.h b/livox_ros_driver2/3rdparty/rapidjson/reader.h
new file mode 100644
index 0000000..3a7203a
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/reader.h
@@ -0,0 +1,2458 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_READER_H_
+#define RAPIDJSON_READER_H_
+
+/*! \file reader.h */
+
+#include <limits>
+#include "allocators.h"
+#include "encodedstream.h"
+#include "internal/clzll.h"
+#include "internal/meta.h"
+#include "internal/stack.h"
+#include "internal/strtod.h"
+#include "stream.h"
+
+#if defined(RAPIDJSON_SIMD) && defined(_MSC_VER)
+#include <intrin.h>
+#pragma intrinsic(_BitScanForward)
+#endif
+#ifdef RAPIDJSON_SSE42
+#include <nmmintrin.h>
+#elif defined(RAPIDJSON_SSE2)
+#include <emmintrin.h>
+#elif defined(RAPIDJSON_NEON)
+#include <arm_neon.h>
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(old - style - cast)
+RAPIDJSON_DIAG_OFF(padded)
+RAPIDJSON_DIAG_OFF(switch - enum)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4127)  // conditional expression is constant
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#define RAPIDJSON_NOTHING /* deliberately empty */
+#ifndef RAPIDJSON_PARSE_ERROR_EARLY_RETURN
+#define RAPIDJSON_PARSE_ERROR_EARLY_RETURN(value) \
+  RAPIDJSON_MULTILINEMACRO_BEGIN                  \
+  if (RAPIDJSON_UNLIKELY(HasParseError())) {      \
+    return value;                                 \
+  }                                               \
+  RAPIDJSON_MULTILINEMACRO_END
+#endif
+#define RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID \
+  RAPIDJSON_PARSE_ERROR_EARLY_RETURN(RAPIDJSON_NOTHING)
+//!@endcond
+
+/*! \def RAPIDJSON_PARSE_ERROR_NORETURN
+    \ingroup RAPIDJSON_ERRORS
+    \brief Macro to indicate a parse error.
+    \param parseErrorCode \ref rapidjson::ParseErrorCode of the error
+    \param offset  position of the error in JSON input (\c size_t)
+
+    This macros can be used as a customization point for the internal
+    error handling mechanism of RapidJSON.
+
+    A common usage model is to throw an exception instead of requiring the
+    caller to explicitly check the \ref rapidjson::GenericReader::Parse's
+    return value:
+
+    \code
+    #define RAPIDJSON_PARSE_ERROR_NORETURN(parseErrorCode,offset) \
+       throw ParseException(parseErrorCode, #parseErrorCode, offset)
+
+    #include <stdexcept>               // std::runtime_error
+    #include "rapidjson/error/error.h" // rapidjson::ParseResult
+
+    struct ParseException : std::runtime_error, rapidjson::ParseResult {
+      ParseException(rapidjson::ParseErrorCode code, const char* msg, size_t
+   offset) : std::runtime_error(msg), ParseResult(code, offset) {}
+    };
+
+    #include "rapidjson/reader.h"
+    \endcode
+
+    \see RAPIDJSON_PARSE_ERROR, rapidjson::GenericReader::Parse
+ */
+#ifndef RAPIDJSON_PARSE_ERROR_NORETURN
+#define RAPIDJSON_PARSE_ERROR_NORETURN(parseErrorCode, offset)              \
+  RAPIDJSON_MULTILINEMACRO_BEGIN                                            \
+  RAPIDJSON_ASSERT(!HasParseError()); /* Error can only be assigned once */ \
+  SetParseError(parseErrorCode, offset);                                    \
+  RAPIDJSON_MULTILINEMACRO_END
+#endif
+
+/*! \def RAPIDJSON_PARSE_ERROR
+    \ingroup RAPIDJSON_ERRORS
+    \brief (Internal) macro to indicate and handle a parse error.
+    \param parseErrorCode \ref rapidjson::ParseErrorCode of the error
+    \param offset  position of the error in JSON input (\c size_t)
+
+    Invokes RAPIDJSON_PARSE_ERROR_NORETURN and stops the parsing.
+
+    \see RAPIDJSON_PARSE_ERROR_NORETURN
+    \hideinitializer
+ */
+#ifndef RAPIDJSON_PARSE_ERROR
+#define RAPIDJSON_PARSE_ERROR(parseErrorCode, offset)     \
+  RAPIDJSON_MULTILINEMACRO_BEGIN                          \
+  RAPIDJSON_PARSE_ERROR_NORETURN(parseErrorCode, offset); \
+  RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;                \
+  RAPIDJSON_MULTILINEMACRO_END
+#endif
+
+#include "error/error.h"  // ParseErrorCode, ParseResult
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// ParseFlag
+
+/*! \def RAPIDJSON_PARSE_DEFAULT_FLAGS
+    \ingroup RAPIDJSON_CONFIG
+    \brief User-defined kParseDefaultFlags definition.
+
+    User can define this as any \c ParseFlag combinations.
+*/
+#ifndef RAPIDJSON_PARSE_DEFAULT_FLAGS
+#define RAPIDJSON_PARSE_DEFAULT_FLAGS kParseNoFlags
+#endif
+
+//! Combination of parseFlags
+/*! \see Reader::Parse, Document::Parse, Document::ParseInsitu,
+ * Document::ParseStream
+ */
+enum ParseFlag {
+  kParseNoFlags = 0,               //!< No flags are set.
+  kParseInsituFlag = 1,            //!< In-situ(destructive) parsing.
+  kParseValidateEncodingFlag = 2,  //!< Validate encoding of JSON strings.
+  kParseIterativeFlag = 4,  //!< Iterative(constant complexity in terms of
+                            //!< function call stack size) parsing.
+  kParseStopWhenDoneFlag =
+      8,  //!< After parsing a complete JSON root from stream, stop further
+  //!< processing the rest of stream. When this flag is used, parser will
+  //!< not generate kParseErrorDocumentRootNotSingular error.
+  kParseFullPrecisionFlag =
+      16,  //!< Parse number in full precision (but slower).
+  kParseCommentsFlag =
+      32,  //!< Allow one-line (//) and multi-line (/**/) comments.
+  kParseNumbersAsStringsFlag =
+      64,  //!< Parse all numbers (ints/doubles) as strings.
+  kParseTrailingCommasFlag =
+      128,  //!< Allow trailing commas at the end of objects and arrays.
+  kParseNanAndInfFlag = 256,  //!< Allow parsing NaN, Inf, Infinity, -Inf and
+                              //!-Infinity as doubles.
+  kParseDefaultFlags =
+      RAPIDJSON_PARSE_DEFAULT_FLAGS  //!< Default parse flags. Can be customized
+                                     //!< by defining
+                                     //!< RAPIDJSON_PARSE_DEFAULT_FLAGS
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Handler
+
+/*! \class rapidjson::Handler
+    \brief Concept for receiving events from GenericReader upon parsing.
+    The functions return true if no error occurs. If they return false,
+    the event publisher should terminate the process.
+\code
+concept Handler {
+    typename Ch;
+
+    bool Null();
+    bool Bool(bool b);
+    bool Int(int i);
+    bool Uint(unsigned i);
+    bool Int64(int64_t i);
+    bool Uint64(uint64_t i);
+    bool Double(double d);
+    /// enabled via kParseNumbersAsStringsFlag, string is not null-terminated
+(use length) bool RawNumber(const Ch* str, SizeType length, bool copy); bool
+String(const Ch* str, SizeType length, bool copy); bool StartObject(); bool
+Key(const Ch* str, SizeType length, bool copy); bool EndObject(SizeType
+memberCount); bool StartArray(); bool EndArray(SizeType elementCount);
+};
+\endcode
+*/
+///////////////////////////////////////////////////////////////////////////////
+// BaseReaderHandler
+
+//! Default implementation of Handler.
+/*! This can be used as base class of any reader handler.
+    \note implements Handler concept
+*/
+template <typename Encoding = UTF8<>, typename Derived = void>
+struct BaseReaderHandler {
+  typedef typename Encoding::Ch Ch;
+
+  typedef
+      typename internal::SelectIf<internal::IsSame<Derived, void>,
+                                  BaseReaderHandler, Derived>::Type Override;
+
+  bool Default() { return true; }
+  bool Null() { return static_cast<Override &>(*this).Default(); }
+  bool Bool(bool) { return static_cast<Override &>(*this).Default(); }
+  bool Int(int) { return static_cast<Override &>(*this).Default(); }
+  bool Uint(unsigned) { return static_cast<Override &>(*this).Default(); }
+  bool Int64(int64_t) { return static_cast<Override &>(*this).Default(); }
+  bool Uint64(uint64_t) { return static_cast<Override &>(*this).Default(); }
+  bool Double(double) { return static_cast<Override &>(*this).Default(); }
+  /// enabled via kParseNumbersAsStringsFlag, string is not null-terminated (use
+  /// length)
+  bool RawNumber(const Ch *str, SizeType len, bool copy) {
+    return static_cast<Override &>(*this).String(str, len, copy);
+  }
+  bool String(const Ch *, SizeType, bool) {
+    return static_cast<Override &>(*this).Default();
+  }
+  bool StartObject() { return static_cast<Override &>(*this).Default(); }
+  bool Key(const Ch *str, SizeType len, bool copy) {
+    return static_cast<Override &>(*this).String(str, len, copy);
+  }
+  bool EndObject(SizeType) { return static_cast<Override &>(*this).Default(); }
+  bool StartArray() { return static_cast<Override &>(*this).Default(); }
+  bool EndArray(SizeType) { return static_cast<Override &>(*this).Default(); }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// StreamLocalCopy
+
+namespace internal {
+
+template <typename Stream, int = StreamTraits<Stream>::copyOptimization>
+class StreamLocalCopy;
+
+//! Do copy optimization.
+template <typename Stream>
+class StreamLocalCopy<Stream, 1> {
+ public:
+  StreamLocalCopy(Stream &original) : s(original), original_(original) {}
+  ~StreamLocalCopy() { original_ = s; }
+
+  Stream s;
+
+ private:
+  StreamLocalCopy &operator=(const StreamLocalCopy &) /* = delete */;
+
+  Stream &original_;
+};
+
+//! Keep reference.
+template <typename Stream>
+class StreamLocalCopy<Stream, 0> {
+ public:
+  StreamLocalCopy(Stream &original) : s(original) {}
+
+  Stream &s;
+
+ private:
+  StreamLocalCopy &operator=(const StreamLocalCopy &) /* = delete */;
+};
+
+}  // namespace internal
+
+///////////////////////////////////////////////////////////////////////////////
+// SkipWhitespace
+
+//! Skip the JSON white spaces in a stream.
+/*! \param is A input stream for skipping white spaces.
+    \note This function has SSE2/SSE4.2 specialization.
+*/
+template <typename InputStream>
+void SkipWhitespace(InputStream &is) {
+  internal::StreamLocalCopy<InputStream> copy(is);
+  InputStream &s(copy.s);
+
+  typename InputStream::Ch c;
+  while ((c = s.Peek()) == ' ' || c == '\n' || c == '\r' || c == '\t') s.Take();
+}
+
+inline const char *SkipWhitespace(const char *p, const char *end) {
+  while (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')) ++p;
+  return p;
+}
+
+#ifdef RAPIDJSON_SSE42
+//! Skip whitespace with SSE 4.2 pcmpistrm instruction, testing 16 8-byte
+//! characters at once.
+inline const char *SkipWhitespace_SIMD(const char *p) {
+  // Fast return for single non-whitespace
+  if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')
+    ++p;
+  else
+    return p;
+
+  // 16-byte align to the next boundary
+  const char *nextAligned = reinterpret_cast<const char *>(
+      (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+  while (p != nextAligned)
+    if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')
+      ++p;
+    else
+      return p;
+
+  // The rest of string using SIMD
+  static const char whitespace[16] = " \n\r\t";
+  const __m128i w =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&whitespace[0]));
+
+  for (;; p += 16) {
+    const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
+    const int r = _mm_cmpistri(w, s, _SIDD_UBYTE_OPS | _SIDD_CMP_EQUAL_ANY |
+                                         _SIDD_LEAST_SIGNIFICANT |
+                                         _SIDD_NEGATIVE_POLARITY);
+    if (r != 16)  // some of characters is non-whitespace
+      return p + r;
+  }
+}
+
+inline const char *SkipWhitespace_SIMD(const char *p, const char *end) {
+  // Fast return for single non-whitespace
+  if (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t'))
+    ++p;
+  else
+    return p;
+
+  // The middle of string using SIMD
+  static const char whitespace[16] = " \n\r\t";
+  const __m128i w =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&whitespace[0]));
+
+  for (; p <= end - 16; p += 16) {
+    const __m128i s = _mm_loadu_si128(reinterpret_cast<const __m128i *>(p));
+    const int r = _mm_cmpistri(w, s, _SIDD_UBYTE_OPS | _SIDD_CMP_EQUAL_ANY |
+                                         _SIDD_LEAST_SIGNIFICANT |
+                                         _SIDD_NEGATIVE_POLARITY);
+    if (r != 16)  // some of characters is non-whitespace
+      return p + r;
+  }
+
+  return SkipWhitespace(p, end);
+}
+
+#elif defined(RAPIDJSON_SSE2)
+
+//! Skip whitespace with SSE2 instructions, testing 16 8-byte characters at
+//! once.
+inline const char *SkipWhitespace_SIMD(const char *p) {
+  // Fast return for single non-whitespace
+  if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')
+    ++p;
+  else
+    return p;
+
+  // 16-byte align to the next boundary
+  const char *nextAligned = reinterpret_cast<const char *>(
+      (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+  while (p != nextAligned)
+    if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')
+      ++p;
+    else
+      return p;
+
+// The rest of string
+#define C16(c) \
+  { c, c, c, c, c, c, c, c, c, c, c, c, c, c, c, c }
+  static const char whitespaces[4][16] = {C16(' '), C16('\n'), C16('\r'),
+                                          C16('\t')};
+#undef C16
+
+  const __m128i w0 =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&whitespaces[0][0]));
+  const __m128i w1 =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&whitespaces[1][0]));
+  const __m128i w2 =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&whitespaces[2][0]));
+  const __m128i w3 =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&whitespaces[3][0]));
+
+  for (;; p += 16) {
+    const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
+    __m128i x = _mm_cmpeq_epi8(s, w0);
+    x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w1));
+    x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w2));
+    x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w3));
+    unsigned short r = static_cast<unsigned short>(~_mm_movemask_epi8(x));
+    if (r != 0) {  // some of characters may be non-whitespace
+#ifdef _MSC_VER  // Find the index of first non-whitespace
+      unsigned long offset;
+      _BitScanForward(&offset, r);
+      return p + offset;
+#else
+      return p + __builtin_ffs(r) - 1;
+#endif
+    }
+  }
+}
+
+inline const char *SkipWhitespace_SIMD(const char *p, const char *end) {
+  // Fast return for single non-whitespace
+  if (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t'))
+    ++p;
+  else
+    return p;
+
+// The rest of string
+#define C16(c) \
+  { c, c, c, c, c, c, c, c, c, c, c, c, c, c, c, c }
+  static const char whitespaces[4][16] = {C16(' '), C16('\n'), C16('\r'),
+                                          C16('\t')};
+#undef C16
+
+  const __m128i w0 =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&whitespaces[0][0]));
+  const __m128i w1 =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&whitespaces[1][0]));
+  const __m128i w2 =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&whitespaces[2][0]));
+  const __m128i w3 =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&whitespaces[3][0]));
+
+  for (; p <= end - 16; p += 16) {
+    const __m128i s = _mm_loadu_si128(reinterpret_cast<const __m128i *>(p));
+    __m128i x = _mm_cmpeq_epi8(s, w0);
+    x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w1));
+    x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w2));
+    x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w3));
+    unsigned short r = static_cast<unsigned short>(~_mm_movemask_epi8(x));
+    if (r != 0) {  // some of characters may be non-whitespace
+#ifdef _MSC_VER  // Find the index of first non-whitespace
+      unsigned long offset;
+      _BitScanForward(&offset, r);
+      return p + offset;
+#else
+      return p + __builtin_ffs(r) - 1;
+#endif
+    }
+  }
+
+  return SkipWhitespace(p, end);
+}
+
+#elif defined(RAPIDJSON_NEON)
+
+//! Skip whitespace with ARM Neon instructions, testing 16 8-byte characters at
+//! once.
+inline const char *SkipWhitespace_SIMD(const char *p) {
+  // Fast return for single non-whitespace
+  if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')
+    ++p;
+  else
+    return p;
+
+  // 16-byte align to the next boundary
+  const char *nextAligned = reinterpret_cast<const char *>(
+      (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+  while (p != nextAligned)
+    if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')
+      ++p;
+    else
+      return p;
+
+  const uint8x16_t w0 = vmovq_n_u8(' ');
+  const uint8x16_t w1 = vmovq_n_u8('\n');
+  const uint8x16_t w2 = vmovq_n_u8('\r');
+  const uint8x16_t w3 = vmovq_n_u8('\t');
+
+  for (;; p += 16) {
+    const uint8x16_t s = vld1q_u8(reinterpret_cast<const uint8_t *>(p));
+    uint8x16_t x = vceqq_u8(s, w0);
+    x = vorrq_u8(x, vceqq_u8(s, w1));
+    x = vorrq_u8(x, vceqq_u8(s, w2));
+    x = vorrq_u8(x, vceqq_u8(s, w3));
+
+    x = vmvnq_u8(x);                                             // Negate
+    x = vrev64q_u8(x);                                           // Rev in 64
+    uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0);   // extract
+    uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1);  // extract
+
+    if (low == 0) {
+      if (high != 0) {
+        uint32_t lz = RAPIDJSON_CLZLL(high);
+        return p + 8 + (lz >> 3);
+      }
+    } else {
+      uint32_t lz = RAPIDJSON_CLZLL(low);
+      return p + (lz >> 3);
+    }
+  }
+}
+
+inline const char *SkipWhitespace_SIMD(const char *p, const char *end) {
+  // Fast return for single non-whitespace
+  if (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t'))
+    ++p;
+  else
+    return p;
+
+  const uint8x16_t w0 = vmovq_n_u8(' ');
+  const uint8x16_t w1 = vmovq_n_u8('\n');
+  const uint8x16_t w2 = vmovq_n_u8('\r');
+  const uint8x16_t w3 = vmovq_n_u8('\t');
+
+  for (; p <= end - 16; p += 16) {
+    const uint8x16_t s = vld1q_u8(reinterpret_cast<const uint8_t *>(p));
+    uint8x16_t x = vceqq_u8(s, w0);
+    x = vorrq_u8(x, vceqq_u8(s, w1));
+    x = vorrq_u8(x, vceqq_u8(s, w2));
+    x = vorrq_u8(x, vceqq_u8(s, w3));
+
+    x = vmvnq_u8(x);                                             // Negate
+    x = vrev64q_u8(x);                                           // Rev in 64
+    uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0);   // extract
+    uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1);  // extract
+
+    if (low == 0) {
+      if (high != 0) {
+        uint32_t lz = RAPIDJSON_CLZLL(high);
+        return p + 8 + (lz >> 3);
+      }
+    } else {
+      uint32_t lz = RAPIDJSON_CLZLL(low);
+      return p + (lz >> 3);
+    }
+  }
+
+  return SkipWhitespace(p, end);
+}
+
+#endif  // RAPIDJSON_NEON
+
+#ifdef RAPIDJSON_SIMD
+//! Template function specialization for InsituStringStream
+template <>
+inline void SkipWhitespace(InsituStringStream &is) {
+  is.src_ = const_cast<char *>(SkipWhitespace_SIMD(is.src_));
+}
+
+//! Template function specialization for StringStream
+template <>
+inline void SkipWhitespace(StringStream &is) {
+  is.src_ = SkipWhitespace_SIMD(is.src_);
+}
+
+template <>
+inline void SkipWhitespace(EncodedInputStream<UTF8<>, MemoryStream> &is) {
+  is.is_.src_ = SkipWhitespace_SIMD(is.is_.src_, is.is_.end_);
+}
+#endif  // RAPIDJSON_SIMD
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericReader
+
+//! SAX-style JSON parser. Use \ref Reader for UTF8 encoding and default
+//! allocator.
+/*! GenericReader parses JSON text from a stream, and send events synchronously
+   to an object implementing Handler concept.
+
+    It needs to allocate a stack for storing a single decoded string during
+    non-destructive parsing.
+
+    For in-situ parsing, the decoded string is directly written to the source
+    text string, no temporary buffer is required.
+
+    A GenericReader object can be reused for parsing multiple JSON text.
+
+    \tparam SourceEncoding Encoding of the input stream.
+    \tparam TargetEncoding Encoding of the parse output.
+    \tparam StackAllocator Allocator type for stack.
+*/
+template <typename SourceEncoding, typename TargetEncoding,
+          typename StackAllocator = CrtAllocator>
+class GenericReader {
+ public:
+  typedef typename SourceEncoding::Ch Ch;  //!< SourceEncoding character type
+
+  //! Constructor.
+  /*! \param stackAllocator Optional allocator for allocating stack memory.
+     (Only use for non-destructive parsing) \param stackCapacity stack capacity
+     in bytes for storing a single decoded string.  (Only use for
+     non-destructive parsing)
+  */
+  GenericReader(StackAllocator *stackAllocator = 0,
+                size_t stackCapacity = kDefaultStackCapacity)
+      : stack_(stackAllocator, stackCapacity),
+        parseResult_(),
+        state_(IterativeParsingStartState) {}
+
+  //! Parse JSON text.
+  /*! \tparam parseFlags Combination of \ref ParseFlag.
+      \tparam InputStream Type of input stream, implementing Stream concept.
+      \tparam Handler Type of handler, implementing Handler concept.
+      \param is Input stream to be parsed.
+      \param handler The handler to receive events.
+      \return Whether the parsing is successful.
+  */
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  ParseResult Parse(InputStream &is, Handler &handler) {
+    if (parseFlags & kParseIterativeFlag)
+      return IterativeParse<parseFlags>(is, handler);
+
+    parseResult_.Clear();
+
+    ClearStackOnExit scope(*this);
+
+    SkipWhitespaceAndComments<parseFlags>(is);
+    RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_);
+
+    if (RAPIDJSON_UNLIKELY(is.Peek() == '\0')) {
+      RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorDocumentEmpty, is.Tell());
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_);
+    } else {
+      ParseValue<parseFlags>(is, handler);
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_);
+
+      if (!(parseFlags & kParseStopWhenDoneFlag)) {
+        SkipWhitespaceAndComments<parseFlags>(is);
+        RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_);
+
+        if (RAPIDJSON_UNLIKELY(is.Peek() != '\0')) {
+          RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorDocumentRootNotSingular,
+                                         is.Tell());
+          RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_);
+        }
+      }
+    }
+
+    return parseResult_;
+  }
+
+  //! Parse JSON text (with \ref kParseDefaultFlags)
+  /*! \tparam InputStream Type of input stream, implementing Stream concept
+      \tparam Handler Type of handler, implementing Handler concept.
+      \param is Input stream to be parsed.
+      \param handler The handler to receive events.
+      \return Whether the parsing is successful.
+  */
+  template <typename InputStream, typename Handler>
+  ParseResult Parse(InputStream &is, Handler &handler) {
+    return Parse<kParseDefaultFlags>(is, handler);
+  }
+
+  //! Initialize JSON text token-by-token parsing
+  /*!
+   */
+  void IterativeParseInit() {
+    parseResult_.Clear();
+    state_ = IterativeParsingStartState;
+  }
+
+  //! Parse one token from JSON text
+  /*! \tparam InputStream Type of input stream, implementing Stream concept
+      \tparam Handler Type of handler, implementing Handler concept.
+      \param is Input stream to be parsed.
+      \param handler The handler to receive events.
+      \return Whether the parsing is successful.
+   */
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  bool IterativeParseNext(InputStream &is, Handler &handler) {
+    while (RAPIDJSON_LIKELY(is.Peek() != '\0')) {
+      SkipWhitespaceAndComments<parseFlags>(is);
+
+      Token t = Tokenize(is.Peek());
+      IterativeParsingState n = Predict(state_, t);
+      IterativeParsingState d = Transit<parseFlags>(state_, t, n, is, handler);
+
+      // If we've finished or hit an error...
+      if (RAPIDJSON_UNLIKELY(IsIterativeParsingCompleteState(d))) {
+        // Report errors.
+        if (d == IterativeParsingErrorState) {
+          HandleError(state_, is);
+          return false;
+        }
+
+        // Transition to the finish state.
+        RAPIDJSON_ASSERT(d == IterativeParsingFinishState);
+        state_ = d;
+
+        // If StopWhenDone is not set...
+        if (!(parseFlags & kParseStopWhenDoneFlag)) {
+          // ... and extra non-whitespace data is found...
+          SkipWhitespaceAndComments<parseFlags>(is);
+          if (is.Peek() != '\0') {
+            // ... this is considered an error.
+            HandleError(state_, is);
+            return false;
+          }
+        }
+
+        // Success! We are done!
+        return true;
+      }
+
+      // Transition to the new state.
+      state_ = d;
+
+      // If we parsed anything other than a delimiter, we invoked the handler,
+      // so we can return true now.
+      if (!IsIterativeParsingDelimiterState(n)) return true;
+    }
+
+    // We reached the end of file.
+    stack_.Clear();
+
+    if (state_ != IterativeParsingFinishState) {
+      HandleError(state_, is);
+      return false;
+    }
+
+    return true;
+  }
+
+  //! Check if token-by-token parsing JSON text is complete
+  /*! \return Whether the JSON has been fully decoded.
+   */
+  RAPIDJSON_FORCEINLINE bool IterativeParseComplete() const {
+    return IsIterativeParsingCompleteState(state_);
+  }
+
+  //! Whether a parse error has occurred in the last parsing.
+  bool HasParseError() const { return parseResult_.IsError(); }
+
+  //! Get the \ref ParseErrorCode of last parsing.
+  ParseErrorCode GetParseErrorCode() const { return parseResult_.Code(); }
+
+  //! Get the position of last parsing error in input, 0 otherwise.
+  size_t GetErrorOffset() const { return parseResult_.Offset(); }
+
+ protected:
+  void SetParseError(ParseErrorCode code, size_t offset) {
+    parseResult_.Set(code, offset);
+  }
+
+ private:
+  // Prohibit copy constructor & assignment operator.
+  GenericReader(const GenericReader &);
+  GenericReader &operator=(const GenericReader &);
+
+  void ClearStack() { stack_.Clear(); }
+
+  // clear stack on any exit from ParseStream, e.g. due to exception
+  struct ClearStackOnExit {
+    explicit ClearStackOnExit(GenericReader &r) : r_(r) {}
+    ~ClearStackOnExit() { r_.ClearStack(); }
+
+   private:
+    GenericReader &r_;
+    ClearStackOnExit(const ClearStackOnExit &);
+    ClearStackOnExit &operator=(const ClearStackOnExit &);
+  };
+
+  template <unsigned parseFlags, typename InputStream>
+  void SkipWhitespaceAndComments(InputStream &is) {
+    SkipWhitespace(is);
+
+    if (parseFlags & kParseCommentsFlag) {
+      while (RAPIDJSON_UNLIKELY(Consume(is, '/'))) {
+        if (Consume(is, '*')) {
+          while (true) {
+            if (RAPIDJSON_UNLIKELY(is.Peek() == '\0'))
+              RAPIDJSON_PARSE_ERROR(kParseErrorUnspecificSyntaxError,
+                                    is.Tell());
+            else if (Consume(is, '*')) {
+              if (Consume(is, '/')) break;
+            } else
+              is.Take();
+          }
+        } else if (RAPIDJSON_LIKELY(Consume(is, '/')))
+          while (is.Peek() != '\0' && is.Take() != '\n') {
+          }
+        else
+          RAPIDJSON_PARSE_ERROR(kParseErrorUnspecificSyntaxError, is.Tell());
+
+        SkipWhitespace(is);
+      }
+    }
+  }
+
+  // Parse object: { string : value, ... }
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  void ParseObject(InputStream &is, Handler &handler) {
+    RAPIDJSON_ASSERT(is.Peek() == '{');
+    is.Take();  // Skip '{'
+
+    if (RAPIDJSON_UNLIKELY(!handler.StartObject()))
+      RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell());
+
+    SkipWhitespaceAndComments<parseFlags>(is);
+    RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+
+    if (Consume(is, '}')) {
+      if (RAPIDJSON_UNLIKELY(!handler.EndObject(0)))  // empty object
+        RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell());
+      return;
+    }
+
+    for (SizeType memberCount = 0;;) {
+      if (RAPIDJSON_UNLIKELY(is.Peek() != '"'))
+        RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissName, is.Tell());
+
+      ParseString<parseFlags>(is, handler, true);
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+
+      SkipWhitespaceAndComments<parseFlags>(is);
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+
+      if (RAPIDJSON_UNLIKELY(!Consume(is, ':')))
+        RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissColon, is.Tell());
+
+      SkipWhitespaceAndComments<parseFlags>(is);
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+
+      ParseValue<parseFlags>(is, handler);
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+
+      SkipWhitespaceAndComments<parseFlags>(is);
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+
+      ++memberCount;
+
+      switch (is.Peek()) {
+        case ',':
+          is.Take();
+          SkipWhitespaceAndComments<parseFlags>(is);
+          RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+          break;
+        case '}':
+          is.Take();
+          if (RAPIDJSON_UNLIKELY(!handler.EndObject(memberCount)))
+            RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell());
+          return;
+        default:
+          RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissCommaOrCurlyBracket,
+                                is.Tell());
+          break;  // This useless break is only for making warning and coverage
+                  // happy
+      }
+
+      if (parseFlags & kParseTrailingCommasFlag) {
+        if (is.Peek() == '}') {
+          if (RAPIDJSON_UNLIKELY(!handler.EndObject(memberCount)))
+            RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell());
+          is.Take();
+          return;
+        }
+      }
+    }
+  }
+
+  // Parse array: [ value, ... ]
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  void ParseArray(InputStream &is, Handler &handler) {
+    RAPIDJSON_ASSERT(is.Peek() == '[');
+    is.Take();  // Skip '['
+
+    if (RAPIDJSON_UNLIKELY(!handler.StartArray()))
+      RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell());
+
+    SkipWhitespaceAndComments<parseFlags>(is);
+    RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+
+    if (Consume(is, ']')) {
+      if (RAPIDJSON_UNLIKELY(!handler.EndArray(0)))  // empty array
+        RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell());
+      return;
+    }
+
+    for (SizeType elementCount = 0;;) {
+      ParseValue<parseFlags>(is, handler);
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+
+      ++elementCount;
+      SkipWhitespaceAndComments<parseFlags>(is);
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+
+      if (Consume(is, ',')) {
+        SkipWhitespaceAndComments<parseFlags>(is);
+        RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+      } else if (Consume(is, ']')) {
+        if (RAPIDJSON_UNLIKELY(!handler.EndArray(elementCount)))
+          RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell());
+        return;
+      } else
+        RAPIDJSON_PARSE_ERROR(kParseErrorArrayMissCommaOrSquareBracket,
+                              is.Tell());
+
+      if (parseFlags & kParseTrailingCommasFlag) {
+        if (is.Peek() == ']') {
+          if (RAPIDJSON_UNLIKELY(!handler.EndArray(elementCount)))
+            RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell());
+          is.Take();
+          return;
+        }
+      }
+    }
+  }
+
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  void ParseNull(InputStream &is, Handler &handler) {
+    RAPIDJSON_ASSERT(is.Peek() == 'n');
+    is.Take();
+
+    if (RAPIDJSON_LIKELY(Consume(is, 'u') && Consume(is, 'l') &&
+                         Consume(is, 'l'))) {
+      if (RAPIDJSON_UNLIKELY(!handler.Null()))
+        RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell());
+    } else
+      RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell());
+  }
+
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  void ParseTrue(InputStream &is, Handler &handler) {
+    RAPIDJSON_ASSERT(is.Peek() == 't');
+    is.Take();
+
+    if (RAPIDJSON_LIKELY(Consume(is, 'r') && Consume(is, 'u') &&
+                         Consume(is, 'e'))) {
+      if (RAPIDJSON_UNLIKELY(!handler.Bool(true)))
+        RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell());
+    } else
+      RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell());
+  }
+
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  void ParseFalse(InputStream &is, Handler &handler) {
+    RAPIDJSON_ASSERT(is.Peek() == 'f');
+    is.Take();
+
+    if (RAPIDJSON_LIKELY(Consume(is, 'a') && Consume(is, 'l') &&
+                         Consume(is, 's') && Consume(is, 'e'))) {
+      if (RAPIDJSON_UNLIKELY(!handler.Bool(false)))
+        RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell());
+    } else
+      RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell());
+  }
+
+  template <typename InputStream>
+  RAPIDJSON_FORCEINLINE static bool Consume(InputStream &is,
+                                            typename InputStream::Ch expect) {
+    if (RAPIDJSON_LIKELY(is.Peek() == expect)) {
+      is.Take();
+      return true;
+    } else
+      return false;
+  }
+
+  // Helper function to parse four hexadecimal digits in \uXXXX in
+  // ParseString().
+  template <typename InputStream>
+  unsigned ParseHex4(InputStream &is, size_t escapeOffset) {
+    unsigned codepoint = 0;
+    for (int i = 0; i < 4; i++) {
+      Ch c = is.Peek();
+      codepoint <<= 4;
+      codepoint += static_cast<unsigned>(c);
+      if (c >= '0' && c <= '9')
+        codepoint -= '0';
+      else if (c >= 'A' && c <= 'F')
+        codepoint -= 'A' - 10;
+      else if (c >= 'a' && c <= 'f')
+        codepoint -= 'a' - 10;
+      else {
+        RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorStringUnicodeEscapeInvalidHex,
+                                       escapeOffset);
+        RAPIDJSON_PARSE_ERROR_EARLY_RETURN(0);
+      }
+      is.Take();
+    }
+    return codepoint;
+  }
+
+  template <typename CharType>
+  class StackStream {
+   public:
+    typedef CharType Ch;
+
+    StackStream(internal::Stack<StackAllocator> &stack)
+        : stack_(stack), length_(0) {}
+    RAPIDJSON_FORCEINLINE void Put(Ch c) {
+      *stack_.template Push<Ch>() = c;
+      ++length_;
+    }
+
+    RAPIDJSON_FORCEINLINE void *Push(SizeType count) {
+      length_ += count;
+      return stack_.template Push<Ch>(count);
+    }
+
+    size_t Length() const { return length_; }
+
+    Ch *Pop() { return stack_.template Pop<Ch>(length_); }
+
+   private:
+    StackStream(const StackStream &);
+    StackStream &operator=(const StackStream &);
+
+    internal::Stack<StackAllocator> &stack_;
+    SizeType length_;
+  };
+
+  // Parse string and generate String event. Different code paths for
+  // kParseInsituFlag.
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  void ParseString(InputStream &is, Handler &handler, bool isKey = false) {
+    internal::StreamLocalCopy<InputStream> copy(is);
+    InputStream &s(copy.s);
+
+    RAPIDJSON_ASSERT(s.Peek() == '\"');
+    s.Take();  // Skip '\"'
+
+    bool success = false;
+    if (parseFlags & kParseInsituFlag) {
+      typename InputStream::Ch *head = s.PutBegin();
+      ParseStringToStream<parseFlags, SourceEncoding, SourceEncoding>(s, s);
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+      size_t length = s.PutEnd(head) - 1;
+      RAPIDJSON_ASSERT(length <= 0xFFFFFFFF);
+      const typename TargetEncoding::Ch *const str =
+          reinterpret_cast<typename TargetEncoding::Ch *>(head);
+      success = (isKey ? handler.Key(str, SizeType(length), false)
+                       : handler.String(str, SizeType(length), false));
+    } else {
+      StackStream<typename TargetEncoding::Ch> stackStream(stack_);
+      ParseStringToStream<parseFlags, SourceEncoding, TargetEncoding>(
+          s, stackStream);
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+      SizeType length = static_cast<SizeType>(stackStream.Length()) - 1;
+      const typename TargetEncoding::Ch *const str = stackStream.Pop();
+      success = (isKey ? handler.Key(str, length, true)
+                       : handler.String(str, length, true));
+    }
+    if (RAPIDJSON_UNLIKELY(!success))
+      RAPIDJSON_PARSE_ERROR(kParseErrorTermination, s.Tell());
+  }
+
+  // Parse string to an output is
+  // This function handles the prefix/suffix double quotes, escaping, and
+  // optional encoding validation.
+  template <unsigned parseFlags, typename SEncoding, typename TEncoding,
+            typename InputStream, typename OutputStream>
+  RAPIDJSON_FORCEINLINE void ParseStringToStream(InputStream &is,
+                                                 OutputStream &os) {
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#define Z16 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+    static const char escape[256] = {
+        Z16, Z16, 0,    0,    '\"', 0,   0,    0,   0,   0,   0,  0,    0,
+        0,   0,   0,    0,    '/',  Z16, Z16,  0,   0,   0,   0,  0,    0,
+        0,   0,   0,    0,    0,    0,   '\\', 0,   0,   0,   0,  0,    '\b',
+        0,   0,   0,    '\f', 0,    0,   0,    0,   0,   0,   0,  '\n', 0,
+        0,   0,   '\r', 0,    '\t', 0,   0,    0,   0,   0,   0,  0,    0,
+        0,   0,   0,    Z16,  Z16,  Z16, Z16,  Z16, Z16, Z16, Z16};
+#undef Z16
+    //!@endcond
+
+    for (;;) {
+      // Scan and copy string before "\\\"" or < 0x20. This is an optional
+      // optimzation.
+      if (!(parseFlags & kParseValidateEncodingFlag))
+        ScanCopyUnescapedString(is, os);
+
+      Ch c = is.Peek();
+      if (RAPIDJSON_UNLIKELY(c == '\\')) {  // Escape
+        size_t escapeOffset = is.Tell();    // For invalid escaping, report the
+                                            // initial '\\' as error offset
+        is.Take();
+        Ch e = is.Peek();
+        if ((sizeof(Ch) == 1 || unsigned(e) < 256) &&
+            RAPIDJSON_LIKELY(escape[static_cast<unsigned char>(e)])) {
+          is.Take();
+          os.Put(static_cast<typename TEncoding::Ch>(
+              escape[static_cast<unsigned char>(e)]));
+        } else if (RAPIDJSON_LIKELY(e == 'u')) {  // Unicode
+          is.Take();
+          unsigned codepoint = ParseHex4(is, escapeOffset);
+          RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+          if (RAPIDJSON_UNLIKELY(codepoint >= 0xD800 && codepoint <= 0xDBFF)) {
+            // Handle UTF-16 surrogate pair
+            if (RAPIDJSON_UNLIKELY(!Consume(is, '\\') || !Consume(is, 'u')))
+              RAPIDJSON_PARSE_ERROR(kParseErrorStringUnicodeSurrogateInvalid,
+                                    escapeOffset);
+            unsigned codepoint2 = ParseHex4(is, escapeOffset);
+            RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID;
+            if (RAPIDJSON_UNLIKELY(codepoint2 < 0xDC00 || codepoint2 > 0xDFFF))
+              RAPIDJSON_PARSE_ERROR(kParseErrorStringUnicodeSurrogateInvalid,
+                                    escapeOffset);
+            codepoint = (((codepoint - 0xD800) << 10) | (codepoint2 - 0xDC00)) +
+                        0x10000;
+          }
+          TEncoding::Encode(os, codepoint);
+        } else
+          RAPIDJSON_PARSE_ERROR(kParseErrorStringEscapeInvalid, escapeOffset);
+      } else if (RAPIDJSON_UNLIKELY(c == '"')) {  // Closing double quote
+        is.Take();
+        os.Put('\0');  // null-terminate the string
+        return;
+      } else if (RAPIDJSON_UNLIKELY(static_cast<unsigned>(c) <
+                                    0x20)) {  // RFC 4627: unescaped = %x20-21 /
+                                              // %x23-5B / %x5D-10FFFF
+        if (c == '\0')
+          RAPIDJSON_PARSE_ERROR(kParseErrorStringMissQuotationMark, is.Tell());
+        else
+          RAPIDJSON_PARSE_ERROR(kParseErrorStringInvalidEncoding, is.Tell());
+      } else {
+        size_t offset = is.Tell();
+        if (RAPIDJSON_UNLIKELY(
+                (parseFlags & kParseValidateEncodingFlag
+                     ? !Transcoder<SEncoding, TEncoding>::Validate(is, os)
+                     : !Transcoder<SEncoding, TEncoding>::Transcode(is, os))))
+          RAPIDJSON_PARSE_ERROR(kParseErrorStringInvalidEncoding, offset);
+      }
+    }
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(InputStream &,
+                                                            OutputStream &) {
+    // Do nothing for generic version
+  }
+
+#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42)
+  // StringStream -> StackStream<char>
+  static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(
+      StringStream &is, StackStream<char> &os) {
+    const char *p = is.src_;
+
+    // Scan one by one until alignment (unaligned load may cross page boundary
+    // and cause crash)
+    const char *nextAligned = reinterpret_cast<const char *>(
+        (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+    while (p != nextAligned)
+      if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') ||
+          RAPIDJSON_UNLIKELY(static_cast<unsigned>(*p) < 0x20)) {
+        is.src_ = p;
+        return;
+      } else
+        os.Put(*p++);
+
+    // The rest of string using SIMD
+    static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"',
+                                    '\"', '\"', '\"', '\"', '\"', '\"',
+                                    '\"', '\"', '\"', '\"'};
+    static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\',
+                                    '\\', '\\', '\\', '\\', '\\', '\\',
+                                    '\\', '\\', '\\', '\\'};
+    static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+                                   0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+                                   0x1F, 0x1F, 0x1F, 0x1F};
+    const __m128i dq =
+        _mm_loadu_si128(reinterpret_cast<const __m128i *>(&dquote[0]));
+    const __m128i bs =
+        _mm_loadu_si128(reinterpret_cast<const __m128i *>(&bslash[0]));
+    const __m128i sp =
+        _mm_loadu_si128(reinterpret_cast<const __m128i *>(&space[0]));
+
+    for (;; p += 16) {
+      const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
+      const __m128i t1 = _mm_cmpeq_epi8(s, dq);
+      const __m128i t2 = _mm_cmpeq_epi8(s, bs);
+      const __m128i t3 = _mm_cmpeq_epi8(
+          _mm_max_epu8(s, sp), sp);  // s < 0x20 <=> max(s, 0x1F) == 0x1F
+      const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3);
+      unsigned short r = static_cast<unsigned short>(_mm_movemask_epi8(x));
+      if (RAPIDJSON_UNLIKELY(r != 0)) {  // some of characters is escaped
+        SizeType length;
+#ifdef _MSC_VER  // Find the index of first escaped
+        unsigned long offset;
+        _BitScanForward(&offset, r);
+        length = offset;
+#else
+        length = static_cast<SizeType>(__builtin_ffs(r) - 1);
+#endif
+        if (length != 0) {
+          char *q = reinterpret_cast<char *>(os.Push(length));
+          for (size_t i = 0; i < length; i++) q[i] = p[i];
+
+          p += length;
+        }
+        break;
+      }
+      _mm_storeu_si128(reinterpret_cast<__m128i *>(os.Push(16)), s);
+    }
+
+    is.src_ = p;
+  }
+
+  // InsituStringStream -> InsituStringStream
+  static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(
+      InsituStringStream &is, InsituStringStream &os) {
+    RAPIDJSON_ASSERT(&is == &os);
+    (void)os;
+
+    if (is.src_ == is.dst_) {
+      SkipUnescapedString(is);
+      return;
+    }
+
+    char *p = is.src_;
+    char *q = is.dst_;
+
+    // Scan one by one until alignment (unaligned load may cross page boundary
+    // and cause crash)
+    const char *nextAligned = reinterpret_cast<const char *>(
+        (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+    while (p != nextAligned)
+      if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') ||
+          RAPIDJSON_UNLIKELY(static_cast<unsigned>(*p) < 0x20)) {
+        is.src_ = p;
+        is.dst_ = q;
+        return;
+      } else
+        *q++ = *p++;
+
+    // The rest of string using SIMD
+    static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"',
+                                    '\"', '\"', '\"', '\"', '\"', '\"',
+                                    '\"', '\"', '\"', '\"'};
+    static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\',
+                                    '\\', '\\', '\\', '\\', '\\', '\\',
+                                    '\\', '\\', '\\', '\\'};
+    static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+                                   0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+                                   0x1F, 0x1F, 0x1F, 0x1F};
+    const __m128i dq =
+        _mm_loadu_si128(reinterpret_cast<const __m128i *>(&dquote[0]));
+    const __m128i bs =
+        _mm_loadu_si128(reinterpret_cast<const __m128i *>(&bslash[0]));
+    const __m128i sp =
+        _mm_loadu_si128(reinterpret_cast<const __m128i *>(&space[0]));
+
+    for (;; p += 16, q += 16) {
+      const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
+      const __m128i t1 = _mm_cmpeq_epi8(s, dq);
+      const __m128i t2 = _mm_cmpeq_epi8(s, bs);
+      const __m128i t3 = _mm_cmpeq_epi8(
+          _mm_max_epu8(s, sp), sp);  // s < 0x20 <=> max(s, 0x1F) == 0x1F
+      const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3);
+      unsigned short r = static_cast<unsigned short>(_mm_movemask_epi8(x));
+      if (RAPIDJSON_UNLIKELY(r != 0)) {  // some of characters is escaped
+        size_t length;
+#ifdef _MSC_VER  // Find the index of first escaped
+        unsigned long offset;
+        _BitScanForward(&offset, r);
+        length = offset;
+#else
+        length = static_cast<size_t>(__builtin_ffs(r) - 1);
+#endif
+        for (const char *pend = p + length; p != pend;) *q++ = *p++;
+        break;
+      }
+      _mm_storeu_si128(reinterpret_cast<__m128i *>(q), s);
+    }
+
+    is.src_ = p;
+    is.dst_ = q;
+  }
+
+  // When read/write pointers are the same for insitu stream, just skip
+  // unescaped characters
+  static RAPIDJSON_FORCEINLINE void SkipUnescapedString(
+      InsituStringStream &is) {
+    RAPIDJSON_ASSERT(is.src_ == is.dst_);
+    char *p = is.src_;
+
+    // Scan one by one until alignment (unaligned load may cross page boundary
+    // and cause crash)
+    const char *nextAligned = reinterpret_cast<const char *>(
+        (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+    for (; p != nextAligned; p++)
+      if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') ||
+          RAPIDJSON_UNLIKELY(static_cast<unsigned>(*p) < 0x20)) {
+        is.src_ = is.dst_ = p;
+        return;
+      }
+
+    // The rest of string using SIMD
+    static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"',
+                                    '\"', '\"', '\"', '\"', '\"', '\"',
+                                    '\"', '\"', '\"', '\"'};
+    static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\',
+                                    '\\', '\\', '\\', '\\', '\\', '\\',
+                                    '\\', '\\', '\\', '\\'};
+    static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+                                   0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+                                   0x1F, 0x1F, 0x1F, 0x1F};
+    const __m128i dq =
+        _mm_loadu_si128(reinterpret_cast<const __m128i *>(&dquote[0]));
+    const __m128i bs =
+        _mm_loadu_si128(reinterpret_cast<const __m128i *>(&bslash[0]));
+    const __m128i sp =
+        _mm_loadu_si128(reinterpret_cast<const __m128i *>(&space[0]));
+
+    for (;; p += 16) {
+      const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
+      const __m128i t1 = _mm_cmpeq_epi8(s, dq);
+      const __m128i t2 = _mm_cmpeq_epi8(s, bs);
+      const __m128i t3 = _mm_cmpeq_epi8(
+          _mm_max_epu8(s, sp), sp);  // s < 0x20 <=> max(s, 0x1F) == 0x1F
+      const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3);
+      unsigned short r = static_cast<unsigned short>(_mm_movemask_epi8(x));
+      if (RAPIDJSON_UNLIKELY(r != 0)) {  // some of characters is escaped
+        size_t length;
+#ifdef _MSC_VER  // Find the index of first escaped
+        unsigned long offset;
+        _BitScanForward(&offset, r);
+        length = offset;
+#else
+        length = static_cast<size_t>(__builtin_ffs(r) - 1);
+#endif
+        p += length;
+        break;
+      }
+    }
+
+    is.src_ = is.dst_ = p;
+  }
+#elif defined(RAPIDJSON_NEON)
+  // StringStream -> StackStream<char>
+  static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(
+      StringStream &is, StackStream<char> &os) {
+    const char *p = is.src_;
+
+    // Scan one by one until alignment (unaligned load may cross page boundary
+    // and cause crash)
+    const char *nextAligned = reinterpret_cast<const char *>(
+        (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+    while (p != nextAligned)
+      if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') ||
+          RAPIDJSON_UNLIKELY(static_cast<unsigned>(*p) < 0x20)) {
+        is.src_ = p;
+        return;
+      } else
+        os.Put(*p++);
+
+    // The rest of string using SIMD
+    const uint8x16_t s0 = vmovq_n_u8('"');
+    const uint8x16_t s1 = vmovq_n_u8('\\');
+    const uint8x16_t s2 = vmovq_n_u8('\b');
+    const uint8x16_t s3 = vmovq_n_u8(32);
+
+    for (;; p += 16) {
+      const uint8x16_t s = vld1q_u8(reinterpret_cast<const uint8_t *>(p));
+      uint8x16_t x = vceqq_u8(s, s0);
+      x = vorrq_u8(x, vceqq_u8(s, s1));
+      x = vorrq_u8(x, vceqq_u8(s, s2));
+      x = vorrq_u8(x, vcltq_u8(s, s3));
+
+      x = vrev64q_u8(x);                                           // Rev in 64
+      uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0);   // extract
+      uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1);  // extract
+
+      SizeType length = 0;
+      bool escaped = false;
+      if (low == 0) {
+        if (high != 0) {
+          uint32_t lz = RAPIDJSON_CLZLL(high);
+          length = 8 + (lz >> 3);
+          escaped = true;
+        }
+      } else {
+        uint32_t lz = RAPIDJSON_CLZLL(low);
+        length = lz >> 3;
+        escaped = true;
+      }
+      if (RAPIDJSON_UNLIKELY(escaped)) {  // some of characters is escaped
+        if (length != 0) {
+          char *q = reinterpret_cast<char *>(os.Push(length));
+          for (size_t i = 0; i < length; i++) q[i] = p[i];
+
+          p += length;
+        }
+        break;
+      }
+      vst1q_u8(reinterpret_cast<uint8_t *>(os.Push(16)), s);
+    }
+
+    is.src_ = p;
+  }
+
+  // InsituStringStream -> InsituStringStream
+  static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(
+      InsituStringStream &is, InsituStringStream &os) {
+    RAPIDJSON_ASSERT(&is == &os);
+    (void)os;
+
+    if (is.src_ == is.dst_) {
+      SkipUnescapedString(is);
+      return;
+    }
+
+    char *p = is.src_;
+    char *q = is.dst_;
+
+    // Scan one by one until alignment (unaligned load may cross page boundary
+    // and cause crash)
+    const char *nextAligned = reinterpret_cast<const char *>(
+        (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+    while (p != nextAligned)
+      if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') ||
+          RAPIDJSON_UNLIKELY(static_cast<unsigned>(*p) < 0x20)) {
+        is.src_ = p;
+        is.dst_ = q;
+        return;
+      } else
+        *q++ = *p++;
+
+    // The rest of string using SIMD
+    const uint8x16_t s0 = vmovq_n_u8('"');
+    const uint8x16_t s1 = vmovq_n_u8('\\');
+    const uint8x16_t s2 = vmovq_n_u8('\b');
+    const uint8x16_t s3 = vmovq_n_u8(32);
+
+    for (;; p += 16, q += 16) {
+      const uint8x16_t s = vld1q_u8(reinterpret_cast<uint8_t *>(p));
+      uint8x16_t x = vceqq_u8(s, s0);
+      x = vorrq_u8(x, vceqq_u8(s, s1));
+      x = vorrq_u8(x, vceqq_u8(s, s2));
+      x = vorrq_u8(x, vcltq_u8(s, s3));
+
+      x = vrev64q_u8(x);                                           // Rev in 64
+      uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0);   // extract
+      uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1);  // extract
+
+      SizeType length = 0;
+      bool escaped = false;
+      if (low == 0) {
+        if (high != 0) {
+          uint32_t lz = RAPIDJSON_CLZLL(high);
+          length = 8 + (lz >> 3);
+          escaped = true;
+        }
+      } else {
+        uint32_t lz = RAPIDJSON_CLZLL(low);
+        length = lz >> 3;
+        escaped = true;
+      }
+      if (RAPIDJSON_UNLIKELY(escaped)) {  // some of characters is escaped
+        for (const char *pend = p + length; p != pend;) {
+          *q++ = *p++;
+        }
+        break;
+      }
+      vst1q_u8(reinterpret_cast<uint8_t *>(q), s);
+    }
+
+    is.src_ = p;
+    is.dst_ = q;
+  }
+
+  // When read/write pointers are the same for insitu stream, just skip
+  // unescaped characters
+  static RAPIDJSON_FORCEINLINE void SkipUnescapedString(
+      InsituStringStream &is) {
+    RAPIDJSON_ASSERT(is.src_ == is.dst_);
+    char *p = is.src_;
+
+    // Scan one by one until alignment (unaligned load may cross page boundary
+    // and cause crash)
+    const char *nextAligned = reinterpret_cast<const char *>(
+        (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+    for (; p != nextAligned; p++)
+      if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') ||
+          RAPIDJSON_UNLIKELY(static_cast<unsigned>(*p) < 0x20)) {
+        is.src_ = is.dst_ = p;
+        return;
+      }
+
+    // The rest of string using SIMD
+    const uint8x16_t s0 = vmovq_n_u8('"');
+    const uint8x16_t s1 = vmovq_n_u8('\\');
+    const uint8x16_t s2 = vmovq_n_u8('\b');
+    const uint8x16_t s3 = vmovq_n_u8(32);
+
+    for (;; p += 16) {
+      const uint8x16_t s = vld1q_u8(reinterpret_cast<uint8_t *>(p));
+      uint8x16_t x = vceqq_u8(s, s0);
+      x = vorrq_u8(x, vceqq_u8(s, s1));
+      x = vorrq_u8(x, vceqq_u8(s, s2));
+      x = vorrq_u8(x, vcltq_u8(s, s3));
+
+      x = vrev64q_u8(x);                                           // Rev in 64
+      uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0);   // extract
+      uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1);  // extract
+
+      if (low == 0) {
+        if (high != 0) {
+          uint32_t lz = RAPIDJSON_CLZLL(high);
+          p += 8 + (lz >> 3);
+          break;
+        }
+      } else {
+        uint32_t lz = RAPIDJSON_CLZLL(low);
+        p += lz >> 3;
+        break;
+      }
+    }
+
+    is.src_ = is.dst_ = p;
+  }
+#endif  // RAPIDJSON_NEON
+
+  template <typename InputStream, bool backup, bool pushOnTake>
+  class NumberStream;
+
+  template <typename InputStream>
+  class NumberStream<InputStream, false, false> {
+   public:
+    typedef typename InputStream::Ch Ch;
+
+    NumberStream(GenericReader &reader, InputStream &s) : is(s) {
+      (void)reader;
+    }
+
+    RAPIDJSON_FORCEINLINE Ch Peek() const { return is.Peek(); }
+    RAPIDJSON_FORCEINLINE Ch TakePush() { return is.Take(); }
+    RAPIDJSON_FORCEINLINE Ch Take() { return is.Take(); }
+    RAPIDJSON_FORCEINLINE void Push(char) {}
+
+    size_t Tell() { return is.Tell(); }
+    size_t Length() { return 0; }
+    const char *Pop() { return 0; }
+
+   protected:
+    NumberStream &operator=(const NumberStream &);
+
+    InputStream &is;
+  };
+
+  template <typename InputStream>
+  class NumberStream<InputStream, true, false>
+      : public NumberStream<InputStream, false, false> {
+    typedef NumberStream<InputStream, false, false> Base;
+
+   public:
+    NumberStream(GenericReader &reader, InputStream &is)
+        : Base(reader, is), stackStream(reader.stack_) {}
+
+    RAPIDJSON_FORCEINLINE Ch TakePush() {
+      stackStream.Put(static_cast<char>(Base::is.Peek()));
+      return Base::is.Take();
+    }
+
+    RAPIDJSON_FORCEINLINE void Push(char c) { stackStream.Put(c); }
+
+    size_t Length() { return stackStream.Length(); }
+
+    const char *Pop() {
+      stackStream.Put('\0');
+      return stackStream.Pop();
+    }
+
+   private:
+    StackStream<char> stackStream;
+  };
+
+  template <typename InputStream>
+  class NumberStream<InputStream, true, true>
+      : public NumberStream<InputStream, true, false> {
+    typedef NumberStream<InputStream, true, false> Base;
+
+   public:
+    NumberStream(GenericReader &reader, InputStream &is) : Base(reader, is) {}
+
+    RAPIDJSON_FORCEINLINE Ch Take() { return Base::TakePush(); }
+  };
+
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  void ParseNumber(InputStream &is, Handler &handler) {
+    internal::StreamLocalCopy<InputStream> copy(is);
+    NumberStream<InputStream,
+                 ((parseFlags & kParseNumbersAsStringsFlag) != 0)
+                     ? ((parseFlags & kParseInsituFlag) == 0)
+                     : ((parseFlags & kParseFullPrecisionFlag) != 0),
+                 (parseFlags & kParseNumbersAsStringsFlag) != 0 &&
+                     (parseFlags & kParseInsituFlag) == 0>
+        s(*this, copy.s);
+
+    size_t startOffset = s.Tell();
+    double d = 0.0;
+    bool useNanOrInf = false;
+
+    // Parse minus
+    bool minus = Consume(s, '-');
+
+    // Parse int: zero / ( digit1-9 *DIGIT )
+    unsigned i = 0;
+    uint64_t i64 = 0;
+    bool use64bit = false;
+    int significandDigit = 0;
+    if (RAPIDJSON_UNLIKELY(s.Peek() == '0')) {
+      i = 0;
+      s.TakePush();
+    } else if (RAPIDJSON_LIKELY(s.Peek() >= '1' && s.Peek() <= '9')) {
+      i = static_cast<unsigned>(s.TakePush() - '0');
+
+      if (minus)
+        while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) {
+          if (RAPIDJSON_UNLIKELY(i >= 214748364)) {  // 2^31 = 2147483648
+            if (RAPIDJSON_LIKELY(i != 214748364 || s.Peek() > '8')) {
+              i64 = i;
+              use64bit = true;
+              break;
+            }
+          }
+          i = i * 10 + static_cast<unsigned>(s.TakePush() - '0');
+          significandDigit++;
+        }
+      else
+        while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) {
+          if (RAPIDJSON_UNLIKELY(i >= 429496729)) {  // 2^32 - 1 = 4294967295
+            if (RAPIDJSON_LIKELY(i != 429496729 || s.Peek() > '5')) {
+              i64 = i;
+              use64bit = true;
+              break;
+            }
+          }
+          i = i * 10 + static_cast<unsigned>(s.TakePush() - '0');
+          significandDigit++;
+        }
+    }
+    // Parse NaN or Infinity here
+    else if ((parseFlags & kParseNanAndInfFlag) &&
+             RAPIDJSON_LIKELY((s.Peek() == 'I' || s.Peek() == 'N'))) {
+      if (Consume(s, 'N')) {
+        if (Consume(s, 'a') && Consume(s, 'N')) {
+          d = std::numeric_limits<double>::quiet_NaN();
+          useNanOrInf = true;
+        }
+      } else if (RAPIDJSON_LIKELY(Consume(s, 'I'))) {
+        if (Consume(s, 'n') && Consume(s, 'f')) {
+          d = (minus ? -std::numeric_limits<double>::infinity()
+                     : std::numeric_limits<double>::infinity());
+          useNanOrInf = true;
+
+          if (RAPIDJSON_UNLIKELY(s.Peek() == 'i' &&
+                                 !(Consume(s, 'i') && Consume(s, 'n') &&
+                                   Consume(s, 'i') && Consume(s, 't') &&
+                                   Consume(s, 'y')))) {
+            RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, s.Tell());
+          }
+        }
+      }
+
+      if (RAPIDJSON_UNLIKELY(!useNanOrInf)) {
+        RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, s.Tell());
+      }
+    } else
+      RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, s.Tell());
+
+    // Parse 64bit int
+    bool useDouble = false;
+    if (use64bit) {
+      if (minus)
+        while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) {
+          if (RAPIDJSON_UNLIKELY(
+                  i64 >=
+                  RAPIDJSON_UINT64_C2(
+                      0x0CCCCCCC, 0xCCCCCCCC)))  // 2^63 = 9223372036854775808
+            if (RAPIDJSON_LIKELY(
+                    i64 != RAPIDJSON_UINT64_C2(0x0CCCCCCC, 0xCCCCCCCC) ||
+                    s.Peek() > '8')) {
+              d = static_cast<double>(i64);
+              useDouble = true;
+              break;
+            }
+          i64 = i64 * 10 + static_cast<unsigned>(s.TakePush() - '0');
+          significandDigit++;
+        }
+      else
+        while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) {
+          if (RAPIDJSON_UNLIKELY(
+                  i64 >= RAPIDJSON_UINT64_C2(
+                             0x19999999,
+                             0x99999999)))  // 2^64 - 1 = 18446744073709551615
+            if (RAPIDJSON_LIKELY(
+                    i64 != RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) ||
+                    s.Peek() > '5')) {
+              d = static_cast<double>(i64);
+              useDouble = true;
+              break;
+            }
+          i64 = i64 * 10 + static_cast<unsigned>(s.TakePush() - '0');
+          significandDigit++;
+        }
+    }
+
+    // Force double for big integer
+    if (useDouble) {
+      while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) {
+        d = d * 10 + (s.TakePush() - '0');
+      }
+    }
+
+    // Parse frac = decimal-point 1*DIGIT
+    int expFrac = 0;
+    size_t decimalPosition;
+    if (Consume(s, '.')) {
+      decimalPosition = s.Length();
+
+      if (RAPIDJSON_UNLIKELY(!(s.Peek() >= '0' && s.Peek() <= '9')))
+        RAPIDJSON_PARSE_ERROR(kParseErrorNumberMissFraction, s.Tell());
+
+      if (!useDouble) {
+#if RAPIDJSON_64BIT
+        // Use i64 to store significand in 64-bit architecture
+        if (!use64bit) i64 = i;
+
+        while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) {
+          if (i64 > RAPIDJSON_UINT64_C2(0x1FFFFF,
+                                        0xFFFFFFFF))  // 2^53 - 1 for fast path
+            break;
+          else {
+            i64 = i64 * 10 + static_cast<unsigned>(s.TakePush() - '0');
+            --expFrac;
+            if (i64 != 0) significandDigit++;
+          }
+        }
+
+        d = static_cast<double>(i64);
+#else
+        // Use double to store significand in 32-bit architecture
+        d = static_cast<double>(use64bit ? i64 : i);
+#endif
+        useDouble = true;
+      }
+
+      while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) {
+        if (significandDigit < 17) {
+          d = d * 10.0 + (s.TakePush() - '0');
+          --expFrac;
+          if (RAPIDJSON_LIKELY(d > 0.0)) significandDigit++;
+        } else
+          s.TakePush();
+      }
+    } else
+      decimalPosition = s.Length();  // decimal position at the end of integer.
+
+    // Parse exp = e [ minus / plus ] 1*DIGIT
+    int exp = 0;
+    if (Consume(s, 'e') || Consume(s, 'E')) {
+      if (!useDouble) {
+        d = static_cast<double>(use64bit ? i64 : i);
+        useDouble = true;
+      }
+
+      bool expMinus = false;
+      if (Consume(s, '+'))
+        ;
+      else if (Consume(s, '-'))
+        expMinus = true;
+
+      if (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) {
+        exp = static_cast<int>(s.Take() - '0');
+        if (expMinus) {
+          // (exp + expFrac) must not underflow int => we're detecting when -exp
+          // gets dangerously close to INT_MIN (a pessimistic next digit 9 would
+          // push it into underflow territory):
+          //
+          //        -(exp * 10 + 9) + expFrac >= INT_MIN
+          //   <=>  exp <= (expFrac - INT_MIN - 9) / 10
+          RAPIDJSON_ASSERT(expFrac <= 0);
+          int maxExp = (expFrac + 2147483639) / 10;
+
+          while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) {
+            exp = exp * 10 + static_cast<int>(s.Take() - '0');
+            if (RAPIDJSON_UNLIKELY(exp > maxExp)) {
+              while (RAPIDJSON_UNLIKELY(
+                  s.Peek() >= '0' &&
+                  s.Peek() <= '9'))  // Consume the rest of exponent
+                s.Take();
+            }
+          }
+        } else {  // positive exp
+          int maxExp = 308 - expFrac;
+          while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) {
+            exp = exp * 10 + static_cast<int>(s.Take() - '0');
+            if (RAPIDJSON_UNLIKELY(exp > maxExp))
+              RAPIDJSON_PARSE_ERROR(kParseErrorNumberTooBig, startOffset);
+          }
+        }
+      } else
+        RAPIDJSON_PARSE_ERROR(kParseErrorNumberMissExponent, s.Tell());
+
+      if (expMinus) exp = -exp;
+    }
+
+    // Finish parsing, call event according to the type of number.
+    bool cont = true;
+
+    if (parseFlags & kParseNumbersAsStringsFlag) {
+      if (parseFlags & kParseInsituFlag) {
+        s.Pop();  // Pop stack no matter if it will be used or not.
+        typename InputStream::Ch *head = is.PutBegin();
+        const size_t length = s.Tell() - startOffset;
+        RAPIDJSON_ASSERT(length <= 0xFFFFFFFF);
+        // unable to insert the \0 character here, it will erase the comma after
+        // this number
+        const typename TargetEncoding::Ch *const str =
+            reinterpret_cast<typename TargetEncoding::Ch *>(head);
+        cont = handler.RawNumber(str, SizeType(length), false);
+      } else {
+        SizeType numCharsToCopy = static_cast<SizeType>(s.Length());
+        StringStream srcStream(s.Pop());
+        StackStream<typename TargetEncoding::Ch> dstStream(stack_);
+        while (numCharsToCopy--) {
+          Transcoder<UTF8<>, TargetEncoding>::Transcode(srcStream, dstStream);
+        }
+        dstStream.Put('\0');
+        const typename TargetEncoding::Ch *str = dstStream.Pop();
+        const SizeType length = static_cast<SizeType>(dstStream.Length()) - 1;
+        cont = handler.RawNumber(str, SizeType(length), true);
+      }
+    } else {
+      size_t length = s.Length();
+      const char *decimal =
+          s.Pop();  // Pop stack no matter if it will be used or not.
+
+      if (useDouble) {
+        int p = exp + expFrac;
+        if (parseFlags & kParseFullPrecisionFlag)
+          d = internal::StrtodFullPrecision(d, p, decimal, length,
+                                            decimalPosition, exp);
+        else
+          d = internal::StrtodNormalPrecision(d, p);
+
+        // Use > max, instead of == inf, to fix bogus warning -Wfloat-equal
+        if (d > (std::numeric_limits<double>::max)()) {
+          // Overflow
+          // TODO: internal::StrtodX should report overflow (or underflow)
+          RAPIDJSON_PARSE_ERROR(kParseErrorNumberTooBig, startOffset);
+        }
+
+        cont = handler.Double(minus ? -d : d);
+      } else if (useNanOrInf) {
+        cont = handler.Double(d);
+      } else {
+        if (use64bit) {
+          if (minus)
+            cont = handler.Int64(static_cast<int64_t>(~i64 + 1));
+          else
+            cont = handler.Uint64(i64);
+        } else {
+          if (minus)
+            cont = handler.Int(static_cast<int32_t>(~i + 1));
+          else
+            cont = handler.Uint(i);
+        }
+      }
+    }
+    if (RAPIDJSON_UNLIKELY(!cont))
+      RAPIDJSON_PARSE_ERROR(kParseErrorTermination, startOffset);
+  }
+
+  // Parse any JSON value
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  void ParseValue(InputStream &is, Handler &handler) {
+    switch (is.Peek()) {
+      case 'n':
+        ParseNull<parseFlags>(is, handler);
+        break;
+      case 't':
+        ParseTrue<parseFlags>(is, handler);
+        break;
+      case 'f':
+        ParseFalse<parseFlags>(is, handler);
+        break;
+      case '"':
+        ParseString<parseFlags>(is, handler);
+        break;
+      case '{':
+        ParseObject<parseFlags>(is, handler);
+        break;
+      case '[':
+        ParseArray<parseFlags>(is, handler);
+        break;
+      default:
+        ParseNumber<parseFlags>(is, handler);
+        break;
+    }
+  }
+
+  // Iterative Parsing
+
+  // States
+  enum IterativeParsingState {
+    IterativeParsingFinishState = 0,  // sink states at top
+    IterativeParsingErrorState,       // sink states at top
+    IterativeParsingStartState,
+
+    // Object states
+    IterativeParsingObjectInitialState,
+    IterativeParsingMemberKeyState,
+    IterativeParsingMemberValueState,
+    IterativeParsingObjectFinishState,
+
+    // Array states
+    IterativeParsingArrayInitialState,
+    IterativeParsingElementState,
+    IterativeParsingArrayFinishState,
+
+    // Single value state
+    IterativeParsingValueState,
+
+    // Delimiter states (at bottom)
+    IterativeParsingElementDelimiterState,
+    IterativeParsingMemberDelimiterState,
+    IterativeParsingKeyValueDelimiterState,
+
+    cIterativeParsingStateCount
+  };
+
+  // Tokens
+  enum Token {
+    LeftBracketToken = 0,
+    RightBracketToken,
+
+    LeftCurlyBracketToken,
+    RightCurlyBracketToken,
+
+    CommaToken,
+    ColonToken,
+
+    StringToken,
+    FalseToken,
+    TrueToken,
+    NullToken,
+    NumberToken,
+
+    kTokenCount
+  };
+
+  RAPIDJSON_FORCEINLINE Token Tokenize(Ch c) const {
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#define N NumberToken
+#define N16 N, N, N, N, N, N, N, N, N, N, N, N, N, N, N, N
+    // Maps from ASCII to Token
+    static const unsigned char tokenMap[256] = {
+        N16,  // 00~0F
+        N16,  // 10~1F
+        N,           N,
+        StringToken, N,
+        N,           N,
+        N,           N,
+        N,           N,
+        N,           N,
+        CommaToken,  N,
+        N,           N,  // 20~2F
+        N,           N,
+        N,           N,
+        N,           N,
+        N,           N,
+        N,           N,
+        ColonToken,  N,
+        N,           N,
+        N,           N,  // 30~3F
+        N16,             // 40~4F
+        N,           N,
+        N,           N,
+        N,           N,
+        N,           N,
+        N,           N,
+        N,           LeftBracketToken,
+        N,           RightBracketToken,
+        N,           N,  // 50~5F
+        N,           N,
+        N,           N,
+        N,           N,
+        FalseToken,  N,
+        N,           N,
+        N,           N,
+        N,           N,
+        NullToken,   N,  // 60~6F
+        N,           N,
+        N,           N,
+        TrueToken,   N,
+        N,           N,
+        N,           N,
+        N,           LeftCurlyBracketToken,
+        N,           RightCurlyBracketToken,
+        N,           N,  // 70~7F
+        N16,         N16,
+        N16,         N16,
+        N16,         N16,
+        N16,         N16  // 80~FF
+    };
+#undef N
+#undef N16
+    //!@endcond
+
+    if (sizeof(Ch) == 1 || static_cast<unsigned>(c) < 256)
+      return static_cast<Token>(tokenMap[static_cast<unsigned char>(c)]);
+    else
+      return NumberToken;
+  }
+
+  RAPIDJSON_FORCEINLINE IterativeParsingState
+  Predict(IterativeParsingState state, Token token) const {
+    // current state x one lookahead token -> new state
+    static const char G[cIterativeParsingStateCount][kTokenCount] = {
+        // Finish(sink state)
+        {IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState},
+        // Error(sink state)
+        {IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState},
+        // Start
+        {
+            IterativeParsingArrayInitialState,   // Left bracket
+            IterativeParsingErrorState,          // Right bracket
+            IterativeParsingObjectInitialState,  // Left curly bracket
+            IterativeParsingErrorState,          // Right curly bracket
+            IterativeParsingErrorState,          // Comma
+            IterativeParsingErrorState,          // Colon
+            IterativeParsingValueState,          // String
+            IterativeParsingValueState,          // False
+            IterativeParsingValueState,          // True
+            IterativeParsingValueState,          // Null
+            IterativeParsingValueState           // Number
+        },
+        // ObjectInitial
+        {
+            IterativeParsingErrorState,         // Left bracket
+            IterativeParsingErrorState,         // Right bracket
+            IterativeParsingErrorState,         // Left curly bracket
+            IterativeParsingObjectFinishState,  // Right curly bracket
+            IterativeParsingErrorState,         // Comma
+            IterativeParsingErrorState,         // Colon
+            IterativeParsingMemberKeyState,     // String
+            IterativeParsingErrorState,         // False
+            IterativeParsingErrorState,         // True
+            IterativeParsingErrorState,         // Null
+            IterativeParsingErrorState          // Number
+        },
+        // MemberKey
+        {
+            IterativeParsingErrorState,              // Left bracket
+            IterativeParsingErrorState,              // Right bracket
+            IterativeParsingErrorState,              // Left curly bracket
+            IterativeParsingErrorState,              // Right curly bracket
+            IterativeParsingErrorState,              // Comma
+            IterativeParsingKeyValueDelimiterState,  // Colon
+            IterativeParsingErrorState,              // String
+            IterativeParsingErrorState,              // False
+            IterativeParsingErrorState,              // True
+            IterativeParsingErrorState,              // Null
+            IterativeParsingErrorState               // Number
+        },
+        // MemberValue
+        {
+            IterativeParsingErrorState,            // Left bracket
+            IterativeParsingErrorState,            // Right bracket
+            IterativeParsingErrorState,            // Left curly bracket
+            IterativeParsingObjectFinishState,     // Right curly bracket
+            IterativeParsingMemberDelimiterState,  // Comma
+            IterativeParsingErrorState,            // Colon
+            IterativeParsingErrorState,            // String
+            IterativeParsingErrorState,            // False
+            IterativeParsingErrorState,            // True
+            IterativeParsingErrorState,            // Null
+            IterativeParsingErrorState             // Number
+        },
+        // ObjectFinish(sink state)
+        {IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState},
+        // ArrayInitial
+        {
+            IterativeParsingArrayInitialState,   // Left bracket(push Element
+                                                 // state)
+            IterativeParsingArrayFinishState,    // Right bracket
+            IterativeParsingObjectInitialState,  // Left curly bracket(push
+                                                 // Element state)
+            IterativeParsingErrorState,          // Right curly bracket
+            IterativeParsingErrorState,          // Comma
+            IterativeParsingErrorState,          // Colon
+            IterativeParsingElementState,        // String
+            IterativeParsingElementState,        // False
+            IterativeParsingElementState,        // True
+            IterativeParsingElementState,        // Null
+            IterativeParsingElementState         // Number
+        },
+        // Element
+        {
+            IterativeParsingErrorState,             // Left bracket
+            IterativeParsingArrayFinishState,       // Right bracket
+            IterativeParsingErrorState,             // Left curly bracket
+            IterativeParsingErrorState,             // Right curly bracket
+            IterativeParsingElementDelimiterState,  // Comma
+            IterativeParsingErrorState,             // Colon
+            IterativeParsingErrorState,             // String
+            IterativeParsingErrorState,             // False
+            IterativeParsingErrorState,             // True
+            IterativeParsingErrorState,             // Null
+            IterativeParsingErrorState              // Number
+        },
+        // ArrayFinish(sink state)
+        {IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState},
+        // Single Value (sink state)
+        {IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState, IterativeParsingErrorState,
+         IterativeParsingErrorState},
+        // ElementDelimiter
+        {
+            IterativeParsingArrayInitialState,   // Left bracket(push Element
+                                                 // state)
+            IterativeParsingArrayFinishState,    // Right bracket
+            IterativeParsingObjectInitialState,  // Left curly bracket(push
+                                                 // Element state)
+            IterativeParsingErrorState,          // Right curly bracket
+            IterativeParsingErrorState,          // Comma
+            IterativeParsingErrorState,          // Colon
+            IterativeParsingElementState,        // String
+            IterativeParsingElementState,        // False
+            IterativeParsingElementState,        // True
+            IterativeParsingElementState,        // Null
+            IterativeParsingElementState         // Number
+        },
+        // MemberDelimiter
+        {
+            IterativeParsingErrorState,         // Left bracket
+            IterativeParsingErrorState,         // Right bracket
+            IterativeParsingErrorState,         // Left curly bracket
+            IterativeParsingObjectFinishState,  // Right curly bracket
+            IterativeParsingErrorState,         // Comma
+            IterativeParsingErrorState,         // Colon
+            IterativeParsingMemberKeyState,     // String
+            IterativeParsingErrorState,         // False
+            IterativeParsingErrorState,         // True
+            IterativeParsingErrorState,         // Null
+            IterativeParsingErrorState          // Number
+        },
+        // KeyValueDelimiter
+        {
+            IterativeParsingArrayInitialState,  // Left bracket(push MemberValue
+                                                // state)
+            IterativeParsingErrorState,         // Right bracket
+            IterativeParsingObjectInitialState,  // Left curly bracket(push
+                                                 // MemberValue state)
+            IterativeParsingErrorState,          // Right curly bracket
+            IterativeParsingErrorState,          // Comma
+            IterativeParsingErrorState,          // Colon
+            IterativeParsingMemberValueState,    // String
+            IterativeParsingMemberValueState,    // False
+            IterativeParsingMemberValueState,    // True
+            IterativeParsingMemberValueState,    // Null
+            IterativeParsingMemberValueState     // Number
+        },
+    };  // End of G
+
+    return static_cast<IterativeParsingState>(G[state][token]);
+  }
+
+  // Make an advance in the token stream and state based on the candidate
+  // destination state which was returned by Transit(). May return a new state
+  // on state pop.
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  RAPIDJSON_FORCEINLINE IterativeParsingState Transit(IterativeParsingState src,
+                                                      Token token,
+                                                      IterativeParsingState dst,
+                                                      InputStream &is,
+                                                      Handler &handler) {
+    (void)token;
+
+    switch (dst) {
+      case IterativeParsingErrorState:
+        return dst;
+
+      case IterativeParsingObjectInitialState:
+      case IterativeParsingArrayInitialState: {
+        // Push the state(Element or MemeberValue) if we are nested in another
+        // array or value of member. In this way we can get the correct state on
+        // ObjectFinish or ArrayFinish by frame pop.
+        IterativeParsingState n = src;
+        if (src == IterativeParsingArrayInitialState ||
+            src == IterativeParsingElementDelimiterState)
+          n = IterativeParsingElementState;
+        else if (src == IterativeParsingKeyValueDelimiterState)
+          n = IterativeParsingMemberValueState;
+        // Push current state.
+        *stack_.template Push<SizeType>(1) = n;
+        // Initialize and push the member/element count.
+        *stack_.template Push<SizeType>(1) = 0;
+        // Call handler
+        bool hr = (dst == IterativeParsingObjectInitialState)
+                      ? handler.StartObject()
+                      : handler.StartArray();
+        // On handler short circuits the parsing.
+        if (!hr) {
+          RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorTermination, is.Tell());
+          return IterativeParsingErrorState;
+        } else {
+          is.Take();
+          return dst;
+        }
+      }
+
+      case IterativeParsingMemberKeyState:
+        ParseString<parseFlags>(is, handler, true);
+        if (HasParseError())
+          return IterativeParsingErrorState;
+        else
+          return dst;
+
+      case IterativeParsingKeyValueDelimiterState:
+        RAPIDJSON_ASSERT(token == ColonToken);
+        is.Take();
+        return dst;
+
+      case IterativeParsingMemberValueState:
+        // Must be non-compound value. Or it would be ObjectInitial or
+        // ArrayInitial state.
+        ParseValue<parseFlags>(is, handler);
+        if (HasParseError()) {
+          return IterativeParsingErrorState;
+        }
+        return dst;
+
+      case IterativeParsingElementState:
+        // Must be non-compound value. Or it would be ObjectInitial or
+        // ArrayInitial state.
+        ParseValue<parseFlags>(is, handler);
+        if (HasParseError()) {
+          return IterativeParsingErrorState;
+        }
+        return dst;
+
+      case IterativeParsingMemberDelimiterState:
+      case IterativeParsingElementDelimiterState:
+        is.Take();
+        // Update member/element count.
+        *stack_.template Top<SizeType>() = *stack_.template Top<SizeType>() + 1;
+        return dst;
+
+      case IterativeParsingObjectFinishState: {
+        // Transit from delimiter is only allowed when trailing commas are
+        // enabled
+        if (!(parseFlags & kParseTrailingCommasFlag) &&
+            src == IterativeParsingMemberDelimiterState) {
+          RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorObjectMissName, is.Tell());
+          return IterativeParsingErrorState;
+        }
+        // Get member count.
+        SizeType c = *stack_.template Pop<SizeType>(1);
+        // If the object is not empty, count the last member.
+        if (src == IterativeParsingMemberValueState) ++c;
+        // Restore the state.
+        IterativeParsingState n = static_cast<IterativeParsingState>(
+            *stack_.template Pop<SizeType>(1));
+        // Transit to Finish state if this is the topmost scope.
+        if (n == IterativeParsingStartState) n = IterativeParsingFinishState;
+        // Call handler
+        bool hr = handler.EndObject(c);
+        // On handler short circuits the parsing.
+        if (!hr) {
+          RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorTermination, is.Tell());
+          return IterativeParsingErrorState;
+        } else {
+          is.Take();
+          return n;
+        }
+      }
+
+      case IterativeParsingArrayFinishState: {
+        // Transit from delimiter is only allowed when trailing commas are
+        // enabled
+        if (!(parseFlags & kParseTrailingCommasFlag) &&
+            src == IterativeParsingElementDelimiterState) {
+          RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorValueInvalid, is.Tell());
+          return IterativeParsingErrorState;
+        }
+        // Get element count.
+        SizeType c = *stack_.template Pop<SizeType>(1);
+        // If the array is not empty, count the last element.
+        if (src == IterativeParsingElementState) ++c;
+        // Restore the state.
+        IterativeParsingState n = static_cast<IterativeParsingState>(
+            *stack_.template Pop<SizeType>(1));
+        // Transit to Finish state if this is the topmost scope.
+        if (n == IterativeParsingStartState) n = IterativeParsingFinishState;
+        // Call handler
+        bool hr = handler.EndArray(c);
+        // On handler short circuits the parsing.
+        if (!hr) {
+          RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorTermination, is.Tell());
+          return IterativeParsingErrorState;
+        } else {
+          is.Take();
+          return n;
+        }
+      }
+
+      default:
+        // This branch is for IterativeParsingValueState actually.
+        // Use `default:` rather than
+        // `case IterativeParsingValueState:` is for code coverage.
+
+        // The IterativeParsingStartState is not enumerated in this switch-case.
+        // It is impossible for that case. And it can be caught by following
+        // assertion.
+
+        // The IterativeParsingFinishState is not enumerated in this switch-case
+        // either. It is a "derivative" state which cannot triggered from
+        // Predict() directly. Therefore it cannot happen here. And it can be
+        // caught by following assertion.
+        RAPIDJSON_ASSERT(dst == IterativeParsingValueState);
+
+        // Must be non-compound value. Or it would be ObjectInitial or
+        // ArrayInitial state.
+        ParseValue<parseFlags>(is, handler);
+        if (HasParseError()) {
+          return IterativeParsingErrorState;
+        }
+        return IterativeParsingFinishState;
+    }
+  }
+
+  template <typename InputStream>
+  void HandleError(IterativeParsingState src, InputStream &is) {
+    if (HasParseError()) {
+      // Error flag has been set.
+      return;
+    }
+
+    switch (src) {
+      case IterativeParsingStartState:
+        RAPIDJSON_PARSE_ERROR(kParseErrorDocumentEmpty, is.Tell());
+        return;
+      case IterativeParsingFinishState:
+        RAPIDJSON_PARSE_ERROR(kParseErrorDocumentRootNotSingular, is.Tell());
+        return;
+      case IterativeParsingObjectInitialState:
+      case IterativeParsingMemberDelimiterState:
+        RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissName, is.Tell());
+        return;
+      case IterativeParsingMemberKeyState:
+        RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissColon, is.Tell());
+        return;
+      case IterativeParsingMemberValueState:
+        RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissCommaOrCurlyBracket,
+                              is.Tell());
+        return;
+      case IterativeParsingKeyValueDelimiterState:
+      case IterativeParsingArrayInitialState:
+      case IterativeParsingElementDelimiterState:
+        RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell());
+        return;
+      default:
+        RAPIDJSON_ASSERT(src == IterativeParsingElementState);
+        RAPIDJSON_PARSE_ERROR(kParseErrorArrayMissCommaOrSquareBracket,
+                              is.Tell());
+        return;
+    }
+  }
+
+  RAPIDJSON_FORCEINLINE bool IsIterativeParsingDelimiterState(
+      IterativeParsingState s) const {
+    return s >= IterativeParsingElementDelimiterState;
+  }
+
+  RAPIDJSON_FORCEINLINE bool IsIterativeParsingCompleteState(
+      IterativeParsingState s) const {
+    return s <= IterativeParsingErrorState;
+  }
+
+  template <unsigned parseFlags, typename InputStream, typename Handler>
+  ParseResult IterativeParse(InputStream &is, Handler &handler) {
+    parseResult_.Clear();
+    ClearStackOnExit scope(*this);
+    IterativeParsingState state = IterativeParsingStartState;
+
+    SkipWhitespaceAndComments<parseFlags>(is);
+    RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_);
+    while (is.Peek() != '\0') {
+      Token t = Tokenize(is.Peek());
+      IterativeParsingState n = Predict(state, t);
+      IterativeParsingState d = Transit<parseFlags>(state, t, n, is, handler);
+
+      if (d == IterativeParsingErrorState) {
+        HandleError(state, is);
+        break;
+      }
+
+      state = d;
+
+      // Do not further consume streams if a root JSON has been parsed.
+      if ((parseFlags & kParseStopWhenDoneFlag) &&
+          state == IterativeParsingFinishState)
+        break;
+
+      SkipWhitespaceAndComments<parseFlags>(is);
+      RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_);
+    }
+
+    // Handle the end of file.
+    if (state != IterativeParsingFinishState) HandleError(state, is);
+
+    return parseResult_;
+  }
+
+  static const size_t kDefaultStackCapacity =
+      256;  //!< Default stack capacity in bytes for storing a single decoded
+            //!< string.
+  internal::Stack<StackAllocator>
+      stack_;  //!< A stack for storing decoded string temporarily during
+               //!< non-destructive parsing.
+  ParseResult parseResult_;
+  IterativeParsingState state_;
+};  // class GenericReader
+
+//! Reader with UTF8 encoding and default allocator.
+typedef GenericReader<UTF8<>, UTF8<>> Reader;
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__) || defined(_MSC_VER)
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_READER_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/schema.h b/livox_ros_driver2/3rdparty/rapidjson/schema.h
new file mode 100644
index 0000000..b9dc8c8
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/schema.h
@@ -0,0 +1,2743 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available->
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip-> All
+// rights reserved->
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License-> You may obtain a copy of the License
+// at
+//
+// http://opensource->org/licenses/MIT
+//
+// 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->
+
+#ifndef RAPIDJSON_SCHEMA_H_
+#define RAPIDJSON_SCHEMA_H_
+
+#include <cmath>  // abs, floor
+#include "document.h"
+#include "pointer.h"
+#include "stringbuffer.h"
+
+#if !defined(RAPIDJSON_SCHEMA_USE_INTERNALREGEX)
+#define RAPIDJSON_SCHEMA_USE_INTERNALREGEX 1
+#else
+#define RAPIDJSON_SCHEMA_USE_INTERNALREGEX 0
+#endif
+
+#if !RAPIDJSON_SCHEMA_USE_INTERNALREGEX &&    \
+    defined(RAPIDJSON_SCHEMA_USE_STDREGEX) && \
+    (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800))
+#define RAPIDJSON_SCHEMA_USE_STDREGEX 1
+#else
+#define RAPIDJSON_SCHEMA_USE_STDREGEX 0
+#endif
+
+#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX
+#include "internal/regex.h"
+#elif RAPIDJSON_SCHEMA_USE_STDREGEX
+#include <regex>
+#endif
+
+#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX || RAPIDJSON_SCHEMA_USE_STDREGEX
+#define RAPIDJSON_SCHEMA_HAS_REGEX 1
+#else
+#define RAPIDJSON_SCHEMA_HAS_REGEX 0
+#endif
+
+#ifndef RAPIDJSON_SCHEMA_VERBOSE
+#define RAPIDJSON_SCHEMA_VERBOSE 0
+#endif
+
+#if RAPIDJSON_SCHEMA_VERBOSE
+#include "stringbuffer.h"
+#endif
+
+RAPIDJSON_DIAG_PUSH
+
+#if defined(__GNUC__)
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_OFF(weak - vtables)
+RAPIDJSON_DIAG_OFF(exit - time - destructors)
+RAPIDJSON_DIAG_OFF(c++ 98 - compat - pedantic)
+RAPIDJSON_DIAG_OFF(variadic - macros)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// Verbose Utilities
+
+#if RAPIDJSON_SCHEMA_VERBOSE
+
+namespace internal {
+
+inline void PrintInvalidKeyword(const char *keyword) {
+  printf("Fail keyword: %s\n", keyword);
+}
+
+inline void PrintInvalidKeyword(const wchar_t *keyword) {
+  wprintf(L"Fail keyword: %ls\n", keyword);
+}
+
+inline void PrintInvalidDocument(const char *document) {
+  printf("Fail document: %s\n\n", document);
+}
+
+inline void PrintInvalidDocument(const wchar_t *document) {
+  wprintf(L"Fail document: %ls\n\n", document);
+}
+
+inline void PrintValidatorPointers(unsigned depth, const char *s,
+                                   const char *d) {
+  printf("S: %*s%s\nD: %*s%s\n\n", depth * 4, " ", s, depth * 4, " ", d);
+}
+
+inline void PrintValidatorPointers(unsigned depth, const wchar_t *s,
+                                   const wchar_t *d) {
+  wprintf(L"S: %*ls%ls\nD: %*ls%ls\n\n", depth * 4, L" ", s, depth * 4, L" ",
+          d);
+}
+
+}  // namespace internal
+
+#endif  // RAPIDJSON_SCHEMA_VERBOSE
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_INVALID_KEYWORD_RETURN
+
+#if RAPIDJSON_SCHEMA_VERBOSE
+#define RAPIDJSON_INVALID_KEYWORD_VERBOSE(keyword) \
+  internal::PrintInvalidKeyword(keyword)
+#else
+#define RAPIDJSON_INVALID_KEYWORD_VERBOSE(keyword)
+#endif
+
+#define RAPIDJSON_INVALID_KEYWORD_RETURN(keyword)         \
+  RAPIDJSON_MULTILINEMACRO_BEGIN                          \
+  context.invalidKeyword = keyword.GetString();           \
+  RAPIDJSON_INVALID_KEYWORD_VERBOSE(keyword.GetString()); \
+  return false;                                           \
+  RAPIDJSON_MULTILINEMACRO_END
+
+///////////////////////////////////////////////////////////////////////////////
+// Forward declarations
+
+template <typename ValueType, typename Allocator>
+class GenericSchemaDocument;
+
+namespace internal {
+
+template <typename SchemaDocumentType>
+class Schema;
+
+///////////////////////////////////////////////////////////////////////////////
+// ISchemaValidator
+
+class ISchemaValidator {
+ public:
+  virtual ~ISchemaValidator() {}
+  virtual bool IsValid() const = 0;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// ISchemaStateFactory
+
+template <typename SchemaType>
+class ISchemaStateFactory {
+ public:
+  virtual ~ISchemaStateFactory() {}
+  virtual ISchemaValidator *CreateSchemaValidator(const SchemaType &) = 0;
+  virtual void DestroySchemaValidator(ISchemaValidator *validator) = 0;
+  virtual void *CreateHasher() = 0;
+  virtual uint64_t GetHashCode(void *hasher) = 0;
+  virtual void DestroryHasher(void *hasher) = 0;
+  virtual void *MallocState(size_t size) = 0;
+  virtual void FreeState(void *p) = 0;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// IValidationErrorHandler
+
+template <typename SchemaType>
+class IValidationErrorHandler {
+ public:
+  typedef typename SchemaType::Ch Ch;
+  typedef typename SchemaType::SValue SValue;
+
+  virtual ~IValidationErrorHandler() {}
+
+  virtual void NotMultipleOf(int64_t actual, const SValue &expected) = 0;
+  virtual void NotMultipleOf(uint64_t actual, const SValue &expected) = 0;
+  virtual void NotMultipleOf(double actual, const SValue &expected) = 0;
+  virtual void AboveMaximum(int64_t actual, const SValue &expected,
+                            bool exclusive) = 0;
+  virtual void AboveMaximum(uint64_t actual, const SValue &expected,
+                            bool exclusive) = 0;
+  virtual void AboveMaximum(double actual, const SValue &expected,
+                            bool exclusive) = 0;
+  virtual void BelowMinimum(int64_t actual, const SValue &expected,
+                            bool exclusive) = 0;
+  virtual void BelowMinimum(uint64_t actual, const SValue &expected,
+                            bool exclusive) = 0;
+  virtual void BelowMinimum(double actual, const SValue &expected,
+                            bool exclusive) = 0;
+
+  virtual void TooLong(const Ch *str, SizeType length, SizeType expected) = 0;
+  virtual void TooShort(const Ch *str, SizeType length, SizeType expected) = 0;
+  virtual void DoesNotMatch(const Ch *str, SizeType length) = 0;
+
+  virtual void DisallowedItem(SizeType index) = 0;
+  virtual void TooFewItems(SizeType actualCount, SizeType expectedCount) = 0;
+  virtual void TooManyItems(SizeType actualCount, SizeType expectedCount) = 0;
+  virtual void DuplicateItems(SizeType index1, SizeType index2) = 0;
+
+  virtual void TooManyProperties(SizeType actualCount,
+                                 SizeType expectedCount) = 0;
+  virtual void TooFewProperties(SizeType actualCount,
+                                SizeType expectedCount) = 0;
+  virtual void StartMissingProperties() = 0;
+  virtual void AddMissingProperty(const SValue &name) = 0;
+  virtual bool EndMissingProperties() = 0;
+  virtual void PropertyViolations(ISchemaValidator **subvalidators,
+                                  SizeType count) = 0;
+  virtual void DisallowedProperty(const Ch *name, SizeType length) = 0;
+
+  virtual void StartDependencyErrors() = 0;
+  virtual void StartMissingDependentProperties() = 0;
+  virtual void AddMissingDependentProperty(const SValue &targetName) = 0;
+  virtual void EndMissingDependentProperties(const SValue &sourceName) = 0;
+  virtual void AddDependencySchemaError(const SValue &souceName,
+                                        ISchemaValidator *subvalidator) = 0;
+  virtual bool EndDependencyErrors() = 0;
+
+  virtual void DisallowedValue() = 0;
+  virtual void StartDisallowedType() = 0;
+  virtual void AddExpectedType(
+      const typename SchemaType::ValueType &expectedType) = 0;
+  virtual void EndDisallowedType(
+      const typename SchemaType::ValueType &actualType) = 0;
+  virtual void NotAllOf(ISchemaValidator **subvalidators, SizeType count) = 0;
+  virtual void NoneOf(ISchemaValidator **subvalidators, SizeType count) = 0;
+  virtual void NotOneOf(ISchemaValidator **subvalidators, SizeType count) = 0;
+  virtual void Disallowed() = 0;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Hasher
+
+// For comparison of compound value
+template <typename Encoding, typename Allocator>
+class Hasher {
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  Hasher(Allocator *allocator = 0, size_t stackCapacity = kDefaultSize)
+      : stack_(allocator, stackCapacity) {}
+
+  bool Null() { return WriteType(kNullType); }
+  bool Bool(bool b) { return WriteType(b ? kTrueType : kFalseType); }
+  bool Int(int i) {
+    Number n;
+    n.u.i = i;
+    n.d = static_cast<double>(i);
+    return WriteNumber(n);
+  }
+  bool Uint(unsigned u) {
+    Number n;
+    n.u.u = u;
+    n.d = static_cast<double>(u);
+    return WriteNumber(n);
+  }
+  bool Int64(int64_t i) {
+    Number n;
+    n.u.i = i;
+    n.d = static_cast<double>(i);
+    return WriteNumber(n);
+  }
+  bool Uint64(uint64_t u) {
+    Number n;
+    n.u.u = u;
+    n.d = static_cast<double>(u);
+    return WriteNumber(n);
+  }
+  bool Double(double d) {
+    Number n;
+    if (d < 0)
+      n.u.i = static_cast<int64_t>(d);
+    else
+      n.u.u = static_cast<uint64_t>(d);
+    n.d = d;
+    return WriteNumber(n);
+  }
+
+  bool RawNumber(const Ch *str, SizeType len, bool) {
+    WriteBuffer(kNumberType, str, len * sizeof(Ch));
+    return true;
+  }
+
+  bool String(const Ch *str, SizeType len, bool) {
+    WriteBuffer(kStringType, str, len * sizeof(Ch));
+    return true;
+  }
+
+  bool StartObject() { return true; }
+  bool Key(const Ch *str, SizeType len, bool copy) {
+    return String(str, len, copy);
+  }
+  bool EndObject(SizeType memberCount) {
+    uint64_t h = Hash(0, kObjectType);
+    uint64_t *kv = stack_.template Pop<uint64_t>(memberCount * 2);
+    for (SizeType i = 0; i < memberCount; i++)
+      h ^= Hash(kv[i * 2],
+                kv[i * 2 + 1]);  // Use xor to achieve member order insensitive
+    *stack_.template Push<uint64_t>() = h;
+    return true;
+  }
+
+  bool StartArray() { return true; }
+  bool EndArray(SizeType elementCount) {
+    uint64_t h = Hash(0, kArrayType);
+    uint64_t *e = stack_.template Pop<uint64_t>(elementCount);
+    for (SizeType i = 0; i < elementCount; i++)
+      h = Hash(h, e[i]);  // Use hash to achieve element order sensitive
+    *stack_.template Push<uint64_t>() = h;
+    return true;
+  }
+
+  bool IsValid() const { return stack_.GetSize() == sizeof(uint64_t); }
+
+  uint64_t GetHashCode() const {
+    RAPIDJSON_ASSERT(IsValid());
+    return *stack_.template Top<uint64_t>();
+  }
+
+ private:
+  static const size_t kDefaultSize = 256;
+  struct Number {
+    union U {
+      uint64_t u;
+      int64_t i;
+    } u;
+    double d;
+  };
+
+  bool WriteType(Type type) { return WriteBuffer(type, 0, 0); }
+
+  bool WriteNumber(const Number &n) {
+    return WriteBuffer(kNumberType, &n, sizeof(n));
+  }
+
+  bool WriteBuffer(Type type, const void *data, size_t len) {
+    // FNV-1a from http://isthe.com/chongo/tech/comp/fnv/
+    uint64_t h = Hash(RAPIDJSON_UINT64_C2(0x84222325, 0xcbf29ce4), type);
+    const unsigned char *d = static_cast<const unsigned char *>(data);
+    for (size_t i = 0; i < len; i++) h = Hash(h, d[i]);
+    *stack_.template Push<uint64_t>() = h;
+    return true;
+  }
+
+  static uint64_t Hash(uint64_t h, uint64_t d) {
+    static const uint64_t kPrime = RAPIDJSON_UINT64_C2(0x00000100, 0x000001b3);
+    h ^= d;
+    h *= kPrime;
+    return h;
+  }
+
+  Stack<Allocator> stack_;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// SchemaValidationContext
+
+template <typename SchemaDocumentType>
+struct SchemaValidationContext {
+  typedef Schema<SchemaDocumentType> SchemaType;
+  typedef ISchemaStateFactory<SchemaType> SchemaValidatorFactoryType;
+  typedef IValidationErrorHandler<SchemaType> ErrorHandlerType;
+  typedef typename SchemaType::ValueType ValueType;
+  typedef typename ValueType::Ch Ch;
+
+  enum PatternValidatorType {
+    kPatternValidatorOnly,
+    kPatternValidatorWithProperty,
+    kPatternValidatorWithAdditionalProperty
+  };
+
+  SchemaValidationContext(SchemaValidatorFactoryType &f, ErrorHandlerType &eh,
+                          const SchemaType *s)
+      : factory(f),
+        error_handler(eh),
+        schema(s),
+        valueSchema(),
+        invalidKeyword(),
+        hasher(),
+        arrayElementHashCodes(),
+        validators(),
+        validatorCount(),
+        patternPropertiesValidators(),
+        patternPropertiesValidatorCount(),
+        patternPropertiesSchemas(),
+        patternPropertiesSchemaCount(),
+        valuePatternValidatorType(kPatternValidatorOnly),
+        propertyExist(),
+        inArray(false),
+        valueUniqueness(false),
+        arrayUniqueness(false) {}
+
+  ~SchemaValidationContext() {
+    if (hasher) factory.DestroryHasher(hasher);
+    if (validators) {
+      for (SizeType i = 0; i < validatorCount; i++)
+        factory.DestroySchemaValidator(validators[i]);
+      factory.FreeState(validators);
+    }
+    if (patternPropertiesValidators) {
+      for (SizeType i = 0; i < patternPropertiesValidatorCount; i++)
+        factory.DestroySchemaValidator(patternPropertiesValidators[i]);
+      factory.FreeState(patternPropertiesValidators);
+    }
+    if (patternPropertiesSchemas) factory.FreeState(patternPropertiesSchemas);
+    if (propertyExist) factory.FreeState(propertyExist);
+  }
+
+  SchemaValidatorFactoryType &factory;
+  ErrorHandlerType &error_handler;
+  const SchemaType *schema;
+  const SchemaType *valueSchema;
+  const Ch *invalidKeyword;
+  void *hasher;                 // Only validator access
+  void *arrayElementHashCodes;  // Only validator access this
+  ISchemaValidator **validators;
+  SizeType validatorCount;
+  ISchemaValidator **patternPropertiesValidators;
+  SizeType patternPropertiesValidatorCount;
+  const SchemaType **patternPropertiesSchemas;
+  SizeType patternPropertiesSchemaCount;
+  PatternValidatorType valuePatternValidatorType;
+  PatternValidatorType objectPatternValidatorType;
+  SizeType arrayElementIndex;
+  bool *propertyExist;
+  bool inArray;
+  bool valueUniqueness;
+  bool arrayUniqueness;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Schema
+
+template <typename SchemaDocumentType>
+class Schema {
+ public:
+  typedef typename SchemaDocumentType::ValueType ValueType;
+  typedef typename SchemaDocumentType::AllocatorType AllocatorType;
+  typedef typename SchemaDocumentType::PointerType PointerType;
+  typedef typename ValueType::EncodingType EncodingType;
+  typedef typename EncodingType::Ch Ch;
+  typedef SchemaValidationContext<SchemaDocumentType> Context;
+  typedef Schema<SchemaDocumentType> SchemaType;
+  typedef GenericValue<EncodingType, AllocatorType> SValue;
+  typedef IValidationErrorHandler<Schema> ErrorHandler;
+  friend class GenericSchemaDocument<ValueType, AllocatorType>;
+
+  Schema(SchemaDocumentType *schemaDocument, const PointerType &p,
+         const ValueType &value, const ValueType &document,
+         AllocatorType *allocator)
+      : allocator_(allocator),
+        uri_(schemaDocument->GetURI(), *allocator),
+        pointer_(p, allocator),
+        typeless_(schemaDocument->GetTypeless()),
+        enum_(),
+        enumCount_(),
+        not_(),
+        type_((1 << kTotalSchemaType) - 1),  // typeless
+        validatorCount_(),
+        notValidatorIndex_(),
+        properties_(),
+        additionalPropertiesSchema_(),
+        patternProperties_(),
+        patternPropertyCount_(),
+        propertyCount_(),
+        minProperties_(),
+        maxProperties_(SizeType(~0)),
+        additionalProperties_(true),
+        hasDependencies_(),
+        hasRequired_(),
+        hasSchemaDependencies_(),
+        additionalItemsSchema_(),
+        itemsList_(),
+        itemsTuple_(),
+        itemsTupleCount_(),
+        minItems_(),
+        maxItems_(SizeType(~0)),
+        additionalItems_(true),
+        uniqueItems_(false),
+        pattern_(),
+        minLength_(0),
+        maxLength_(~SizeType(0)),
+        exclusiveMinimum_(false),
+        exclusiveMaximum_(false),
+        defaultValueLength_(0) {
+    typedef typename SchemaDocumentType::ValueType ValueType;
+    typedef typename ValueType::ConstValueIterator ConstValueIterator;
+    typedef typename ValueType::ConstMemberIterator ConstMemberIterator;
+
+    if (!value.IsObject()) return;
+
+    if (const ValueType *v = GetMember(value, GetTypeString())) {
+      type_ = 0;
+      if (v->IsString())
+        AddType(*v);
+      else if (v->IsArray())
+        for (ConstValueIterator itr = v->Begin(); itr != v->End(); ++itr)
+          AddType(*itr);
+    }
+
+    if (const ValueType *v = GetMember(value, GetEnumString()))
+      if (v->IsArray() && v->Size() > 0) {
+        enum_ = static_cast<uint64_t *>(
+            allocator_->Malloc(sizeof(uint64_t) * v->Size()));
+        for (ConstValueIterator itr = v->Begin(); itr != v->End(); ++itr) {
+          typedef Hasher<EncodingType, MemoryPoolAllocator<>> EnumHasherType;
+          char buffer[256u + 24];
+          MemoryPoolAllocator<> hasherAllocator(buffer, sizeof(buffer));
+          EnumHasherType h(&hasherAllocator, 256);
+          itr->Accept(h);
+          enum_[enumCount_++] = h.GetHashCode();
+        }
+      }
+
+    if (schemaDocument) {
+      AssignIfExist(allOf_, *schemaDocument, p, value, GetAllOfString(),
+                    document);
+      AssignIfExist(anyOf_, *schemaDocument, p, value, GetAnyOfString(),
+                    document);
+      AssignIfExist(oneOf_, *schemaDocument, p, value, GetOneOfString(),
+                    document);
+    }
+
+    if (const ValueType *v = GetMember(value, GetNotString())) {
+      schemaDocument->CreateSchema(&not_, p.Append(GetNotString(), allocator_),
+                                   *v, document);
+      notValidatorIndex_ = validatorCount_;
+      validatorCount_++;
+    }
+
+    // Object
+
+    const ValueType *properties = GetMember(value, GetPropertiesString());
+    const ValueType *required = GetMember(value, GetRequiredString());
+    const ValueType *dependencies = GetMember(value, GetDependenciesString());
+    {
+      // Gather properties from properties/required/dependencies
+      SValue allProperties(kArrayType);
+
+      if (properties && properties->IsObject())
+        for (ConstMemberIterator itr = properties->MemberBegin();
+             itr != properties->MemberEnd(); ++itr)
+          AddUniqueElement(allProperties, itr->name);
+
+      if (required && required->IsArray())
+        for (ConstValueIterator itr = required->Begin(); itr != required->End();
+             ++itr)
+          if (itr->IsString()) AddUniqueElement(allProperties, *itr);
+
+      if (dependencies && dependencies->IsObject())
+        for (ConstMemberIterator itr = dependencies->MemberBegin();
+             itr != dependencies->MemberEnd(); ++itr) {
+          AddUniqueElement(allProperties, itr->name);
+          if (itr->value.IsArray())
+            for (ConstValueIterator i = itr->value.Begin();
+                 i != itr->value.End(); ++i)
+              if (i->IsString()) AddUniqueElement(allProperties, *i);
+        }
+
+      if (allProperties.Size() > 0) {
+        propertyCount_ = allProperties.Size();
+        properties_ = static_cast<Property *>(
+            allocator_->Malloc(sizeof(Property) * propertyCount_));
+        for (SizeType i = 0; i < propertyCount_; i++) {
+          new (&properties_[i]) Property();
+          properties_[i].name = allProperties[i];
+          properties_[i].schema = typeless_;
+        }
+      }
+    }
+
+    if (properties && properties->IsObject()) {
+      PointerType q = p.Append(GetPropertiesString(), allocator_);
+      for (ConstMemberIterator itr = properties->MemberBegin();
+           itr != properties->MemberEnd(); ++itr) {
+        SizeType index;
+        if (FindPropertyIndex(itr->name, &index))
+          schemaDocument->CreateSchema(&properties_[index].schema,
+                                       q.Append(itr->name, allocator_),
+                                       itr->value, document);
+      }
+    }
+
+    if (const ValueType *v = GetMember(value, GetPatternPropertiesString())) {
+      PointerType q = p.Append(GetPatternPropertiesString(), allocator_);
+      patternProperties_ = static_cast<PatternProperty *>(
+          allocator_->Malloc(sizeof(PatternProperty) * v->MemberCount()));
+      patternPropertyCount_ = 0;
+
+      for (ConstMemberIterator itr = v->MemberBegin(); itr != v->MemberEnd();
+           ++itr) {
+        new (&patternProperties_[patternPropertyCount_]) PatternProperty();
+        patternProperties_[patternPropertyCount_].pattern =
+            CreatePattern(itr->name);
+        schemaDocument->CreateSchema(
+            &patternProperties_[patternPropertyCount_].schema,
+            q.Append(itr->name, allocator_), itr->value, document);
+        patternPropertyCount_++;
+      }
+    }
+
+    if (required && required->IsArray())
+      for (ConstValueIterator itr = required->Begin(); itr != required->End();
+           ++itr)
+        if (itr->IsString()) {
+          SizeType index;
+          if (FindPropertyIndex(*itr, &index)) {
+            properties_[index].required = true;
+            hasRequired_ = true;
+          }
+        }
+
+    if (dependencies && dependencies->IsObject()) {
+      PointerType q = p.Append(GetDependenciesString(), allocator_);
+      hasDependencies_ = true;
+      for (ConstMemberIterator itr = dependencies->MemberBegin();
+           itr != dependencies->MemberEnd(); ++itr) {
+        SizeType sourceIndex;
+        if (FindPropertyIndex(itr->name, &sourceIndex)) {
+          if (itr->value.IsArray()) {
+            properties_[sourceIndex].dependencies = static_cast<bool *>(
+                allocator_->Malloc(sizeof(bool) * propertyCount_));
+            std::memset(properties_[sourceIndex].dependencies, 0,
+                        sizeof(bool) * propertyCount_);
+            for (ConstValueIterator targetItr = itr->value.Begin();
+                 targetItr != itr->value.End(); ++targetItr) {
+              SizeType targetIndex;
+              if (FindPropertyIndex(*targetItr, &targetIndex))
+                properties_[sourceIndex].dependencies[targetIndex] = true;
+            }
+          } else if (itr->value.IsObject()) {
+            hasSchemaDependencies_ = true;
+            schemaDocument->CreateSchema(
+                &properties_[sourceIndex].dependenciesSchema,
+                q.Append(itr->name, allocator_), itr->value, document);
+            properties_[sourceIndex].dependenciesValidatorIndex =
+                validatorCount_;
+            validatorCount_++;
+          }
+        }
+      }
+    }
+
+    if (const ValueType *v =
+            GetMember(value, GetAdditionalPropertiesString())) {
+      if (v->IsBool())
+        additionalProperties_ = v->GetBool();
+      else if (v->IsObject())
+        schemaDocument->CreateSchema(
+            &additionalPropertiesSchema_,
+            p.Append(GetAdditionalPropertiesString(), allocator_), *v,
+            document);
+    }
+
+    AssignIfExist(minProperties_, value, GetMinPropertiesString());
+    AssignIfExist(maxProperties_, value, GetMaxPropertiesString());
+
+    // Array
+    if (const ValueType *v = GetMember(value, GetItemsString())) {
+      PointerType q = p.Append(GetItemsString(), allocator_);
+      if (v->IsObject())  // List validation
+        schemaDocument->CreateSchema(&itemsList_, q, *v, document);
+      else if (v->IsArray()) {  // Tuple validation
+        itemsTuple_ = static_cast<const Schema **>(
+            allocator_->Malloc(sizeof(const Schema *) * v->Size()));
+        SizeType index = 0;
+        for (ConstValueIterator itr = v->Begin(); itr != v->End();
+             ++itr, index++)
+          schemaDocument->CreateSchema(&itemsTuple_[itemsTupleCount_++],
+                                       q.Append(index, allocator_), *itr,
+                                       document);
+      }
+    }
+
+    AssignIfExist(minItems_, value, GetMinItemsString());
+    AssignIfExist(maxItems_, value, GetMaxItemsString());
+
+    if (const ValueType *v = GetMember(value, GetAdditionalItemsString())) {
+      if (v->IsBool())
+        additionalItems_ = v->GetBool();
+      else if (v->IsObject())
+        schemaDocument->CreateSchema(
+            &additionalItemsSchema_,
+            p.Append(GetAdditionalItemsString(), allocator_), *v, document);
+    }
+
+    AssignIfExist(uniqueItems_, value, GetUniqueItemsString());
+
+    // String
+    AssignIfExist(minLength_, value, GetMinLengthString());
+    AssignIfExist(maxLength_, value, GetMaxLengthString());
+
+    if (const ValueType *v = GetMember(value, GetPatternString()))
+      pattern_ = CreatePattern(*v);
+
+    // Number
+    if (const ValueType *v = GetMember(value, GetMinimumString()))
+      if (v->IsNumber()) minimum_.CopyFrom(*v, *allocator_);
+
+    if (const ValueType *v = GetMember(value, GetMaximumString()))
+      if (v->IsNumber()) maximum_.CopyFrom(*v, *allocator_);
+
+    AssignIfExist(exclusiveMinimum_, value, GetExclusiveMinimumString());
+    AssignIfExist(exclusiveMaximum_, value, GetExclusiveMaximumString());
+
+    if (const ValueType *v = GetMember(value, GetMultipleOfString()))
+      if (v->IsNumber() && v->GetDouble() > 0.0)
+        multipleOf_.CopyFrom(*v, *allocator_);
+
+    // Default
+    if (const ValueType *v = GetMember(value, GetDefaultValueString()))
+      if (v->IsString()) defaultValueLength_ = v->GetStringLength();
+  }
+
+  ~Schema() {
+    AllocatorType::Free(enum_);
+    if (properties_) {
+      for (SizeType i = 0; i < propertyCount_; i++) properties_[i].~Property();
+      AllocatorType::Free(properties_);
+    }
+    if (patternProperties_) {
+      for (SizeType i = 0; i < patternPropertyCount_; i++)
+        patternProperties_[i].~PatternProperty();
+      AllocatorType::Free(patternProperties_);
+    }
+    AllocatorType::Free(itemsTuple_);
+#if RAPIDJSON_SCHEMA_HAS_REGEX
+    if (pattern_) {
+      pattern_->~RegexType();
+      AllocatorType::Free(pattern_);
+    }
+#endif
+  }
+
+  const SValue &GetURI() const { return uri_; }
+
+  const PointerType &GetPointer() const { return pointer_; }
+
+  bool BeginValue(Context &context) const {
+    if (context.inArray) {
+      if (uniqueItems_) context.valueUniqueness = true;
+
+      if (itemsList_)
+        context.valueSchema = itemsList_;
+      else if (itemsTuple_) {
+        if (context.arrayElementIndex < itemsTupleCount_)
+          context.valueSchema = itemsTuple_[context.arrayElementIndex];
+        else if (additionalItemsSchema_)
+          context.valueSchema = additionalItemsSchema_;
+        else if (additionalItems_)
+          context.valueSchema = typeless_;
+        else {
+          context.error_handler.DisallowedItem(context.arrayElementIndex);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetItemsString());
+        }
+      } else
+        context.valueSchema = typeless_;
+
+      context.arrayElementIndex++;
+    }
+    return true;
+  }
+
+  RAPIDJSON_FORCEINLINE bool EndValue(Context &context) const {
+    if (context.patternPropertiesValidatorCount > 0) {
+      bool otherValid = false;
+      SizeType count = context.patternPropertiesValidatorCount;
+      if (context.objectPatternValidatorType != Context::kPatternValidatorOnly)
+        otherValid = context.patternPropertiesValidators[--count]->IsValid();
+
+      bool patternValid = true;
+      for (SizeType i = 0; i < count; i++)
+        if (!context.patternPropertiesValidators[i]->IsValid()) {
+          patternValid = false;
+          break;
+        }
+
+      if (context.objectPatternValidatorType ==
+          Context::kPatternValidatorOnly) {
+        if (!patternValid) {
+          context.error_handler.PropertyViolations(
+              context.patternPropertiesValidators, count);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternPropertiesString());
+        }
+      } else if (context.objectPatternValidatorType ==
+                 Context::kPatternValidatorWithProperty) {
+        if (!patternValid || !otherValid) {
+          context.error_handler.PropertyViolations(
+              context.patternPropertiesValidators, count + 1);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternPropertiesString());
+        }
+      } else if (!patternValid &&
+                 !otherValid) {  // kPatternValidatorWithAdditionalProperty)
+        context.error_handler.PropertyViolations(
+            context.patternPropertiesValidators, count + 1);
+        RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternPropertiesString());
+      }
+    }
+
+    if (enum_) {
+      const uint64_t h = context.factory.GetHashCode(context.hasher);
+      for (SizeType i = 0; i < enumCount_; i++)
+        if (enum_[i] == h) goto foundEnum;
+      context.error_handler.DisallowedValue();
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetEnumString());
+    foundEnum:;
+    }
+
+    if (allOf_.schemas)
+      for (SizeType i = allOf_.begin; i < allOf_.begin + allOf_.count; i++)
+        if (!context.validators[i]->IsValid()) {
+          context.error_handler.NotAllOf(&context.validators[allOf_.begin],
+                                         allOf_.count);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetAllOfString());
+        }
+
+    if (anyOf_.schemas) {
+      for (SizeType i = anyOf_.begin; i < anyOf_.begin + anyOf_.count; i++)
+        if (context.validators[i]->IsValid()) goto foundAny;
+      context.error_handler.NoneOf(&context.validators[anyOf_.begin],
+                                   anyOf_.count);
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetAnyOfString());
+    foundAny:;
+    }
+
+    if (oneOf_.schemas) {
+      bool oneValid = false;
+      for (SizeType i = oneOf_.begin; i < oneOf_.begin + oneOf_.count; i++)
+        if (context.validators[i]->IsValid()) {
+          if (oneValid) {
+            context.error_handler.NotOneOf(&context.validators[oneOf_.begin],
+                                           oneOf_.count);
+            RAPIDJSON_INVALID_KEYWORD_RETURN(GetOneOfString());
+          } else
+            oneValid = true;
+        }
+      if (!oneValid) {
+        context.error_handler.NotOneOf(&context.validators[oneOf_.begin],
+                                       oneOf_.count);
+        RAPIDJSON_INVALID_KEYWORD_RETURN(GetOneOfString());
+      }
+    }
+
+    if (not_ && context.validators[notValidatorIndex_]->IsValid()) {
+      context.error_handler.Disallowed();
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetNotString());
+    }
+
+    return true;
+  }
+
+  bool Null(Context &context) const {
+    if (!(type_ & (1 << kNullSchemaType))) {
+      DisallowedType(context, GetNullString());
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString());
+    }
+    return CreateParallelValidator(context);
+  }
+
+  bool Bool(Context &context, bool) const {
+    if (!(type_ & (1 << kBooleanSchemaType))) {
+      DisallowedType(context, GetBooleanString());
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString());
+    }
+    return CreateParallelValidator(context);
+  }
+
+  bool Int(Context &context, int i) const {
+    if (!CheckInt(context, i)) return false;
+    return CreateParallelValidator(context);
+  }
+
+  bool Uint(Context &context, unsigned u) const {
+    if (!CheckUint(context, u)) return false;
+    return CreateParallelValidator(context);
+  }
+
+  bool Int64(Context &context, int64_t i) const {
+    if (!CheckInt(context, i)) return false;
+    return CreateParallelValidator(context);
+  }
+
+  bool Uint64(Context &context, uint64_t u) const {
+    if (!CheckUint(context, u)) return false;
+    return CreateParallelValidator(context);
+  }
+
+  bool Double(Context &context, double d) const {
+    if (!(type_ & (1 << kNumberSchemaType))) {
+      DisallowedType(context, GetNumberString());
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString());
+    }
+
+    if (!minimum_.IsNull() && !CheckDoubleMinimum(context, d)) return false;
+
+    if (!maximum_.IsNull() && !CheckDoubleMaximum(context, d)) return false;
+
+    if (!multipleOf_.IsNull() && !CheckDoubleMultipleOf(context, d))
+      return false;
+
+    return CreateParallelValidator(context);
+  }
+
+  bool String(Context &context, const Ch *str, SizeType length, bool) const {
+    if (!(type_ & (1 << kStringSchemaType))) {
+      DisallowedType(context, GetStringString());
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString());
+    }
+
+    if (minLength_ != 0 || maxLength_ != SizeType(~0)) {
+      SizeType count;
+      if (internal::CountStringCodePoint<EncodingType>(str, length, &count)) {
+        if (count < minLength_) {
+          context.error_handler.TooShort(str, length, minLength_);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinLengthString());
+        }
+        if (count > maxLength_) {
+          context.error_handler.TooLong(str, length, maxLength_);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaxLengthString());
+        }
+      }
+    }
+
+    if (pattern_ && !IsPatternMatch(pattern_, str, length)) {
+      context.error_handler.DoesNotMatch(str, length);
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternString());
+    }
+
+    return CreateParallelValidator(context);
+  }
+
+  bool StartObject(Context &context) const {
+    if (!(type_ & (1 << kObjectSchemaType))) {
+      DisallowedType(context, GetObjectString());
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString());
+    }
+
+    if (hasDependencies_ || hasRequired_) {
+      context.propertyExist = static_cast<bool *>(
+          context.factory.MallocState(sizeof(bool) * propertyCount_));
+      std::memset(context.propertyExist, 0, sizeof(bool) * propertyCount_);
+    }
+
+    if (patternProperties_) {  // pre-allocate schema array
+      SizeType count =
+          patternPropertyCount_ + 1;  // extra for valuePatternValidatorType
+      context.patternPropertiesSchemas = static_cast<const SchemaType **>(
+          context.factory.MallocState(sizeof(const SchemaType *) * count));
+      context.patternPropertiesSchemaCount = 0;
+      std::memset(context.patternPropertiesSchemas, 0,
+                  sizeof(SchemaType *) * count);
+    }
+
+    return CreateParallelValidator(context);
+  }
+
+  bool Key(Context &context, const Ch *str, SizeType len, bool) const {
+    if (patternProperties_) {
+      context.patternPropertiesSchemaCount = 0;
+      for (SizeType i = 0; i < patternPropertyCount_; i++)
+        if (patternProperties_[i].pattern &&
+            IsPatternMatch(patternProperties_[i].pattern, str, len)) {
+          context.patternPropertiesSchemas
+              [context.patternPropertiesSchemaCount++] =
+              patternProperties_[i].schema;
+          context.valueSchema = typeless_;
+        }
+    }
+
+    SizeType index = 0;
+    if (FindPropertyIndex(ValueType(str, len).Move(), &index)) {
+      if (context.patternPropertiesSchemaCount > 0) {
+        context
+            .patternPropertiesSchemas[context.patternPropertiesSchemaCount++] =
+            properties_[index].schema;
+        context.valueSchema = typeless_;
+        context.valuePatternValidatorType =
+            Context::kPatternValidatorWithProperty;
+      } else
+        context.valueSchema = properties_[index].schema;
+
+      if (context.propertyExist) context.propertyExist[index] = true;
+
+      return true;
+    }
+
+    if (additionalPropertiesSchema_) {
+      if (additionalPropertiesSchema_ &&
+          context.patternPropertiesSchemaCount > 0) {
+        context
+            .patternPropertiesSchemas[context.patternPropertiesSchemaCount++] =
+            additionalPropertiesSchema_;
+        context.valueSchema = typeless_;
+        context.valuePatternValidatorType =
+            Context::kPatternValidatorWithAdditionalProperty;
+      } else
+        context.valueSchema = additionalPropertiesSchema_;
+      return true;
+    } else if (additionalProperties_) {
+      context.valueSchema = typeless_;
+      return true;
+    }
+
+    if (context.patternPropertiesSchemaCount ==
+        0) {  // patternProperties are not additional properties
+      context.error_handler.DisallowedProperty(str, len);
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetAdditionalPropertiesString());
+    }
+
+    return true;
+  }
+
+  bool EndObject(Context &context, SizeType memberCount) const {
+    if (hasRequired_) {
+      context.error_handler.StartMissingProperties();
+      for (SizeType index = 0; index < propertyCount_; index++)
+        if (properties_[index].required && !context.propertyExist[index])
+          if (properties_[index].schema->defaultValueLength_ == 0)
+            context.error_handler.AddMissingProperty(properties_[index].name);
+      if (context.error_handler.EndMissingProperties())
+        RAPIDJSON_INVALID_KEYWORD_RETURN(GetRequiredString());
+    }
+
+    if (memberCount < minProperties_) {
+      context.error_handler.TooFewProperties(memberCount, minProperties_);
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinPropertiesString());
+    }
+
+    if (memberCount > maxProperties_) {
+      context.error_handler.TooManyProperties(memberCount, maxProperties_);
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaxPropertiesString());
+    }
+
+    if (hasDependencies_) {
+      context.error_handler.StartDependencyErrors();
+      for (SizeType sourceIndex = 0; sourceIndex < propertyCount_;
+           sourceIndex++) {
+        const Property &source = properties_[sourceIndex];
+        if (context.propertyExist[sourceIndex]) {
+          if (source.dependencies) {
+            context.error_handler.StartMissingDependentProperties();
+            for (SizeType targetIndex = 0; targetIndex < propertyCount_;
+                 targetIndex++)
+              if (source.dependencies[targetIndex] &&
+                  !context.propertyExist[targetIndex])
+                context.error_handler.AddMissingDependentProperty(
+                    properties_[targetIndex].name);
+            context.error_handler.EndMissingDependentProperties(source.name);
+          } else if (source.dependenciesSchema) {
+            ISchemaValidator *dependenciesValidator =
+                context.validators[source.dependenciesValidatorIndex];
+            if (!dependenciesValidator->IsValid())
+              context.error_handler.AddDependencySchemaError(
+                  source.name, dependenciesValidator);
+          }
+        }
+      }
+      if (context.error_handler.EndDependencyErrors())
+        RAPIDJSON_INVALID_KEYWORD_RETURN(GetDependenciesString());
+    }
+
+    return true;
+  }
+
+  bool StartArray(Context &context) const {
+    if (!(type_ & (1 << kArraySchemaType))) {
+      DisallowedType(context, GetArrayString());
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString());
+    }
+
+    context.arrayElementIndex = 0;
+    context.inArray = true;
+
+    return CreateParallelValidator(context);
+  }
+
+  bool EndArray(Context &context, SizeType elementCount) const {
+    context.inArray = false;
+
+    if (elementCount < minItems_) {
+      context.error_handler.TooFewItems(elementCount, minItems_);
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinItemsString());
+    }
+
+    if (elementCount > maxItems_) {
+      context.error_handler.TooManyItems(elementCount, maxItems_);
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaxItemsString());
+    }
+
+    return true;
+  }
+
+// Generate functions for string literal according to Ch
+#define RAPIDJSON_STRING_(name, ...)                           \
+  static const ValueType &Get##name##String() {                \
+    static const Ch s[] = {__VA_ARGS__, '\0'};                 \
+    static const ValueType v(                                  \
+        s, static_cast<SizeType>(sizeof(s) / sizeof(Ch) - 1)); \
+    return v;                                                  \
+  }
+
+  RAPIDJSON_STRING_(Null, 'n', 'u', 'l', 'l')
+  RAPIDJSON_STRING_(Boolean, 'b', 'o', 'o', 'l', 'e', 'a', 'n')
+  RAPIDJSON_STRING_(Object, 'o', 'b', 'j', 'e', 'c', 't')
+  RAPIDJSON_STRING_(Array, 'a', 'r', 'r', 'a', 'y')
+  RAPIDJSON_STRING_(String, 's', 't', 'r', 'i', 'n', 'g')
+  RAPIDJSON_STRING_(Number, 'n', 'u', 'm', 'b', 'e', 'r')
+  RAPIDJSON_STRING_(Integer, 'i', 'n', 't', 'e', 'g', 'e', 'r')
+  RAPIDJSON_STRING_(Type, 't', 'y', 'p', 'e')
+  RAPIDJSON_STRING_(Enum, 'e', 'n', 'u', 'm')
+  RAPIDJSON_STRING_(AllOf, 'a', 'l', 'l', 'O', 'f')
+  RAPIDJSON_STRING_(AnyOf, 'a', 'n', 'y', 'O', 'f')
+  RAPIDJSON_STRING_(OneOf, 'o', 'n', 'e', 'O', 'f')
+  RAPIDJSON_STRING_(Not, 'n', 'o', 't')
+  RAPIDJSON_STRING_(Properties, 'p', 'r', 'o', 'p', 'e', 'r', 't', 'i', 'e',
+                    's')
+  RAPIDJSON_STRING_(Required, 'r', 'e', 'q', 'u', 'i', 'r', 'e', 'd')
+  RAPIDJSON_STRING_(Dependencies, 'd', 'e', 'p', 'e', 'n', 'd', 'e', 'n', 'c',
+                    'i', 'e', 's')
+  RAPIDJSON_STRING_(PatternProperties, 'p', 'a', 't', 't', 'e', 'r', 'n', 'P',
+                    'r', 'o', 'p', 'e', 'r', 't', 'i', 'e', 's')
+  RAPIDJSON_STRING_(AdditionalProperties, 'a', 'd', 'd', 'i', 't', 'i', 'o',
+                    'n', 'a', 'l', 'P', 'r', 'o', 'p', 'e', 'r', 't', 'i', 'e',
+                    's')
+  RAPIDJSON_STRING_(MinProperties, 'm', 'i', 'n', 'P', 'r', 'o', 'p', 'e', 'r',
+                    't', 'i', 'e', 's')
+  RAPIDJSON_STRING_(MaxProperties, 'm', 'a', 'x', 'P', 'r', 'o', 'p', 'e', 'r',
+                    't', 'i', 'e', 's')
+  RAPIDJSON_STRING_(Items, 'i', 't', 'e', 'm', 's')
+  RAPIDJSON_STRING_(MinItems, 'm', 'i', 'n', 'I', 't', 'e', 'm', 's')
+  RAPIDJSON_STRING_(MaxItems, 'm', 'a', 'x', 'I', 't', 'e', 'm', 's')
+  RAPIDJSON_STRING_(AdditionalItems, 'a', 'd', 'd', 'i', 't', 'i', 'o', 'n',
+                    'a', 'l', 'I', 't', 'e', 'm', 's')
+  RAPIDJSON_STRING_(UniqueItems, 'u', 'n', 'i', 'q', 'u', 'e', 'I', 't', 'e',
+                    'm', 's')
+  RAPIDJSON_STRING_(MinLength, 'm', 'i', 'n', 'L', 'e', 'n', 'g', 't', 'h')
+  RAPIDJSON_STRING_(MaxLength, 'm', 'a', 'x', 'L', 'e', 'n', 'g', 't', 'h')
+  RAPIDJSON_STRING_(Pattern, 'p', 'a', 't', 't', 'e', 'r', 'n')
+  RAPIDJSON_STRING_(Minimum, 'm', 'i', 'n', 'i', 'm', 'u', 'm')
+  RAPIDJSON_STRING_(Maximum, 'm', 'a', 'x', 'i', 'm', 'u', 'm')
+  RAPIDJSON_STRING_(ExclusiveMinimum, 'e', 'x', 'c', 'l', 'u', 's', 'i', 'v',
+                    'e', 'M', 'i', 'n', 'i', 'm', 'u', 'm')
+  RAPIDJSON_STRING_(ExclusiveMaximum, 'e', 'x', 'c', 'l', 'u', 's', 'i', 'v',
+                    'e', 'M', 'a', 'x', 'i', 'm', 'u', 'm')
+  RAPIDJSON_STRING_(MultipleOf, 'm', 'u', 'l', 't', 'i', 'p', 'l', 'e', 'O',
+                    'f')
+  RAPIDJSON_STRING_(DefaultValue, 'd', 'e', 'f', 'a', 'u', 'l', 't')
+
+#undef RAPIDJSON_STRING_
+
+ private:
+  enum SchemaValueType {
+    kNullSchemaType,
+    kBooleanSchemaType,
+    kObjectSchemaType,
+    kArraySchemaType,
+    kStringSchemaType,
+    kNumberSchemaType,
+    kIntegerSchemaType,
+    kTotalSchemaType
+  };
+
+#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX
+  typedef internal::GenericRegex<EncodingType, AllocatorType> RegexType;
+#elif RAPIDJSON_SCHEMA_USE_STDREGEX
+  typedef std::basic_regex<Ch> RegexType;
+#else
+  typedef char RegexType;
+#endif
+
+  struct SchemaArray {
+    SchemaArray() : schemas(), count() {}
+    ~SchemaArray() { AllocatorType::Free(schemas); }
+    const SchemaType **schemas;
+    SizeType begin;  // begin index of context.validators
+    SizeType count;
+  };
+
+  template <typename V1, typename V2>
+  void AddUniqueElement(V1 &a, const V2 &v) {
+    for (typename V1::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr)
+      if (*itr == v) return;
+    V1 c(v, *allocator_);
+    a.PushBack(c, *allocator_);
+  }
+
+  static const ValueType *GetMember(const ValueType &value,
+                                    const ValueType &name) {
+    typename ValueType::ConstMemberIterator itr = value.FindMember(name);
+    return itr != value.MemberEnd() ? &(itr->value) : 0;
+  }
+
+  static void AssignIfExist(bool &out, const ValueType &value,
+                            const ValueType &name) {
+    if (const ValueType *v = GetMember(value, name))
+      if (v->IsBool()) out = v->GetBool();
+  }
+
+  static void AssignIfExist(SizeType &out, const ValueType &value,
+                            const ValueType &name) {
+    if (const ValueType *v = GetMember(value, name))
+      if (v->IsUint64() && v->GetUint64() <= SizeType(~0))
+        out = static_cast<SizeType>(v->GetUint64());
+  }
+
+  void AssignIfExist(SchemaArray &out, SchemaDocumentType &schemaDocument,
+                     const PointerType &p, const ValueType &value,
+                     const ValueType &name, const ValueType &document) {
+    if (const ValueType *v = GetMember(value, name)) {
+      if (v->IsArray() && v->Size() > 0) {
+        PointerType q = p.Append(name, allocator_);
+        out.count = v->Size();
+        out.schemas = static_cast<const Schema **>(
+            allocator_->Malloc(out.count * sizeof(const Schema *)));
+        memset(out.schemas, 0, sizeof(Schema *) * out.count);
+        for (SizeType i = 0; i < out.count; i++)
+          schemaDocument.CreateSchema(&out.schemas[i], q.Append(i, allocator_),
+                                      (*v)[i], document);
+        out.begin = validatorCount_;
+        validatorCount_ += out.count;
+      }
+    }
+  }
+
+#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX
+  template <typename ValueType>
+  RegexType *CreatePattern(const ValueType &value) {
+    if (value.IsString()) {
+      RegexType *r = new (allocator_->Malloc(sizeof(RegexType)))
+          RegexType(value.GetString(), allocator_);
+      if (!r->IsValid()) {
+        r->~RegexType();
+        AllocatorType::Free(r);
+        r = 0;
+      }
+      return r;
+    }
+    return 0;
+  }
+
+  static bool IsPatternMatch(const RegexType *pattern, const Ch *str,
+                             SizeType) {
+    GenericRegexSearch<RegexType> rs(*pattern);
+    return rs.Search(str);
+  }
+#elif RAPIDJSON_SCHEMA_USE_STDREGEX
+  template <typename ValueType>
+  RegexType *CreatePattern(const ValueType &value) {
+    if (value.IsString()) {
+      RegexType *r =
+          static_cast<RegexType *>(allocator_->Malloc(sizeof(RegexType)));
+      try {
+        return new (r)
+            RegexType(value.GetString(), std::size_t(value.GetStringLength()),
+                      std::regex_constants::ECMAScript);
+      } catch (const std::regex_error &) {
+        AllocatorType::Free(r);
+      }
+    }
+    return 0;
+  }
+
+  static bool IsPatternMatch(const RegexType *pattern, const Ch *str,
+                             SizeType length) {
+    std::match_results<const Ch *> r;
+    return std::regex_search(str, str + length, r, *pattern);
+  }
+#else
+  template <typename ValueType>
+  RegexType *CreatePattern(const ValueType &) {
+    return 0;
+  }
+
+  static bool IsPatternMatch(const RegexType *, const Ch *, SizeType) {
+    return true;
+  }
+#endif  // RAPIDJSON_SCHEMA_USE_STDREGEX
+
+  void AddType(const ValueType &type) {
+    if (type == GetNullString())
+      type_ |= 1 << kNullSchemaType;
+    else if (type == GetBooleanString())
+      type_ |= 1 << kBooleanSchemaType;
+    else if (type == GetObjectString())
+      type_ |= 1 << kObjectSchemaType;
+    else if (type == GetArrayString())
+      type_ |= 1 << kArraySchemaType;
+    else if (type == GetStringString())
+      type_ |= 1 << kStringSchemaType;
+    else if (type == GetIntegerString())
+      type_ |= 1 << kIntegerSchemaType;
+    else if (type == GetNumberString())
+      type_ |= (1 << kNumberSchemaType) | (1 << kIntegerSchemaType);
+  }
+
+  bool CreateParallelValidator(Context &context) const {
+    if (enum_ || context.arrayUniqueness)
+      context.hasher = context.factory.CreateHasher();
+
+    if (validatorCount_) {
+      RAPIDJSON_ASSERT(context.validators == 0);
+      context.validators =
+          static_cast<ISchemaValidator **>(context.factory.MallocState(
+              sizeof(ISchemaValidator *) * validatorCount_));
+      context.validatorCount = validatorCount_;
+
+      if (allOf_.schemas) CreateSchemaValidators(context, allOf_);
+
+      if (anyOf_.schemas) CreateSchemaValidators(context, anyOf_);
+
+      if (oneOf_.schemas) CreateSchemaValidators(context, oneOf_);
+
+      if (not_)
+        context.validators[notValidatorIndex_] =
+            context.factory.CreateSchemaValidator(*not_);
+
+      if (hasSchemaDependencies_) {
+        for (SizeType i = 0; i < propertyCount_; i++)
+          if (properties_[i].dependenciesSchema)
+            context.validators[properties_[i].dependenciesValidatorIndex] =
+                context.factory.CreateSchemaValidator(
+                    *properties_[i].dependenciesSchema);
+      }
+    }
+
+    return true;
+  }
+
+  void CreateSchemaValidators(Context &context,
+                              const SchemaArray &schemas) const {
+    for (SizeType i = 0; i < schemas.count; i++)
+      context.validators[schemas.begin + i] =
+          context.factory.CreateSchemaValidator(*schemas.schemas[i]);
+  }
+
+  // O(n)
+  bool FindPropertyIndex(const ValueType &name, SizeType *outIndex) const {
+    SizeType len = name.GetStringLength();
+    const Ch *str = name.GetString();
+    for (SizeType index = 0; index < propertyCount_; index++)
+      if (properties_[index].name.GetStringLength() == len &&
+          (std::memcmp(properties_[index].name.GetString(), str,
+                       sizeof(Ch) * len) == 0)) {
+        *outIndex = index;
+        return true;
+      }
+    return false;
+  }
+
+  bool CheckInt(Context &context, int64_t i) const {
+    if (!(type_ & ((1 << kIntegerSchemaType) | (1 << kNumberSchemaType)))) {
+      DisallowedType(context, GetIntegerString());
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString());
+    }
+
+    if (!minimum_.IsNull()) {
+      if (minimum_.IsInt64()) {
+        if (exclusiveMinimum_ ? i <= minimum_.GetInt64()
+                              : i < minimum_.GetInt64()) {
+          context.error_handler.BelowMinimum(i, minimum_, exclusiveMinimum_);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinimumString());
+        }
+      } else if (minimum_.IsUint64()) {
+        context.error_handler.BelowMinimum(i, minimum_, exclusiveMinimum_);
+        RAPIDJSON_INVALID_KEYWORD_RETURN(
+            GetMinimumString());  // i <= max(int64_t) < minimum.GetUint64()
+      } else if (!CheckDoubleMinimum(context, static_cast<double>(i)))
+        return false;
+    }
+
+    if (!maximum_.IsNull()) {
+      if (maximum_.IsInt64()) {
+        if (exclusiveMaximum_ ? i >= maximum_.GetInt64()
+                              : i > maximum_.GetInt64()) {
+          context.error_handler.AboveMaximum(i, maximum_, exclusiveMaximum_);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaximumString());
+        }
+      } else if (maximum_.IsUint64()) {
+      }
+      /* do nothing */  // i <= max(int64_t) < maximum_.GetUint64()
+      else if (!CheckDoubleMaximum(context, static_cast<double>(i)))
+        return false;
+    }
+
+    if (!multipleOf_.IsNull()) {
+      if (multipleOf_.IsUint64()) {
+        if (static_cast<uint64_t>(i >= 0 ? i : -i) % multipleOf_.GetUint64() !=
+            0) {
+          context.error_handler.NotMultipleOf(i, multipleOf_);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetMultipleOfString());
+        }
+      } else if (!CheckDoubleMultipleOf(context, static_cast<double>(i)))
+        return false;
+    }
+
+    return true;
+  }
+
+  bool CheckUint(Context &context, uint64_t i) const {
+    if (!(type_ & ((1 << kIntegerSchemaType) | (1 << kNumberSchemaType)))) {
+      DisallowedType(context, GetIntegerString());
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString());
+    }
+
+    if (!minimum_.IsNull()) {
+      if (minimum_.IsUint64()) {
+        if (exclusiveMinimum_ ? i <= minimum_.GetUint64()
+                              : i < minimum_.GetUint64()) {
+          context.error_handler.BelowMinimum(i, minimum_, exclusiveMinimum_);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinimumString());
+        }
+      } else if (minimum_.IsInt64())
+        /* do nothing */;  // i >= 0 > minimum.Getint64()
+      else if (!CheckDoubleMinimum(context, static_cast<double>(i)))
+        return false;
+    }
+
+    if (!maximum_.IsNull()) {
+      if (maximum_.IsUint64()) {
+        if (exclusiveMaximum_ ? i >= maximum_.GetUint64()
+                              : i > maximum_.GetUint64()) {
+          context.error_handler.AboveMaximum(i, maximum_, exclusiveMaximum_);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaximumString());
+        }
+      } else if (maximum_.IsInt64()) {
+        context.error_handler.AboveMaximum(i, maximum_, exclusiveMaximum_);
+        RAPIDJSON_INVALID_KEYWORD_RETURN(
+            GetMaximumString());  // i >= 0 > maximum_
+      } else if (!CheckDoubleMaximum(context, static_cast<double>(i)))
+        return false;
+    }
+
+    if (!multipleOf_.IsNull()) {
+      if (multipleOf_.IsUint64()) {
+        if (i % multipleOf_.GetUint64() != 0) {
+          context.error_handler.NotMultipleOf(i, multipleOf_);
+          RAPIDJSON_INVALID_KEYWORD_RETURN(GetMultipleOfString());
+        }
+      } else if (!CheckDoubleMultipleOf(context, static_cast<double>(i)))
+        return false;
+    }
+
+    return true;
+  }
+
+  bool CheckDoubleMinimum(Context &context, double d) const {
+    if (exclusiveMinimum_ ? d <= minimum_.GetDouble()
+                          : d < minimum_.GetDouble()) {
+      context.error_handler.BelowMinimum(d, minimum_, exclusiveMinimum_);
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinimumString());
+    }
+    return true;
+  }
+
+  bool CheckDoubleMaximum(Context &context, double d) const {
+    if (exclusiveMaximum_ ? d >= maximum_.GetDouble()
+                          : d > maximum_.GetDouble()) {
+      context.error_handler.AboveMaximum(d, maximum_, exclusiveMaximum_);
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaximumString());
+    }
+    return true;
+  }
+
+  bool CheckDoubleMultipleOf(Context &context, double d) const {
+    double a = std::abs(d), b = std::abs(multipleOf_.GetDouble());
+    double q = std::floor(a / b);
+    double r = a - q * b;
+    if (r > 0.0) {
+      context.error_handler.NotMultipleOf(d, multipleOf_);
+      RAPIDJSON_INVALID_KEYWORD_RETURN(GetMultipleOfString());
+    }
+    return true;
+  }
+
+  void DisallowedType(Context &context, const ValueType &actualType) const {
+    ErrorHandler &eh = context.error_handler;
+    eh.StartDisallowedType();
+
+    if (type_ & (1 << kNullSchemaType)) eh.AddExpectedType(GetNullString());
+    if (type_ & (1 << kBooleanSchemaType))
+      eh.AddExpectedType(GetBooleanString());
+    if (type_ & (1 << kObjectSchemaType)) eh.AddExpectedType(GetObjectString());
+    if (type_ & (1 << kArraySchemaType)) eh.AddExpectedType(GetArrayString());
+    if (type_ & (1 << kStringSchemaType)) eh.AddExpectedType(GetStringString());
+
+    if (type_ & (1 << kNumberSchemaType))
+      eh.AddExpectedType(GetNumberString());
+    else if (type_ & (1 << kIntegerSchemaType))
+      eh.AddExpectedType(GetIntegerString());
+
+    eh.EndDisallowedType(actualType);
+  }
+
+  struct Property {
+    Property()
+        : schema(),
+          dependenciesSchema(),
+          dependenciesValidatorIndex(),
+          dependencies(),
+          required(false) {}
+    ~Property() { AllocatorType::Free(dependencies); }
+    SValue name;
+    const SchemaType *schema;
+    const SchemaType *dependenciesSchema;
+    SizeType dependenciesValidatorIndex;
+    bool *dependencies;
+    bool required;
+  };
+
+  struct PatternProperty {
+    PatternProperty() : schema(), pattern() {}
+    ~PatternProperty() {
+      if (pattern) {
+        pattern->~RegexType();
+        AllocatorType::Free(pattern);
+      }
+    }
+    const SchemaType *schema;
+    RegexType *pattern;
+  };
+
+  AllocatorType *allocator_;
+  SValue uri_;
+  PointerType pointer_;
+  const SchemaType *typeless_;
+  uint64_t *enum_;
+  SizeType enumCount_;
+  SchemaArray allOf_;
+  SchemaArray anyOf_;
+  SchemaArray oneOf_;
+  const SchemaType *not_;
+  unsigned type_;  // bitmask of kSchemaType
+  SizeType validatorCount_;
+  SizeType notValidatorIndex_;
+
+  Property *properties_;
+  const SchemaType *additionalPropertiesSchema_;
+  PatternProperty *patternProperties_;
+  SizeType patternPropertyCount_;
+  SizeType propertyCount_;
+  SizeType minProperties_;
+  SizeType maxProperties_;
+  bool additionalProperties_;
+  bool hasDependencies_;
+  bool hasRequired_;
+  bool hasSchemaDependencies_;
+
+  const SchemaType *additionalItemsSchema_;
+  const SchemaType *itemsList_;
+  const SchemaType **itemsTuple_;
+  SizeType itemsTupleCount_;
+  SizeType minItems_;
+  SizeType maxItems_;
+  bool additionalItems_;
+  bool uniqueItems_;
+
+  RegexType *pattern_;
+  SizeType minLength_;
+  SizeType maxLength_;
+
+  SValue minimum_;
+  SValue maximum_;
+  SValue multipleOf_;
+  bool exclusiveMinimum_;
+  bool exclusiveMaximum_;
+
+  SizeType defaultValueLength_;
+};
+
+template <typename Stack, typename Ch>
+struct TokenHelper {
+  RAPIDJSON_FORCEINLINE static void AppendIndexToken(Stack &documentStack,
+                                                     SizeType index) {
+    *documentStack.template Push<Ch>() = '/';
+    char buffer[21];
+    size_t length =
+        static_cast<size_t>((sizeof(SizeType) == 4 ? u32toa(index, buffer)
+                                                   : u64toa(index, buffer)) -
+                            buffer);
+    for (size_t i = 0; i < length; i++)
+      *documentStack.template Push<Ch>() = static_cast<Ch>(buffer[i]);
+  }
+};
+
+// Partial specialized version for char to prevent buffer copying.
+template <typename Stack>
+struct TokenHelper<Stack, char> {
+  RAPIDJSON_FORCEINLINE static void AppendIndexToken(Stack &documentStack,
+                                                     SizeType index) {
+    if (sizeof(SizeType) == 4) {
+      char *buffer = documentStack.template Push<char>(1 + 10);  // '/' + uint
+      *buffer++ = '/';
+      const char *end = internal::u32toa(index, buffer);
+      documentStack.template Pop<char>(
+          static_cast<size_t>(10 - (end - buffer)));
+    } else {
+      char *buffer = documentStack.template Push<char>(1 + 20);  // '/' + uint64
+      *buffer++ = '/';
+      const char *end = internal::u64toa(index, buffer);
+      documentStack.template Pop<char>(
+          static_cast<size_t>(20 - (end - buffer)));
+    }
+  }
+};
+
+}  // namespace internal
+
+///////////////////////////////////////////////////////////////////////////////
+// IGenericRemoteSchemaDocumentProvider
+
+template <typename SchemaDocumentType>
+class IGenericRemoteSchemaDocumentProvider {
+ public:
+  typedef typename SchemaDocumentType::Ch Ch;
+
+  virtual ~IGenericRemoteSchemaDocumentProvider() {}
+  virtual const SchemaDocumentType *GetRemoteDocument(const Ch *uri,
+                                                      SizeType length) = 0;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericSchemaDocument
+
+//! JSON schema document.
+/*!
+    A JSON schema document is a compiled version of a JSON schema.
+    It is basically a tree of internal::Schema.
+
+    \note This is an immutable class (i.e. its instance cannot be modified after
+   construction). \tparam ValueT Type of JSON value (e.g. \c Value ), which also
+   determine the encoding. \tparam Allocator Allocator type for allocating
+   memory of this document.
+*/
+template <typename ValueT, typename Allocator = CrtAllocator>
+class GenericSchemaDocument {
+ public:
+  typedef ValueT ValueType;
+  typedef IGenericRemoteSchemaDocumentProvider<GenericSchemaDocument>
+      IRemoteSchemaDocumentProviderType;
+  typedef Allocator AllocatorType;
+  typedef typename ValueType::EncodingType EncodingType;
+  typedef typename EncodingType::Ch Ch;
+  typedef internal::Schema<GenericSchemaDocument> SchemaType;
+  typedef GenericPointer<ValueType, Allocator> PointerType;
+  typedef GenericValue<EncodingType, Allocator> URIType;
+  friend class internal::Schema<GenericSchemaDocument>;
+  template <typename, typename, typename>
+  friend class GenericSchemaValidator;
+
+  //! Constructor.
+  /*!
+      Compile a JSON document into schema document.
+
+      \param document A JSON document as source.
+      \param uri The base URI of this schema document for purposes of violation
+     reporting. \param uriLength Length of \c name, in code points. \param
+     remoteProvider An optional remote schema document provider for resolving
+     remote reference. Can be null. \param allocator An optional allocator
+     instance for allocating memory. Can be null.
+  */
+  explicit GenericSchemaDocument(
+      const ValueType &document, const Ch *uri = 0, SizeType uriLength = 0,
+      IRemoteSchemaDocumentProviderType *remoteProvider = 0,
+      Allocator *allocator = 0)
+      : remoteProvider_(remoteProvider),
+        allocator_(allocator),
+        ownAllocator_(),
+        root_(),
+        typeless_(),
+        schemaMap_(allocator, kInitialSchemaMapSize),
+        schemaRef_(allocator, kInitialSchemaRefSize) {
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+
+    Ch noUri[1] = {0};
+    uri_.SetString(uri ? uri : noUri, uriLength, *allocator_);
+
+    typeless_ =
+        static_cast<SchemaType *>(allocator_->Malloc(sizeof(SchemaType)));
+    new (typeless_)
+        SchemaType(this, PointerType(), ValueType(kObjectType).Move(),
+                   ValueType(kObjectType).Move(), allocator_);
+
+    // Generate root schema, it will call CreateSchema() to create sub-schemas,
+    // And call AddRefSchema() if there are $ref.
+    CreateSchemaRecursive(&root_, PointerType(), document, document);
+
+    // Resolve $ref
+    while (!schemaRef_.Empty()) {
+      SchemaRefEntry *refEntry = schemaRef_.template Pop<SchemaRefEntry>(1);
+      if (const SchemaType *s = GetSchema(refEntry->target)) {
+        if (refEntry->schema) *refEntry->schema = s;
+
+        // Create entry in map if not exist
+        if (!GetSchema(refEntry->source)) {
+          new (schemaMap_.template Push<SchemaEntry>()) SchemaEntry(
+              refEntry->source, const_cast<SchemaType *>(s), false, allocator_);
+        }
+      } else if (refEntry->schema)
+        *refEntry->schema = typeless_;
+
+      refEntry->~SchemaRefEntry();
+    }
+
+    RAPIDJSON_ASSERT(root_ != 0);
+
+    schemaRef_.ShrinkToFit();  // Deallocate all memory for ref
+  }
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  //! Move constructor in C++11
+  GenericSchemaDocument(GenericSchemaDocument &&rhs) RAPIDJSON_NOEXCEPT
+      : remoteProvider_(rhs.remoteProvider_),
+        allocator_(rhs.allocator_),
+        ownAllocator_(rhs.ownAllocator_),
+        root_(rhs.root_),
+        typeless_(rhs.typeless_),
+        schemaMap_(std::move(rhs.schemaMap_)),
+        schemaRef_(std::move(rhs.schemaRef_)),
+        uri_(std::move(rhs.uri_)) {
+    rhs.remoteProvider_ = 0;
+    rhs.allocator_ = 0;
+    rhs.ownAllocator_ = 0;
+    rhs.typeless_ = 0;
+  }
+#endif
+
+  //! Destructor
+  ~GenericSchemaDocument() {
+    while (!schemaMap_.Empty())
+      schemaMap_.template Pop<SchemaEntry>(1)->~SchemaEntry();
+
+    if (typeless_) {
+      typeless_->~SchemaType();
+      Allocator::Free(typeless_);
+    }
+
+    RAPIDJSON_DELETE(ownAllocator_);
+  }
+
+  const URIType &GetURI() const { return uri_; }
+
+  //! Get the root schema.
+  const SchemaType &GetRoot() const { return *root_; }
+
+ private:
+  //! Prohibit copying
+  GenericSchemaDocument(const GenericSchemaDocument &);
+  //! Prohibit assignment
+  GenericSchemaDocument &operator=(const GenericSchemaDocument &);
+
+  struct SchemaRefEntry {
+    SchemaRefEntry(const PointerType &s, const PointerType &t,
+                   const SchemaType **outSchema, Allocator *allocator)
+        : source(s, allocator), target(t, allocator), schema(outSchema) {}
+    PointerType source;
+    PointerType target;
+    const SchemaType **schema;
+  };
+
+  struct SchemaEntry {
+    SchemaEntry(const PointerType &p, SchemaType *s, bool o,
+                Allocator *allocator)
+        : pointer(p, allocator), schema(s), owned(o) {}
+    ~SchemaEntry() {
+      if (owned) {
+        schema->~SchemaType();
+        Allocator::Free(schema);
+      }
+    }
+    PointerType pointer;
+    SchemaType *schema;
+    bool owned;
+  };
+
+  void CreateSchemaRecursive(const SchemaType **schema,
+                             const PointerType &pointer, const ValueType &v,
+                             const ValueType &document) {
+    if (schema) *schema = typeless_;
+
+    if (v.GetType() == kObjectType) {
+      const SchemaType *s = GetSchema(pointer);
+      if (!s) CreateSchema(schema, pointer, v, document);
+
+      for (typename ValueType::ConstMemberIterator itr = v.MemberBegin();
+           itr != v.MemberEnd(); ++itr)
+        CreateSchemaRecursive(0, pointer.Append(itr->name, allocator_),
+                              itr->value, document);
+    } else if (v.GetType() == kArrayType)
+      for (SizeType i = 0; i < v.Size(); i++)
+        CreateSchemaRecursive(0, pointer.Append(i, allocator_), v[i], document);
+  }
+
+  void CreateSchema(const SchemaType **schema, const PointerType &pointer,
+                    const ValueType &v, const ValueType &document) {
+    RAPIDJSON_ASSERT(pointer.IsValid());
+    if (v.IsObject()) {
+      if (!HandleRefSchema(pointer, schema, v, document)) {
+        SchemaType *s = new (allocator_->Malloc(sizeof(SchemaType)))
+            SchemaType(this, pointer, v, document, allocator_);
+        new (schemaMap_.template Push<SchemaEntry>())
+            SchemaEntry(pointer, s, true, allocator_);
+        if (schema) *schema = s;
+      }
+    }
+  }
+
+  bool HandleRefSchema(const PointerType &source, const SchemaType **schema,
+                       const ValueType &v, const ValueType &document) {
+    static const Ch kRefString[] = {'$', 'r', 'e', 'f', '\0'};
+    static const ValueType kRefValue(kRefString, 4);
+
+    typename ValueType::ConstMemberIterator itr = v.FindMember(kRefValue);
+    if (itr == v.MemberEnd()) return false;
+
+    if (itr->value.IsString()) {
+      SizeType len = itr->value.GetStringLength();
+      if (len > 0) {
+        const Ch *s = itr->value.GetString();
+        SizeType i = 0;
+        while (i < len && s[i] != '#')  // Find the first #
+          i++;
+
+        if (i > 0) {  // Remote reference, resolve immediately
+          if (remoteProvider_) {
+            if (const GenericSchemaDocument *remoteDocument =
+                    remoteProvider_->GetRemoteDocument(s, i)) {
+              PointerType pointer(&s[i], len - i, allocator_);
+              if (pointer.IsValid()) {
+                if (const SchemaType *sc = remoteDocument->GetSchema(pointer)) {
+                  if (schema) *schema = sc;
+                  new (schemaMap_.template Push<SchemaEntry>()) SchemaEntry(
+                      source, const_cast<SchemaType *>(sc), false, allocator_);
+                  return true;
+                }
+              }
+            }
+          }
+        } else if (s[i] == '#') {  // Local reference, defer resolution
+          PointerType pointer(&s[i], len - i, allocator_);
+          if (pointer.IsValid()) {
+            if (const ValueType *nv = pointer.Get(document))
+              if (HandleRefSchema(source, schema, *nv, document)) return true;
+
+            new (schemaRef_.template Push<SchemaRefEntry>())
+                SchemaRefEntry(source, pointer, schema, allocator_);
+            return true;
+          }
+        }
+      }
+    }
+    return false;
+  }
+
+  const SchemaType *GetSchema(const PointerType &pointer) const {
+    for (const SchemaEntry *target = schemaMap_.template Bottom<SchemaEntry>();
+         target != schemaMap_.template End<SchemaEntry>(); ++target)
+      if (pointer == target->pointer) return target->schema;
+    return 0;
+  }
+
+  PointerType GetPointer(const SchemaType *schema) const {
+    for (const SchemaEntry *target = schemaMap_.template Bottom<SchemaEntry>();
+         target != schemaMap_.template End<SchemaEntry>(); ++target)
+      if (schema == target->schema) return target->pointer;
+    return PointerType();
+  }
+
+  const SchemaType *GetTypeless() const { return typeless_; }
+
+  static const size_t kInitialSchemaMapSize = 64;
+  static const size_t kInitialSchemaRefSize = 64;
+
+  IRemoteSchemaDocumentProviderType *remoteProvider_;
+  Allocator *allocator_;
+  Allocator *ownAllocator_;
+  const SchemaType *root_;  //!< Root schema.
+  SchemaType *typeless_;
+  internal::Stack<Allocator> schemaMap_;  // Stores created Pointer -> Schemas
+  internal::Stack<Allocator>
+      schemaRef_;  // Stores Pointer from $ref and schema which holds the $ref
+  URIType uri_;
+};
+
+//! GenericSchemaDocument using Value type.
+typedef GenericSchemaDocument<Value> SchemaDocument;
+//! IGenericRemoteSchemaDocumentProvider using SchemaDocument.
+typedef IGenericRemoteSchemaDocumentProvider<SchemaDocument>
+    IRemoteSchemaDocumentProvider;
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericSchemaValidator
+
+//! JSON Schema Validator.
+/*!
+    A SAX style JSON schema validator.
+    It uses a \c GenericSchemaDocument to validate SAX events.
+    It delegates the incoming SAX events to an output handler.
+    The default output handler does nothing.
+    It can be reused multiple times by calling \c Reset().
+
+    \tparam SchemaDocumentType Type of schema document.
+    \tparam OutputHandler Type of output handler. Default handler does nothing.
+    \tparam StateAllocator Allocator for storing the internal validation states.
+*/
+template <typename SchemaDocumentType,
+          typename OutputHandler = BaseReaderHandler<
+              typename SchemaDocumentType::SchemaType::EncodingType>,
+          typename StateAllocator = CrtAllocator>
+class GenericSchemaValidator : public internal::ISchemaStateFactory<
+                                   typename SchemaDocumentType::SchemaType>,
+                               public internal::ISchemaValidator,
+                               public internal::IValidationErrorHandler<
+                                   typename SchemaDocumentType::SchemaType> {
+ public:
+  typedef typename SchemaDocumentType::SchemaType SchemaType;
+  typedef typename SchemaDocumentType::PointerType PointerType;
+  typedef typename SchemaType::EncodingType EncodingType;
+  typedef typename SchemaType::SValue SValue;
+  typedef typename EncodingType::Ch Ch;
+  typedef GenericStringRef<Ch> StringRefType;
+  typedef GenericValue<EncodingType, StateAllocator> ValueType;
+
+  //! Constructor without output handler.
+  /*!
+      \param schemaDocument The schema document to conform to.
+      \param allocator Optional allocator for storing internal validation
+     states. \param schemaStackCapacity Optional initial capacity of schema path
+     stack. \param documentStackCapacity Optional initial capacity of document
+     path stack.
+  */
+  GenericSchemaValidator(
+      const SchemaDocumentType &schemaDocument, StateAllocator *allocator = 0,
+      size_t schemaStackCapacity = kDefaultSchemaStackCapacity,
+      size_t documentStackCapacity = kDefaultDocumentStackCapacity)
+      : schemaDocument_(&schemaDocument),
+        root_(schemaDocument.GetRoot()),
+        stateAllocator_(allocator),
+        ownStateAllocator_(0),
+        schemaStack_(allocator, schemaStackCapacity),
+        documentStack_(allocator, documentStackCapacity),
+        outputHandler_(0),
+        error_(kObjectType),
+        currentError_(),
+        missingDependents_(),
+        valid_(true)
+#if RAPIDJSON_SCHEMA_VERBOSE
+        ,
+        depth_(0)
+#endif
+  {
+  }
+
+  //! Constructor with output handler.
+  /*!
+      \param schemaDocument The schema document to conform to.
+      \param allocator Optional allocator for storing internal validation
+     states. \param schemaStackCapacity Optional initial capacity of schema path
+     stack. \param documentStackCapacity Optional initial capacity of document
+     path stack.
+  */
+  GenericSchemaValidator(
+      const SchemaDocumentType &schemaDocument, OutputHandler &outputHandler,
+      StateAllocator *allocator = 0,
+      size_t schemaStackCapacity = kDefaultSchemaStackCapacity,
+      size_t documentStackCapacity = kDefaultDocumentStackCapacity)
+      : schemaDocument_(&schemaDocument),
+        root_(schemaDocument.GetRoot()),
+        stateAllocator_(allocator),
+        ownStateAllocator_(0),
+        schemaStack_(allocator, schemaStackCapacity),
+        documentStack_(allocator, documentStackCapacity),
+        outputHandler_(&outputHandler),
+        error_(kObjectType),
+        currentError_(),
+        missingDependents_(),
+        valid_(true)
+#if RAPIDJSON_SCHEMA_VERBOSE
+        ,
+        depth_(0)
+#endif
+  {
+  }
+
+  //! Destructor.
+  ~GenericSchemaValidator() {
+    Reset();
+    RAPIDJSON_DELETE(ownStateAllocator_);
+  }
+
+  //! Reset the internal states.
+  void Reset() {
+    while (!schemaStack_.Empty()) PopSchema();
+    documentStack_.Clear();
+    error_.SetObject();
+    currentError_.SetNull();
+    missingDependents_.SetNull();
+    valid_ = true;
+  }
+
+  //! Checks whether the current state is valid.
+  // Implementation of ISchemaValidator
+  virtual bool IsValid() const { return valid_; }
+
+  //! Gets the error object.
+  ValueType &GetError() { return error_; }
+  const ValueType &GetError() const { return error_; }
+
+  //! Gets the JSON pointer pointed to the invalid schema.
+  PointerType GetInvalidSchemaPointer() const {
+    return schemaStack_.Empty() ? PointerType() : CurrentSchema().GetPointer();
+  }
+
+  //! Gets the keyword of invalid schema.
+  const Ch *GetInvalidSchemaKeyword() const {
+    return schemaStack_.Empty() ? 0 : CurrentContext().invalidKeyword;
+  }
+
+  //! Gets the JSON pointer pointed to the invalid value.
+  PointerType GetInvalidDocumentPointer() const {
+    if (documentStack_.Empty()) {
+      return PointerType();
+    } else {
+      return PointerType(documentStack_.template Bottom<Ch>(),
+                         documentStack_.GetSize() / sizeof(Ch));
+    }
+  }
+
+  void NotMultipleOf(int64_t actual, const SValue &expected) {
+    AddNumberError(SchemaType::GetMultipleOfString(), ValueType(actual).Move(),
+                   expected);
+  }
+  void NotMultipleOf(uint64_t actual, const SValue &expected) {
+    AddNumberError(SchemaType::GetMultipleOfString(), ValueType(actual).Move(),
+                   expected);
+  }
+  void NotMultipleOf(double actual, const SValue &expected) {
+    AddNumberError(SchemaType::GetMultipleOfString(), ValueType(actual).Move(),
+                   expected);
+  }
+  void AboveMaximum(int64_t actual, const SValue &expected, bool exclusive) {
+    AddNumberError(SchemaType::GetMaximumString(), ValueType(actual).Move(),
+                   expected,
+                   exclusive ? &SchemaType::GetExclusiveMaximumString : 0);
+  }
+  void AboveMaximum(uint64_t actual, const SValue &expected, bool exclusive) {
+    AddNumberError(SchemaType::GetMaximumString(), ValueType(actual).Move(),
+                   expected,
+                   exclusive ? &SchemaType::GetExclusiveMaximumString : 0);
+  }
+  void AboveMaximum(double actual, const SValue &expected, bool exclusive) {
+    AddNumberError(SchemaType::GetMaximumString(), ValueType(actual).Move(),
+                   expected,
+                   exclusive ? &SchemaType::GetExclusiveMaximumString : 0);
+  }
+  void BelowMinimum(int64_t actual, const SValue &expected, bool exclusive) {
+    AddNumberError(SchemaType::GetMinimumString(), ValueType(actual).Move(),
+                   expected,
+                   exclusive ? &SchemaType::GetExclusiveMinimumString : 0);
+  }
+  void BelowMinimum(uint64_t actual, const SValue &expected, bool exclusive) {
+    AddNumberError(SchemaType::GetMinimumString(), ValueType(actual).Move(),
+                   expected,
+                   exclusive ? &SchemaType::GetExclusiveMinimumString : 0);
+  }
+  void BelowMinimum(double actual, const SValue &expected, bool exclusive) {
+    AddNumberError(SchemaType::GetMinimumString(), ValueType(actual).Move(),
+                   expected,
+                   exclusive ? &SchemaType::GetExclusiveMinimumString : 0);
+  }
+
+  void TooLong(const Ch *str, SizeType length, SizeType expected) {
+    AddNumberError(SchemaType::GetMaxLengthString(),
+                   ValueType(str, length, GetStateAllocator()).Move(),
+                   SValue(expected).Move());
+  }
+  void TooShort(const Ch *str, SizeType length, SizeType expected) {
+    AddNumberError(SchemaType::GetMinLengthString(),
+                   ValueType(str, length, GetStateAllocator()).Move(),
+                   SValue(expected).Move());
+  }
+  void DoesNotMatch(const Ch *str, SizeType length) {
+    currentError_.SetObject();
+    currentError_.AddMember(GetActualString(),
+                            ValueType(str, length, GetStateAllocator()).Move(),
+                            GetStateAllocator());
+    AddCurrentError(SchemaType::GetPatternString());
+  }
+
+  void DisallowedItem(SizeType index) {
+    currentError_.SetObject();
+    currentError_.AddMember(GetDisallowedString(), ValueType(index).Move(),
+                            GetStateAllocator());
+    AddCurrentError(SchemaType::GetAdditionalItemsString(), true);
+  }
+  void TooFewItems(SizeType actualCount, SizeType expectedCount) {
+    AddNumberError(SchemaType::GetMinItemsString(),
+                   ValueType(actualCount).Move(), SValue(expectedCount).Move());
+  }
+  void TooManyItems(SizeType actualCount, SizeType expectedCount) {
+    AddNumberError(SchemaType::GetMaxItemsString(),
+                   ValueType(actualCount).Move(), SValue(expectedCount).Move());
+  }
+  void DuplicateItems(SizeType index1, SizeType index2) {
+    ValueType duplicates(kArrayType);
+    duplicates.PushBack(index1, GetStateAllocator());
+    duplicates.PushBack(index2, GetStateAllocator());
+    currentError_.SetObject();
+    currentError_.AddMember(GetDuplicatesString(), duplicates,
+                            GetStateAllocator());
+    AddCurrentError(SchemaType::GetUniqueItemsString(), true);
+  }
+
+  void TooManyProperties(SizeType actualCount, SizeType expectedCount) {
+    AddNumberError(SchemaType::GetMaxPropertiesString(),
+                   ValueType(actualCount).Move(), SValue(expectedCount).Move());
+  }
+  void TooFewProperties(SizeType actualCount, SizeType expectedCount) {
+    AddNumberError(SchemaType::GetMinPropertiesString(),
+                   ValueType(actualCount).Move(), SValue(expectedCount).Move());
+  }
+  void StartMissingProperties() { currentError_.SetArray(); }
+  void AddMissingProperty(const SValue &name) {
+    currentError_.PushBack(ValueType(name, GetStateAllocator()).Move(),
+                           GetStateAllocator());
+  }
+  bool EndMissingProperties() {
+    if (currentError_.Empty()) return false;
+    ValueType error(kObjectType);
+    error.AddMember(GetMissingString(), currentError_, GetStateAllocator());
+    currentError_ = error;
+    AddCurrentError(SchemaType::GetRequiredString());
+    return true;
+  }
+  void PropertyViolations(ISchemaValidator **subvalidators, SizeType count) {
+    for (SizeType i = 0; i < count; ++i)
+      MergeError(
+          static_cast<GenericSchemaValidator *>(subvalidators[i])->GetError());
+  }
+  void DisallowedProperty(const Ch *name, SizeType length) {
+    currentError_.SetObject();
+    currentError_.AddMember(GetDisallowedString(),
+                            ValueType(name, length, GetStateAllocator()).Move(),
+                            GetStateAllocator());
+    AddCurrentError(SchemaType::GetAdditionalPropertiesString(), true);
+  }
+
+  void StartDependencyErrors() { currentError_.SetObject(); }
+  void StartMissingDependentProperties() { missingDependents_.SetArray(); }
+  void AddMissingDependentProperty(const SValue &targetName) {
+    missingDependents_.PushBack(
+        ValueType(targetName, GetStateAllocator()).Move(), GetStateAllocator());
+  }
+  void EndMissingDependentProperties(const SValue &sourceName) {
+    if (!missingDependents_.Empty())
+      currentError_.AddMember(ValueType(sourceName, GetStateAllocator()).Move(),
+                              missingDependents_, GetStateAllocator());
+  }
+  void AddDependencySchemaError(const SValue &sourceName,
+                                ISchemaValidator *subvalidator) {
+    currentError_.AddMember(
+        ValueType(sourceName, GetStateAllocator()).Move(),
+        static_cast<GenericSchemaValidator *>(subvalidator)->GetError(),
+        GetStateAllocator());
+  }
+  bool EndDependencyErrors() {
+    if (currentError_.ObjectEmpty()) return false;
+    ValueType error(kObjectType);
+    error.AddMember(GetErrorsString(), currentError_, GetStateAllocator());
+    currentError_ = error;
+    AddCurrentError(SchemaType::GetDependenciesString());
+    return true;
+  }
+
+  void DisallowedValue() {
+    currentError_.SetObject();
+    AddCurrentError(SchemaType::GetEnumString());
+  }
+  void StartDisallowedType() { currentError_.SetArray(); }
+  void AddExpectedType(const typename SchemaType::ValueType &expectedType) {
+    currentError_.PushBack(ValueType(expectedType, GetStateAllocator()).Move(),
+                           GetStateAllocator());
+  }
+  void EndDisallowedType(const typename SchemaType::ValueType &actualType) {
+    ValueType error(kObjectType);
+    error.AddMember(GetExpectedString(), currentError_, GetStateAllocator());
+    error.AddMember(GetActualString(),
+                    ValueType(actualType, GetStateAllocator()).Move(),
+                    GetStateAllocator());
+    currentError_ = error;
+    AddCurrentError(SchemaType::GetTypeString());
+  }
+  void NotAllOf(ISchemaValidator **subvalidators, SizeType count) {
+    for (SizeType i = 0; i < count; ++i) {
+      MergeError(
+          static_cast<GenericSchemaValidator *>(subvalidators[i])->GetError());
+    }
+  }
+  void NoneOf(ISchemaValidator **subvalidators, SizeType count) {
+    AddErrorArray(SchemaType::GetAnyOfString(), subvalidators, count);
+  }
+  void NotOneOf(ISchemaValidator **subvalidators, SizeType count) {
+    AddErrorArray(SchemaType::GetOneOfString(), subvalidators, count);
+  }
+  void Disallowed() {
+    currentError_.SetObject();
+    AddCurrentError(SchemaType::GetNotString());
+  }
+
+#define RAPIDJSON_STRING_(name, ...)                           \
+  static const StringRefType &Get##name##String() {            \
+    static const Ch s[] = {__VA_ARGS__, '\0'};                 \
+    static const StringRefType v(                              \
+        s, static_cast<SizeType>(sizeof(s) / sizeof(Ch) - 1)); \
+    return v;                                                  \
+  }
+
+  RAPIDJSON_STRING_(InstanceRef, 'i', 'n', 's', 't', 'a', 'n', 'c', 'e', 'R',
+                    'e', 'f')
+  RAPIDJSON_STRING_(SchemaRef, 's', 'c', 'h', 'e', 'm', 'a', 'R', 'e', 'f')
+  RAPIDJSON_STRING_(Expected, 'e', 'x', 'p', 'e', 'c', 't', 'e', 'd')
+  RAPIDJSON_STRING_(Actual, 'a', 'c', 't', 'u', 'a', 'l')
+  RAPIDJSON_STRING_(Disallowed, 'd', 'i', 's', 'a', 'l', 'l', 'o', 'w', 'e',
+                    'd')
+  RAPIDJSON_STRING_(Missing, 'm', 'i', 's', 's', 'i', 'n', 'g')
+  RAPIDJSON_STRING_(Errors, 'e', 'r', 'r', 'o', 'r', 's')
+  RAPIDJSON_STRING_(Duplicates, 'd', 'u', 'p', 'l', 'i', 'c', 'a', 't', 'e',
+                    's')
+
+#undef RAPIDJSON_STRING_
+
+#if RAPIDJSON_SCHEMA_VERBOSE
+#define RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_()                        \
+  RAPIDJSON_MULTILINEMACRO_BEGIN                                        \
+  *documentStack_.template Push<Ch>() = '\0';                           \
+  documentStack_.template Pop<Ch>(1);                                   \
+  internal::PrintInvalidDocument(documentStack_.template Bottom<Ch>()); \
+  RAPIDJSON_MULTILINEMACRO_END
+#else
+#define RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_()
+#endif
+
+#define RAPIDJSON_SCHEMA_HANDLE_BEGIN_(method, arg1)   \
+  if (!valid_) return false;                           \
+  if (!BeginValue() || !CurrentSchema().method arg1) { \
+    RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_();          \
+    return valid_ = false;                             \
+  }
+
+#define RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(method, arg2)                    \
+  for (Context *context = schemaStack_.template Bottom<Context>();         \
+       context != schemaStack_.template End<Context>(); context++) {       \
+    if (context->hasher)                                                   \
+      static_cast<HasherType *>(context->hasher)->method arg2;             \
+    if (context->validators)                                               \
+      for (SizeType i_ = 0; i_ < context->validatorCount; i_++)            \
+        static_cast<GenericSchemaValidator *>(context->validators[i_])     \
+            ->method arg2;                                                 \
+    if (context->patternPropertiesValidators)                              \
+      for (SizeType i_ = 0; i_ < context->patternPropertiesValidatorCount; \
+           i_++)                                                           \
+        static_cast<GenericSchemaValidator *>(                             \
+            context->patternPropertiesValidators[i_])                      \
+            ->method arg2;                                                 \
+  }
+
+#define RAPIDJSON_SCHEMA_HANDLE_END_(method, arg2) \
+  return valid_ = EndValue() && (!outputHandler_ || outputHandler_->method arg2)
+
+#define RAPIDJSON_SCHEMA_HANDLE_VALUE_(method, arg1, arg2) \
+  RAPIDJSON_SCHEMA_HANDLE_BEGIN_(method, arg1);            \
+  RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(method, arg2);         \
+  RAPIDJSON_SCHEMA_HANDLE_END_(method, arg2)
+
+  bool Null() { RAPIDJSON_SCHEMA_HANDLE_VALUE_(Null, (CurrentContext()), ()); }
+  bool Bool(bool b) {
+    RAPIDJSON_SCHEMA_HANDLE_VALUE_(Bool, (CurrentContext(), b), (b));
+  }
+  bool Int(int i) {
+    RAPIDJSON_SCHEMA_HANDLE_VALUE_(Int, (CurrentContext(), i), (i));
+  }
+  bool Uint(unsigned u) {
+    RAPIDJSON_SCHEMA_HANDLE_VALUE_(Uint, (CurrentContext(), u), (u));
+  }
+  bool Int64(int64_t i) {
+    RAPIDJSON_SCHEMA_HANDLE_VALUE_(Int64, (CurrentContext(), i), (i));
+  }
+  bool Uint64(uint64_t u) {
+    RAPIDJSON_SCHEMA_HANDLE_VALUE_(Uint64, (CurrentContext(), u), (u));
+  }
+  bool Double(double d) {
+    RAPIDJSON_SCHEMA_HANDLE_VALUE_(Double, (CurrentContext(), d), (d));
+  }
+  bool RawNumber(const Ch *str, SizeType length, bool copy) {
+    RAPIDJSON_SCHEMA_HANDLE_VALUE_(
+        String, (CurrentContext(), str, length, copy), (str, length, copy));
+  }
+  bool String(const Ch *str, SizeType length, bool copy) {
+    RAPIDJSON_SCHEMA_HANDLE_VALUE_(
+        String, (CurrentContext(), str, length, copy), (str, length, copy));
+  }
+
+  bool StartObject() {
+    RAPIDJSON_SCHEMA_HANDLE_BEGIN_(StartObject, (CurrentContext()));
+    RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(StartObject, ());
+    return valid_ = !outputHandler_ || outputHandler_->StartObject();
+  }
+
+  bool Key(const Ch *str, SizeType len, bool copy) {
+    if (!valid_) return false;
+    AppendToken(str, len);
+    if (!CurrentSchema().Key(CurrentContext(), str, len, copy))
+      return valid_ = false;
+    RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(Key, (str, len, copy));
+    return valid_ = !outputHandler_ || outputHandler_->Key(str, len, copy);
+  }
+
+  bool EndObject(SizeType memberCount) {
+    if (!valid_) return false;
+    RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(EndObject, (memberCount));
+    if (!CurrentSchema().EndObject(CurrentContext(), memberCount))
+      return valid_ = false;
+    RAPIDJSON_SCHEMA_HANDLE_END_(EndObject, (memberCount));
+  }
+
+  bool StartArray() {
+    RAPIDJSON_SCHEMA_HANDLE_BEGIN_(StartArray, (CurrentContext()));
+    RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(StartArray, ());
+    return valid_ = !outputHandler_ || outputHandler_->StartArray();
+  }
+
+  bool EndArray(SizeType elementCount) {
+    if (!valid_) return false;
+    RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(EndArray, (elementCount));
+    if (!CurrentSchema().EndArray(CurrentContext(), elementCount))
+      return valid_ = false;
+    RAPIDJSON_SCHEMA_HANDLE_END_(EndArray, (elementCount));
+  }
+
+#undef RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_
+#undef RAPIDJSON_SCHEMA_HANDLE_BEGIN_
+#undef RAPIDJSON_SCHEMA_HANDLE_PARALLEL_
+#undef RAPIDJSON_SCHEMA_HANDLE_VALUE_
+
+  // Implementation of ISchemaStateFactory<SchemaType>
+  virtual ISchemaValidator *CreateSchemaValidator(const SchemaType &root) {
+    return new (GetStateAllocator().Malloc(sizeof(GenericSchemaValidator)))
+        GenericSchemaValidator(*schemaDocument_, root,
+                               documentStack_.template Bottom<char>(),
+                               documentStack_.GetSize(),
+#if RAPIDJSON_SCHEMA_VERBOSE
+                               depth_ + 1,
+#endif
+                               &GetStateAllocator());
+  }
+
+  virtual void DestroySchemaValidator(ISchemaValidator *validator) {
+    GenericSchemaValidator *v =
+        static_cast<GenericSchemaValidator *>(validator);
+    v->~GenericSchemaValidator();
+    StateAllocator::Free(v);
+  }
+
+  virtual void *CreateHasher() {
+    return new (GetStateAllocator().Malloc(sizeof(HasherType)))
+        HasherType(&GetStateAllocator());
+  }
+
+  virtual uint64_t GetHashCode(void *hasher) {
+    return static_cast<HasherType *>(hasher)->GetHashCode();
+  }
+
+  virtual void DestroryHasher(void *hasher) {
+    HasherType *h = static_cast<HasherType *>(hasher);
+    h->~HasherType();
+    StateAllocator::Free(h);
+  }
+
+  virtual void *MallocState(size_t size) {
+    return GetStateAllocator().Malloc(size);
+  }
+
+  virtual void FreeState(void *p) { StateAllocator::Free(p); }
+
+ private:
+  typedef typename SchemaType::Context Context;
+  typedef GenericValue<UTF8<>, StateAllocator> HashCodeArray;
+  typedef internal::Hasher<EncodingType, StateAllocator> HasherType;
+
+  GenericSchemaValidator(
+      const SchemaDocumentType &schemaDocument, const SchemaType &root,
+      const char *basePath, size_t basePathSize,
+#if RAPIDJSON_SCHEMA_VERBOSE
+      unsigned depth,
+#endif
+      StateAllocator *allocator = 0,
+      size_t schemaStackCapacity = kDefaultSchemaStackCapacity,
+      size_t documentStackCapacity = kDefaultDocumentStackCapacity)
+      : schemaDocument_(&schemaDocument),
+        root_(root),
+        stateAllocator_(allocator),
+        ownStateAllocator_(0),
+        schemaStack_(allocator, schemaStackCapacity),
+        documentStack_(allocator, documentStackCapacity),
+        outputHandler_(0),
+        error_(kObjectType),
+        currentError_(),
+        missingDependents_(),
+        valid_(true)
+#if RAPIDJSON_SCHEMA_VERBOSE
+        ,
+        depth_(depth)
+#endif
+  {
+    if (basePath && basePathSize)
+      memcpy(documentStack_.template Push<char>(basePathSize), basePath,
+             basePathSize);
+  }
+
+  StateAllocator &GetStateAllocator() {
+    if (!stateAllocator_)
+      stateAllocator_ = ownStateAllocator_ = RAPIDJSON_NEW(StateAllocator)();
+    return *stateAllocator_;
+  }
+
+  bool BeginValue() {
+    if (schemaStack_.Empty())
+      PushSchema(root_);
+    else {
+      if (CurrentContext().inArray)
+        internal::TokenHelper<internal::Stack<StateAllocator>,
+                              Ch>::AppendIndexToken(documentStack_,
+                                                    CurrentContext()
+                                                        .arrayElementIndex);
+
+      if (!CurrentSchema().BeginValue(CurrentContext())) return false;
+
+      SizeType count = CurrentContext().patternPropertiesSchemaCount;
+      const SchemaType **sa = CurrentContext().patternPropertiesSchemas;
+      typename Context::PatternValidatorType patternValidatorType =
+          CurrentContext().valuePatternValidatorType;
+      bool valueUniqueness = CurrentContext().valueUniqueness;
+      RAPIDJSON_ASSERT(CurrentContext().valueSchema);
+      PushSchema(*CurrentContext().valueSchema);
+
+      if (count > 0) {
+        CurrentContext().objectPatternValidatorType = patternValidatorType;
+        ISchemaValidator **&va = CurrentContext().patternPropertiesValidators;
+        SizeType &validatorCount =
+            CurrentContext().patternPropertiesValidatorCount;
+        va = static_cast<ISchemaValidator **>(
+            MallocState(sizeof(ISchemaValidator *) * count));
+        for (SizeType i = 0; i < count; i++)
+          va[validatorCount++] = CreateSchemaValidator(*sa[i]);
+      }
+
+      CurrentContext().arrayUniqueness = valueUniqueness;
+    }
+    return true;
+  }
+
+  bool EndValue() {
+    if (!CurrentSchema().EndValue(CurrentContext())) return false;
+
+#if RAPIDJSON_SCHEMA_VERBOSE
+    GenericStringBuffer<EncodingType> sb;
+    schemaDocument_->GetPointer(&CurrentSchema()).Stringify(sb);
+
+    *documentStack_.template Push<Ch>() = '\0';
+    documentStack_.template Pop<Ch>(1);
+    internal::PrintValidatorPointers(depth_, sb.GetString(),
+                                     documentStack_.template Bottom<Ch>());
+#endif
+
+    uint64_t h =
+        CurrentContext().arrayUniqueness
+            ? static_cast<HasherType *>(CurrentContext().hasher)->GetHashCode()
+            : 0;
+
+    PopSchema();
+
+    if (!schemaStack_.Empty()) {
+      Context &context = CurrentContext();
+      if (context.valueUniqueness) {
+        HashCodeArray *a =
+            static_cast<HashCodeArray *>(context.arrayElementHashCodes);
+        if (!a)
+          CurrentContext().arrayElementHashCodes = a =
+              new (GetStateAllocator().Malloc(sizeof(HashCodeArray)))
+                  HashCodeArray(kArrayType);
+        for (typename HashCodeArray::ConstValueIterator itr = a->Begin();
+             itr != a->End(); ++itr)
+          if (itr->GetUint64() == h) {
+            DuplicateItems(static_cast<SizeType>(itr - a->Begin()), a->Size());
+            RAPIDJSON_INVALID_KEYWORD_RETURN(
+                SchemaType::GetUniqueItemsString());
+          }
+        a->PushBack(h, GetStateAllocator());
+      }
+    }
+
+    // Remove the last token of document pointer
+    while (!documentStack_.Empty() &&
+           *documentStack_.template Pop<Ch>(1) != '/')
+      ;
+
+    return true;
+  }
+
+  void AppendToken(const Ch *str, SizeType len) {
+    documentStack_.template Reserve<Ch>(
+        1 +
+        len * 2);  // worst case all characters are escaped as two characters
+    *documentStack_.template PushUnsafe<Ch>() = '/';
+    for (SizeType i = 0; i < len; i++) {
+      if (str[i] == '~') {
+        *documentStack_.template PushUnsafe<Ch>() = '~';
+        *documentStack_.template PushUnsafe<Ch>() = '0';
+      } else if (str[i] == '/') {
+        *documentStack_.template PushUnsafe<Ch>() = '~';
+        *documentStack_.template PushUnsafe<Ch>() = '1';
+      } else
+        *documentStack_.template PushUnsafe<Ch>() = str[i];
+    }
+  }
+
+  RAPIDJSON_FORCEINLINE void PushSchema(const SchemaType &schema) {
+    new (schemaStack_.template Push<Context>()) Context(*this, *this, &schema);
+  }
+
+  RAPIDJSON_FORCEINLINE void PopSchema() {
+    Context *c = schemaStack_.template Pop<Context>(1);
+    if (HashCodeArray *a =
+            static_cast<HashCodeArray *>(c->arrayElementHashCodes)) {
+      a->~HashCodeArray();
+      StateAllocator::Free(a);
+    }
+    c->~Context();
+  }
+
+  void AddErrorLocation(ValueType &result, bool parent) {
+    GenericStringBuffer<EncodingType> sb;
+    PointerType instancePointer = GetInvalidDocumentPointer();
+    ((parent && instancePointer.GetTokenCount() > 0)
+         ? PointerType(instancePointer.GetTokens(),
+                       instancePointer.GetTokenCount() - 1)
+         : instancePointer)
+        .StringifyUriFragment(sb);
+    ValueType instanceRef(sb.GetString(),
+                          static_cast<SizeType>(sb.GetSize() / sizeof(Ch)),
+                          GetStateAllocator());
+    result.AddMember(GetInstanceRefString(), instanceRef, GetStateAllocator());
+    sb.Clear();
+    memcpy(sb.Push(CurrentSchema().GetURI().GetStringLength()),
+           CurrentSchema().GetURI().GetString(),
+           CurrentSchema().GetURI().GetStringLength() * sizeof(Ch));
+    GetInvalidSchemaPointer().StringifyUriFragment(sb);
+    ValueType schemaRef(sb.GetString(),
+                        static_cast<SizeType>(sb.GetSize() / sizeof(Ch)),
+                        GetStateAllocator());
+    result.AddMember(GetSchemaRefString(), schemaRef, GetStateAllocator());
+  }
+
+  void AddError(ValueType &keyword, ValueType &error) {
+    typename ValueType::MemberIterator member = error_.FindMember(keyword);
+    if (member == error_.MemberEnd())
+      error_.AddMember(keyword, error, GetStateAllocator());
+    else {
+      if (member->value.IsObject()) {
+        ValueType errors(kArrayType);
+        errors.PushBack(member->value, GetStateAllocator());
+        member->value = errors;
+      }
+      member->value.PushBack(error, GetStateAllocator());
+    }
+  }
+
+  void AddCurrentError(const typename SchemaType::ValueType &keyword,
+                       bool parent = false) {
+    AddErrorLocation(currentError_, parent);
+    AddError(ValueType(keyword, GetStateAllocator(), false).Move(),
+             currentError_);
+  }
+
+  void MergeError(ValueType &other) {
+    for (typename ValueType::MemberIterator it = other.MemberBegin(),
+                                            end = other.MemberEnd();
+         it != end; ++it) {
+      AddError(it->name, it->value);
+    }
+  }
+
+  void AddNumberError(
+      const typename SchemaType::ValueType &keyword, ValueType &actual,
+      const SValue &expected,
+      const typename SchemaType::ValueType &(*exclusive)() = 0) {
+    currentError_.SetObject();
+    currentError_.AddMember(GetActualString(), actual, GetStateAllocator());
+    currentError_.AddMember(GetExpectedString(),
+                            ValueType(expected, GetStateAllocator()).Move(),
+                            GetStateAllocator());
+    if (exclusive)
+      currentError_.AddMember(
+          ValueType(exclusive(), GetStateAllocator()).Move(), true,
+          GetStateAllocator());
+    AddCurrentError(keyword);
+  }
+
+  void AddErrorArray(const typename SchemaType::ValueType &keyword,
+                     ISchemaValidator **subvalidators, SizeType count) {
+    ValueType errors(kArrayType);
+    for (SizeType i = 0; i < count; ++i)
+      errors.PushBack(
+          static_cast<GenericSchemaValidator *>(subvalidators[i])->GetError(),
+          GetStateAllocator());
+    currentError_.SetObject();
+    currentError_.AddMember(GetErrorsString(), errors, GetStateAllocator());
+    AddCurrentError(keyword);
+  }
+
+  const SchemaType &CurrentSchema() const {
+    return *schemaStack_.template Top<Context>()->schema;
+  }
+  Context &CurrentContext() { return *schemaStack_.template Top<Context>(); }
+  const Context &CurrentContext() const {
+    return *schemaStack_.template Top<Context>();
+  }
+
+  static const size_t kDefaultSchemaStackCapacity = 1024;
+  static const size_t kDefaultDocumentStackCapacity = 256;
+  const SchemaDocumentType *schemaDocument_;
+  const SchemaType &root_;
+  StateAllocator *stateAllocator_;
+  StateAllocator *ownStateAllocator_;
+  internal::Stack<StateAllocator>
+      schemaStack_;  //!< stack to store the current path of schema
+                     //!< (BaseSchemaType *)
+  internal::Stack<StateAllocator>
+      documentStack_;  //!< stack to store the current path of validating
+                       //!< document (Ch)
+  OutputHandler *outputHandler_;
+  ValueType error_;
+  ValueType currentError_;
+  ValueType missingDependents_;
+  bool valid_;
+#if RAPIDJSON_SCHEMA_VERBOSE
+  unsigned depth_;
+#endif
+};
+
+typedef GenericSchemaValidator<SchemaDocument> SchemaValidator;
+
+///////////////////////////////////////////////////////////////////////////////
+// SchemaValidatingReader
+
+//! A helper class for parsing with validation.
+/*!
+    This helper class is a functor, designed as a parameter of \ref
+   GenericDocument::Populate().
+
+    \tparam parseFlags Combination of \ref ParseFlag.
+    \tparam InputStream Type of input stream, implementing Stream concept.
+    \tparam SourceEncoding Encoding of the input stream.
+    \tparam SchemaDocumentType Type of schema document.
+    \tparam StackAllocator Allocator type for stack.
+*/
+template <unsigned parseFlags, typename InputStream, typename SourceEncoding,
+          typename SchemaDocumentType = SchemaDocument,
+          typename StackAllocator = CrtAllocator>
+class SchemaValidatingReader {
+ public:
+  typedef typename SchemaDocumentType::PointerType PointerType;
+  typedef typename InputStream::Ch Ch;
+  typedef GenericValue<SourceEncoding, StackAllocator> ValueType;
+
+  //! Constructor
+  /*!
+      \param is Input stream.
+      \param sd Schema document.
+  */
+  SchemaValidatingReader(InputStream &is, const SchemaDocumentType &sd)
+      : is_(is),
+        sd_(sd),
+        invalidSchemaKeyword_(),
+        error_(kObjectType),
+        isValid_(true) {}
+
+  template <typename Handler>
+  bool operator()(Handler &handler) {
+    GenericReader<SourceEncoding, typename SchemaDocumentType::EncodingType,
+                  StackAllocator>
+        reader;
+    GenericSchemaValidator<SchemaDocumentType, Handler> validator(sd_, handler);
+    parseResult_ = reader.template Parse<parseFlags>(is_, validator);
+
+    isValid_ = validator.IsValid();
+    if (isValid_) {
+      invalidSchemaPointer_ = PointerType();
+      invalidSchemaKeyword_ = 0;
+      invalidDocumentPointer_ = PointerType();
+      error_.SetObject();
+    } else {
+      invalidSchemaPointer_ = validator.GetInvalidSchemaPointer();
+      invalidSchemaKeyword_ = validator.GetInvalidSchemaKeyword();
+      invalidDocumentPointer_ = validator.GetInvalidDocumentPointer();
+      error_.CopyFrom(validator.GetError(), allocator_);
+    }
+
+    return parseResult_;
+  }
+
+  const ParseResult &GetParseResult() const { return parseResult_; }
+  bool IsValid() const { return isValid_; }
+  const PointerType &GetInvalidSchemaPointer() const {
+    return invalidSchemaPointer_;
+  }
+  const Ch *GetInvalidSchemaKeyword() const { return invalidSchemaKeyword_; }
+  const PointerType &GetInvalidDocumentPointer() const {
+    return invalidDocumentPointer_;
+  }
+  const ValueType &GetError() const { return error_; }
+
+ private:
+  InputStream &is_;
+  const SchemaDocumentType &sd_;
+
+  ParseResult parseResult_;
+  PointerType invalidSchemaPointer_;
+  const Ch *invalidSchemaKeyword_;
+  PointerType invalidDocumentPointer_;
+  StackAllocator allocator_;
+  ValueType error_;
+  bool isValid_;
+};
+
+RAPIDJSON_NAMESPACE_END
+RAPIDJSON_DIAG_POP
+
+#endif  // RAPIDJSON_SCHEMA_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/stream.h b/livox_ros_driver2/3rdparty/rapidjson/stream.h
new file mode 100644
index 0000000..a446c1a
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/stream.h
@@ -0,0 +1,242 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#include "rapidjson.h"
+
+#ifndef RAPIDJSON_STREAM_H_
+#define RAPIDJSON_STREAM_H_
+
+#include "encodings.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+//  Stream
+
+/*! \class rapidjson::Stream
+    \brief Concept for reading and writing characters.
+
+    For read-only stream, no need to implement PutBegin(), Put(), Flush() and
+PutEnd().
+
+    For write-only stream, only need to implement Put() and Flush().
+
+\code
+concept Stream {
+    typename Ch;    //!< Character type of the stream.
+
+    //! Read the current character from stream without moving the read cursor.
+    Ch Peek() const;
+
+    //! Read the current character from stream and moving the read cursor to
+next character. Ch Take();
+
+    //! Get the current read cursor.
+    //! \return Number of characters read from start.
+    size_t Tell();
+
+    //! Begin writing operation at the current read pointer.
+    //! \return The begin writer pointer.
+    Ch* PutBegin();
+
+    //! Write a character.
+    void Put(Ch c);
+
+    //! Flush the buffer.
+    void Flush();
+
+    //! End the writing operation.
+    //! \param begin The begin write pointer returned by PutBegin().
+    //! \return Number of characters written.
+    size_t PutEnd(Ch* begin);
+}
+\endcode
+*/
+
+//! Provides additional information for stream.
+/*!
+    By using traits pattern, this type provides a default configuration for
+   stream. For custom stream, this type can be specialized for other
+   configuration. See TEST(Reader, CustomStringStream) in readertest.cpp for
+   example.
+*/
+template <typename Stream>
+struct StreamTraits {
+  //! Whether to make local copy of stream for optimization during parsing.
+  /*!
+      By default, for safety, streams do not use local copy optimization.
+      Stream that can be copied fast should specialize this, like
+     StreamTraits<StringStream>.
+  */
+  enum { copyOptimization = 0 };
+};
+
+//! Reserve n characters for writing to a stream.
+template <typename Stream>
+inline void PutReserve(Stream &stream, size_t count) {
+  (void)stream;
+  (void)count;
+}
+
+//! Write character to a stream, presuming buffer is reserved.
+template <typename Stream>
+inline void PutUnsafe(Stream &stream, typename Stream::Ch c) {
+  stream.Put(c);
+}
+
+//! Put N copies of a character to a stream.
+template <typename Stream, typename Ch>
+inline void PutN(Stream &stream, Ch c, size_t n) {
+  PutReserve(stream, n);
+  for (size_t i = 0; i < n; i++) PutUnsafe(stream, c);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericStreamWrapper
+
+//! A Stream Wrapper
+/*! \tThis string stream is a wrapper for any stream by just forwarding any
+    \treceived message to the origin stream.
+    \note implements Stream concept
+*/
+
+#if defined(_MSC_VER) && _MSC_VER <= 1800
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
+#endif
+
+template <typename InputStream, typename Encoding = UTF8<>>
+class GenericStreamWrapper {
+ public:
+  typedef typename Encoding::Ch Ch;
+  GenericStreamWrapper(InputStream &is) : is_(is) {}
+
+  Ch Peek() const { return is_.Peek(); }
+  Ch Take() { return is_.Take(); }
+  size_t Tell() { return is_.Tell(); }
+  Ch *PutBegin() { return is_.PutBegin(); }
+  void Put(Ch ch) { is_.Put(ch); }
+  void Flush() { is_.Flush(); }
+  size_t PutEnd(Ch *ch) { return is_.PutEnd(ch); }
+
+  // wrapper for MemoryStream
+  const Ch *Peek4() const { return is_.Peek4(); }
+
+  // wrapper for AutoUTFInputStream
+  UTFType GetType() const { return is_.GetType(); }
+  bool HasBOM() const { return is_.HasBOM(); }
+
+ protected:
+  InputStream &is_;
+};
+
+#if defined(_MSC_VER) && _MSC_VER <= 1800
+RAPIDJSON_DIAG_POP
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// StringStream
+
+//! Read-only string stream.
+/*! \note implements Stream concept
+ */
+template <typename Encoding>
+struct GenericStringStream {
+  typedef typename Encoding::Ch Ch;
+
+  GenericStringStream(const Ch *src) : src_(src), head_(src) {}
+
+  Ch Peek() const { return *src_; }
+  Ch Take() { return *src_++; }
+  size_t Tell() const { return static_cast<size_t>(src_ - head_); }
+
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+  const Ch *src_;   //!< Current read position.
+  const Ch *head_;  //!< Original head of the string.
+};
+
+template <typename Encoding>
+struct StreamTraits<GenericStringStream<Encoding>> {
+  enum { copyOptimization = 1 };
+};
+
+//! String stream with UTF8 encoding.
+typedef GenericStringStream<UTF8<>> StringStream;
+
+///////////////////////////////////////////////////////////////////////////////
+// InsituStringStream
+
+//! A read-write string stream.
+/*! This string stream is particularly designed for in-situ parsing.
+    \note implements Stream concept
+*/
+template <typename Encoding>
+struct GenericInsituStringStream {
+  typedef typename Encoding::Ch Ch;
+
+  GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {}
+
+  // Read
+  Ch Peek() { return *src_; }
+  Ch Take() { return *src_++; }
+  size_t Tell() { return static_cast<size_t>(src_ - head_); }
+
+  // Write
+  void Put(Ch c) {
+    RAPIDJSON_ASSERT(dst_ != 0);
+    *dst_++ = c;
+  }
+
+  Ch *PutBegin() { return dst_ = src_; }
+  size_t PutEnd(Ch *begin) { return static_cast<size_t>(dst_ - begin); }
+  void Flush() {}
+
+  Ch *Push(size_t count) {
+    Ch *begin = dst_;
+    dst_ += count;
+    return begin;
+  }
+  void Pop(size_t count) { dst_ -= count; }
+
+  Ch *src_;
+  Ch *dst_;
+  Ch *head_;
+};
+
+template <typename Encoding>
+struct StreamTraits<GenericInsituStringStream<Encoding>> {
+  enum { copyOptimization = 1 };
+};
+
+//! Insitu string stream with UTF8 encoding.
+typedef GenericInsituStringStream<UTF8<>> InsituStringStream;
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_STREAM_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/stringbuffer.h b/livox_ros_driver2/3rdparty/rapidjson/stringbuffer.h
new file mode 100644
index 0000000..42b2bb1
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/stringbuffer.h
@@ -0,0 +1,130 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_STRINGBUFFER_H_
+#define RAPIDJSON_STRINGBUFFER_H_
+
+#include "internal/stack.h"
+#include "stream.h"
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+#include <utility>  // std::move
+#endif
+
+#include "internal/stack.h"
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Represents an in-memory output stream.
+/*!
+    \tparam Encoding Encoding of the stream.
+    \tparam Allocator type for allocating memory buffer.
+    \note implements Stream concept
+*/
+template <typename Encoding, typename Allocator = CrtAllocator>
+class GenericStringBuffer {
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  GenericStringBuffer(Allocator *allocator = 0,
+                      size_t capacity = kDefaultCapacity)
+      : stack_(allocator, capacity) {}
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  GenericStringBuffer(GenericStringBuffer &&rhs)
+      : stack_(std::move(rhs.stack_)) {}
+  GenericStringBuffer &operator=(GenericStringBuffer &&rhs) {
+    if (&rhs != this) stack_ = std::move(rhs.stack_);
+    return *this;
+  }
+#endif
+
+  void Put(Ch c) { *stack_.template Push<Ch>() = c; }
+  void PutUnsafe(Ch c) { *stack_.template PushUnsafe<Ch>() = c; }
+  void Flush() {}
+
+  void Clear() { stack_.Clear(); }
+  void ShrinkToFit() {
+    // Push and pop a null terminator. This is safe.
+    *stack_.template Push<Ch>() = '\0';
+    stack_.ShrinkToFit();
+    stack_.template Pop<Ch>(1);
+  }
+
+  void Reserve(size_t count) { stack_.template Reserve<Ch>(count); }
+  Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
+  Ch *PushUnsafe(size_t count) { return stack_.template PushUnsafe<Ch>(count); }
+  void Pop(size_t count) { stack_.template Pop<Ch>(count); }
+
+  const Ch *GetString() const {
+    // Push and pop a null terminator. This is safe.
+    *stack_.template Push<Ch>() = '\0';
+    stack_.template Pop<Ch>(1);
+
+    return stack_.template Bottom<Ch>();
+  }
+
+  //! Get the size of string in bytes in the string buffer.
+  size_t GetSize() const { return stack_.GetSize(); }
+
+  //! Get the length of string in Ch in the string buffer.
+  size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); }
+
+  static const size_t kDefaultCapacity = 256;
+  mutable internal::Stack<Allocator> stack_;
+
+ private:
+  // Prohibit copy constructor & assignment operator.
+  GenericStringBuffer(const GenericStringBuffer &);
+  GenericStringBuffer &operator=(const GenericStringBuffer &);
+};
+
+//! String buffer with UTF8 encoding
+typedef GenericStringBuffer<UTF8<>> StringBuffer;
+
+template <typename Encoding, typename Allocator>
+inline void PutReserve(GenericStringBuffer<Encoding, Allocator> &stream,
+                       size_t count) {
+  stream.Reserve(count);
+}
+
+template <typename Encoding, typename Allocator>
+inline void PutUnsafe(GenericStringBuffer<Encoding, Allocator> &stream,
+                      typename Encoding::Ch c) {
+  stream.PutUnsafe(c);
+}
+
+//! Implement specialized version of PutN() with memset() for better
+//! performance.
+template <>
+inline void PutN(GenericStringBuffer<UTF8<>> &stream, char c, size_t n) {
+  std::memset(stream.stack_.Push<char>(n), c, n * sizeof(c));
+}
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_STRINGBUFFER_H_
diff --git a/livox_ros_driver2/3rdparty/rapidjson/writer.h b/livox_ros_driver2/3rdparty/rapidjson/writer.h
new file mode 100644
index 0000000..69f9b05
--- /dev/null
+++ b/livox_ros_driver2/3rdparty/rapidjson/writer.h
@@ -0,0 +1,811 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// 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.
+
+#ifndef RAPIDJSON_WRITER_H_
+#define RAPIDJSON_WRITER_H_
+
+#include <new>  // placement new
+#include "internal/clzll.h"
+#include "internal/dtoa.h"
+#include "internal/itoa.h"
+#include "internal/meta.h"
+#include "internal/stack.h"
+#include "internal/strfunc.h"
+#include "stream.h"
+#include "stringbuffer.h"
+
+#if defined(RAPIDJSON_SIMD) && defined(_MSC_VER)
+#include <intrin.h>
+#pragma intrinsic(_BitScanForward)
+#endif
+#ifdef RAPIDJSON_SSE42
+#include <nmmintrin.h>
+#elif defined(RAPIDJSON_SSE2)
+#include <emmintrin.h>
+#elif defined(RAPIDJSON_NEON)
+#include <arm_neon.h>
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+RAPIDJSON_DIAG_OFF(unreachable - code)
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4127)  // conditional expression is constant
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// WriteFlag
+
+/*! \def RAPIDJSON_WRITE_DEFAULT_FLAGS
+    \ingroup RAPIDJSON_CONFIG
+    \brief User-defined kWriteDefaultFlags definition.
+
+    User can define this as any \c WriteFlag combinations.
+*/
+#ifndef RAPIDJSON_WRITE_DEFAULT_FLAGS
+#define RAPIDJSON_WRITE_DEFAULT_FLAGS kWriteNoFlags
+#endif
+
+//! Combination of writeFlags
+enum WriteFlag {
+  kWriteNoFlags = 0,               //!< No flags are set.
+  kWriteValidateEncodingFlag = 1,  //!< Validate encoding of JSON strings.
+  kWriteNanAndInfFlag = 2,  //!< Allow writing of Infinity, -Infinity and NaN.
+  kWriteDefaultFlags =
+      RAPIDJSON_WRITE_DEFAULT_FLAGS  //!< Default write flags. Can be customized
+                                     //!< by defining
+                                     //!< RAPIDJSON_WRITE_DEFAULT_FLAGS
+};
+
+//! JSON writer
+/*! Writer implements the concept Handler.
+    It generates JSON text by events to an output os.
+
+    User may programmatically calls the functions of a writer to generate JSON
+   text.
+
+    On the other side, a writer can also be passed to objects that generates
+   events,
+
+    for example Reader::Parse() and Document::Accept().
+
+    \tparam OutputStream Type of output stream.
+    \tparam SourceEncoding Encoding of source string.
+    \tparam TargetEncoding Encoding of output stream.
+    \tparam StackAllocator Type of allocator for allocating memory of stack.
+    \note implements Handler concept
+*/
+template <typename OutputStream, typename SourceEncoding = UTF8<>,
+          typename TargetEncoding = UTF8<>,
+          typename StackAllocator = CrtAllocator,
+          unsigned writeFlags = kWriteDefaultFlags>
+class Writer {
+ public:
+  typedef typename SourceEncoding::Ch Ch;
+
+  static const int kDefaultMaxDecimalPlaces = 324;
+
+  //! Constructor
+  /*! \param os Output stream.
+      \param stackAllocator User supplied allocator. If it is null, it will
+     create a private one. \param levelDepth Initial capacity of stack.
+  */
+  explicit Writer(OutputStream &os, StackAllocator *stackAllocator = 0,
+                  size_t levelDepth = kDefaultLevelDepth)
+      : os_(&os),
+        level_stack_(stackAllocator, levelDepth * sizeof(Level)),
+        maxDecimalPlaces_(kDefaultMaxDecimalPlaces),
+        hasRoot_(false) {}
+
+  explicit Writer(StackAllocator *allocator = 0,
+                  size_t levelDepth = kDefaultLevelDepth)
+      : os_(0),
+        level_stack_(allocator, levelDepth * sizeof(Level)),
+        maxDecimalPlaces_(kDefaultMaxDecimalPlaces),
+        hasRoot_(false) {}
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  Writer(Writer &&rhs)
+      : os_(rhs.os_),
+        level_stack_(std::move(rhs.level_stack_)),
+        maxDecimalPlaces_(rhs.maxDecimalPlaces_),
+        hasRoot_(rhs.hasRoot_) {
+    rhs.os_ = 0;
+  }
+#endif
+
+  //! Reset the writer with a new stream.
+  /*!
+      This function reset the writer with a new stream and default settings,
+      in order to make a Writer object reusable for output multiple JSONs.
+
+      \param os New output stream.
+      \code
+      Writer<OutputStream> writer(os1);
+      writer.StartObject();
+      // ...
+      writer.EndObject();
+
+      writer.Reset(os2);
+      writer.StartObject();
+      // ...
+      writer.EndObject();
+      \endcode
+  */
+  void Reset(OutputStream &os) {
+    os_ = &os;
+    hasRoot_ = false;
+    level_stack_.Clear();
+  }
+
+  //! Checks whether the output is a complete JSON.
+  /*!
+      A complete JSON has a complete root object or array.
+  */
+  bool IsComplete() const { return hasRoot_ && level_stack_.Empty(); }
+
+  int GetMaxDecimalPlaces() const { return maxDecimalPlaces_; }
+
+  //! Sets the maximum number of decimal places for double output.
+  /*!
+      This setting truncates the output with specified number of decimal places.
+
+      For example,
+
+      \code
+      writer.SetMaxDecimalPlaces(3);
+      writer.StartArray();
+      writer.Double(0.12345);                 // "0.123"
+      writer.Double(0.0001);                  // "0.0"
+      writer.Double(1.234567890123456e30);    // "1.234567890123456e30" (do not
+     truncate significand for positive exponent) writer.Double(1.23e-4); //
+     "0.0"                  (do truncate significand for negative exponent)
+      writer.EndArray();
+      \endcode
+
+      The default setting does not truncate any decimal places. You can restore
+     to this setting by calling \code
+      writer.SetMaxDecimalPlaces(Writer::kDefaultMaxDecimalPlaces);
+      \endcode
+  */
+  void SetMaxDecimalPlaces(int maxDecimalPlaces) {
+    maxDecimalPlaces_ = maxDecimalPlaces;
+  }
+
+  /*!@name Implementation of Handler
+      \see Handler
+  */
+  //@{
+
+  bool Null() {
+    Prefix(kNullType);
+    return EndValue(WriteNull());
+  }
+  bool Bool(bool b) {
+    Prefix(b ? kTrueType : kFalseType);
+    return EndValue(WriteBool(b));
+  }
+  bool Int(int i) {
+    Prefix(kNumberType);
+    return EndValue(WriteInt(i));
+  }
+  bool Uint(unsigned u) {
+    Prefix(kNumberType);
+    return EndValue(WriteUint(u));
+  }
+  bool Int64(int64_t i64) {
+    Prefix(kNumberType);
+    return EndValue(WriteInt64(i64));
+  }
+  bool Uint64(uint64_t u64) {
+    Prefix(kNumberType);
+    return EndValue(WriteUint64(u64));
+  }
+
+  //! Writes the given \c double value to the stream
+  /*!
+      \param d The value to be written.
+      \return Whether it is succeed.
+  */
+  bool Double(double d) {
+    Prefix(kNumberType);
+    return EndValue(WriteDouble(d));
+  }
+
+  bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
+    RAPIDJSON_ASSERT(str != 0);
+    (void)copy;
+    Prefix(kNumberType);
+    return EndValue(WriteString(str, length));
+  }
+
+  bool String(const Ch *str, SizeType length, bool copy = false) {
+    RAPIDJSON_ASSERT(str != 0);
+    (void)copy;
+    Prefix(kStringType);
+    return EndValue(WriteString(str, length));
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool String(const std::basic_string<Ch> &str) {
+    return String(str.data(), SizeType(str.size()));
+  }
+#endif
+
+  bool StartObject() {
+    Prefix(kObjectType);
+    new (level_stack_.template Push<Level>()) Level(false);
+    return WriteStartObject();
+  }
+
+  bool Key(const Ch *str, SizeType length, bool copy = false) {
+    return String(str, length, copy);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool Key(const std::basic_string<Ch> &str) {
+    return Key(str.data(), SizeType(str.size()));
+  }
+#endif
+
+  bool EndObject(SizeType memberCount = 0) {
+    (void)memberCount;
+    RAPIDJSON_ASSERT(level_stack_.GetSize() >=
+                     sizeof(Level));  // not inside an Object
+    RAPIDJSON_ASSERT(!level_stack_.template Top<Level>()
+                          ->inArray);  // currently inside an Array, not Object
+    RAPIDJSON_ASSERT(0 ==
+                     level_stack_.template Top<Level>()->valueCount %
+                         2);  // Object has a Key without a Value
+    level_stack_.template Pop<Level>(1);
+    return EndValue(WriteEndObject());
+  }
+
+  bool StartArray() {
+    Prefix(kArrayType);
+    new (level_stack_.template Push<Level>()) Level(true);
+    return WriteStartArray();
+  }
+
+  bool EndArray(SizeType elementCount = 0) {
+    (void)elementCount;
+    RAPIDJSON_ASSERT(level_stack_.GetSize() >= sizeof(Level));
+    RAPIDJSON_ASSERT(level_stack_.template Top<Level>()->inArray);
+    level_stack_.template Pop<Level>(1);
+    return EndValue(WriteEndArray());
+  }
+  //@}
+
+  /*! @name Convenience extensions */
+  //@{
+
+  //! Simpler but slower overload.
+  bool String(const Ch *const &str) {
+    return String(str, internal::StrLen(str));
+  }
+  bool Key(const Ch *const &str) { return Key(str, internal::StrLen(str)); }
+
+  //@}
+
+  //! Write a raw JSON value.
+  /*!
+      For user to write a stringified JSON as a value.
+
+      \param json A well-formed JSON value. It should not contain null character
+     within [0, length - 1] range. \param length Length of the json. \param type
+     Type of the root of json.
+  */
+  bool RawValue(const Ch *json, size_t length, Type type) {
+    RAPIDJSON_ASSERT(json != 0);
+    Prefix(type);
+    return EndValue(WriteRawValue(json, length));
+  }
+
+  //! Flush the output stream.
+  /*!
+      Allows the user to flush the output stream immediately.
+   */
+  void Flush() { os_->Flush(); }
+
+ protected:
+  //! Information for each nested level
+  struct Level {
+    Level(bool inArray_) : valueCount(0), inArray(inArray_) {}
+    size_t valueCount;  //!< number of values in this level
+    bool inArray;       //!< true if in array, otherwise in object
+  };
+
+  static const size_t kDefaultLevelDepth = 32;
+
+  bool WriteNull() {
+    PutReserve(*os_, 4);
+    PutUnsafe(*os_, 'n');
+    PutUnsafe(*os_, 'u');
+    PutUnsafe(*os_, 'l');
+    PutUnsafe(*os_, 'l');
+    return true;
+  }
+
+  bool WriteBool(bool b) {
+    if (b) {
+      PutReserve(*os_, 4);
+      PutUnsafe(*os_, 't');
+      PutUnsafe(*os_, 'r');
+      PutUnsafe(*os_, 'u');
+      PutUnsafe(*os_, 'e');
+    } else {
+      PutReserve(*os_, 5);
+      PutUnsafe(*os_, 'f');
+      PutUnsafe(*os_, 'a');
+      PutUnsafe(*os_, 'l');
+      PutUnsafe(*os_, 's');
+      PutUnsafe(*os_, 'e');
+    }
+    return true;
+  }
+
+  bool WriteInt(int i) {
+    char buffer[11];
+    const char *end = internal::i32toa(i, buffer);
+    PutReserve(*os_, static_cast<size_t>(end - buffer));
+    for (const char *p = buffer; p != end; ++p)
+      PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
+    return true;
+  }
+
+  bool WriteUint(unsigned u) {
+    char buffer[10];
+    const char *end = internal::u32toa(u, buffer);
+    PutReserve(*os_, static_cast<size_t>(end - buffer));
+    for (const char *p = buffer; p != end; ++p)
+      PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
+    return true;
+  }
+
+  bool WriteInt64(int64_t i64) {
+    char buffer[21];
+    const char *end = internal::i64toa(i64, buffer);
+    PutReserve(*os_, static_cast<size_t>(end - buffer));
+    for (const char *p = buffer; p != end; ++p)
+      PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
+    return true;
+  }
+
+  bool WriteUint64(uint64_t u64) {
+    char buffer[20];
+    char *end = internal::u64toa(u64, buffer);
+    PutReserve(*os_, static_cast<size_t>(end - buffer));
+    for (char *p = buffer; p != end; ++p)
+      PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
+    return true;
+  }
+
+  bool WriteDouble(double d) {
+    if (internal::Double(d).IsNanOrInf()) {
+      if (!(writeFlags & kWriteNanAndInfFlag)) return false;
+      if (internal::Double(d).IsNan()) {
+        PutReserve(*os_, 3);
+        PutUnsafe(*os_, 'N');
+        PutUnsafe(*os_, 'a');
+        PutUnsafe(*os_, 'N');
+        return true;
+      }
+      if (internal::Double(d).Sign()) {
+        PutReserve(*os_, 9);
+        PutUnsafe(*os_, '-');
+      } else
+        PutReserve(*os_, 8);
+      PutUnsafe(*os_, 'I');
+      PutUnsafe(*os_, 'n');
+      PutUnsafe(*os_, 'f');
+      PutUnsafe(*os_, 'i');
+      PutUnsafe(*os_, 'n');
+      PutUnsafe(*os_, 'i');
+      PutUnsafe(*os_, 't');
+      PutUnsafe(*os_, 'y');
+      return true;
+    }
+
+    char buffer[25];
+    char *end = internal::dtoa(d, buffer, maxDecimalPlaces_);
+    PutReserve(*os_, static_cast<size_t>(end - buffer));
+    for (char *p = buffer; p != end; ++p)
+      PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
+    return true;
+  }
+
+  bool WriteString(const Ch *str, SizeType length) {
+    static const typename OutputStream::Ch hexDigits[16] = {
+        '0', '1', '2', '3', '4', '5', '6', '7',
+        '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
+    static const char escape[256] = {
+#define Z16 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+        // 0    1    2    3    4    5    6    7    8    9    A    B    C    D E
+        // F
+        'u', 'u', 'u',  'u', 'u', 'u', 'u', 'u', 'b', 't',
+        'n', 'u', 'f',  'r', 'u', 'u',  // 00
+        'u', 'u', 'u',  'u', 'u', 'u', 'u', 'u', 'u', 'u',
+        'u', 'u', 'u',  'u', 'u', 'u',  // 10
+        0,   0,   '"',  0,   0,   0,   0,   0,   0,   0,
+        0,   0,   0,    0,   0,   0,  // 20
+        Z16, Z16,                     // 30~4F
+        0,   0,   0,    0,   0,   0,   0,   0,   0,   0,
+        0,   0,   '\\', 0,   0,   0,                       // 50
+        Z16, Z16, Z16,  Z16, Z16, Z16, Z16, Z16, Z16, Z16  // 60~FF
+#undef Z16
+    };
+
+    if (TargetEncoding::supportUnicode)
+      PutReserve(*os_, 2 + length * 6);  // "\uxxxx..."
+    else
+      PutReserve(*os_, 2 + length * 12);  // "\uxxxx\uyyyy..."
+
+    PutUnsafe(*os_, '\"');
+    GenericStringStream<SourceEncoding> is(str);
+    while (ScanWriteUnescapedString(is, length)) {
+      const Ch c = is.Peek();
+      if (!TargetEncoding::supportUnicode && static_cast<unsigned>(c) >= 0x80) {
+        // Unicode escaping
+        unsigned codepoint;
+        if (RAPIDJSON_UNLIKELY(!SourceEncoding::Decode(is, &codepoint)))
+          return false;
+        PutUnsafe(*os_, '\\');
+        PutUnsafe(*os_, 'u');
+        if (codepoint <= 0xD7FF ||
+            (codepoint >= 0xE000 && codepoint <= 0xFFFF)) {
+          PutUnsafe(*os_, hexDigits[(codepoint >> 12) & 15]);
+          PutUnsafe(*os_, hexDigits[(codepoint >> 8) & 15]);
+          PutUnsafe(*os_, hexDigits[(codepoint >> 4) & 15]);
+          PutUnsafe(*os_, hexDigits[(codepoint)&15]);
+        } else {
+          RAPIDJSON_ASSERT(codepoint >= 0x010000 && codepoint <= 0x10FFFF);
+          // Surrogate pair
+          unsigned s = codepoint - 0x010000;
+          unsigned lead = (s >> 10) + 0xD800;
+          unsigned trail = (s & 0x3FF) + 0xDC00;
+          PutUnsafe(*os_, hexDigits[(lead >> 12) & 15]);
+          PutUnsafe(*os_, hexDigits[(lead >> 8) & 15]);
+          PutUnsafe(*os_, hexDigits[(lead >> 4) & 15]);
+          PutUnsafe(*os_, hexDigits[(lead)&15]);
+          PutUnsafe(*os_, '\\');
+          PutUnsafe(*os_, 'u');
+          PutUnsafe(*os_, hexDigits[(trail >> 12) & 15]);
+          PutUnsafe(*os_, hexDigits[(trail >> 8) & 15]);
+          PutUnsafe(*os_, hexDigits[(trail >> 4) & 15]);
+          PutUnsafe(*os_, hexDigits[(trail)&15]);
+        }
+      } else if ((sizeof(Ch) == 1 || static_cast<unsigned>(c) < 256) &&
+                 RAPIDJSON_UNLIKELY(escape[static_cast<unsigned char>(c)])) {
+        is.Take();
+        PutUnsafe(*os_, '\\');
+        PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(
+                            escape[static_cast<unsigned char>(c)]));
+        if (escape[static_cast<unsigned char>(c)] == 'u') {
+          PutUnsafe(*os_, '0');
+          PutUnsafe(*os_, '0');
+          PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) >> 4]);
+          PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) & 0xF]);
+        }
+      } else if (RAPIDJSON_UNLIKELY(!(
+                     writeFlags & kWriteValidateEncodingFlag
+                         ? Transcoder<SourceEncoding, TargetEncoding>::Validate(
+                               is, *os_)
+                         : Transcoder<SourceEncoding,
+                                      TargetEncoding>::TranscodeUnsafe(is,
+                                                                       *os_))))
+        return false;
+    }
+    PutUnsafe(*os_, '\"');
+    return true;
+  }
+
+  bool ScanWriteUnescapedString(GenericStringStream<SourceEncoding> &is,
+                                size_t length) {
+    return RAPIDJSON_LIKELY(is.Tell() < length);
+  }
+
+  bool WriteStartObject() {
+    os_->Put('{');
+    return true;
+  }
+  bool WriteEndObject() {
+    os_->Put('}');
+    return true;
+  }
+  bool WriteStartArray() {
+    os_->Put('[');
+    return true;
+  }
+  bool WriteEndArray() {
+    os_->Put(']');
+    return true;
+  }
+
+  bool WriteRawValue(const Ch *json, size_t length) {
+    PutReserve(*os_, length);
+    GenericStringStream<SourceEncoding> is(json);
+    while (RAPIDJSON_LIKELY(is.Tell() < length)) {
+      RAPIDJSON_ASSERT(is.Peek() != '\0');
+      if (RAPIDJSON_UNLIKELY(!(
+              writeFlags & kWriteValidateEncodingFlag
+                  ? Transcoder<SourceEncoding, TargetEncoding>::Validate(is,
+                                                                         *os_)
+                  : Transcoder<SourceEncoding, TargetEncoding>::TranscodeUnsafe(
+                        is, *os_))))
+        return false;
+    }
+    return true;
+  }
+
+  void Prefix(Type type) {
+    (void)type;
+    if (RAPIDJSON_LIKELY(level_stack_.GetSize() !=
+                         0)) {  // this value is not at root
+      Level *level = level_stack_.template Top<Level>();
+      if (level->valueCount > 0) {
+        if (level->inArray)
+          os_->Put(',');  // add comma if it is not the first element in array
+        else              // in object
+          os_->Put((level->valueCount % 2 == 0) ? ',' : ':');
+      }
+      if (!level->inArray && level->valueCount % 2 == 0)
+        RAPIDJSON_ASSERT(type == kStringType);  // if it's in object, then even
+                                                // number should be a name
+      level->valueCount++;
+    } else {
+      RAPIDJSON_ASSERT(!hasRoot_);  // Should only has one and only one root.
+      hasRoot_ = true;
+    }
+  }
+
+  // Flush the value if it is the top level one.
+  bool EndValue(bool ret) {
+    if (RAPIDJSON_UNLIKELY(level_stack_.Empty()))  // end of json text
+      Flush();
+    return ret;
+  }
+
+  OutputStream *os_;
+  internal::Stack<StackAllocator> level_stack_;
+  int maxDecimalPlaces_;
+  bool hasRoot_;
+
+ private:
+  // Prohibit copy constructor & assignment operator.
+  Writer(const Writer &);
+  Writer &operator=(const Writer &);
+};
+
+// Full specialization for StringStream to prevent memory copying
+
+template <>
+inline bool Writer<StringBuffer>::WriteInt(int i) {
+  char *buffer = os_->Push(11);
+  const char *end = internal::i32toa(i, buffer);
+  os_->Pop(static_cast<size_t>(11 - (end - buffer)));
+  return true;
+}
+
+template <>
+inline bool Writer<StringBuffer>::WriteUint(unsigned u) {
+  char *buffer = os_->Push(10);
+  const char *end = internal::u32toa(u, buffer);
+  os_->Pop(static_cast<size_t>(10 - (end - buffer)));
+  return true;
+}
+
+template <>
+inline bool Writer<StringBuffer>::WriteInt64(int64_t i64) {
+  char *buffer = os_->Push(21);
+  const char *end = internal::i64toa(i64, buffer);
+  os_->Pop(static_cast<size_t>(21 - (end - buffer)));
+  return true;
+}
+
+template <>
+inline bool Writer<StringBuffer>::WriteUint64(uint64_t u) {
+  char *buffer = os_->Push(20);
+  const char *end = internal::u64toa(u, buffer);
+  os_->Pop(static_cast<size_t>(20 - (end - buffer)));
+  return true;
+}
+
+template <>
+inline bool Writer<StringBuffer>::WriteDouble(double d) {
+  if (internal::Double(d).IsNanOrInf()) {
+    // Note: This code path can only be reached if
+    // (RAPIDJSON_WRITE_DEFAULT_FLAGS & kWriteNanAndInfFlag).
+    if (!(kWriteDefaultFlags & kWriteNanAndInfFlag)) return false;
+    if (internal::Double(d).IsNan()) {
+      PutReserve(*os_, 3);
+      PutUnsafe(*os_, 'N');
+      PutUnsafe(*os_, 'a');
+      PutUnsafe(*os_, 'N');
+      return true;
+    }
+    if (internal::Double(d).Sign()) {
+      PutReserve(*os_, 9);
+      PutUnsafe(*os_, '-');
+    } else
+      PutReserve(*os_, 8);
+    PutUnsafe(*os_, 'I');
+    PutUnsafe(*os_, 'n');
+    PutUnsafe(*os_, 'f');
+    PutUnsafe(*os_, 'i');
+    PutUnsafe(*os_, 'n');
+    PutUnsafe(*os_, 'i');
+    PutUnsafe(*os_, 't');
+    PutUnsafe(*os_, 'y');
+    return true;
+  }
+
+  char *buffer = os_->Push(25);
+  char *end = internal::dtoa(d, buffer, maxDecimalPlaces_);
+  os_->Pop(static_cast<size_t>(25 - (end - buffer)));
+  return true;
+}
+
+#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42)
+template <>
+inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
+                                                           size_t length) {
+  if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
+
+  if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
+
+  const char *p = is.src_;
+  const char *end = is.head_ + length;
+  const char *nextAligned = reinterpret_cast<const char *>(
+      (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+  const char *endAligned = reinterpret_cast<const char *>(
+      reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
+  if (nextAligned > end) return true;
+
+  while (p != nextAligned)
+    if (*p < 0x20 || *p == '\"' || *p == '\\') {
+      is.src_ = p;
+      return RAPIDJSON_LIKELY(is.Tell() < length);
+    } else
+      os_->PutUnsafe(*p++);
+
+  // The rest of string using SIMD
+  static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"',
+                                  '\"', '\"', '\"', '\"', '\"', '\"',
+                                  '\"', '\"', '\"', '\"'};
+  static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\',
+                                  '\\', '\\', '\\', '\\', '\\', '\\',
+                                  '\\', '\\', '\\', '\\'};
+  static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+                                 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+                                 0x1F, 0x1F, 0x1F, 0x1F};
+  const __m128i dq =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&dquote[0]));
+  const __m128i bs =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&bslash[0]));
+  const __m128i sp =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&space[0]));
+
+  for (; p != endAligned; p += 16) {
+    const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
+    const __m128i t1 = _mm_cmpeq_epi8(s, dq);
+    const __m128i t2 = _mm_cmpeq_epi8(s, bs);
+    const __m128i t3 = _mm_cmpeq_epi8(_mm_max_epu8(s, sp),
+                                      sp);  // s < 0x20 <=> max(s, 0x1F) == 0x1F
+    const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3);
+    unsigned short r = static_cast<unsigned short>(_mm_movemask_epi8(x));
+    if (RAPIDJSON_UNLIKELY(r != 0)) {  // some of characters is escaped
+      SizeType len;
+#ifdef _MSC_VER  // Find the index of first escaped
+      unsigned long offset;
+      _BitScanForward(&offset, r);
+      len = offset;
+#else
+      len = static_cast<SizeType>(__builtin_ffs(r) - 1);
+#endif
+      char *q = reinterpret_cast<char *>(os_->PushUnsafe(len));
+      for (size_t i = 0; i < len; i++) q[i] = p[i];
+
+      p += len;
+      break;
+    }
+    _mm_storeu_si128(reinterpret_cast<__m128i *>(os_->PushUnsafe(16)), s);
+  }
+
+  is.src_ = p;
+  return RAPIDJSON_LIKELY(is.Tell() < length);
+}
+#elif defined(RAPIDJSON_NEON)
+template <>
+inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
+                                                           size_t length) {
+  if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
+
+  if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
+
+  const char *p = is.src_;
+  const char *end = is.head_ + length;
+  const char *nextAligned = reinterpret_cast<const char *>(
+      (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+  const char *endAligned = reinterpret_cast<const char *>(
+      reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
+  if (nextAligned > end) return true;
+
+  while (p != nextAligned)
+    if (*p < 0x20 || *p == '\"' || *p == '\\') {
+      is.src_ = p;
+      return RAPIDJSON_LIKELY(is.Tell() < length);
+    } else
+      os_->PutUnsafe(*p++);
+
+  // The rest of string using SIMD
+  const uint8x16_t s0 = vmovq_n_u8('"');
+  const uint8x16_t s1 = vmovq_n_u8('\\');
+  const uint8x16_t s2 = vmovq_n_u8('\b');
+  const uint8x16_t s3 = vmovq_n_u8(32);
+
+  for (; p != endAligned; p += 16) {
+    const uint8x16_t s = vld1q_u8(reinterpret_cast<const uint8_t *>(p));
+    uint8x16_t x = vceqq_u8(s, s0);
+    x = vorrq_u8(x, vceqq_u8(s, s1));
+    x = vorrq_u8(x, vceqq_u8(s, s2));
+    x = vorrq_u8(x, vcltq_u8(s, s3));
+
+    x = vrev64q_u8(x);                                           // Rev in 64
+    uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0);   // extract
+    uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1);  // extract
+
+    SizeType len = 0;
+    bool escaped = false;
+    if (low == 0) {
+      if (high != 0) {
+        uint32_t lz = RAPIDJSON_CLZLL(high);
+        len = 8 + (lz >> 3);
+        escaped = true;
+      }
+    } else {
+      uint32_t lz = RAPIDJSON_CLZLL(low);
+      len = lz >> 3;
+      escaped = true;
+    }
+    if (RAPIDJSON_UNLIKELY(escaped)) {  // some of characters is escaped
+      char *q = reinterpret_cast<char *>(os_->PushUnsafe(len));
+      for (size_t i = 0; i < len; i++) q[i] = p[i];
+
+      p += len;
+      break;
+    }
+    vst1q_u8(reinterpret_cast<uint8_t *>(os_->PushUnsafe(16)), s);
+  }
+
+  is.src_ = p;
+  return RAPIDJSON_LIKELY(is.Tell() < length);
+}
+#endif  // RAPIDJSON_NEON
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(_MSC_VER) || defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_RAPIDJSON_H_
diff --git a/livox_ros_driver2/CHANGELOG.md b/livox_ros_driver2/CHANGELOG.md
new file mode 100644
index 0000000..df75257
--- /dev/null
+++ b/livox_ros_driver2/CHANGELOG.md
@@ -0,0 +1,57 @@
+# Changelog
+
+All notable changes to this project will be documented in this file.
+
+## [1.2.4]
+### Fixed
+- Optimize framing performance
+
+## [1.2.3]
+### Fixed
+- Optimize framing logic and reduce CPU usage
+- Fixed some known issues
+
+## [1.2.1]
+### Fixed
+- Fix offset time error regarding CustomMsg format message publishment.
+
+## [1.2.0]
+### Added
+- Revise the frame segmentation logic.
+- (Notice!!!) Add Timestamp to each point in Livox pointcloud2 (PointXYZRTLT) format. The PointXYZRTL format has been updated to PointXYZRTLT format. Compatibility needs to be considered.
+### Fixed
+- Improve support for gPTP and GPS synchronizations.
+
+--- 
+## [1.1.3]
+### Fixed
+- Improve performance when running in ROS2 Humble.
+
+--- 
+## [1.1.2]
+### Changed
+- Change publish frequency range to [0.5Hz, 10 Hz].
+### Fixed
+- Fix a high CPU-usage problem.
+
+--- 
+## [1.1.1]
+### Added
+- Offer valid line-number info in the point cloud data of MID-360 Lidar.
+- Enable IMU by default.
+### Changed
+- Update the README slightly.
+
+--- 
+## [1.0.0]
+### Added
+- Support Mid-360 Lidar.
+- Support for Ubuntu 22.04 ROS2 humble.
+- Support multi-topic fuction, the suffix of the topic name corresponds to the ip address of each Lidar. 
+### Changed
+- Remove the embedded SDK.
+- Constraint: Livox ROS Driver 2 for ROS2 does not support message passing with PCL native data types.
+### Fixed
+- Fix IMU packet loss.
+- Fix some conflicts with livox ros driver.
+- Fixed HAP Lidar publishing PointCloud2 and CustomMsg format point clouds with no line number.
diff --git a/livox_ros_driver2/CMakeLists.txt b/livox_ros_driver2/CMakeLists.txt
new file mode 100644
index 0000000..99a8ccc
--- /dev/null
+++ b/livox_ros_driver2/CMakeLists.txt
@@ -0,0 +1,336 @@
+# judge which cmake codes to use 
+if(ROS_EDITION STREQUAL "ROS1")
+
+  # Copyright(c) 2019 livoxtech limited.
+
+  cmake_minimum_required(VERSION 3.0)
+
+
+  #---------------------------------------------------------------------------------------
+  # Start livox_ros_driver2 project
+  #---------------------------------------------------------------------------------------
+  include(cmake/version.cmake)
+  project(livox_ros_driver2 VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
+  message(STATUS "livox_ros_driver2 version: ${LIVOX_ROS_DRIVER2_VERSION}")
+
+  #---------------------------------------------------------------------------------------
+  # Add ROS Version MACRO
+  #---------------------------------------------------------------------------------------
+  add_definitions(-DBUILDING_ROS1)
+
+  #---------------------------------------------------------------------------------------
+  # find package and the dependecy
+  #---------------------------------------------------------------------------------------
+  find_package(Boost 1.54 REQUIRED COMPONENTS
+    system
+    thread
+    chrono
+  )
+
+  ## Find catkin macros and libraries
+  ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+  ## is used, also find other catkin packages
+  find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    rospy
+    sensor_msgs
+    std_msgs
+    message_generation
+    rosbag
+    pcl_ros
+  )
+
+  ## Find pcl lib
+  find_package(PCL REQUIRED)
+
+  ## Generate messages in the 'msg' folder
+  add_message_files(FILES
+    CustomPoint.msg
+    CustomMsg.msg
+  #   Message2.msg
+  )
+
+  ## Generate added messages and services with any dependencies listed here
+  generate_messages(DEPENDENCIES
+    std_msgs
+  )
+
+  find_package(PkgConfig)
+  pkg_check_modules(APR apr-1)
+  if (APR_FOUND)
+    message(${APR_INCLUDE_DIRS})
+    message(${APR_LIBRARIES})
+  endif (APR_FOUND)
+
+  ###################################
+  ## catkin specific configuration ##
+  ###################################
+  ## The catkin_package macro generates cmake config files for your package
+  ## Declare things to be passed to dependent projects
+  ## INCLUDE_DIRS: uncomment this if your package contains header files
+  ## LIBRARIES: libraries you create in this project that dependent projects als    o need
+  ## CATKIN_DEPENDS: catkin_packages dependent projects also need
+  ## DEPENDS: system dependencies of this project that dependent projects also n    eed
+  catkin_package(CATKIN_DEPENDS
+    roscpp rospy std_msgs message_runtime
+    pcl_ros
+  )
+
+  #---------------------------------------------------------------------------------------
+  # Set default build to release
+  #---------------------------------------------------------------------------------------
+  if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose Release or Debug" FORCE)
+  endif()
+
+  #---------------------------------------------------------------------------------------
+  # Compiler config
+  #---------------------------------------------------------------------------------------
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+
+  ## make sure the livox_lidar_sdk_static library is installed
+  find_library(LIVOX_LIDAR_SDK_LIBRARY  liblivox_lidar_sdk_static.a    /usr/local/lib)
+
+  ## PCL library
+  link_directories(${PCL_LIBRARY_DIRS})
+  add_definitions(${PCL_DEFINITIONS})
+
+  #---------------------------------------------------------------------------------------
+  # generate excutable and add libraries
+  #---------------------------------------------------------------------------------------
+  add_executable(${PROJECT_NAME}_node
+    ""
+  )
+
+  #---------------------------------------------------------------------------------------
+  # precompile macro and compile option
+  #---------------------------------------------------------------------------------------
+  target_compile_options(${PROJECT_NAME}_node
+    PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall>
+  )
+
+  #---------------------------------------------------------------------------------------
+  # add projects that depend on
+  #---------------------------------------------------------------------------------------
+  add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp)
+
+  #---------------------------------------------------------------------------------------
+  # source file
+  #---------------------------------------------------------------------------------------
+  target_sources(${PROJECT_NAME}_node
+    PRIVATE
+    src/driver_node.cpp
+    src/lds.cpp
+    src/lds_lidar.cpp
+    src/lddc.cpp
+    src/livox_ros_driver2.cpp
+
+    src/comm/comm.cpp
+    src/comm/ldq.cpp
+    src/comm/semaphore.cpp
+    src/comm/lidar_imu_data_queue.cpp
+    src/comm/cache_index.cpp
+    src/comm/pub_handler.cpp
+
+    src/parse_cfg_file/parse_cfg_file.cpp
+    src/parse_cfg_file/parse_livox_lidar_cfg.cpp
+
+    src/call_back/lidar_common_callback.cpp
+    src/call_back/livox_lidar_callback.cpp
+  )
+
+  #---------------------------------------------------------------------------------------
+  # include file
+  #---------------------------------------------------------------------------------------
+  target_include_directories(${PROJECT_NAME}_node
+    PUBLIC
+    ${catkin_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+    ${APR_INCLUDE_DIRS}
+    3rdparty
+    src
+  )
+
+  #---------------------------------------------------------------------------------------
+  # link libraries
+  #---------------------------------------------------------------------------------------
+  target_link_libraries(${PROJECT_NAME}_node
+    ${LIVOX_LIDAR_SDK_LIBRARY}
+    ${Boost_LIBRARY}
+    ${catkin_LIBRARIES}
+    ${PCL_LIBRARIES}
+    ${APR_LIBRARIES}
+  )
+
+
+  #---------------------------------------------------------------------------------------
+  # Install
+  #---------------------------------------------------------------------------------------
+
+  install(TARGETS ${PROJECT_NAME}_node
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+
+  install(DIRECTORY launch_ROS1/
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch_ROS1
+  )
+
+  #---------------------------------------------------------------------------------------
+  # end of CMakeList.txt
+  #---------------------------------------------------------------------------------------
+
+
+else(ROS_EDITION STREQUAL "ROS2")
+
+  # Copyright(c) 2020 livoxtech limited.
+
+  cmake_minimum_required(VERSION 3.14)
+  project(livox_ros_driver2)
+
+  # Default to C99
+  if(NOT CMAKE_C_STANDARD)
+    set(CMAKE_C_STANDARD 99)
+  endif()
+
+  # Default to C++14
+  if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 14)
+  endif()
+
+  list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules")
+
+  if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
+  endif()
+
+  # Printf version info
+  include(cmake/version.cmake)
+  project(${PROJECT_NAME} VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
+  message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}")
+
+  #---------------------------------------------------------------------------------------
+  # Add ROS Version MACRO
+  #---------------------------------------------------------------------------------------
+  add_definitions(-DBUILDING_ROS2)
+
+  # find dependencies
+  # uncomment the following section in order to fill in
+  # further dependencies manually.
+  # find_package(<dependency> REQUIRED)
+  find_package(ament_cmake_auto REQUIRED)
+  ament_auto_find_build_dependencies()
+  find_package(PCL REQUIRED)
+  find_package(std_msgs REQUIRED)
+  find_package(builtin_interfaces REQUIRED)
+  find_package(rosidl_default_generators REQUIRED)
+
+  # check apr
+  find_package(PkgConfig)
+  pkg_check_modules(APR apr-1)
+  if (APR_FOUND)
+    message(${APR_INCLUDE_DIRS})
+    message(${APR_LIBRARIES})
+  endif (APR_FOUND)
+
+  # generate custom msg headers
+  set(LIVOX_INTERFACES livox_interfaces2)
+  rosidl_generate_interfaces(${LIVOX_INTERFACES}
+    "msg/CustomPoint.msg"
+    "msg/CustomMsg.msg"
+    DEPENDENCIES builtin_interfaces std_msgs
+    LIBRARY_NAME ${PROJECT_NAME}
+  )
+
+  ## make sure the livox_lidar_sdk_shared library is installed
+  find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED)
+
+  ##
+  find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR
+    NAMES "livox_lidar_api.h" "livox_lidar_def.h"
+    REQUIRED)
+
+  ## PCL library
+  link_directories(${PCL_LIBRARY_DIRS})
+  add_definitions(${PCL_DEFINITIONS})
+
+  # livox ros2 driver target
+  ament_auto_add_library(${PROJECT_NAME} SHARED
+    src/livox_ros_driver2.cpp
+    src/lddc.cpp
+    src/driver_node.cpp
+    src/lds.cpp
+    src/lds_lidar.cpp
+
+    src/comm/comm.cpp
+    src/comm/ldq.cpp
+    src/comm/semaphore.cpp
+    src/comm/lidar_imu_data_queue.cpp
+    src/comm/cache_index.cpp
+    src/comm/pub_handler.cpp
+
+    src/parse_cfg_file/parse_cfg_file.cpp
+    src/parse_cfg_file/parse_livox_lidar_cfg.cpp
+
+    src/call_back/lidar_common_callback.cpp
+    src/call_back/livox_lidar_callback.cpp
+  )
+
+  target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS})
+
+  # get include directories of custom msg headers
+  if(HUMBLE_ROS STREQUAL "humble")
+    rosidl_get_typesupport_target(cpp_typesupport_target
+    ${LIVOX_INTERFACES} "rosidl_typesupport_cpp")
+    target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}")
+  else()
+    set(LIVOX_INTERFACE_TARGET "${LIVOX_INTERFACES}__rosidl_typesupport_cpp")
+    add_dependencies(${PROJECT_NAME} ${LIVOX_INTERFACES})
+    get_target_property(LIVOX_INTERFACES_INCLUDE_DIRECTORIES ${LIVOX_INTERFACE_TARGET} INTERFACE_INCLUDE_DIRECTORIES)
+  endif()
+
+  # include file direcotry
+  target_include_directories(${PROJECT_NAME} PUBLIC
+    ${PCL_INCLUDE_DIRS}
+    ${APR_INCLUDE_DIRS}
+    ${LIVOX_LIDAR_SDK_INCLUDE_DIR}
+    ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES}   # for custom msgs
+    3rdparty
+    src
+  )
+
+  # link libraries
+  target_link_libraries(${PROJECT_NAME}
+    ${LIVOX_LIDAR_SDK_LIBRARY}
+    ${LIVOX_INTERFACE_TARGET}   # for custom msgs
+    ${PPT_LIBRARY}
+    ${Boost_LIBRARY}
+    ${PCL_LIBRARIES}
+    ${APR_LIBRARIES}
+  )
+
+  rclcpp_components_register_node(${PROJECT_NAME}
+    PLUGIN "livox_ros::DriverNode"
+    EXECUTABLE ${PROJECT_NAME}_node
+  )
+
+  if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    # the following line skips the linter which checks for copyrights
+    # uncomment the line when a copyright and license is not present in all source files
+    #set(ament_cmake_copyright_FOUND TRUE)
+    # the following line skips cpplint (only works in a git repo)
+    # uncomment the line when this package is not in a git repo
+    #set(ament_cmake_cpplint_FOUND TRUE)
+    ament_lint_auto_find_test_dependencies()
+  endif()
+
+  ament_auto_package(INSTALL_TO_SHARE
+    config
+    launch_ROS2
+  )
+
+endif()
\ No newline at end of file
diff --git a/livox_ros_driver2/LICENSE.txt b/livox_ros_driver2/LICENSE.txt
new file mode 100644
index 0000000..9e0f5db
--- /dev/null
+++ b/livox_ros_driver2/LICENSE.txt
@@ -0,0 +1,412 @@
+The following portions of the LIVOX’s Livox ROS Driver2 (“Software” referred to in the terms below) are made available to you under the terms of the MIT License provided below and is also available at https://opensource.org/licenses/MIT.  
+
+livox_ros_driver2
+├── build.sh
+├── cmake
+│   └── version.cmake
+├── CMakeLists.txt
+├── config
+│   ├── display_point_cloud_ROS1.rviz
+│   ├── display_point_cloud_ROS2.rviz
+│   ├── HAP_config.json
+│   ├── MID360_config.json
+│   └── mixed_HAP_MID360_config.json
+├── launch_ROS1
+│   ├── msg_HAP.launch
+│   ├── msg_MID360.launch
+│   ├── msg_mixed.launch
+│   ├── rviz_HAP.launch
+│   ├── rviz_MID360.launch
+│   └── rviz_mixed.launch
+├── launch_ROS2
+│   ├── msg_HAP_launch.py
+│   ├── msg_MID360_launch.py
+│   ├── rviz_HAP_launch.py
+│   ├── rviz_MID360_launch.py
+│   └── rviz_mixed.py
+├── msg
+│   ├── CustomMsg.msg
+│   └── CustomPoint.msg
+├── package_ROS1.xml
+├── package_ROS2.xml
+├── package.xml
+├── README.md
+└── src
+    ├── call_back
+    │   ├── lidar_common_callback.cpp
+    │   ├── lidar_common_callback.h
+    │   ├── livox_lidar_callback.cpp
+    │   └── livox_lidar_callback.h
+    ├── comm
+    │   ├── cache_index.cpp
+    │   ├── cache_index.h
+    │   ├── comm.cpp
+    │   ├── comm.h
+    │   ├── ldq.cpp
+    │   ├── ldq.h
+    │   ├── lidar_imu_data_queue.cpp
+    │   ├── lidar_imu_data_queue.h
+    │   ├── pub_handler.cpp
+    │   ├── pub_handler.h
+    │   ├── semaphore.cpp
+    │   └── semaphore.h
+    ├── driver_node.cpp
+    ├── driver_node.h
+    ├── include
+    │   ├── livox_ros_driver2.h
+    │   ├── ros1_headers.h
+    │   ├── ros2_headers.h
+    │   └── ros_headers.h
+    ├── lddc.cpp
+    ├── lddc.h
+    ├── lds.cpp
+    ├── lds.h
+    ├── lds_lidar.cpp
+    ├── lds_lidar.h
+    ├── livox_ros_driver2.cpp
+    └── parse_cfg_file
+        ├── parse_cfg_file.cpp
+        ├── parse_cfg_file.h
+        ├── parse_livox_lidar_cfg.cpp
+        └── parse_livox_lidar_cfg.h
+
+---------------------------------
+
+The MIT License (MIT)
+
+Copyright (c) 2022 Livox. All rights reserved.
+
+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.
+
+===============================================================
+
+LIVOX’s Livox ROS Driver2 uses unmodified source code of RapidJSON (https://github.com/Tencent/rapidjson), which is also licensed under MIT license. A copy of the MIT license is provided below and is also available at https://opensource.org/licenses/MIT.
+
+livox_ros_driver2
+├── 3rdparty
+│   └── rapidjson
+│       ├── allocators.h
+│       ├── cursorstreamwrapper.h
+│       ├── document.h
+│       ├── encodedstream.h
+│       ├── encodings.h
+│       ├── error
+│       │   ├── en.h
+│       │   └── error.h
+│       ├── filereadstream.h
+│       ├── filewritestream.h
+│       ├── fwd.h
+│       ├── internal
+│       │   ├── biginteger.h
+│       │   ├── clzll.h
+│       │   ├── diyfp.h
+│       │   ├── dtoa.h
+│       │   ├── ieee754.h
+│       │   ├── itoa.h
+│       │   ├── meta.h
+│       │   ├── pow10.h
+│       │   ├── regex.h
+│       │   ├── stack.h
+│       │   ├── strfunc.h
+│       │   ├── strtod.h
+│       │   └── swap.h
+│       ├── istreamwrapper.h
+│       ├── memorybuffer.h
+│       ├── memorystream.h
+│       ├── msinttypes
+│       │   ├── inttypes.h
+│       │   └── stdint.h
+│       ├── ostreamwrapper.h
+│       ├── pointer.h
+│       ├── prettywriter.h
+│       ├── rapidjson.h
+│       ├── reader.h
+│       ├── schema.h
+│       ├── stream.h
+│       ├── stringbuffer.h
+│       └── writer.h
+
+-------------------------------------------------------------
+
+Tencent is pleased to support the open source community by making RapidJSON
+available.
+
+Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+rights reserved.
+
+Licensed under the MIT License (the "License"); you may not use this file
+except in compliance with the License. You may obtain a copy of the License
+at
+
+http://opensource.org/licenses/MIT
+
+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.
+
+===============================================================
+
+LIVOX’s Livox ROS Driver2 uses unmodified libraries and interfaces of ROS (https://www.ros.org/), which is also licensed under 3-Clause-BSD license. A copy of the 3-Clause-BSD license is provided below and is also available at https://opensource.org/licenses/BSD-3-Clause.
+
+-------------------------------------------------------------
+
+The 3-Clause BSD License
+
+Copyright (c) 2001 - 2009, The Board of Trustees of the University of Illinois.
+All rights reserved.
+
+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 University of Illinois
+  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.
+
+===============================================================
+
+LIVOX’s Livox ROS Driver2 uses unmodified libraries and interfaces of ROS2-rclcpp (https://github.com/ros2), which is also licensed under Apache License 2.0. A copy of the Apache License 2.0 is provided below and is also available at https://www.apache.org/licenses/LICENSE-2.0.
+
+-------------------------------------------------------------
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   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.
diff --git a/livox_ros_driver2/README.md b/livox_ros_driver2/README.md
new file mode 100644
index 0000000..ab7c0ad
--- /dev/null
+++ b/livox_ros_driver2/README.md
@@ -0,0 +1,545 @@
+# Livox ROS Driver 2
+
+Livox ROS Driver 2 is the 2nd-generation driver package used to connect LiDAR products produced by Livox, applicable for ROS (noetic recommended) and ROS2 (foxy or humble recommended).
+
+  **Note :**
+
+  As a debugging tool, Livox ROS Driver is not recommended for mass production but limited to test scenarios. You should optimize the code based on the original source to meet your various needs.
+
+## 1. Preparation
+
+### 1.1 OS requirements
+
+  * Ubuntu 18.04 for ROS Melodic;
+  * Ubuntu 20.04 for ROS Noetic and ROS2 Foxy;
+  * Ubuntu 22.04 for ROS2 Humble;
+
+  **Tips:**
+
+  Colcon is a build tool used in ROS2.
+
+  How to install colcon: [Colcon installation instructions](https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Colcon-Tutorial.html)
+
+### 1.2 Install ROS & ROS2
+
+For ROS Melodic installation, please refer to:
+[ROS Melodic installation instructions](https://wiki.ros.org/melodic/Installation)
+
+For ROS Noetic installation, please refer to:
+[ROS Noetic installation instructions](https://wiki.ros.org/noetic/Installation)
+
+For ROS2 Foxy installation, please refer to:
+[ROS Foxy installation instructions](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html)
+
+For ROS2 Humble installation, please refer to:
+[ROS Humble installation instructions](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html)
+
+Desktop-Full installation is recommend.
+
+## 2. Build & Run Livox ROS Driver 2
+
+### 2.1 Clone Livox ROS Driver 2 source code:
+
+```shell
+git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2
+```
+
+  **Note :**
+
+  Be sure to clone the source code in a '[work_space]/src/' folder (as shown above), otherwise compilation errors will occur due to the compilation tool restriction.
+
+### 2.2 Build & install the Livox-SDK2
+
+  **Note :**
+
+  Please follow the guidance of installation in the [Livox-SDK2/README.md](https://github.com/Livox-SDK/Livox-SDK2/blob/master/README.md)
+
+### 2.3 Build the Livox ROS Driver 2:
+
+#### For ROS (take Noetic as an example):
+```shell
+source /opt/ros/noetic/setup.sh
+./build.sh ROS1
+```
+
+#### For ROS2 Foxy:
+```shell
+source /opt/ros/foxy/setup.sh
+./build.sh ROS2
+```
+
+#### For ROS2 Humble:
+```shell
+source /opt/ros/humble/setup.sh
+./build.sh humble
+```
+
+### 2.4 Run Livox ROS Driver 2:
+
+#### For ROS:
+
+```shell
+source ../../devel/setup.sh
+roslaunch livox_ros_driver2 [launch file]
+```
+
+in which,  
+
+* **livox_ros_driver2** : is the ROS package name of Livox ROS Driver 2;
+* **[launch file]** : is the ROS launch file you want to use; the 'launch_ROS1' folder contains several launch samples for your reference;  
+
+An rviz launch example for HAP LiDAR would be:
+
+```shell
+roslaunch livox_ros_driver2 rviz_HAP.launch
+```
+
+#### For ROS2:
+```shell
+source ../../install/setup.sh
+ros2 launch livox_ros_driver2 [launch file]
+```
+
+in which,  
+
+* **[launch file]** : is the ROS2 launch file you want to use; the 'launch_ROS2' folder contains several launch samples for your reference.
+
+A rviz launch example for HAP LiDAR would be:
+
+```shell
+ros2 launch livox_ros_driver2 rviz_HAP_launch.py
+```
+
+## 3. Launch file and livox_ros_driver2 internal parameter configuration instructions
+
+### 3.1 Launch file configuration instructions
+
+Launch files of ROS are in the "ws_livox/src/livox_ros_driver2/launch_ROS1" directory and launch files of ROS2 are in the "ws_livox/src/livox_ros_driver2/launch_ROS2" directory. Different launch files have different configuration parameter values and are used in different scenarios:
+
+| launch file name          | Description                                                  |
+| ------------------------- | ------------------------------------------------------------ |
+| rviz_HAP.launch   | Connect to HAP LiDAR device<br>Publish pointcloud2 format  data<br>Autoload rviz |
+| msg_HAP.launch     | Connect to HAP LiDAR device<br>Publish livox customized pointcloud data|
+| rviz_MID360.launch        | Connect to MID360 LiDAR device<br>Publish pointcloud2 format data <br>Autoload rviz|
+| msg_MID360.launch          | Connect to MID360 LiDAR device<br>Publish livox customized pointcloud data |
+| rviz_mixed.launch    | Connect to HAP and MID360 LiDAR device<br>Publish pointcloud2 format data <br>Autoload rviz|
+| msg_mixed.launch      | Connect to HAP and MID360 LiDAR device<br>Publish livox customized pointcloud data |
+
+### 3.2 Livox ros driver 2 internal main parameter configuration instructions
+
+All internal parameters of Livox_ros_driver2 are in the launch file. Below are detailed descriptions of the three commonly used parameters :
+
+| Parameter    | Detailed description                                         | Default |
+| ------------ | ------------------------------------------------------------ | ------- |
+| publish_freq | Set the frequency of point cloud publish <br>Floating-point data type, recommended values 5.0, 10.0, 20.0, 50.0, etc. The maximum publish frequency is 100.0 Hz.| 10.0    |
+| multi_topic  | If the LiDAR device has an independent topic to publish pointcloud data<br>0 -- All LiDAR devices use the same topic to publish pointcloud data<br>1 -- Each LiDAR device has its own topic to publish point cloud data | 0       |
+| xfer_format  | Set pointcloud format<br>0 -- Livox pointcloud2(PointXYZRTLT) pointcloud format<br>1 -- Livox customized pointcloud format<br>2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS) | 0       |
+
+  **Note :**
+
+  Other parameters not mentioned in this table are not suggested to be changed unless fully understood.
+
+&ensp;&ensp;&ensp;&ensp;***Livox_ros_driver2 pointcloud data detailed description :***
+
+1. Livox pointcloud2 (PointXYZRTLT) point cloud format, as follows :
+
+```c
+float32 x               # X axis, unit:m
+float32 y               # Y axis, unit:m
+float32 z               # Z axis, unit:m
+float32 intensity       # the value is reflectivity, 0.0~255.0
+uint8   tag             # livox tag
+uint8   line            # laser number in lidar
+float64 timestamp       # Timestamp of point
+```
+  **Note :**
+
+  The number of points in the frame may be different, but each point provides a timestamp.
+
+2. Livox customized data package format, as follows :
+
+```c
+std_msgs/Header header     # ROS standard message header
+uint64          timebase   # The time of first point
+uint32          point_num  # Total number of pointclouds
+uint8           lidar_id   # Lidar device id number
+uint8[3]        rsvd       # Reserved use
+CustomPoint[]   points     # Pointcloud data
+```
+
+&ensp;&ensp;&ensp;&ensp;Customized Point Cloud (CustomPoint) format in the above customized data package :
+
+```c
+uint32  offset_time     # offset time relative to the base time
+float32 x               # X axis, unit:m
+float32 y               # Y axis, unit:m
+float32 z               # Z axis, unit:m
+uint8   reflectivity    # reflectivity, 0~255
+uint8   tag             # livox tag
+uint8   line            # laser number in lidar
+```
+
+3. The standard pointcloud2 (pcl :: PointXYZI) format in the PCL library (only ROS can publish):
+
+&ensp;&ensp;&ensp;&ensp;Please refer to the pcl :: PointXYZI data structure in the point_types.hpp file of the PCL library.
+
+## 4. LiDAR config
+
+LiDAR Configurations (such as ip, port, data type... etc.) can be set via a json-style config file. Config files for single HAP, Mid360 and mixed-LiDARs are in the "config" folder. The parameter naming *'user_config_path'* in launch files indicates such json file path.
+
+1. Follow is a configuration example for HAP LiDAR (located in config/HAP_config.json):
+
+```json
+{
+  "lidar_summary_info" : {
+    "lidar_type": 8  # protocol type index, please don't revise this value
+  },
+  "HAP": {
+    "device_type" : "HAP",
+    "lidar_ipaddr": "",
+    "lidar_net_info" : {
+      "cmd_data_port": 56000,  # command port
+      "push_msg_port": 0,
+      "point_data_port": 57000,
+      "imu_data_port": 58000,
+      "log_data_port": 59000
+    },
+    "host_net_info" : {
+      "cmd_data_ip" : "192.168.1.5",  # host ip (it can be revised)
+      "cmd_data_port": 56000,
+      "push_msg_ip": "",
+      "push_msg_port": 0,
+      "point_data_ip": "192.168.1.5",  # host ip
+      "point_data_port": 57000,
+      "imu_data_ip" : "192.168.1.5",  # host ip
+      "imu_data_port": 58000,
+      "log_data_ip" : "",
+      "log_data_port": 59000
+    }
+  },
+  "lidar_configs" : [
+    {
+      "ip" : "192.168.1.100",  # ip of the LiDAR you want to config
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "blind_spot_set" : 50,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    }
+  ]
+}
+```
+
+The parameter attributes in the above json file are described in the following table :
+
+**LiDAR configuration parameter**
+| Parameter                  | Type    | Description                                                  | Default         |
+| :------------------------- | ------- | ------------------------------------------------------------ | --------------- |
+| ip             | String  | Ip of the LiDAR you want to config | 192.168.1.100 |
+| pcl_data_type             | Int | Choose the resolution of the point cloud data to send<br>1 -- Cartesian coordinate data (32 bits)<br>2 -- Cartesian coordinate data (16 bits) <br>3 --Spherical coordinate data| 1           |
+| pattern_mode                | Int     | Space scan pattern<br>0 -- non-repeating scanning pattern mode<br>1 -- repeating scanning pattern mode <br>2 -- repeating scanning pattern mode (low scanning rate) | 0               |
+| blind_spot_set (Only for HAP LiDAR)                 | Int     | Set blind spot<br>Range from 50 cm to 200 cm               | 50               |
+| extrinsic_parameter |      | Set extrinsic parameter<br> The data types of "roll" "picth" "yaw" are float <br>  The data types of "x" "y" "z" are int<br>               |
+
+For more infomation about the HAP config, please refer to:
+[HAP Config File Description](https://github.com/Livox-SDK/Livox-SDK2/wiki/hap-config-file-description)
+
+2. When connecting multiple LiDARs, add objects corresponding to different LiDARs to the "lidar_configs" array. Examples of mixed-LiDARs config file contents are as follows :
+
+```json
+{
+  "lidar_summary_info" : {
+    "lidar_type": 8  # protocol type index, please don't revise this value
+  },
+  "HAP": {
+    "lidar_net_info" : {  # HAP ports, please don't revise these values
+      "cmd_data_port": 56000,  # HAP command port
+      "push_msg_port": 0,
+      "point_data_port": 57000,
+      "imu_data_port": 58000,
+      "log_data_port": 59000
+    },
+    "host_net_info" : {
+      "cmd_data_ip" : "192.168.1.5",  # host ip
+      "cmd_data_port": 56000,
+      "push_msg_ip": "",
+      "push_msg_port": 0,
+      "point_data_ip": "192.168.1.5",  # host ip
+      "point_data_port": 57000,
+      "imu_data_ip" : "192.168.1.5",  # host ip
+      "imu_data_port": 58000,
+      "log_data_ip" : "",
+      "log_data_port": 59000
+    }
+  },
+  "MID360": {
+    "lidar_net_info" : {  # Mid360 ports, please don't revise these values
+      "cmd_data_port": 56100,  # Mid360 command port
+      "push_msg_port": 56200,
+      "point_data_port": 56300,
+      "imu_data_port": 56400,
+      "log_data_port": 56500
+    },
+    "host_net_info" : {
+      "cmd_data_ip" : "192.168.1.5",  # host ip
+      "cmd_data_port": 56101,
+      "push_msg_ip": "192.168.1.5",  # host ip
+      "push_msg_port": 56201,
+      "point_data_ip": "192.168.1.5",  # host ip
+      "point_data_port": 56301,
+      "imu_data_ip" : "192.168.1.5",  # host ip
+      "imu_data_port": 56401,
+      "log_data_ip" : "",
+      "log_data_port": 56501
+    }
+  },
+  "lidar_configs" : [
+    {
+      "ip" : "192.168.1.100",  # ip of the HAP you want to config
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "blind_spot_set" : 50,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    },
+    {
+      "ip" : "192.168.1.12",  # ip of the Mid360 you want to config
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    }
+  ]
+}
+```
+3. when multiple nics on the host connect to multiple LiDARs, you need to add objects corresponding to different LiDARs to the lidar_configs array. Run different luanch files separately, and the following is an example of mixing lidar configuration file contents:
+
+**MID360_config1:**
+```json
+{
+  "lidar_summary_info" : {
+    "lidar_type": 8  # protocol type index，please don't revise this value
+  },
+    "MID360": {
+        "lidar_net_info": {
+            "cmd_data_port": 56100, # command port
+            "push_msg_port": 56200, 
+            "point_data_port": 56300,
+            "imu_data_port": 56400,
+            "log_data_port": 56500
+        },
+        "host_net_info": [
+            {
+                "lidar_ip": ["192.168.1.100"], # Lidar ip
+                "host_ip": "192.168.1.5", # host ip
+                "cmd_data_port": 56101,
+                "push_msg_port": 56201,
+                "point_data_port": 56301,
+                "imu_data_port": 56401,
+                "log_data_port": 56501
+            }
+        ]
+    },
+    "lidar_configs": [
+        {
+            "ip": "192.168.1.100", # ip of the LiDAR you want to config
+            "pcl_data_type": 1,
+            "pattern_mode": 0,
+            "extrinsic_parameter": {
+                "roll": 0.0,
+                "pitch": 0.0,
+                "yaw": 0.0,
+                "x": 0,
+                "y": 0,
+                "z": 0
+            }
+        }
+    ]
+}
+```
+**MID360_config2:**
+```json
+{
+  "lidar_summary_info" : {
+    "lidar_type": 8  # protocol type index，please don't revise this value
+  },
+    "MID360": {
+        "lidar_net_info": {
+            "cmd_data_port": 56100, # command port
+            "push_msg_port": 56200, 
+            "point_data_port": 56300,
+            "imu_data_port": 56400,
+            "log_data_port": 56500
+        },
+        "host_net_info": [
+            {
+                "lidar_ip": ["192.168.2.100"], # Lidar ip
+                "host_ip": "192.168.2.5", # host ip
+                "cmd_data_port": 56101,
+                "push_msg_port": 56201,
+                "point_data_port": 56301,
+                "imu_data_port": 56401,
+                "log_data_port": 56501
+            }
+        ]
+    },
+    "lidar_configs": [
+        {
+            "ip": "192.168.2.100", # ip of the LiDAR you want to config
+            "pcl_data_type": 1,
+            "pattern_mode": 0,
+            "extrinsic_parameter": {
+                "roll": 0.0,
+                "pitch": 0.0,
+                "yaw": 0.0,
+                "x": 0,
+                "y": 0,
+                "z": 0
+            }
+        }
+    ]
+}
+```
+**Launch1:**
+```
+<launch>
+    <!--user configure parameters for ros start-->
+    <arg name="lvx_file_path" default="livox_test.lvx"/>
+    <arg name="bd_list" default="100000000000000"/>
+    <arg name="xfer_format" default="0"/>
+    <arg name="multi_topic" default="1"/>
+    <arg name="data_src" default="0"/>
+    <arg name="publish_freq" default="10.0"/>
+    <arg name="output_type" default="0"/>
+    <arg name="rviz_enable" default="true"/>
+    <arg name="rosbag_enable" default="false"/>
+    <arg name="cmdline_arg" default="$(arg bd_list)"/>
+    <arg name="msg_frame_id" default="livox_frame"/>
+    <arg name="lidar_bag" default="true"/>
+    <arg name="imu_bag" default="true"/>
+    <!--user configure parameters for ros end--> 
+
+    <param name="xfer_format" value="$(arg xfer_format)"/>
+    <param name="multi_topic" value="$(arg multi_topic)"/>
+    <param name="data_src" value="$(arg data_src)"/>
+    <param name="publish_freq" type="double" value="$(arg publish_freq)"/>
+    <param name="output_data_type" value="$(arg output_type)"/>
+    <param name="cmdline_str" type="string" value="$(arg bd_list)"/>
+    <param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
+    <param name="user_config_path" type="string" value="$(find livox_ros_driver2)/config/MID360_config1.json"/> # Mid360 MID360_config1 name
+    <param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
+    <param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
+    <param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
+
+    <node name="livox_lidar_publisher1" pkg="livox_ros_driver2"
+          type="livox_ros_driver2_node" required="true"
+          output="screen" args="$(arg cmdline_arg)"/>
+
+    <group if="$(arg rviz_enable)">
+        <node name="livox_rviz" pkg="rviz" type="rviz" respawn="true"
+                args="-d $(find livox_ros_driver2)/config/display_point_cloud_ROS1.rviz"/>
+    </group>
+
+    <group if="$(arg rosbag_enable)">
+        <node pkg="rosbag" type="record" name="record" output="screen"
+                args="-a"/>
+    </group>
+
+</launch>
+```
+**Launch2:**
+```
+<launch>
+    <!--user configure parameters for ros start-->
+    <arg name="lvx_file_path" default="livox_test.lvx"/>
+    <arg name="bd_list" default="100000000000000"/>
+    <arg name="xfer_format" default="0"/>
+    <arg name="multi_topic" default="1"/>
+    <arg name="data_src" default="0"/>
+    <arg name="publish_freq" default="10.0"/>
+    <arg name="output_type" default="0"/>
+    <arg name="rviz_enable" default="true"/>
+    <arg name="rosbag_enable" default="false"/>
+    <arg name="cmdline_arg" default="$(arg bd_list)"/>
+    <arg name="msg_frame_id" default="livox_frame"/>
+    <arg name="lidar_bag" default="true"/>
+    <arg name="imu_bag" default="true"/>
+    <!--user configure parameters for ros end--> 
+
+    <param name="xfer_format" value="$(arg xfer_format)"/>
+    <param name="multi_topic" value="$(arg multi_topic)"/>
+    <param name="data_src" value="$(arg data_src)"/>
+    <param name="publish_freq" type="double" value="$(arg publish_freq)"/>
+    <param name="output_data_type" value="$(arg output_type)"/>
+    <param name="cmdline_str" type="string" value="$(arg bd_list)"/>
+    <param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
+    <param name="user_config_path" type="string" value="$(find livox_ros_driver2)/config/MID360_config2.json"/> # Mid360 MID360_config2 name
+    <param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
+    <param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
+    <param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
+
+    <node name="livox_lidar_publisher2" pkg="livox_ros_driver2"
+          type="livox_ros_driver2_node" required="true"
+          output="screen" args="$(arg cmdline_arg)"/>
+
+    <group if="$(arg rviz_enable)">
+        <node name="livox_rviz" pkg="rviz" type="rviz" respawn="true"
+                args="-d $(find livox_ros_driver2)/config/display_point_cloud_ROS1.rviz"/>
+    </group>
+
+    <group if="$(arg rosbag_enable)">
+        <node pkg="rosbag" type="record" name="record" output="screen"
+                args="-a"/>
+    </group>
+
+</launch>
+
+```
+
+## 5. Supported LiDAR list
+
+* HAP
+* Mid360
+* (more types are comming soon...)
+
+## 6. FAQ
+
+### 6.1 launch with "livox_lidar_rviz_HAP.launch" but no point cloud display on the grid?
+
+Please check the "Global Options - Fixed Frame" field in the RViz "Display" pannel. Set the field value to "livox_frame" and check the "PointCloud2" option in the pannel.
+
+### 6.2 launch with command "ros2 launch livox_lidar_rviz_HAP_launch.py" but cannot open shared object file "liblivox_sdk_shared.so" ?
+
+Please add '/usr/local/lib' to the env LD_LIBRARY_PATH.
+
+* If you want to add to current terminal:
+
+  ```shell
+  export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib
+  ```
+
+* If you want to add to current user:
+
+  ```shell
+  vim ~/.bashrc
+  export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib
+  source ~/.bashrc
+  ```
diff --git a/livox_ros_driver2/build.sh b/livox_ros_driver2/build.sh
new file mode 100755
index 0000000..3ce316c
--- /dev/null
+++ b/livox_ros_driver2/build.sh
@@ -0,0 +1,70 @@
+#!/bin/bash
+
+readonly VERSION_ROS1="ROS1"
+readonly VERSION_ROS2="ROS2"
+readonly VERSION_HUMBLE="humble"
+
+pushd `pwd` > /dev/null
+cd `dirname $0`
+echo "Working Path: "`pwd`
+
+ROS_VERSION=""
+ROS_HUMBLE=""
+
+# Set working ROS version
+if [ "$1" = "ROS2" ]; then
+    ROS_VERSION=${VERSION_ROS2}
+elif [ "$1" = "humble" ]; then
+    ROS_VERSION=${VERSION_ROS2}
+    ROS_HUMBLE=${VERSION_HUMBLE}
+elif [ "$1" = "ROS1" ]; then
+    ROS_VERSION=${VERSION_ROS1}
+else
+    echo "Invalid Argument"
+    exit
+fi
+echo "ROS version is: "$ROS_VERSION
+
+# clear `build/` folder.
+# TODO: Do not clear these folders, if the last build is based on the same ROS version.
+rm -rf ../../build/
+rm -rf ../../devel/
+rm -rf ../../install/
+# clear src/CMakeLists.txt if it exists.
+if [ -f ../CMakeLists.txt ]; then
+    rm -f ../CMakeLists.txt
+fi
+
+# exit
+
+# substitute the files/folders: CMakeList.txt, package.xml(s)
+if [ ${ROS_VERSION} = ${VERSION_ROS1} ]; then
+    if [ -f package.xml ]; then
+        rm package.xml
+    fi
+    cp -f package_ROS1.xml package.xml
+elif [ ${ROS_VERSION} = ${VERSION_ROS2} ]; then
+    if [ -f package.xml ]; then
+        rm package.xml
+    fi
+    cp -f package_ROS2.xml package.xml
+    cp -rf launch_ROS2/ launch/
+fi
+
+# build
+pushd `pwd` > /dev/null
+if [ $ROS_VERSION = ${VERSION_ROS1} ]; then
+    cd ../../
+    catkin_make -DROS_EDITION=${VERSION_ROS1}
+elif [ $ROS_VERSION = ${VERSION_ROS2} ]; then
+    cd ../../
+    colcon build --cmake-args -DROS_EDITION=${VERSION_ROS2} -DHUMBLE_ROS=${ROS_HUMBLE}
+fi
+popd > /dev/null
+
+# remove the substituted folders/files
+if [ $ROS_VERSION = ${VERSION_ROS2} ]; then
+    rm -rf launch/
+fi
+
+popd > /dev/null
diff --git a/livox_ros_driver2/cmake/version.cmake b/livox_ros_driver2/cmake/version.cmake
new file mode 100644
index 0000000..908c19b
--- /dev/null
+++ b/livox_ros_driver2/cmake/version.cmake
@@ -0,0 +1,16 @@
+#---------------------------------------------------------------------------------------
+# Get livox_ros_driver2 version from include/livox_ros_driver2.h
+#---------------------------------------------------------------------------------------
+file(READ "${CMAKE_CURRENT_LIST_DIR}/../src/include/livox_ros_driver2.h" LIVOX_ROS_DRIVER2_VERSION_FILE)
+string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MAJOR ([0-9]+)" _  "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
+set(ver_major ${CMAKE_MATCH_1})
+
+string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MINOR ([0-9]+)" _  "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
+set(ver_minor ${CMAKE_MATCH_1})
+string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_PATCH ([0-9]+)" _  "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
+set(ver_patch ${CMAKE_MATCH_1})
+
+if (NOT DEFINED ver_major OR NOT DEFINED ver_minor OR NOT DEFINED ver_patch)
+    message(FATAL_ERROR "Could not extract valid version from include/livox_ros_driver2.h")
+endif()
+set (LIVOX_ROS_DRIVER2_VERSION "${ver_major}.${ver_minor}.${ver_patch}")
diff --git a/livox_ros_driver2/config/HAP_config.json b/livox_ros_driver2/config/HAP_config.json
new file mode 100644
index 0000000..090ab9f
--- /dev/null
+++ b/livox_ros_driver2/config/HAP_config.json
@@ -0,0 +1,42 @@
+{
+  "lidar_summary_info" : {
+    "lidar_type": 8
+  },
+  "HAP": {
+    "lidar_net_info" : {
+      "cmd_data_port": 56000,
+      "push_msg_port": 0,
+      "point_data_port": 57000,
+      "imu_data_port": 58000,
+      "log_data_port": 59000
+    },
+    "host_net_info" : {
+      "cmd_data_ip" : "192.168.1.5",
+      "cmd_data_port": 56000,
+      "push_msg_ip": "",
+      "push_msg_port": 0,
+      "point_data_ip": "192.168.1.5",
+      "point_data_port": 57000,
+      "imu_data_ip" : "192.168.1.5",
+      "imu_data_port": 58000,
+      "log_data_ip" : "",
+      "log_data_port": 59000
+    }
+  },
+  "lidar_configs" : [
+    {
+      "ip" : "192.168.1.100",
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    }
+  ]
+}
+
diff --git a/livox_ros_driver2/config/MID360_config.json b/livox_ros_driver2/config/MID360_config.json
new file mode 100644
index 0000000..b393130
--- /dev/null
+++ b/livox_ros_driver2/config/MID360_config.json
@@ -0,0 +1,42 @@
+{
+  "lidar_summary_info" : {
+    "lidar_type": 8
+  },
+  "MID360": {
+    "lidar_net_info" : {
+      "cmd_data_port": 56100,
+      "push_msg_port": 56200,
+      "point_data_port": 56300,
+      "imu_data_port": 56400,
+      "log_data_port": 56500
+    },
+    "host_net_info" : {
+      "cmd_data_ip" : "192.168.1.50",
+      "cmd_data_port": 56101,
+      "push_msg_ip": "192.168.1.50",
+      "push_msg_port": 56201,
+      "point_data_ip": "192.168.1.50",
+      "point_data_port": 56301,
+      "imu_data_ip" : "192.168.1.50",
+      "imu_data_port": 56401,
+      "log_data_ip" : "",
+      "log_data_port": 56501
+    }
+  },
+  "lidar_configs" : [
+    {
+      "ip" : "192.168.1.3",
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    }
+  ]
+}
+
diff --git a/livox_ros_driver2/config/display_point_cloud_ROS1.rviz b/livox_ros_driver2/config/display_point_cloud_ROS1.rviz
new file mode 100644
index 0000000..6239170
--- /dev/null
+++ b/livox_ros_driver2/config/display_point_cloud_ROS1.rviz
@@ -0,0 +1,172 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /PointCloud21
+      Splitter Ratio: 0.500694990158081
+    Tree Height: 728
+  - 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: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 0.4569999873638153
+        Min Value: -0.367000013589859
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 1
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 2
+      Size (m): 0.004999999888241291
+      Style: Points
+      Topic: /livox/lidar
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: livox_frame
+    Frame Rate: 40
+  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: 25.80008888244629
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.2672550082206726
+        Y: 0.061853598803281784
+        Z: 0.15087400376796722
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5597995519638062
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.065610408782959
+    Saved:
+      - Class: rviz/Orbit
+        Distance: 10
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Focal Point:
+          X: 0
+          Y: 0
+          Z: 0
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: Orbit
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 1.1103999614715576
+        Target Frame: <Fixed Frame>
+        Value: Orbit (rviz)
+        Yaw: 0.5703970193862915
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1025
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001c400000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005730000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1853
+  X: 67
+  Y: 27
diff --git a/livox_ros_driver2/config/display_point_cloud_ROS2.rviz b/livox_ros_driver2/config/display_point_cloud_ROS2.rviz
new file mode 100644
index 0000000..c044254
--- /dev/null
+++ b/livox_ros_driver2/config/display_point_cloud_ROS2.rviz
@@ -0,0 +1,137 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 796
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0.20000000298023224
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 150
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /livox/lidar
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: livox_frame
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - 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
+      Topic: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 14.215983390808105
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.9503982067108154
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 2.6603963375091553
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1025
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015f000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c3000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1853
+  X: 67
+  Y: 27
diff --git a/livox_ros_driver2/config/mixed_HAP_MID360_config.json b/livox_ros_driver2/config/mixed_HAP_MID360_config.json
new file mode 100644
index 0000000..0539b1e
--- /dev/null
+++ b/livox_ros_driver2/config/mixed_HAP_MID360_config.json
@@ -0,0 +1,76 @@
+{
+  "lidar_summary_info" : {
+    "lidar_type": 8
+  },
+  "HAP": {
+    "lidar_net_info" : {
+      "cmd_data_port": 56000,
+      "push_msg_port": 0,
+      "point_data_port": 57000,
+      "imu_data_port": 58000,
+      "log_data_port": 59000
+    },
+    "host_net_info" : {
+      "cmd_data_ip" : "192.168.1.5",
+      "cmd_data_port": 56000,
+      "push_msg_ip": "",
+      "push_msg_port": 0,
+      "point_data_ip": "192.168.1.5",
+      "point_data_port": 57000,
+      "imu_data_ip" : "192.168.1.5",
+      "imu_data_port": 58000,
+      "log_data_ip" : "",
+      "log_data_port": 59000
+    }
+  },
+  "MID360": {
+    "lidar_net_info" : {
+      "cmd_data_port": 56100,
+      "push_msg_port": 56200,
+      "point_data_port": 56300,
+      "imu_data_port": 56400,
+      "log_data_port": 56500
+    },
+    "host_net_info" : {
+      "cmd_data_ip" : "192.168.1.5",
+      "cmd_data_port": 56101,
+      "push_msg_ip": "192.168.1.5",
+      "push_msg_port": 56201,
+      "point_data_ip": "192.168.1.5",
+      "point_data_port": 56301,
+      "imu_data_ip" : "192.168.1.5",
+      "imu_data_port": 56401,
+      "log_data_ip" : "",
+      "log_data_port": 56501
+    }
+  },
+  "lidar_configs" : [
+    {
+      "ip" : "192.168.1.100",
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    },
+    {
+      "ip" : "192.168.1.12",
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    }
+  ]
+}
+
diff --git a/livox_ros_driver2/launch_ROS2/msg_HAP_launch.py b/livox_ros_driver2/launch_ROS2/msg_HAP_launch.py
new file mode 100644
index 0000000..2ff3d66
--- /dev/null
+++ b/livox_ros_driver2/launch_ROS2/msg_HAP_launch.py
@@ -0,0 +1,55 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch
+
+################### user configure parameters for ros2 start ###################
+xfer_format   = 1    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'livox_frame'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+rviz_config_path = os.path.join(cur_config_path, 'livox_lidar.rviz')
+user_config_path = os.path.join(cur_config_path, 'HAP_config.json')
+################### user configure parameters for ros2 end #####################
+
+livox_ros2_params = [
+    {"xfer_format": xfer_format},
+    {"multi_topic": multi_topic},
+    {"data_src": data_src},
+    {"publish_freq": publish_freq},
+    {"output_data_type": output_type},
+    {"frame_id": frame_id},
+    {"lvx_file_path": lvx_file_path},
+    {"user_config_path": user_config_path},
+    {"cmdline_input_bd_code": cmdline_bd_code}
+]
+
+
+def generate_launch_description():
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    return LaunchDescription([
+        livox_driver,
+        # launch.actions.RegisterEventHandler(
+        #     event_handler=launch.event_handlers.OnProcessExit(
+        #         target_action=livox_rviz,
+        #         on_exit=[
+        #             launch.actions.EmitEvent(event=launch.events.Shutdown()),
+        #         ]
+        #     )
+        # )
+    ])
diff --git a/livox_ros_driver2/launch_ROS2/msg_MID360_launch.py b/livox_ros_driver2/launch_ROS2/msg_MID360_launch.py
new file mode 100644
index 0000000..8492c49
--- /dev/null
+++ b/livox_ros_driver2/launch_ROS2/msg_MID360_launch.py
@@ -0,0 +1,54 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch
+
+################### user configure parameters for ros2 start ###################
+xfer_format   = 1    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'livox_frame'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+user_config_path = os.path.join(cur_config_path, 'MID360_config.json')
+################### user configure parameters for ros2 end #####################
+
+livox_ros2_params = [
+    {"xfer_format": xfer_format},
+    {"multi_topic": multi_topic},
+    {"data_src": data_src},
+    {"publish_freq": publish_freq},
+    {"output_data_type": output_type},
+    {"frame_id": frame_id},
+    {"lvx_file_path": lvx_file_path},
+    {"user_config_path": user_config_path},
+    {"cmdline_input_bd_code": cmdline_bd_code}
+]
+
+
+def generate_launch_description():
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    return LaunchDescription([
+        livox_driver,
+        # launch.actions.RegisterEventHandler(
+        #     event_handler=launch.event_handlers.OnProcessExit(
+        #         target_action=livox_rviz,
+        #         on_exit=[
+        #             launch.actions.EmitEvent(event=launch.events.Shutdown()),
+        #         ]
+        #     )
+        # )
+    ])
diff --git a/livox_ros_driver2/launch_ROS2/rviz_HAP_launch.py b/livox_ros_driver2/launch_ROS2/rviz_HAP_launch.py
new file mode 100644
index 0000000..834a545
--- /dev/null
+++ b/livox_ros_driver2/launch_ROS2/rviz_HAP_launch.py
@@ -0,0 +1,63 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch
+
+################### user configure parameters for ros2 start ###################
+xfer_format   = 0    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'livox_frame'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz')
+user_config_path = os.path.join(cur_config_path, 'HAP_config.json')
+################### user configure parameters for ros2 end #####################
+
+livox_ros2_params = [
+    {"xfer_format": xfer_format},
+    {"multi_topic": multi_topic},
+    {"data_src": data_src},
+    {"publish_freq": publish_freq},
+    {"output_data_type": output_type},
+    {"frame_id": frame_id},
+    {"lvx_file_path": lvx_file_path},
+    {"user_config_path": user_config_path},
+    {"cmdline_input_bd_code": cmdline_bd_code}
+]
+
+
+def generate_launch_description():
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    livox_rviz = Node(
+            package='rviz2',
+            executable='rviz2',
+            output='screen',
+            arguments=['--display-config', rviz_config_path]
+        )
+
+    return LaunchDescription([
+        livox_driver,
+        livox_rviz,
+        # launch.actions.RegisterEventHandler(
+        #     event_handler=launch.event_handlers.OnProcessExit(
+        #         target_action=livox_rviz,
+        #         on_exit=[
+        #             launch.actions.EmitEvent(event=launch.events.Shutdown()),
+        #         ]
+        #     )
+        # )
+    ])
diff --git a/livox_ros_driver2/launch_ROS2/rviz_MID360_launch.py b/livox_ros_driver2/launch_ROS2/rviz_MID360_launch.py
new file mode 100644
index 0000000..31d3480
--- /dev/null
+++ b/livox_ros_driver2/launch_ROS2/rviz_MID360_launch.py
@@ -0,0 +1,63 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch
+
+################### user configure parameters for ros2 start ###################
+xfer_format   = 0    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'livox_frame'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz')
+user_config_path = os.path.join(cur_config_path, 'MID360_config.json')
+################### user configure parameters for ros2 end #####################
+
+livox_ros2_params = [
+    {"xfer_format": xfer_format},
+    {"multi_topic": multi_topic},
+    {"data_src": data_src},
+    {"publish_freq": publish_freq},
+    {"output_data_type": output_type},
+    {"frame_id": frame_id},
+    {"lvx_file_path": lvx_file_path},
+    {"user_config_path": user_config_path},
+    {"cmdline_input_bd_code": cmdline_bd_code}
+]
+
+
+def generate_launch_description():
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    livox_rviz = Node(
+            package='rviz2',
+            executable='rviz2',
+            output='screen',
+            arguments=['--display-config', rviz_config_path]
+        )
+
+    return LaunchDescription([
+        livox_driver,
+        livox_rviz,
+        # launch.actions.RegisterEventHandler(
+        #     event_handler=launch.event_handlers.OnProcessExit(
+        #         target_action=livox_rviz,
+        #         on_exit=[
+        #             launch.actions.EmitEvent(event=launch.events.Shutdown()),
+        #         ]
+        #     )
+        # )
+    ])
diff --git a/livox_ros_driver2/launch_ROS2/rviz_mixed.py b/livox_ros_driver2/launch_ROS2/rviz_mixed.py
new file mode 100644
index 0000000..46a39c7
--- /dev/null
+++ b/livox_ros_driver2/launch_ROS2/rviz_mixed.py
@@ -0,0 +1,63 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch
+
+################### user configure parameters for ros2 start ###################
+xfer_format   = 0    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'livox_frame'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz')
+user_config_path = os.path.join(cur_config_path, 'mixed_HAP_MID360_config.json')
+################### user configure parameters for ros2 end #####################
+
+livox_ros2_params = [
+    {"xfer_format": xfer_format},
+    {"multi_topic": multi_topic},
+    {"data_src": data_src},
+    {"publish_freq": publish_freq},
+    {"output_data_type": output_type},
+    {"frame_id": frame_id},
+    {"lvx_file_path": lvx_file_path},
+    {"user_config_path": user_config_path},
+    {"cmdline_input_bd_code": cmdline_bd_code}
+]
+
+
+def generate_launch_description():
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    livox_rviz = Node(
+            package='rviz2',
+            executable='rviz2',
+            output='screen',
+            arguments=['--display-config', rviz_config_path]
+        )
+
+    return LaunchDescription([
+        livox_driver,
+        livox_rviz,
+        # launch.actions.RegisterEventHandler(
+        #     event_handler=launch.event_handlers.OnProcessExit(
+        #         target_action=livox_rviz,
+        #         on_exit=[
+        #             launch.actions.EmitEvent(event=launch.events.Shutdown()),
+        #         ]
+        #     )
+        # )
+    ])
diff --git a/livox_ros_driver2/msg/CustomMsg.msg b/livox_ros_driver2/msg/CustomMsg.msg
new file mode 100644
index 0000000..91f044b
--- /dev/null
+++ b/livox_ros_driver2/msg/CustomMsg.msg
@@ -0,0 +1,9 @@
+# Livox publish pointcloud msg format.
+
+std_msgs/Header header    # ROS standard message header
+uint64 timebase           # The time of first point
+uint32 point_num          # Total number of pointclouds
+uint8  lidar_id           # Lidar device id number
+uint8[3]  rsvd            # Reserved use
+CustomPoint[] points      # Pointcloud data
+
diff --git a/livox_ros_driver2/msg/CustomPoint.msg b/livox_ros_driver2/msg/CustomPoint.msg
new file mode 100644
index 0000000..34719b2
--- /dev/null
+++ b/livox_ros_driver2/msg/CustomPoint.msg
@@ -0,0 +1,10 @@
+# Livox costom pointcloud format.
+
+uint32 offset_time      # offset time relative to the base time
+float32 x               # X axis, unit:m
+float32 y               # Y axis, unit:m
+float32 z               # Z axis, unit:m
+uint8 reflectivity      # reflectivity, 0~255
+uint8 tag               # livox tag
+uint8 line              # laser number in lidar
+
diff --git a/livox_ros_driver2/package_ROS2.xml b/livox_ros_driver2/package_ROS2.xml
new file mode 100644
index 0000000..96f5762
--- /dev/null
+++ b/livox_ros_driver2/package_ROS2.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>livox_ros_driver2</name>
+  <version>1.0.0</version>
+  <description>The ROS device driver for Livox 3D LiDARs, for ROS2</description>
+  <maintainer email="dev@livoxtech.com">feng</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rcutils</depend>
+  <depend>pcl_conversions</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>libpcl-all-dev</depend>
+
+  <exec_depend>rosbag2</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>git</depend>
+  <depend>apr</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/livox_ros_driver2/src/call_back/lidar_common_callback.cpp b/livox_ros_driver2/src/call_back/lidar_common_callback.cpp
new file mode 100644
index 0000000..43efb24
--- /dev/null
+++ b/livox_ros_driver2/src/call_back/lidar_common_callback.cpp
@@ -0,0 +1,73 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "lidar_common_callback.h"
+
+#include "../lds_lidar.h"
+
+#include <string>
+
+namespace livox_ros {
+
+void LidarCommonCallback::OnLidarPointClounCb(PointFrame* frame, void* client_data) {
+  if (frame == nullptr) {
+    printf("LidarPointCloudCb frame is nullptr.\n");
+    return;
+  }
+
+  if (client_data == nullptr) {
+    printf("Lidar point cloud cb failed, client data is nullptr.\n");
+    return;
+  }
+
+  if (frame->lidar_num ==0) {
+    printf("LidarPointCloudCb lidar_num:%u.\n", frame->lidar_num);
+    return;
+  }
+
+  LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
+  
+  //printf("Lidar point cloud, lidar_num:%u.\n", frame->lidar_num);
+
+  lds_lidar->StoragePointData(frame);
+}
+
+void LidarCommonCallback::LidarImuDataCallback(ImuData* imu_data, void *client_data) {
+  if (imu_data == nullptr) {
+    printf("Imu data is nullptr.\n");
+    return;
+  }
+  if (client_data == nullptr) {
+    printf("Lidar point cloud cb failed, client data is nullptr.\n");
+    return;
+  }
+
+  LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
+  lds_lidar->StorageImuData(imu_data);
+}
+
+} // namespace livox_ros
+
+
+
diff --git a/livox_ros_driver2/src/call_back/lidar_common_callback.h b/livox_ros_driver2/src/call_back/lidar_common_callback.h
new file mode 100644
index 0000000..4ff6803
--- /dev/null
+++ b/livox_ros_driver2/src/call_back/lidar_common_callback.h
@@ -0,0 +1,40 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_
+#define LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_	
+
+#include "comm/comm.h"
+
+namespace livox_ros {
+
+class LidarCommonCallback {
+ public:
+  static void OnLidarPointClounCb(PointFrame* frame, void* client_data);
+  static void LidarImuDataCallback(ImuData* imu_data, void *client_data);
+};
+
+} // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_
diff --git a/livox_ros_driver2/src/call_back/livox_lidar_callback.cpp b/livox_ros_driver2/src/call_back/livox_lidar_callback.cpp
new file mode 100644
index 0000000..60bb237
--- /dev/null
+++ b/livox_ros_driver2/src/call_back/livox_lidar_callback.cpp
@@ -0,0 +1,333 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "livox_lidar_callback.h"
+
+#include <string>
+#include <thread>
+#include <iostream>
+
+namespace livox_ros {
+
+void LivoxLidarCallback::LidarInfoChangeCallback(const uint32_t handle,
+                                           const LivoxLidarInfo* info,
+                                           void* client_data) {
+  if (client_data == nullptr) {
+    std::cout << "lidar info change callback failed, client data is nullptr" << std::endl;
+    return;
+  }
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+
+  LidarDevice* lidar_device = GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "found lidar not defined in the user-defined config, ip: " << IpNumToString(handle) << std::endl;
+    // add lidar device
+    uint8_t index = 0;
+    int8_t ret = lds_lidar->cache_index_.GetFreeIndex(kLivoxLidarType, handle, index);
+    if (ret != 0) {
+      std::cout << "failed to add lidar device, lidar ip: " << IpNumToString(handle) << std::endl;
+      return;
+    }
+    LidarDevice *p_lidar = &(lds_lidar->lidars_[index]);
+    p_lidar->lidar_type = kLivoxLidarType;
+  } else {
+    // set the lidar according to the user-defined config
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+
+    // lock for modify the lidar device set_bits
+    {
+      std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
+      if (config.pcl_data_type != -1 ) {
+        lidar_device->livox_config.set_bits |= kConfigDataType;
+        SetLivoxLidarPclDataType(handle, static_cast<LivoxLidarPointDataType>(config.pcl_data_type),
+                                LivoxLidarCallback::SetDataTypeCallback, lds_lidar);
+        std::cout << "set pcl data type, handle: " << handle << ", data type: "
+                  << static_cast<int32_t>(config.pcl_data_type) << std::endl;
+      }
+      if (config.pattern_mode != -1) {
+        lidar_device->livox_config.set_bits |= kConfigScanPattern;
+        SetLivoxLidarScanPattern(handle, static_cast<LivoxLidarScanPattern>(config.pattern_mode),
+                              LivoxLidarCallback::SetPatternModeCallback, lds_lidar);
+        std::cout << "set scan pattern, handle: " << handle << ", scan pattern: "
+                  << static_cast<int32_t>(config.pattern_mode) << std::endl;
+      }
+      if (config.blind_spot_set != -1) {
+        lidar_device->livox_config.set_bits |= kConfigBlindSpot;
+        SetLivoxLidarBlindSpot(handle, config.blind_spot_set,
+                              LivoxLidarCallback::SetBlindSpotCallback, lds_lidar);
+
+        std::cout << "set blind spot, handle: " << handle << ", blind spot distance: "
+                  << config.blind_spot_set << std::endl;
+      }
+      if (config.dual_emit_en != -1) {
+        lidar_device->livox_config.set_bits |= kConfigDualEmit;
+        SetLivoxLidarDualEmit(handle, (config.dual_emit_en == 0 ? false : true),
+                              LivoxLidarCallback::SetDualEmitCallback, lds_lidar);
+        std::cout << "set dual emit mode, handle: " << handle << ", enable dual emit: "
+                  << static_cast<int32_t>(config.dual_emit_en) << std::endl;
+      }
+    } // free lock for set_bits
+
+    // set extrinsic params into lidar
+    LivoxLidarInstallAttitude attitude {
+      config.extrinsic_param.roll,
+      config.extrinsic_param.pitch,
+      config.extrinsic_param.yaw,
+      config.extrinsic_param.x,
+      config.extrinsic_param.y,
+      config.extrinsic_param.z
+    };
+    SetLivoxLidarInstallAttitude(config.handle, &attitude,
+                                 LivoxLidarCallback::SetAttitudeCallback, lds_lidar);
+  }
+
+  std::cout << "begin to change work mode to 'Normal', handle: " << handle << std::endl;
+  SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr);
+  EnableLivoxLidarImuData(handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar);
+  return;
+}
+
+void LivoxLidarCallback::WorkModeChangedCallback(livox_status status,
+                                                 uint32_t handle,
+                                                 LivoxLidarAsyncControlResponse *response,
+                                                 void *client_data) {
+  if (status != kLivoxLidarStatusSuccess) {
+    std::cout << "failed to change work mode, handle: " << handle << ", try again..."<< std::endl;
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+    SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr);
+    return;
+  }
+  std::cout << "successfully change work mode, handle: " << handle << std::endl;
+  return;
+}
+
+void LivoxLidarCallback::SetDataTypeCallback(livox_status status, uint32_t handle,
+                                             LivoxLidarAsyncControlResponse *response,
+                                             void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set data type since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+
+  if (status == kLivoxLidarStatusSuccess) {
+    std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
+    lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDataType));
+    if (!lidar_device->livox_config.set_bits) {
+      lidar_device->connect_state = kConnectStateSampling;
+    }
+    std::cout << "successfully set data type, handle: " << handle
+              << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+    SetLivoxLidarPclDataType(handle, static_cast<LivoxLidarPointDataType>(config.pcl_data_type),
+                             LivoxLidarCallback::SetDataTypeCallback, client_data);
+    std::cout << "set data type timeout, handle: " << handle
+              << ", try again..." << std::endl;
+  } else {
+    std::cout << "failed to set data type, handle: " << handle
+              << ", return code: " << response->ret_code
+              << ", error key: " << response->error_key << std::endl;
+  }
+  return;
+}
+
+void LivoxLidarCallback::SetPatternModeCallback(livox_status status, uint32_t handle,
+                                                LivoxLidarAsyncControlResponse *response,
+                                                void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set pattern mode since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+
+  if (status == kLivoxLidarStatusSuccess) {
+    std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
+    lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigScanPattern));
+    if (!lidar_device->livox_config.set_bits) {
+      lidar_device->connect_state = kConnectStateSampling;
+    }
+    std::cout << "successfully set pattern mode, handle: " << handle
+              << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+    SetLivoxLidarScanPattern(handle, static_cast<LivoxLidarScanPattern>(config.pattern_mode),
+                             LivoxLidarCallback::SetPatternModeCallback, client_data);
+    std::cout << "set pattern mode timeout, handle: " << handle
+              << ", try again..." << std::endl;
+  } else {
+    std::cout << "failed to set pattern mode, handle: " << handle
+              << ", return code: " << response->ret_code
+              << ", error key: " << response->error_key << std::endl;
+  }
+  return;
+}
+
+void LivoxLidarCallback::SetBlindSpotCallback(livox_status status, uint32_t handle,
+                                              LivoxLidarAsyncControlResponse *response,
+                                              void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set blind spot since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+
+  if (status == kLivoxLidarStatusSuccess) {
+    std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
+    lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigBlindSpot));
+    if (!lidar_device->livox_config.set_bits) {
+      lidar_device->connect_state = kConnectStateSampling;
+    }
+    std::cout << "successfully set blind spot, handle: " << handle
+              << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+    SetLivoxLidarBlindSpot(handle, config.blind_spot_set,
+                           LivoxLidarCallback::SetBlindSpotCallback, client_data);
+    std::cout << "set blind spot timeout, handle: " << handle
+              << ", try again..." << std::endl;
+  } else {
+    std::cout << "failed to set blind spot, handle: " << handle
+              << ", return code: " << response->ret_code
+              << ", error key: " << response->error_key << std::endl;
+  }
+  return;
+}
+
+void LivoxLidarCallback::SetDualEmitCallback(livox_status status, uint32_t handle,
+                                             LivoxLidarAsyncControlResponse *response,
+                                             void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set dual emit mode since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+  if (status == kLivoxLidarStatusSuccess) {
+    std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
+    lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDualEmit));
+    if (!lidar_device->livox_config.set_bits) {
+      lidar_device->connect_state = kConnectStateSampling;
+    }
+    std::cout << "successfully set dual emit mode, handle: " << handle
+              << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+    SetLivoxLidarDualEmit(handle, config.dual_emit_en,
+                          LivoxLidarCallback::SetDualEmitCallback, client_data);
+    std::cout << "set dual emit mode timeout, handle: " << handle
+              << ", try again..." << std::endl;
+  } else {
+    std::cout << "failed to set dual emit mode, handle: " << handle
+              << ", return code: " << response->ret_code
+              << ", error key: " << response->error_key << std::endl;
+  }
+  return;
+}
+
+void LivoxLidarCallback::SetAttitudeCallback(livox_status status, uint32_t handle,
+                                             LivoxLidarAsyncControlResponse *response,
+                                             void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set dual emit mode since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+  if (status == kLivoxLidarStatusSuccess) {
+    std::cout << "successfully set lidar attitude, ip: " << IpNumToString(handle) << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    std::cout << "set lidar attitude timeout, ip: " << IpNumToString(handle)
+              << ", try again..." << std::endl;
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+    LivoxLidarInstallAttitude attitude {
+      config.extrinsic_param.roll,
+      config.extrinsic_param.pitch,
+      config.extrinsic_param.yaw,
+      config.extrinsic_param.x,
+      config.extrinsic_param.y,
+      config.extrinsic_param.z
+    };
+    SetLivoxLidarInstallAttitude(config.handle, &attitude,
+                                 LivoxLidarCallback::SetAttitudeCallback, lds_lidar);
+  } else {
+    std::cout << "failed to set lidar attitude, ip: " << IpNumToString(handle) << std::endl;
+  }
+}
+
+void LivoxLidarCallback::EnableLivoxLidarImuDataCallback(livox_status status, uint32_t handle,
+                                                         LivoxLidarAsyncControlResponse *response,
+                                                         void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set pattern mode since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+
+  if (response == nullptr) {
+    std::cout << "failed to get response since no lidar IMU sensor found, handle: "
+              << handle << std::endl;
+    return;
+  }
+
+  if (status == kLivoxLidarStatusSuccess) {
+    std::cout << "successfully enable Livox Lidar imu, ip: " << IpNumToString(handle) << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    std::cout << "enable Livox Lidar imu timeout, ip: " << IpNumToString(handle)
+              << ", try again..." << std::endl;
+    EnableLivoxLidarImuData(handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar);
+  } else {
+    std::cout << "failed to enable Livox Lidar imu, ip: " << IpNumToString(handle) << std::endl;
+  }
+}
+
+LidarDevice* LivoxLidarCallback::GetLidarDevice(const uint32_t handle, void* client_data) {
+  if (client_data == nullptr) {
+    std::cout << "failed to get lidar device, client data is nullptr" << std::endl;
+    return nullptr;
+  }
+
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+  uint8_t index = 0;
+  int8_t ret = lds_lidar->cache_index_.GetIndex(kLivoxLidarType, handle, index);
+  if (ret != 0) {
+    return nullptr;
+  }
+
+  return &(lds_lidar->lidars_[index]);
+}
+
+} // namespace livox_ros
diff --git a/livox_ros_driver2/src/call_back/livox_lidar_callback.h b/livox_ros_driver2/src/call_back/livox_lidar_callback.h
new file mode 100644
index 0000000..1b6b549
--- /dev/null
+++ b/livox_ros_driver2/src/call_back/livox_lidar_callback.h
@@ -0,0 +1,71 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_
+#define LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_
+
+#include "../lds.h"
+#include "../lds_lidar.h"
+#include "../comm/comm.h"
+
+#include "livox_lidar_api.h"
+#include "livox_lidar_def.h"
+
+namespace livox_ros {
+
+class LivoxLidarCallback {
+ public:
+  static void LidarInfoChangeCallback(const uint32_t handle,
+                                      const LivoxLidarInfo* info,
+                                      void* client_data);
+  static void WorkModeChangedCallback(livox_status status,
+                                      uint32_t handle,
+                                      LivoxLidarAsyncControlResponse *response,
+                                      void *client_data);
+  static void SetDataTypeCallback(livox_status status, uint32_t handle,
+                                  LivoxLidarAsyncControlResponse *response,
+                                  void *client_data);
+  static void SetPatternModeCallback(livox_status status, uint32_t handle,
+                                     LivoxLidarAsyncControlResponse *response,
+                                     void *client_data);
+  static void SetBlindSpotCallback(livox_status status, uint32_t handle,
+                                   LivoxLidarAsyncControlResponse *response,
+                                   void *client_data);
+  static void SetDualEmitCallback(livox_status status, uint32_t handle,
+                                  LivoxLidarAsyncControlResponse *response,
+                                  void *client_data);
+  static void SetAttitudeCallback(livox_status status, uint32_t handle,
+                                  LivoxLidarAsyncControlResponse *response,
+                                  void *client_data);
+  static void EnableLivoxLidarImuDataCallback(livox_status status, uint32_t handle,
+                                  LivoxLidarAsyncControlResponse *response,
+                                  void *client_data);
+
+ private:
+  static LidarDevice* GetLidarDevice(const uint32_t handle, void* client_data);
+};
+
+} // namespace livox_ros
+
+#endif  // LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_
diff --git a/livox_ros_driver2/src/comm/cache_index.cpp b/livox_ros_driver2/src/comm/cache_index.cpp
new file mode 100644
index 0000000..33e6500
--- /dev/null
+++ b/livox_ros_driver2/src/comm/cache_index.cpp
@@ -0,0 +1,121 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "cache_index.h"
+#include "livox_lidar_def.h"
+
+namespace livox_ros {
+
+CacheIndex::CacheIndex() {
+  std::array<bool, kMaxLidarCount> index_cache = {0};
+  index_cache_.swap(index_cache);
+}
+
+int8_t CacheIndex::GetFreeIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) {
+  std::string key;
+  int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key);
+  if (ret != 0) {
+    return -1;
+  }
+  {
+    std::lock_guard<std::mutex> lock(index_mutex_);
+    if (map_index_.find(key) != map_index_.end()) {
+      index = map_index_[key];
+      return 0;
+    }
+  }
+
+  {
+    printf("GetFreeIndex key:%s.\n", key.c_str());
+    std::lock_guard<std::mutex> lock(index_mutex_);
+    for (size_t i = 0; i < kMaxSourceLidar; ++i) {
+      if (!index_cache_[i]) {
+        index_cache_[i] = 1;
+        map_index_[key] = static_cast<uint8_t>(i);
+        index = static_cast<uint8_t>(i);
+        return 0;
+      }
+    }
+  }
+  return -1;
+}
+
+int8_t CacheIndex::GenerateIndexKey(const uint8_t livox_lidar_type, const uint32_t handle, std::string& key) {
+  if (livox_lidar_type == kLivoxLidarType) {
+    key = "livox_lidar_" + std::to_string(handle);
+  } else {
+    printf("Can not generate index, the livox lidar type is unknown, the livox lidar type:%u\n", livox_lidar_type);
+    return -1;
+  }
+  return 0;
+}
+
+int8_t CacheIndex::GetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) {
+  std::string key;
+  int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key);
+  if (ret != 0) {
+    return -1;
+  }
+
+  if (map_index_.find(key) != map_index_.end()) {
+    std::lock_guard<std::mutex> lock(index_mutex_);
+    index = map_index_[key];
+    return 0;
+  }
+  printf("Can not get index, the livox lidar type:%u, handle:%u\n", livox_lidar_type, handle);
+  return -1;
+}
+
+int8_t CacheIndex::LvxGetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) {
+  std::string key;
+  int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key);
+  if (ret != 0) {
+    return -1;
+  }
+
+  if (map_index_.find(key) != map_index_.end()) {
+    index = map_index_[key];
+    return 0;
+  }
+
+  return GetFreeIndex(livox_lidar_type, handle, index);
+}
+
+void CacheIndex::ResetIndex(LidarDevice *lidar) {
+  std::string key;
+  int8_t ret = GenerateIndexKey(lidar->lidar_type, lidar->handle, key);
+  if (ret != 0) {
+    printf("Reset index failed, can not generate index key, lidar type:%u, handle:%u.\n", lidar->lidar_type, lidar->handle);
+    return;
+  }
+
+  if (map_index_.find(key) != map_index_.end()) {
+    uint8_t index = map_index_[key];
+    std::lock_guard<std::mutex> lock(index_mutex_);
+    map_index_.erase(key);
+    index_cache_[index] = 0;
+  }
+}
+
+} // namespace
diff --git a/livox_ros_driver2/src/comm/cache_index.h b/livox_ros_driver2/src/comm/cache_index.h
new file mode 100644
index 0000000..9451eda
--- /dev/null
+++ b/livox_ros_driver2/src/comm/cache_index.h
@@ -0,0 +1,54 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_CACHE_INDEX_H_
+#define LIVOX_ROS_DRIVER_CACHE_INDEX_H_
+
+#include <mutex>
+#include <array>
+#include <map>
+#include <string>
+
+#include "comm/comm.h"
+
+namespace livox_ros {
+
+class CacheIndex {
+ public:
+  CacheIndex();
+  int8_t GetFreeIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index);
+  int8_t GetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index);
+  int8_t GenerateIndexKey(const uint8_t livox_lidar_type, const uint32_t handle, std::string& key);
+  int8_t LvxGetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index);
+  void ResetIndex(LidarDevice *lidar);
+
+ private:
+  std::mutex index_mutex_;
+  std::map<std::string, uint8_t> map_index_; /* key:handle/slot, val:index */
+  std::array<bool, kMaxSourceLidar> index_cache_;
+};
+
+} // namespace livox_ros
+
+# endif // LIVOX_ROS_DRIVER_CACHE_INDEX_H_
diff --git a/livox_ros_driver2/src/comm/comm.cpp b/livox_ros_driver2/src/comm/comm.cpp
new file mode 100644
index 0000000..b3490cc
--- /dev/null
+++ b/livox_ros_driver2/src/comm/comm.cpp
@@ -0,0 +1,70 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "comm/comm.h"
+#include <string.h>
+#include <arpa/inet.h>
+
+namespace livox_ros {
+
+/** Common function --------------------------------------------------------- */
+bool IsFilePathValid(const char *path_str) {
+  int str_len = strlen(path_str);
+
+  if ((str_len > kPathStrMinSize) && (str_len < kPathStrMaxSize)) {
+    return true;
+  } else {
+    return false;
+  }
+}
+
+uint32_t CalculatePacketQueueSize(const double publish_freq) {
+  uint32_t queue_size = 10;
+  if (publish_freq > 10.0) {
+    queue_size = static_cast<uint32_t>(publish_freq) + 1;
+  }
+  return queue_size;
+}
+
+std::string IpNumToString(uint32_t ip_num) {
+  struct in_addr ip;
+  ip.s_addr = ip_num;
+  return std::string(inet_ntoa(ip));
+}
+
+uint32_t IpStringToNum(std::string ip_string) {
+  return static_cast<uint32_t>(inet_addr(ip_string.c_str()));
+}
+
+std::string ReplacePeriodByUnderline(std::string str) {
+  std::size_t pos = str.find(".");
+  while (pos != std::string::npos) {
+    str.replace(pos, 1, "_");
+    pos = str.find(".");
+  }
+  return str;
+}
+
+} // namespace livox_ros
+
diff --git a/livox_ros_driver2/src/comm/comm.h b/livox_ros_driver2/src/comm/comm.h
new file mode 100644
index 0000000..492b39e
--- /dev/null
+++ b/livox_ros_driver2/src/comm/comm.h
@@ -0,0 +1,304 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER2_COMM_H_
+#define LIVOX_ROS_DRIVER2_COMM_H_
+
+#include <math.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <iostream>
+#include <string>
+#include <vector>
+#include <memory>
+#include <map>
+
+#include "lidar_imu_data_queue.h"
+
+namespace livox_ros {
+
+/** Max lidar data source num */
+const uint8_t kMaxSourceLidar = 32;
+
+
+/** Eth packet relative info parama */
+const uint32_t kMaxPointPerEthPacket = 100;
+const uint32_t kMinEthPacketQueueSize = 32;     /**< must be 2^n */
+const uint32_t kMaxEthPacketQueueSize = 131072; /**< must be 2^n */
+const uint32_t kImuEthPacketQueueSize = 256;
+
+/** Max packet length according to Ethernet MTU */
+const uint32_t KEthPacketMaxLength = 1500;
+const uint32_t KEthPacketHeaderLength = 18;     /**< (sizeof(LivoxEthPacket) - 1) */
+const uint32_t KCartesianPointSize = 13;
+const uint32_t KSphericalPointSzie = 9;
+
+const uint64_t kRosTimeMax = 4294967296000000000; /**< 2^32 * 1000000000ns */
+const int64_t kPacketTimeGap = 1000000;           /**< 1ms = 1000000ns */
+/**< the threshold of packet continuous */
+const int64_t kMaxPacketTimeGap = 1700000;
+/**< the threshold of device disconect */
+const int64_t kDeviceDisconnectThreshold = 1000000000;
+const uint32_t kNsPerSecond = 1000000000; /**< 1s  = 1000000000ns */
+const uint32_t kNsTolerantFrameTimeDeviation = 1000000; /**< 1ms  = 1000000ns */
+const uint32_t kRatioOfMsToNs = 1000000; /**< 1ms  = 1000000ns */
+
+const int kPathStrMinSize = 4;   /**< Must more than 4 char */
+const int kPathStrMaxSize = 256; /**< Must less than 256 char */
+const int kBdCodeSize = 15;
+
+const uint32_t kPointXYZRSize = 16;
+const uint32_t kPointXYZRTRSize = 18;
+
+const double PI = 3.14159265358979323846;
+
+constexpr uint32_t kMaxBufferSize = 0x8000;  // 32k bytes
+
+/** Device Line Number **/
+const uint8_t kLineNumberDefault = 1;
+const uint8_t kLineNumberMid360 = 4;
+const uint8_t kLineNumberHAP = 6;    
+
+// SDK related
+typedef enum {
+  kIndustryLidarType = 1,
+  kVehicleLidarType = 2,
+  kDirectLidarType = 4,
+  kLivoxLidarType = 8
+} LidarProtoType;
+
+// SDK related
+/** Timestamp sync mode define. */
+typedef enum {
+  kTimestampTypeNoSync = 0, /**< No sync signal mode. */
+  kTimestampTypeGptpOrPtp = 1,    /**< gPTP or PTP sync mode */
+  kTimestampTypeGps = 2   /**< GPS sync mode. */
+} TimestampType;
+
+/** Lidar connect state */
+typedef enum {
+  kConnectStateOff = 0,
+  kConnectStateOn = 1,
+  kConnectStateConfig = 2,
+  kConnectStateSampling = 3,
+} LidarConnectState;
+
+/** Device data source type */
+typedef enum {
+  kSourceRawLidar = 0, /**< Data from raw lidar. */
+  kSourceRawHub = 1,   /**< Data from lidar hub. */
+  kSourceLvxFile,      /**< Data from parse lvx file. */
+  kSourceUndef,
+} LidarDataSourceType;
+
+typedef enum { kCoordinateCartesian = 0, kCoordinateSpherical } CoordinateType;
+
+typedef enum {
+  kConfigDataType = 1 << 0,
+  kConfigScanPattern = 1 << 1,
+  kConfigBlindSpot = 1 << 2,
+  kConfigDualEmit = 1 << 3,
+  kConfigUnknown
+} LivoxLidarConfigCodeBit;
+
+typedef enum {
+  kNoneExtrinsicParameter,
+  kExtrinsicParameterFromLidar,
+  kExtrinsicParameterFromXml
+} ExtrinsicParameterType;
+
+typedef struct {
+ uint8_t lidar_type {};
+} LidarSummaryInfo;
+
+/** 8bytes stamp to uint64_t stamp */
+typedef union {
+  struct {
+    uint32_t low;
+    uint32_t high;
+  } stamp_word;
+
+  uint8_t stamp_bytes[8];
+  int64_t stamp;
+} LdsStamp;
+
+#pragma pack(1)
+
+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     */
+  double timestamp;   /**< Timestamp of point*/
+} LivoxPointXyzrtlt;
+
+typedef struct {
+  float x;
+  float y;
+  float z;
+  float intensity;
+  uint8_t tag;
+  uint8_t line;
+  uint64_t offset_time;
+} PointXyzlt;
+
+typedef struct {
+  uint32_t handle;
+  uint8_t lidar_type; ////refer to LivoxLidarType
+  uint32_t points_num;
+  PointXyzlt* points;
+} PointPacket;
+
+typedef struct {
+  uint64_t base_time[kMaxSourceLidar] {};
+  uint8_t lidar_num {};
+  PointPacket lidar_point[kMaxSourceLidar] {};
+} PointFrame;
+
+#pragma pack()
+
+typedef struct {
+  LidarProtoType lidar_type;
+  uint32_t handle;
+  uint64_t base_time;
+  uint32_t points_num;
+  std::vector<PointXyzlt> points;
+} StoragePacket;
+
+typedef struct {
+  LidarProtoType lidar_type;
+  uint32_t handle;
+  bool extrinsic_enable;
+  uint32_t point_num;
+  uint8_t data_type;
+  uint8_t line_num;
+  uint64_t time_stamp;
+  uint64_t point_interval;
+  std::vector<uint8_t> raw_data;
+} RawPacket;
+
+typedef struct {
+  StoragePacket *storage_packet;
+  volatile uint32_t rd_idx;
+  volatile uint32_t wr_idx;
+  uint32_t mask;
+  uint32_t size; /**< must be power of 2. */
+} LidarDataQueue;
+
+/*****************************/
+/* About Extrinsic Parameter */
+typedef struct {
+  float roll;  /**< Roll angle, unit: degree. */
+  float pitch; /**< Pitch angle, unit: degree. */
+  float yaw;   /**< Yaw angle, unit: degree. */
+  int32_t x;   /**< X translation, unit: mm. */
+  int32_t y;   /**< Y translation, unit: mm. */
+  int32_t z;   /**< Z translation, unit: mm. */
+} ExtParameter;
+
+typedef float TranslationVector[3]; /**< x, y, z translation, unit: mm. */
+typedef float RotationMatrix[3][3];
+
+typedef struct {
+  TranslationVector trans;
+  RotationMatrix rotation;
+} ExtParameterDetailed;
+
+typedef struct {
+  LidarProtoType lidar_type;
+  uint32_t handle;
+  ExtParameter param;
+} LidarExtParameter;
+
+/** Configuration in json config file for livox lidar */
+typedef struct {
+  char broadcast_code[16];
+  bool enable_connect;
+  bool enable_fan;
+  uint32_t return_mode;
+  uint32_t coordinate;
+  uint32_t imu_rate;
+  uint32_t extrinsic_parameter_source;
+  bool enable_high_sensitivity;
+} UserRawConfig;
+
+typedef struct {
+  bool enable_fan;
+  uint32_t return_mode;
+  uint32_t coordinate;              /**< 0 for CartesianCoordinate; others for SphericalCoordinate. */
+  uint32_t imu_rate;
+  uint32_t extrinsic_parameter_source;
+  bool enable_high_sensitivity;
+  volatile uint32_t set_bits;
+  volatile uint32_t get_bits;
+} UserConfig;
+
+typedef struct {
+  uint32_t handle;
+  int8_t pcl_data_type;
+  int8_t pattern_mode;
+  int32_t blind_spot_set;
+  int8_t dual_emit_en;
+  ExtParameter extrinsic_param;
+  volatile uint32_t set_bits;
+  volatile uint32_t get_bits;
+} UserLivoxLidarConfig;
+
+/** Lidar data source info abstract */
+typedef struct {
+  uint8_t lidar_type;
+  uint32_t handle;
+  // union {
+  //   uint8_t slot : 4; //slot for LivoxLidarType::kVehicleLidarType
+  //   uint8_t handle : 4;  // handle for LivoxLidarType::kIndustryLidarType
+  // };
+  uint8_t data_src;                  /**< From raw lidar or livox file. */
+  volatile LidarConnectState connect_state;
+  // DeviceInfo info;
+
+  LidarDataQueue data;
+  LidarImuDataQueue imu_data;
+
+  uint32_t firmware_ver; /**< Firmware version of lidar  */
+  UserLivoxLidarConfig livox_config;
+} LidarDevice;
+
+constexpr uint32_t kMaxProductType = 10;
+constexpr uint32_t kDeviceTypeLidarMid70 = 6;
+
+/***********************************/
+/* Global function for general use */
+bool IsFilePathValid(const char *path_str);
+uint32_t CalculatePacketQueueSize(const double publish_freq);
+std::string IpNumToString(uint32_t ip_num);
+uint32_t IpStringToNum(std::string ip_string);
+std::string ReplacePeriodByUnderline(std::string str);
+
+} // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER2_COMM_H_
diff --git a/livox_ros_driver2/src/comm/ldq.cpp b/livox_ros_driver2/src/comm/ldq.cpp
new file mode 100644
index 0000000..d70bcee
--- /dev/null
+++ b/livox_ros_driver2/src/comm/ldq.cpp
@@ -0,0 +1,150 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include <stdio.h>
+#include <string.h>
+
+#include "ldq.h"
+
+namespace livox_ros {
+
+/* for pointcloud queue process */
+bool InitQueue(LidarDataQueue *queue, uint32_t queue_size) {
+  if (queue == nullptr) {
+    // ROS_WARN("RosDriver Queue: Initialization failed - invalid queue.");
+    return false;
+  }
+
+  if (!IsPowerOf2(queue_size)) {
+    queue_size = RoundupPowerOf2(queue_size);
+    printf("Init queue, real query size:%u.\n", queue_size);
+  }
+
+  if (queue->storage_packet) {
+    delete[] queue->storage_packet;
+    queue->storage_packet = nullptr;
+  }
+
+  queue->storage_packet = new StoragePacket[queue_size];
+  if (queue->storage_packet == nullptr) {
+    // ROS_WARN("RosDriver Queue: Initialization failed - failed to allocate memory.");
+    return false;
+  }
+
+  queue->rd_idx = 0;
+  queue->wr_idx = 0;
+  queue->size = queue_size;
+  queue->mask = queue_size - 1;
+
+  return true;
+}
+
+bool DeInitQueue(LidarDataQueue *queue) {
+  if (queue == nullptr) {
+    // ROS_WARN("RosDriver Queue: Deinitialization failed - invalid queue.");
+    return false;
+  }
+
+  if (queue->storage_packet) {
+    delete[] queue->storage_packet;
+  }
+
+  queue->rd_idx = 0;
+  queue->wr_idx = 0;
+  queue->size = 0;
+  queue->mask = 0;
+
+  return true;
+}
+
+void ResetQueue(LidarDataQueue *queue) {
+  queue->rd_idx = 0;
+  queue->wr_idx = 0;
+}
+
+bool QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet) {
+  if (queue == nullptr || storage_packet == nullptr) {
+    // ROS_WARN("RosDriver Queue: Invalid pointer parameters.");
+    return false;
+  }
+
+  if (QueueIsEmpty(queue)) {
+    // ROS_WARN("RosDriver Queue: Pop failed, since the queue is empty.");
+    return false;
+  }
+
+  uint32_t rd_idx = queue->rd_idx & queue->mask;
+
+  storage_packet->base_time = queue->storage_packet[rd_idx].base_time;
+  storage_packet->points_num = queue->storage_packet[rd_idx].points_num;
+  storage_packet->points.resize(queue->storage_packet[rd_idx].points_num);
+
+  memcpy(storage_packet->points.data(), queue->storage_packet[rd_idx].points.data(), (storage_packet->points_num) * sizeof(PointXyzlt));
+  return true;
+}
+
+void QueuePopUpdate(LidarDataQueue *queue) {
+  queue->rd_idx++;
+}
+
+bool QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet) {
+  if (!QueuePrePop(queue, storage_packet)) {
+    return false;
+  }
+  QueuePopUpdate(queue);
+
+  return true;
+}
+
+uint32_t QueueUsedSize(LidarDataQueue *queue) {
+  return queue->wr_idx - queue->rd_idx;
+}
+
+uint32_t QueueUnusedSize(LidarDataQueue *queue) {
+  return (queue->size - QueueUsedSize(queue));
+}
+
+bool QueueIsFull(LidarDataQueue *queue) {
+  return ((queue->wr_idx - queue->rd_idx) > queue->mask);
+}
+
+bool QueueIsEmpty(LidarDataQueue *queue) {
+  return (queue->rd_idx == queue->wr_idx);
+}
+
+uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, const uint64_t base_time) {
+  uint32_t wr_idx = queue->wr_idx & queue->mask;
+  PointPacket* lidar_point_data = reinterpret_cast<PointPacket*>(data);
+  queue->storage_packet[wr_idx].base_time = base_time;
+  queue->storage_packet[wr_idx].points_num = lidar_point_data->points_num;
+
+  queue->storage_packet[wr_idx].points.clear();
+  queue->storage_packet[wr_idx].points.resize(lidar_point_data->points_num);
+  memcpy(queue->storage_packet[wr_idx].points.data(), lidar_point_data->points, sizeof(PointXyzlt) * (lidar_point_data->points_num));
+
+  queue->wr_idx++;
+  return 1;
+}
+
+}  // namespace livox_ros
diff --git a/livox_ros_driver2/src/comm/ldq.h b/livox_ros_driver2/src/comm/ldq.h
new file mode 100644
index 0000000..9830a31
--- /dev/null
+++ b/livox_ros_driver2/src/comm/ldq.h
@@ -0,0 +1,66 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_LDQ_H_
+#define LIVOX_ROS_DRIVER_LDQ_H_
+
+#include <stdint.h>
+#include <vector>
+
+#include "comm/comm.h"
+
+namespace livox_ros {
+  
+inline static bool IsPowerOf2(uint32_t size) {
+  return (size != 0) && ((size & (size - 1)) == 0);
+}
+
+inline static uint32_t RoundupPowerOf2(uint32_t size) {
+  uint32_t power2_val = 0;
+  for (int i = 0; i < 32; i++) {
+    power2_val = ((uint32_t)1) << i;
+    if (size <= power2_val) {
+      break;
+    }
+  }
+
+  return power2_val;
+}
+
+/** queue operate function */
+bool InitQueue(LidarDataQueue *queue, uint32_t queue_size);
+bool DeInitQueue(LidarDataQueue *queue);
+void ResetQueue(LidarDataQueue *queue);
+bool QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet);
+void QueuePopUpdate(LidarDataQueue *queue);
+bool QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet);
+uint32_t QueueUsedSize(LidarDataQueue *queue);
+uint32_t QueueUnusedSize(LidarDataQueue *queue);
+bool QueueIsFull(LidarDataQueue *queue);
+bool QueueIsEmpty(LidarDataQueue *queue);
+uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, const uint64_t base_time);
+
+}  // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER_LDQ_H_
diff --git a/livox_ros_driver2/src/comm/lidar_imu_data_queue.cpp b/livox_ros_driver2/src/comm/lidar_imu_data_queue.cpp
new file mode 100644
index 0000000..43de94b
--- /dev/null
+++ b/livox_ros_driver2/src/comm/lidar_imu_data_queue.cpp
@@ -0,0 +1,70 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "lidar_imu_data_queue.h"
+
+namespace livox_ros {
+
+void LidarImuDataQueue::Push(ImuData* imu_data) {
+  ImuData data;
+  data.lidar_type = imu_data->lidar_type;
+  data.handle = imu_data->handle;
+  data.time_stamp = imu_data->time_stamp;
+
+  data.gyro_x = imu_data->gyro_x;
+  data.gyro_y = imu_data->gyro_y;
+  data.gyro_z = imu_data->gyro_z;
+
+  data.acc_x = imu_data->acc_x;
+  data.acc_y = imu_data->acc_y;
+  data.acc_z = imu_data->acc_z;
+
+  std::lock_guard<std::mutex> lock(mutex_);
+  imu_data_queue_.push_back(std::move(data));
+}
+
+bool LidarImuDataQueue::Pop(ImuData& imu_data) {
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (imu_data_queue_.empty()) {
+    return false;
+  }
+  imu_data = imu_data_queue_.front();
+  imu_data_queue_.pop_front();
+  return true;
+}
+
+bool LidarImuDataQueue::Empty() {
+  std::lock_guard<std::mutex> lock(mutex_);
+  return imu_data_queue_.empty();
+}
+
+void LidarImuDataQueue::Clear() {
+  std::list<ImuData> tmp_imu_data_queue;
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    imu_data_queue_.swap(tmp_imu_data_queue);
+  }
+}
+
+} // namespace livox_ros
\ No newline at end of file
diff --git a/livox_ros_driver2/src/comm/lidar_imu_data_queue.h b/livox_ros_driver2/src/comm/lidar_imu_data_queue.h
new file mode 100644
index 0000000..83091f3
--- /dev/null
+++ b/livox_ros_driver2/src/comm/lidar_imu_data_queue.h
@@ -0,0 +1,77 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_
+#define LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_
+
+#include <list>
+#include <mutex>
+#include <cstdint>
+
+namespace livox_ros {
+
+// Based on the IMU Data Type in Livox communication protocol
+// TODO: add a link to the protocol
+typedef struct {
+  float gyro_x;        /**< Gyroscope X axis, Unit:rad/s */
+  float gyro_y;        /**< Gyroscope Y axis, Unit:rad/s */
+  float gyro_z;        /**< Gyroscope Z axis, Unit:rad/s */
+  float acc_x;         /**< Accelerometer X axis, Unit:g */
+  float acc_y;         /**< Accelerometer Y axis, Unit:g */
+  float acc_z;         /**< Accelerometer Z axis, Unit:g */
+} RawImuPoint;
+
+typedef struct {
+  uint8_t lidar_type;
+  uint32_t handle;
+  uint8_t slot;
+  // union {
+  //   uint8_t handle;
+  //   uint8_t slot;
+  // };
+  uint64_t time_stamp;
+  float gyro_x;        /**< Gyroscope X axis, Unit:rad/s */
+  float gyro_y;        /**< Gyroscope Y axis, Unit:rad/s */
+  float gyro_z;        /**< Gyroscope Z axis, Unit:rad/s */
+  float acc_x;         /**< Accelerometer X axis, Unit:g */
+  float acc_y;         /**< Accelerometer Y axis, Unit:g */
+  float acc_z;         /**< Accelerometer Z axis, Unit:g */
+} ImuData;
+
+class LidarImuDataQueue {
+ public:
+  void Push(ImuData* imu_data);
+  bool Pop(ImuData& imu_data);
+  bool Empty();
+  void Clear();
+
+ private:
+  std::mutex mutex_;
+  std::list<ImuData> imu_data_queue_;
+};
+
+} // namespace
+
+#endif // LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_
+
diff --git a/livox_ros_driver2/src/comm/pub_handler.cpp b/livox_ros_driver2/src/comm/pub_handler.cpp
new file mode 100644
index 0000000..07d4d4f
--- /dev/null
+++ b/livox_ros_driver2/src/comm/pub_handler.cpp
@@ -0,0 +1,457 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "pub_handler.h"
+
+#include <cstdlib>
+#include <chrono>
+#include <iostream>
+#include <limits>
+
+namespace livox_ros {
+
+std::atomic<bool> PubHandler::is_timestamp_sync_;
+
+PubHandler &pub_handler() {
+  static PubHandler handler;
+  return handler;
+}
+
+void PubHandler::Init() {
+}
+
+void PubHandler::Uninit() {
+  if (lidar_listen_id_ > 0) {
+    LivoxLidarRemovePointCloudObserver(lidar_listen_id_);
+    lidar_listen_id_ = 0;
+  }
+
+  RequestExit();
+
+  if (point_process_thread_ &&
+    point_process_thread_->joinable()) {
+    point_process_thread_->join();
+    point_process_thread_ = nullptr;
+  } else {
+    /* */
+  }
+}
+
+void PubHandler::RequestExit() {
+  is_quit_.store(true);
+}
+
+void PubHandler::SetPointCloudConfig(const double publish_freq) {
+  publish_interval_ = (kNsPerSecond / (publish_freq * 10)) * 10;
+  publish_interval_tolerance_ = publish_interval_ - kNsTolerantFrameTimeDeviation;
+  publish_interval_ms_ = publish_interval_ / kRatioOfMsToNs;
+  if (!point_process_thread_) {
+    point_process_thread_ = std::make_shared<std::thread>(&PubHandler::RawDataProcess, this);
+  }
+  return;
+}
+
+void PubHandler::SetImuDataCallback(ImuDataCallback cb, void* client_data) {
+  imu_client_data_ = client_data;
+  imu_callback_ = cb;
+}
+
+void PubHandler::AddLidarsExtParam(LidarExtParameter& lidar_param) {
+  std::unique_lock<std::mutex> lock(packet_mutex_);
+  uint32_t id = 0;
+  GetLidarId(lidar_param.lidar_type, lidar_param.handle, id);
+  lidar_extrinsics_[id] = lidar_param;
+}
+
+void PubHandler::ClearAllLidarsExtrinsicParams() {
+  std::unique_lock<std::mutex> lock(packet_mutex_);
+  lidar_extrinsics_.clear();
+}
+
+void PubHandler::SetPointCloudsCallback(PointCloudsCallback cb, void* client_data) {
+  pub_client_data_ = client_data;
+  points_callback_ = cb;
+  lidar_listen_id_ = LivoxLidarAddPointCloudObserver(OnLivoxLidarPointCloudCallback, this);
+}
+
+void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t dev_type,
+                                                LivoxLidarEthernetPacket *data, void *client_data) {
+  PubHandler* self = (PubHandler*)client_data;
+  if (!self) {
+    return;
+  }
+
+  if (data->time_type != kTimestampTypeNoSync) {
+    is_timestamp_sync_.store(true);
+  } else {
+    is_timestamp_sync_.store(false);
+  }
+
+  if (data->data_type == kLivoxLidarImuData) {
+    if (self->imu_callback_) {
+      RawImuPoint* imu = (RawImuPoint*) data->data;
+      ImuData imu_data;
+      imu_data.lidar_type = static_cast<uint8_t>(LidarProtoType::kLivoxLidarType);
+      imu_data.handle = handle;
+      imu_data.time_stamp = GetEthPacketTimestamp(data->time_type,
+                                                  data->timestamp, sizeof(data->timestamp));
+      imu_data.gyro_x = imu->gyro_x;
+      imu_data.gyro_y = imu->gyro_y;
+      imu_data.gyro_z = imu->gyro_z;
+      imu_data.acc_x = imu->acc_x;
+      imu_data.acc_y = imu->acc_y;
+      imu_data.acc_z = imu->acc_z;
+      self->imu_callback_(&imu_data, self->imu_client_data_);
+    }
+    return;
+  }
+  RawPacket packet = {};
+  packet.handle = handle;
+  packet.lidar_type = LidarProtoType::kLivoxLidarType;
+  packet.extrinsic_enable = false; 
+  if (dev_type == LivoxLidarDeviceType::kLivoxLidarTypeIndustrialHAP) {
+    packet.line_num = kLineNumberHAP;
+  } else if (dev_type == LivoxLidarDeviceType::kLivoxLidarTypeMid360) {
+    packet.line_num = kLineNumberMid360;
+  } else {
+    packet.line_num = kLineNumberDefault;
+  }
+  packet.data_type = data->data_type;
+  packet.point_num = data->dot_num;
+  packet.point_interval = data->time_interval * 100 / data->dot_num;  //ns
+  packet.time_stamp = GetEthPacketTimestamp(data->time_type,
+                                            data->timestamp, sizeof(data->timestamp));
+  uint32_t length = data->length - sizeof(LivoxLidarEthernetPacket) + 1;
+  packet.raw_data.insert(packet.raw_data.end(), data->data, data->data + length);
+  {
+    std::unique_lock<std::mutex> lock(self->packet_mutex_);
+    self->raw_packet_queue_.push_back(packet);
+  }
+    self->packet_condition_.notify_one();
+
+  return;
+}
+
+void PubHandler::PublishPointCloud() {
+  //publish point
+  if (points_callback_) {
+    points_callback_(&frame_, pub_client_data_);
+  }
+  return;
+}
+
+void PubHandler::CheckTimer(uint32_t id) {
+
+  if (PubHandler::is_timestamp_sync_.load()) { // Enable time synchronization
+    auto& process_handler = lidar_process_handlers_[id];
+    uint64_t recent_time_ms = process_handler->GetRecentTimeStamp() / kRatioOfMsToNs;
+    if ((recent_time_ms % publish_interval_ms_ != 0) || recent_time_ms == 0) {
+      return;
+    }
+
+    uint64_t diff = process_handler->GetRecentTimeStamp() - process_handler->GetLidarBaseTime();
+    if (diff < publish_interval_tolerance_) {
+      return;
+    }
+
+    frame_.base_time[frame_.lidar_num] = process_handler->GetLidarBaseTime();
+    points_[id].clear();
+    process_handler->GetLidarPointClouds(points_[id]);
+    if (points_[id].empty()) {
+      return;
+    }
+    PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num];
+    lidar_point.lidar_type = LidarProtoType::kLivoxLidarType;  // TODO:
+    lidar_point.handle = id;
+    lidar_point.points_num = points_[id].size();
+    lidar_point.points = points_[id].data();
+    frame_.lidar_num++;
+    
+    if (frame_.lidar_num != 0) {
+      PublishPointCloud();
+      frame_.lidar_num = 0;
+    }
+  } else { // Disable time synchronization
+    auto now_time = std::chrono::high_resolution_clock::now();
+    //First Set
+    static bool first = true;
+    if (first) {
+      last_pub_time_ = now_time;
+      first = false;
+      return;
+    }
+    if (now_time - last_pub_time_ < std::chrono::nanoseconds(publish_interval_)) {
+      return;
+    }
+    last_pub_time_ += std::chrono::nanoseconds(publish_interval_);
+    for (auto &process_handler : lidar_process_handlers_) {
+      frame_.base_time[frame_.lidar_num] = process_handler.second->GetLidarBaseTime();
+      uint32_t handle = process_handler.first;
+      points_[handle].clear();
+      process_handler.second->GetLidarPointClouds(points_[handle]);
+      if (points_[handle].empty()) {
+        continue;
+      }
+      PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num];
+      lidar_point.lidar_type = LidarProtoType::kLivoxLidarType;  // TODO:
+      lidar_point.handle = handle;
+      lidar_point.points_num = points_[handle].size();
+      lidar_point.points = points_[handle].data();
+      frame_.lidar_num++;
+    }
+    PublishPointCloud();
+    frame_.lidar_num = 0;
+  }
+  return;
+}
+
+void PubHandler::RawDataProcess() {
+  RawPacket raw_data;
+  while (!is_quit_.load()) {
+    {
+      std::unique_lock<std::mutex> lock(packet_mutex_);
+      if (raw_packet_queue_.empty()) {
+        packet_condition_.wait_for(lock, std::chrono::milliseconds(500));
+        if (raw_packet_queue_.empty()) {
+          continue;
+        }
+      }
+      raw_data = raw_packet_queue_.front();
+      raw_packet_queue_.pop_front();
+    }
+    uint32_t id = 0;
+    GetLidarId(raw_data.lidar_type, raw_data.handle, id);
+    if (lidar_process_handlers_.find(id) == lidar_process_handlers_.end()) {
+      lidar_process_handlers_[id].reset(new LidarPubHandler());
+    }
+    auto &process_handler = lidar_process_handlers_[id];
+    if (lidar_extrinsics_.find(id) != lidar_extrinsics_.end()) {
+        lidar_process_handlers_[id]->SetLidarsExtParam(lidar_extrinsics_[id]);
+    }
+    process_handler->PointCloudProcess(raw_data);
+    CheckTimer(id);
+  }
+}
+
+bool PubHandler::GetLidarId(LidarProtoType lidar_type, uint32_t handle, uint32_t& id) {
+  if (lidar_type == kLivoxLidarType) {
+    id = handle;
+    return true;
+  }
+  return false;
+}
+
+uint64_t PubHandler::GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size) {
+  LdsStamp time;
+  memcpy(time.stamp_bytes, time_stamp, size);
+
+  if (timestamp_type == kTimestampTypeGptpOrPtp ||
+      timestamp_type == kTimestampTypeGps) {
+    return time.stamp;
+  }
+
+  return std::chrono::high_resolution_clock::now().time_since_epoch().count();
+}
+
+/*******************************/
+/*  LidarPubHandler Definitions*/
+LidarPubHandler::LidarPubHandler() : is_set_extrinsic_params_(false) {}
+
+uint64_t LidarPubHandler::GetLidarBaseTime() {
+  if (points_clouds_.empty()) {
+    return 0;
+  }
+  return points_clouds_.at(0).offset_time;
+}
+
+void LidarPubHandler::GetLidarPointClouds(std::vector<PointXyzlt>& points_clouds) {
+  std::lock_guard<std::mutex> lock(mutex_);
+  points_clouds.swap(points_clouds_);
+}
+
+uint64_t LidarPubHandler::GetRecentTimeStamp() {
+  if (points_clouds_.empty()) {
+    return 0;
+  }
+  return points_clouds_.back().offset_time;
+}
+
+uint32_t LidarPubHandler::GetLidarPointCloudsSize() {
+  std::lock_guard<std::mutex> lock(mutex_);
+  return points_clouds_.size();
+}
+
+//convert to standard format and extrinsic compensate
+void LidarPubHandler::PointCloudProcess(RawPacket & pkt) {
+  if (pkt.lidar_type == LidarProtoType::kLivoxLidarType) {
+    LivoxLidarPointCloudProcess(pkt);
+  } else {
+    static bool flag = false;
+    if (!flag) {
+      std::cout << "error, unsupported protocol type: " << static_cast<int>(pkt.lidar_type) << std::endl;
+      flag = true;      
+    }
+  }
+}
+
+void LidarPubHandler::LivoxLidarPointCloudProcess(RawPacket & pkt) {
+  switch (pkt.data_type) {
+    case kLivoxLidarCartesianCoordinateHighData:
+      ProcessCartesianHighPoint(pkt);
+      break;
+    case kLivoxLidarCartesianCoordinateLowData:
+      ProcessCartesianLowPoint(pkt);
+      break;
+    case kLivoxLidarSphericalCoordinateData:
+      ProcessSphericalPoint(pkt);
+      break;
+    default:
+      std::cout << "unknown data type: " << static_cast<int>(pkt.data_type)
+                << " !!" << std::endl;
+      break;
+  }
+}
+
+void LidarPubHandler::SetLidarsExtParam(LidarExtParameter lidar_param) {
+  if (is_set_extrinsic_params_) {
+    return;
+  }
+  extrinsic_.trans[0] = lidar_param.param.x;
+  extrinsic_.trans[1] = lidar_param.param.y;
+  extrinsic_.trans[2] = lidar_param.param.z;
+
+  double cos_roll = cos(static_cast<double>(lidar_param.param.roll * PI / 180.0));
+  double cos_pitch = cos(static_cast<double>(lidar_param.param.pitch * PI / 180.0));
+  double cos_yaw = cos(static_cast<double>(lidar_param.param.yaw * PI / 180.0));
+  double sin_roll = sin(static_cast<double>(lidar_param.param.roll * PI / 180.0));
+  double sin_pitch = sin(static_cast<double>(lidar_param.param.pitch * PI / 180.0));
+  double sin_yaw = sin(static_cast<double>(lidar_param.param.yaw * PI / 180.0));
+
+  extrinsic_.rotation[0][0] = cos_pitch * cos_yaw;
+  extrinsic_.rotation[0][1] = sin_roll * sin_pitch * cos_yaw - cos_roll * sin_yaw;
+  extrinsic_.rotation[0][2] = cos_roll * sin_pitch * cos_yaw + sin_roll * sin_yaw;
+
+  extrinsic_.rotation[1][0] = cos_pitch * sin_yaw;
+  extrinsic_.rotation[1][1] = sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw;
+  extrinsic_.rotation[1][2] = cos_roll * sin_pitch * sin_yaw - sin_roll * cos_yaw;
+
+  extrinsic_.rotation[2][0] = -sin_pitch;
+  extrinsic_.rotation[2][1] = sin_roll * cos_pitch;
+  extrinsic_.rotation[2][2] = cos_roll * cos_pitch;
+
+  is_set_extrinsic_params_ = true;
+}
+
+void LidarPubHandler::ProcessCartesianHighPoint(RawPacket & pkt) {
+  LivoxLidarCartesianHighRawPoint* raw = (LivoxLidarCartesianHighRawPoint*)pkt.raw_data.data();
+  PointXyzlt point = {};
+  for (uint32_t i = 0; i < pkt.point_num; i++) {
+    if (pkt.extrinsic_enable) {
+      point.x = raw[i].x / 1000.0;
+      point.y = raw[i].y / 1000.0;
+      point.z = raw[i].z / 1000.0;
+    } else {
+      point.x = (raw[i].x * extrinsic_.rotation[0][0] +
+                raw[i].y * extrinsic_.rotation[0][1] +
+                raw[i].z * extrinsic_.rotation[0][2] + extrinsic_.trans[0]) / 1000.0;
+      point.y = (raw[i].x* extrinsic_.rotation[1][0] +
+                raw[i].y * extrinsic_.rotation[1][1] +
+                raw[i].z * extrinsic_.rotation[1][2] + extrinsic_.trans[1]) / 1000.0;
+      point.z = (raw[i].x * extrinsic_.rotation[2][0] +
+                raw[i].y * extrinsic_.rotation[2][1] +
+                raw[i].z * extrinsic_.rotation[2][2] + extrinsic_.trans[2]) / 1000.0;
+    }
+    point.intensity = raw[i].reflectivity;
+    point.line = i % pkt.line_num;
+    point.tag = raw[i].tag;
+    point.offset_time = pkt.time_stamp + i * pkt.point_interval;
+    std::lock_guard<std::mutex> lock(mutex_);
+    points_clouds_.push_back(point);
+  }
+}
+
+void LidarPubHandler::ProcessCartesianLowPoint(RawPacket & pkt) {
+  LivoxLidarCartesianLowRawPoint* raw = (LivoxLidarCartesianLowRawPoint*)pkt.raw_data.data();
+  PointXyzlt point = {};
+  for (uint32_t i = 0; i < pkt.point_num; i++) {
+    if (pkt.extrinsic_enable) {
+      point.x = raw[i].x / 100.0;
+      point.y = raw[i].y / 100.0;
+      point.z = raw[i].z / 100.0;
+    } else {
+      point.x = (raw[i].x * extrinsic_.rotation[0][0] +
+                raw[i].y * extrinsic_.rotation[0][1] +
+                raw[i].z * extrinsic_.rotation[0][2] + extrinsic_.trans[0]) / 100.0;
+      point.y = (raw[i].x* extrinsic_.rotation[1][0] +
+                raw[i].y * extrinsic_.rotation[1][1] +
+                raw[i].z * extrinsic_.rotation[1][2] + extrinsic_.trans[1]) / 100.0;
+      point.z = (raw[i].x * extrinsic_.rotation[2][0] +
+                raw[i].y * extrinsic_.rotation[2][1] +
+                raw[i].z * extrinsic_.rotation[2][2] + extrinsic_.trans[2]) / 100.0;
+    }
+    point.intensity = raw[i].reflectivity;
+    point.line = i % pkt.line_num;
+    point.tag = raw[i].tag;
+    point.offset_time = pkt.time_stamp + i * pkt.point_interval;
+    std::lock_guard<std::mutex> lock(mutex_);
+    points_clouds_.push_back(point);
+  }
+}
+
+void LidarPubHandler::ProcessSphericalPoint(RawPacket& pkt) {
+  LivoxLidarSpherPoint* raw = (LivoxLidarSpherPoint*)pkt.raw_data.data();
+  PointXyzlt point = {};
+  for (uint32_t i = 0; i < pkt.point_num; i++) {
+    double radius = raw[i].depth / 1000.0;
+    double theta = raw[i].theta / 100.0 / 180 * PI;
+    double phi = raw[i].phi / 100.0 / 180 * PI;
+    double src_x = radius * sin(theta) * cos(phi);
+    double src_y = radius * sin(theta) * sin(phi);
+    double src_z = radius * cos(theta);
+    if (pkt.extrinsic_enable) {
+      point.x = src_x;
+      point.y = src_y;
+      point.z = src_z;
+    } else {
+      point.x = src_x * extrinsic_.rotation[0][0] +
+                src_y * extrinsic_.rotation[0][1] +
+                src_z * extrinsic_.rotation[0][2] + (extrinsic_.trans[0] / 1000.0);
+      point.y = src_x * extrinsic_.rotation[1][0] +
+                src_y * extrinsic_.rotation[1][1] +
+                src_z * extrinsic_.rotation[1][2] + (extrinsic_.trans[1] / 1000.0);
+      point.z = src_x * extrinsic_.rotation[2][0] +
+                src_y * extrinsic_.rotation[2][1] +
+                src_z * extrinsic_.rotation[2][2] + (extrinsic_.trans[2] / 1000.0);
+    }
+
+    point.intensity = raw[i].reflectivity;
+    point.line = i % pkt.line_num;
+    point.tag = raw[i].tag;
+    point.offset_time = pkt.time_stamp + i * pkt.point_interval;
+    std::lock_guard<std::mutex> lock(mutex_);
+    points_clouds_.push_back(point);
+  }
+}
+
+} // namespace livox_ros
diff --git a/livox_ros_driver2/src/comm/pub_handler.h b/livox_ros_driver2/src/comm/pub_handler.h
new file mode 100644
index 0000000..9c519a0
--- /dev/null
+++ b/livox_ros_driver2/src/comm/pub_handler.h
@@ -0,0 +1,138 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_DRIVER_PUB_HANDLER_H_
+#define LIVOX_DRIVER_PUB_HANDLER_H_
+
+#include <atomic>
+#include <cstring>
+#include <condition_variable> // std::condition_variable
+#include <deque>
+#include <functional>
+#include <map>
+#include <memory>
+#include <mutex>              // std::mutex
+#include <thread>
+
+#include "livox_lidar_def.h"
+#include "livox_lidar_api.h"
+#include "comm/comm.h"
+
+namespace livox_ros {
+
+class LidarPubHandler {
+ public:
+  LidarPubHandler();
+  ~ LidarPubHandler() {}
+
+  void PointCloudProcess(RawPacket& pkt);
+  void SetLidarsExtParam(LidarExtParameter param);
+  void GetLidarPointClouds(std::vector<PointXyzlt>& points_clouds);
+
+  uint64_t GetRecentTimeStamp();
+  uint32_t GetLidarPointCloudsSize();
+  uint64_t GetLidarBaseTime();
+
+ private:
+  void LivoxLidarPointCloudProcess(RawPacket & pkt);
+  void ProcessCartesianHighPoint(RawPacket & pkt);
+  void ProcessCartesianLowPoint(RawPacket & pkt);
+  void ProcessSphericalPoint(RawPacket & pkt);
+  std::vector<PointXyzlt> points_clouds_;
+  ExtParameterDetailed extrinsic_ = {
+    {0, 0, 0},
+    {
+      {1, 0, 0},
+      {0, 1, 1},
+      {0, 0, 1}
+    }
+  };
+  std::mutex mutex_;
+  std::atomic_bool is_set_extrinsic_params_;
+};
+  
+class PubHandler {
+ public:
+  using PointCloudsCallback = std::function<void(PointFrame*, void *)>;
+  using ImuDataCallback = std::function<void(ImuData*, void*)>;
+  using TimePoint = std::chrono::high_resolution_clock::time_point;
+
+  PubHandler() {}
+
+  ~ PubHandler() { Uninit(); }
+
+  void Uninit();
+  void RequestExit();
+  void Init();
+  void SetPointCloudConfig(const double publish_freq);
+  void SetPointCloudsCallback(PointCloudsCallback cb, void* client_data);
+  void AddLidarsExtParam(LidarExtParameter& extrinsic_params);
+  void ClearAllLidarsExtrinsicParams();
+  void SetImuDataCallback(ImuDataCallback cb, void* client_data);
+
+ private:
+  //thread to process raw data
+  void RawDataProcess();
+  std::atomic<bool> is_quit_{false};
+  std::shared_ptr<std::thread> point_process_thread_;
+  std::mutex packet_mutex_;
+  std::condition_variable packet_condition_;
+
+  //publish callback
+  void CheckTimer(uint32_t id);
+  void PublishPointCloud();
+  static void OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t dev_type,
+                                             LivoxLidarEthernetPacket *data, void *client_data);
+  
+  static bool GetLidarId(LidarProtoType lidar_type, uint32_t handle, uint32_t& id);
+  static uint64_t GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size);
+
+  PointCloudsCallback points_callback_;
+  void* pub_client_data_ = nullptr;
+
+  ImuDataCallback imu_callback_;
+  void* imu_client_data_ = nullptr;
+
+  PointFrame frame_;
+
+  std::deque<RawPacket> raw_packet_queue_;
+
+  //pub config
+  uint64_t publish_interval_ = 100000000; //100 ms
+  uint64_t publish_interval_tolerance_ = 100000000; //100 ms
+  uint64_t publish_interval_ms_ = 100; //100 ms
+  TimePoint last_pub_time_;
+
+  std::map<uint32_t, std::unique_ptr<LidarPubHandler>> lidar_process_handlers_;
+  std::map<uint32_t, std::vector<PointXyzlt>> points_;
+  std::map<uint32_t, LidarExtParameter> lidar_extrinsics_;
+  static std::atomic<bool> is_timestamp_sync_;
+  uint16_t lidar_listen_id_ = 0;
+};
+
+PubHandler &pub_handler();
+
+}  // namespace livox_ros
+
+#endif  // LIVOX_DRIVER_PUB_HANDLER_H_
\ No newline at end of file
diff --git a/livox_ros_driver2/src/comm/semaphore.cpp b/livox_ros_driver2/src/comm/semaphore.cpp
new file mode 100644
index 0000000..941815a
--- /dev/null
+++ b/livox_ros_driver2/src/comm/semaphore.cpp
@@ -0,0 +1,41 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "semaphore.h"
+
+namespace livox_ros {
+
+void Semaphore::Signal() {
+  std::unique_lock<std::mutex> lock(mutex_);
+  ++count_;
+  cv_.notify_one();
+}
+
+void Semaphore::Wait() {
+  std::unique_lock<std::mutex> lock(mutex_);
+  cv_.wait(lock, [=] { return count_ > 0; });
+  --count_;
+}
+
+} // namespace livox_ros
diff --git a/livox_ros_driver2/src/comm/semaphore.h b/livox_ros_driver2/src/comm/semaphore.h
new file mode 100644
index 0000000..b8085cd
--- /dev/null
+++ b/livox_ros_driver2/src/comm/semaphore.h
@@ -0,0 +1,51 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_SEMAPHORE_H_
+#define LIVOX_ROS_DRIVER_SEMAPHORE_H_
+
+#include <mutex>
+#include <condition_variable>
+
+namespace livox_ros {
+
+class Semaphore {
+ public:
+  explicit Semaphore(int count = 0) : count_(count) {
+  }
+  void Signal();
+  void Wait();
+  int GetCount() {
+    return count_;
+  }
+
+ private:
+  std::mutex mutex_;
+  std::condition_variable cv_;
+  volatile int count_;
+};
+
+} // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER_SEMAPHORE_H_
diff --git a/livox_ros_driver2/src/driver_node.cpp b/livox_ros_driver2/src/driver_node.cpp
new file mode 100644
index 0000000..24f0aba
--- /dev/null
+++ b/livox_ros_driver2/src/driver_node.cpp
@@ -0,0 +1,46 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "driver_node.h"
+#include "lddc.h"
+
+namespace livox_ros {
+
+DriverNode& DriverNode::GetNode() noexcept {
+  return *this;
+}
+
+DriverNode::~DriverNode() {
+  lddc_ptr_->lds_->RequestExit();
+  exit_signal_.set_value();
+  pointclouddata_poll_thread_->join();
+  imudata_poll_thread_->join();
+}
+
+} // namespace livox_ros
+
+
+
+
+
diff --git a/livox_ros_driver2/src/driver_node.h b/livox_ros_driver2/src/driver_node.h
new file mode 100644
index 0000000..aaf4f81
--- /dev/null
+++ b/livox_ros_driver2/src/driver_node.h
@@ -0,0 +1,78 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_DRIVER_NODE_H
+#define LIVOX_DRIVER_NODE_H
+
+#include "include/ros_headers.h"
+
+namespace livox_ros {
+
+class Lddc;
+
+#ifdef BUILDING_ROS1
+class DriverNode final : public ros::NodeHandle {
+ public:
+  DriverNode() = default;
+  DriverNode(const DriverNode &) = delete;
+  ~DriverNode();
+  DriverNode &operator=(const DriverNode &) = delete;
+
+  DriverNode& GetNode() noexcept;
+
+  void PointCloudDataPollThread();
+  void ImuDataPollThread();
+
+  std::unique_ptr<Lddc> lddc_ptr_;
+  std::shared_ptr<std::thread> pointclouddata_poll_thread_;
+  std::shared_ptr<std::thread> imudata_poll_thread_;
+  std::shared_future<void> future_;
+  std::promise<void> exit_signal_;
+};
+
+#elif defined BUILDING_ROS2
+class DriverNode final : public rclcpp::Node {
+ public:
+  explicit DriverNode(const rclcpp::NodeOptions& options);
+  DriverNode(const DriverNode &) = delete;
+  ~DriverNode();
+  DriverNode &operator=(const DriverNode &) = delete;
+
+  DriverNode& GetNode() noexcept;
+
+ private:
+  void PointCloudDataPollThread();
+  void ImuDataPollThread();
+
+  std::unique_ptr<Lddc> lddc_ptr_;
+  std::shared_ptr<std::thread> pointclouddata_poll_thread_;
+  std::shared_ptr<std::thread> imudata_poll_thread_;
+  std::shared_future<void> future_;
+  std::promise<void> exit_signal_;
+};
+#endif
+
+} // namespace livox_ros
+
+#endif // LIVOX_DRIVER_NODE_H
\ No newline at end of file
diff --git a/livox_ros_driver2/src/include/livox_ros_driver2.h b/livox_ros_driver2/src/include/livox_ros_driver2.h
new file mode 100644
index 0000000..9160a98
--- /dev/null
+++ b/livox_ros_driver2/src/include/livox_ros_driver2.h
@@ -0,0 +1,40 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER2_INClUDE_H_
+#define LIVOX_ROS_DRIVER2_INClUDE_H_
+
+#define LIVOX_ROS_DRIVER2_VER_MAJOR 1
+#define LIVOX_ROS_DRIVER2_VER_MINOR 2
+#define LIVOX_ROS_DRIVER2_VER_PATCH 4
+
+#define GET_STRING(n) GET_STRING_DIRECT(n)
+#define GET_STRING_DIRECT(n) #n
+
+#define LIVOX_ROS_DRIVER2_VERSION_STRING                      \
+  GET_STRING(LIVOX_ROS_DRIVER2_VER_MAJOR)                     \
+  "." GET_STRING(LIVOX_ROS_DRIVER2_VER_MINOR) "." GET_STRING( \
+      LIVOX_ROS_DRIVER2_VER_PATCH)
+
+#endif  // LIVOX_ROS_DRIVER2_INClUDE_H_
diff --git a/livox_ros_driver2/src/include/ros1_headers.h b/livox_ros_driver2/src/include/ros1_headers.h
new file mode 100644
index 0000000..6510859
--- /dev/null
+++ b/livox_ros_driver2/src/include/ros1_headers.h
@@ -0,0 +1,54 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// 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.
+//
+
+// Denoting headers specifically used for building ROS1 Driver.
+
+#ifndef ROS1_HEADERS_H_
+#define ROS1_HEADERS_H_
+
+#include <thread>
+#include <future>
+
+#include <ros/ros.h>
+#include <rosbag/bag.h>
+#include <pcl_ros/point_cloud.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/PointCloud2.h>
+#include "livox_ros_driver2/CustomMsg.h"
+#include "livox_ros_driver2/CustomPoint.h"
+
+
+#define DRIVER_DEBUG(node, ...) ROS_DEBUG(__VA_ARGS__)
+#define DRIVER_INFO(node, ...) ROS_INFO(__VA_ARGS__)
+#define DRIVER_WARN(node, ...) ROS_WARN(__VA_ARGS__)
+#define DRIVER_ERROR(node, ...) ROS_ERROR(__VA_ARGS__)
+#define DRIVER_FATAL(node, ...) ROS_FATAL(__VA_ARGS__)
+
+#define DRIVER_DEBUG_EXTRA(node, EXTRA, ...) ROS_DEBUG_##EXTRA(__VA_ARGS__)
+#define DRIVER_INFO_EXTRA(node, EXTRA, ...) ROS_INFO_##EXTRA(__VA_ARGS__)
+#define DRIVER_WARN_EXTRA(node, EXTRA, ...) ROS_WARN_##EXTRA(__VA_ARGS__)
+#define DRIVER_ERROR_EXTRA(node, EXTRA, ...) ROS_ERROR_##EXTRA(__VA_ARGS__)
+#define DRIVER_FATAL_EXTRA(node, EXTRA, ...) ROS_FATAL_##EXTRA(__VA_ARGS__)
+
+#endif // ROS1_HEADERS_H_
diff --git a/livox_ros_driver2/src/include/ros2_headers.h b/livox_ros_driver2/src/include/ros2_headers.h
new file mode 100644
index 0000000..1d11236
--- /dev/null
+++ b/livox_ros_driver2/src/include/ros2_headers.h
@@ -0,0 +1,52 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// 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.
+//
+
+// Denoting headers specifically used for building ROS1 Driver.
+
+#ifndef ROS2_HEADERS_H_
+#define ROS2_HEADERS_H_
+
+#include <thread>
+#include <future>
+
+#include <rclcpp/rclcpp.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include "livox_ros_driver2/msg/custom_point.hpp"
+#include "livox_ros_driver2/msg/custom_msg.hpp"
+
+#define DRIVER_DEBUG(node, ...) RCLCPP_DEBUG((node).get_logger(), __VA_ARGS__)
+#define DRIVER_INFO(node, ...) RCLCPP_INFO((node).get_logger(), __VA_ARGS__)
+#define DRIVER_WARN(node, ...) RCLCPP_WARN((node).get_logger(), __VA_ARGS__)
+#define DRIVER_ERROR(node, ...) RCLCPP_ERROR((node).get_logger(), __VA_ARGS__)
+#define DRIVER_FATAL(node, ...) RCLCPP_FATAL((node).get_logger(), __VA_ARGS__)
+
+#define DRIVER_DEBUG_EXTRA(node, EXTRA, ...) RCLCPP_DEBUG_##EXTRA((node).get_logger(), __VA_ARGS__)
+#define DRIVER_INFO_EXTRA(node, EXTRA, ...) RCLCPP_INFO_##EXTRA((node).get_logger(), __VA_ARGS__)
+#define DRIVER_WARN_EXTRA(node, EXTRA, ...) RCLCPP_WARN_##EXTRA((node).get_logger(), __VA_ARGS__)
+#define DRIVER_ERROR_EXTRA(node, EXTRA, ...) RCLCPP_ERROR_##EXTRA((node).get_logger(), __VA_ARGS__)
+#define DRIVER_FATAL_EXTRA(node, EXTRA, ...) RCLCPP_FATAL_##EXTRA((node).get_logger(), __VA_ARGS__)
+
+#endif // ROS2_HEADERS_H_
diff --git a/livox_ros_driver2/src/include/ros_headers.h b/livox_ros_driver2/src/include/ros_headers.h
new file mode 100644
index 0000000..5c8b281
--- /dev/null
+++ b/livox_ros_driver2/src/include/ros_headers.h
@@ -0,0 +1,34 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef ROS_HEADERS_H_
+#define ROS_HEADERS_H_
+
+#ifdef BUILDING_ROS1
+#include "ros1_headers.h"
+#elif defined BUILDING_ROS2
+#include "ros2_headers.h"
+#endif
+
+#endif // ROS_HEADERS_H_
diff --git a/livox_ros_driver2/src/lddc.cpp b/livox_ros_driver2/src/lddc.cpp
new file mode 100644
index 0000000..06195df
--- /dev/null
+++ b/livox_ros_driver2/src/lddc.cpp
@@ -0,0 +1,703 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "lddc.h"
+#include "comm/ldq.h"
+#include "comm/comm.h"
+
+#include <inttypes.h>
+#include <iostream>
+#include <iomanip>
+#include <math.h>
+#include <stdint.h>
+
+#include "include/ros_headers.h"
+
+#include "driver_node.h"
+#include "lds_lidar.h"
+
+namespace livox_ros {
+
+/** Lidar Data Distribute Control--------------------------------------------*/
+#ifdef BUILDING_ROS1
+Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
+    double frq, std::string &frame_id, bool lidar_bag, bool imu_bag)
+    : transfer_format_(format),
+      use_multi_topic_(multi_topic),
+      data_src_(data_src),
+      output_type_(output_type),
+      publish_frq_(frq),
+      frame_id_(frame_id),
+      enable_lidar_bag_(lidar_bag),
+      enable_imu_bag_(imu_bag) {
+  publish_period_ns_ = kNsPerSecond / publish_frq_;
+  lds_ = nullptr;
+  memset(private_pub_, 0, sizeof(private_pub_));
+  memset(private_imu_pub_, 0, sizeof(private_imu_pub_));
+  global_pub_ = nullptr;
+  global_imu_pub_ = nullptr;
+  cur_node_ = nullptr;
+  bag_ = nullptr;
+}
+#elif defined BUILDING_ROS2
+Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
+           double frq, std::string &frame_id)
+    : transfer_format_(format),
+      use_multi_topic_(multi_topic),
+      data_src_(data_src),
+      output_type_(output_type),
+      publish_frq_(frq),
+      frame_id_(frame_id) {
+  publish_period_ns_ = kNsPerSecond / publish_frq_;
+  lds_ = nullptr;
+#if 0
+  bag_ = nullptr;
+#endif
+}
+#endif
+
+Lddc::~Lddc() {
+#ifdef BUILDING_ROS1
+  if (global_pub_) {
+    delete global_pub_;
+  }
+
+  if (global_imu_pub_) {
+    delete global_imu_pub_;
+  }
+#endif
+
+  PrepareExit();
+
+#ifdef BUILDING_ROS1
+  for (uint32_t i = 0; i < kMaxSourceLidar; i++) {
+    if (private_pub_[i]) {
+      delete private_pub_[i];
+    }
+  }
+
+  for (uint32_t i = 0; i < kMaxSourceLidar; i++) {
+    if (private_imu_pub_[i]) {
+      delete private_imu_pub_[i];
+    }
+  }
+#endif
+  std::cout << "lddc destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
+}
+
+int Lddc::RegisterLds(Lds *lds) {
+  if (lds_ == nullptr) {
+    lds_ = lds;
+    return 0;
+  } else {
+    return -1;
+  }
+}
+
+void Lddc::DistributePointCloudData(void) {
+  if (!lds_) {
+    std::cout << "lds is not registered" << std::endl;
+    return;
+  }
+  if (lds_->IsRequestExit()) {
+    std::cout << "DistributePointCloudData is RequestExit" << std::endl;
+    return;
+  }
+  
+  lds_->pcd_semaphore_.Wait();
+  for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
+    uint32_t lidar_id = i;
+    LidarDevice *lidar = &lds_->lidars_[lidar_id];
+    LidarDataQueue *p_queue = &lidar->data;
+    if ((kConnectStateSampling != lidar->connect_state) || (p_queue == nullptr)) {
+      continue;
+    }
+    PollingLidarPointCloudData(lidar_id, lidar);    
+  }
+}
+
+void Lddc::DistributeImuData(void) {
+  if (!lds_) {
+    std::cout << "lds is not registered" << std::endl;
+    return;
+  }
+  if (lds_->IsRequestExit()) {
+    std::cout << "DistributeImuData is RequestExit" << std::endl;
+    return;
+  }
+  
+  lds_->imu_semaphore_.Wait();
+  for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
+    uint32_t lidar_id = i;
+    LidarDevice *lidar = &lds_->lidars_[lidar_id];
+    LidarImuDataQueue *p_queue = &lidar->imu_data;
+    if ((kConnectStateSampling != lidar->connect_state) || (p_queue == nullptr)) {
+      continue;
+    }
+    PollingLidarImuData(lidar_id, lidar);
+  }
+}
+
+void Lddc::PollingLidarPointCloudData(uint8_t index, LidarDevice *lidar) {
+  LidarDataQueue *p_queue = &lidar->data;
+  if (p_queue == nullptr || p_queue->storage_packet == nullptr) {
+    return;
+  }
+
+  while (!lds_->IsRequestExit() && !QueueIsEmpty(p_queue)) {
+    if (kPointCloud2Msg == transfer_format_) {
+      PublishPointcloud2(p_queue, index);
+    } else if (kLivoxCustomMsg == transfer_format_) {
+      PublishCustomPointcloud(p_queue, index);
+    } else if (kPclPxyziMsg == transfer_format_) {
+      PublishPclMsg(p_queue, index);
+    }
+  }
+}
+
+void Lddc::PollingLidarImuData(uint8_t index, LidarDevice *lidar) {
+  LidarImuDataQueue& p_queue = lidar->imu_data;
+  while (!lds_->IsRequestExit() && !p_queue.Empty()) {
+    PublishImuData(p_queue, index);
+  }
+}
+
+void Lddc::PrepareExit(void) {
+#ifdef BUILDING_ROS1
+  if (bag_) {
+    DRIVER_INFO(*cur_node_, "Waiting to save the bag file!");
+    bag_->close();
+    DRIVER_INFO(*cur_node_, "Save the bag file successfully!");
+    bag_ = nullptr;
+  }
+#endif
+  if (lds_) {
+    lds_->PrepareExit();
+    lds_ = nullptr;
+  }
+}
+
+void Lddc::PublishPointcloud2(LidarDataQueue *queue, uint8_t index) {
+  while(!QueueIsEmpty(queue)) {
+    StoragePacket pkg;
+    QueuePop(queue, &pkg);
+    if (pkg.points.empty()) {
+      printf("Publish point cloud2 failed, the pkg points is empty.\n");
+      continue;
+    }
+
+    PointCloud2 cloud;
+    uint64_t timestamp = 0;
+    InitPointcloud2Msg(pkg, cloud, timestamp);
+    PublishPointcloud2Data(index, timestamp, cloud);
+  }
+}
+
+void Lddc::PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index) {
+  while(!QueueIsEmpty(queue)) {
+    StoragePacket pkg;
+    QueuePop(queue, &pkg);
+    if (pkg.points.empty()) {
+      printf("Publish custom point cloud failed, the pkg points is empty.\n");
+      continue;
+    }
+
+    CustomMsg livox_msg;
+    InitCustomMsg(livox_msg, pkg, index);
+    FillPointsToCustomMsg(livox_msg, pkg);
+    PublishCustomPointData(livox_msg, index);
+  }
+}
+
+/* for pcl::pxyzi */
+void Lddc::PublishPclMsg(LidarDataQueue *queue, uint8_t index) {
+#ifdef BUILDING_ROS2
+  static bool first_log = true;
+  if (first_log) {
+    std::cout << "error: message type 'pcl::PointCloud' is NOT supported in ROS2, "
+              << "please modify the 'xfer_format' field in the launch file"
+              << std::endl;
+  }
+  first_log = false;
+  return;
+#endif
+  while(!QueueIsEmpty(queue)) {
+    StoragePacket pkg;
+    QueuePop(queue, &pkg);
+    if (pkg.points.empty()) {
+      printf("Publish point cloud failed, the pkg points is empty.\n");
+      continue;
+    }
+
+    PointCloud cloud;
+    uint64_t timestamp = 0;
+    InitPclMsg(pkg, cloud, timestamp);
+    FillPointsToPclMsg(pkg, cloud);
+    PublishPclData(index, timestamp, cloud);
+  }
+  return;
+}
+
+void Lddc::InitPointcloud2MsgHeader(PointCloud2& cloud) {
+  cloud.header.frame_id.assign(frame_id_);
+  cloud.height = 1;
+  cloud.width = 0;
+  cloud.fields.resize(7);
+  cloud.fields[0].offset = 0;
+  cloud.fields[0].name = "x";
+  cloud.fields[0].count = 1;
+  cloud.fields[0].datatype = PointField::FLOAT32;
+  cloud.fields[1].offset = 4;
+  cloud.fields[1].name = "y";
+  cloud.fields[1].count = 1;
+  cloud.fields[1].datatype = PointField::FLOAT32;
+  cloud.fields[2].offset = 8;
+  cloud.fields[2].name = "z";
+  cloud.fields[2].count = 1;
+  cloud.fields[2].datatype = PointField::FLOAT32;
+  cloud.fields[3].offset = 12;
+  cloud.fields[3].name = "intensity";
+  cloud.fields[3].count = 1;
+  cloud.fields[3].datatype = PointField::FLOAT32;
+  cloud.fields[4].offset = 16;
+  cloud.fields[4].name = "tag";
+  cloud.fields[4].count = 1;
+  cloud.fields[4].datatype = PointField::UINT8;
+  cloud.fields[5].offset = 17;
+  cloud.fields[5].name = "line";
+  cloud.fields[5].count = 1;
+  cloud.fields[5].datatype = PointField::UINT8;
+  cloud.fields[6].offset = 18;
+  cloud.fields[6].name = "timestamp";
+  cloud.fields[6].count = 1;
+  cloud.fields[6].datatype = PointField::FLOAT64;
+  cloud.point_step = sizeof(LivoxPointXyzrtlt);
+}
+
+void Lddc::InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint64_t& timestamp) {
+  InitPointcloud2MsgHeader(cloud);
+
+  cloud.point_step = sizeof(LivoxPointXyzrtlt);
+
+  cloud.width = pkg.points_num;
+  cloud.row_step = cloud.width * cloud.point_step;
+
+  cloud.is_bigendian = false;
+  cloud.is_dense     = true;
+
+  if (!pkg.points.empty()) {
+    timestamp = pkg.base_time;
+  }
+
+  #ifdef BUILDING_ROS1
+      cloud.header.stamp = ros::Time( timestamp / 1000000000.0);
+  #elif defined BUILDING_ROS2
+      cloud.header.stamp = rclcpp::Time(timestamp);
+  #endif
+
+  std::vector<LivoxPointXyzrtlt> points;
+  for (size_t i = 0; i < pkg.points_num; ++i) {
+    LivoxPointXyzrtlt point;
+    point.x = pkg.points[i].x;
+    point.y = pkg.points[i].y;
+    point.z = pkg.points[i].z;
+    point.reflectivity = pkg.points[i].intensity;
+    point.tag = pkg.points[i].tag;
+    point.line = pkg.points[i].line;
+    point.timestamp = static_cast<double>(pkg.points[i].offset_time);
+    points.push_back(std::move(point));
+  }
+  cloud.data.resize(pkg.points_num * sizeof(LivoxPointXyzrtlt));
+  memcpy(cloud.data.data(), points.data(), pkg.points_num * sizeof(LivoxPointXyzrtlt));
+}
+
+void Lddc::PublishPointcloud2Data(const uint8_t index, const uint64_t timestamp, const PointCloud2& cloud) {
+#ifdef BUILDING_ROS1
+  PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index);
+#elif defined BUILDING_ROS2
+  Publisher<PointCloud2>::SharedPtr publisher_ptr =
+    std::dynamic_pointer_cast<Publisher<PointCloud2>>(GetCurrentPublisher(index));
+#endif
+
+  if (kOutputToRos == output_type_) {
+    publisher_ptr->publish(cloud);
+  } else {
+#ifdef BUILDING_ROS1
+    if (bag_ && enable_lidar_bag_) {
+      bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), cloud);
+    }
+#endif
+  }
+}
+
+void Lddc::InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index) {
+  livox_msg.header.frame_id.assign(frame_id_);
+
+#ifdef BUILDING_ROS1
+  static uint32_t msg_seq = 0;
+  livox_msg.header.seq = msg_seq;
+  ++msg_seq;
+#endif
+
+  uint64_t timestamp = 0;
+  if (!pkg.points.empty()) {
+    timestamp = pkg.base_time;
+  }
+  livox_msg.timebase = timestamp;
+
+#ifdef BUILDING_ROS1
+  livox_msg.header.stamp = ros::Time(timestamp / 1000000000.0);
+#elif defined BUILDING_ROS2
+  livox_msg.header.stamp = rclcpp::Time(timestamp);
+#endif
+
+  livox_msg.point_num = pkg.points_num;
+  if (lds_->lidars_[index].lidar_type == kLivoxLidarType) {
+    livox_msg.lidar_id = lds_->lidars_[index].handle;
+  } else {
+    printf("Init custom msg lidar id failed, the index:%u.\n", index);
+    livox_msg.lidar_id = 0;
+  }
+}
+
+void Lddc::FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg) {
+  uint32_t points_num = pkg.points_num;
+  const std::vector<PointXyzlt>& points = pkg.points;
+  for (uint32_t i = 0; i < points_num; ++i) {
+    CustomPoint point;
+    point.x = points[i].x;
+    point.y = points[i].y;
+    point.z = points[i].z;
+    point.reflectivity = points[i].intensity;
+    point.tag = points[i].tag;
+    point.line = points[i].line;
+    point.offset_time = static_cast<uint32_t>(points[i].offset_time - pkg.base_time);
+
+    livox_msg.points.push_back(std::move(point));
+  }
+}
+
+void Lddc::PublishCustomPointData(const CustomMsg& livox_msg, const uint8_t index) {
+#ifdef BUILDING_ROS1
+  PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index);
+#elif defined BUILDING_ROS2
+  Publisher<CustomMsg>::SharedPtr publisher_ptr = std::dynamic_pointer_cast<Publisher<CustomMsg>>(GetCurrentPublisher(index));
+#endif
+
+  if (kOutputToRos == output_type_) {
+    publisher_ptr->publish(livox_msg);
+  } else {
+#ifdef BUILDING_ROS1
+    if (bag_ && enable_lidar_bag_) {
+      bag_->write(publisher_ptr->getTopic(), ros::Time(livox_msg.timebase / 1000000000.0), livox_msg);
+    }
+#endif
+  }
+}
+
+void Lddc::InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp) {
+#ifdef BUILDING_ROS1
+  cloud.header.frame_id.assign(frame_id_);
+  cloud.height = 1;
+  cloud.width = pkg.points_num;
+
+  if (!pkg.points.empty()) {
+    timestamp = pkg.base_time;
+  }
+  cloud.header.stamp = timestamp / 1000.0;  // to pcl ros time stamp
+#elif defined BUILDING_ROS2
+  std::cout << "warning: pcl::PointCloud is not supported in ROS2, "
+            << "please check code logic" 
+            << std::endl;
+#endif
+  return;
+}
+
+void Lddc::FillPointsToPclMsg(const StoragePacket& pkg, PointCloud& pcl_msg) {
+#ifdef BUILDING_ROS1
+  if (pkg.points.empty()) {
+    return;
+  }
+
+  uint32_t points_num = pkg.points_num;
+  const std::vector<PointXyzlt>& points = pkg.points;
+  for (uint32_t i = 0; i < points_num; ++i) {
+    pcl::PointXYZI point;
+    point.x = points[i].x;
+    point.y = points[i].y;
+    point.z = points[i].z;
+    point.intensity = points[i].intensity;
+
+    pcl_msg.points.push_back(std::move(point));
+  }
+#elif defined BUILDING_ROS2
+  std::cout << "warning: pcl::PointCloud is not supported in ROS2, "
+            << "please check code logic" 
+            << std::endl;
+#endif
+  return;
+}
+
+void Lddc::PublishPclData(const uint8_t index, const uint64_t timestamp, const PointCloud& cloud) {
+#ifdef BUILDING_ROS1
+  PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index);
+  if (kOutputToRos == output_type_) {
+    publisher_ptr->publish(cloud);
+  } else {
+    if (bag_ && enable_lidar_bag_) {
+      bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), cloud);
+    }
+  }
+#elif defined BUILDING_ROS2
+  std::cout << "warning: pcl::PointCloud is not supported in ROS2, "
+            << "please check code logic" 
+            << std::endl;
+#endif
+  return;
+}
+
+void Lddc::InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp) {
+  imu_msg.header.frame_id = "livox_frame";
+
+  timestamp = imu_data.time_stamp;
+#ifdef BUILDING_ROS1
+  imu_msg.header.stamp = ros::Time(timestamp / 1000000000.0);  // to ros time stamp
+#elif defined BUILDING_ROS2
+  imu_msg.header.stamp = rclcpp::Time(timestamp);  // to ros time stamp
+#endif
+
+  imu_msg.angular_velocity.x = imu_data.gyro_x;
+  imu_msg.angular_velocity.y = imu_data.gyro_y;
+  imu_msg.angular_velocity.z = imu_data.gyro_z;
+  imu_msg.linear_acceleration.x = imu_data.acc_x;
+  imu_msg.linear_acceleration.y = imu_data.acc_y;
+  imu_msg.linear_acceleration.z = imu_data.acc_z;
+}
+
+void Lddc::PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index) {
+  ImuData imu_data;
+  if (!imu_data_queue.Pop(imu_data)) {
+    //printf("Publish imu data failed, imu data queue pop failed.\n");
+    return;
+  }
+
+  ImuMsg imu_msg;
+  uint64_t timestamp;
+  InitImuMsg(imu_data, imu_msg, timestamp);
+
+#ifdef BUILDING_ROS1
+  PublisherPtr publisher_ptr = GetCurrentImuPublisher(index);
+#elif defined BUILDING_ROS2
+  Publisher<ImuMsg>::SharedPtr publisher_ptr = std::dynamic_pointer_cast<Publisher<ImuMsg>>(GetCurrentImuPublisher(index));
+#endif
+
+  if (kOutputToRos == output_type_) {
+    publisher_ptr->publish(imu_msg);
+  } else {
+#ifdef BUILDING_ROS1
+    if (bag_ && enable_imu_bag_) {
+      bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), imu_msg);
+    }
+#endif
+  }
+}
+
+#ifdef BUILDING_ROS2
+std::shared_ptr<rclcpp::PublisherBase> Lddc::CreatePublisher(uint8_t msg_type,
+    std::string &topic_name, uint32_t queue_size) {
+    if (kPointCloud2Msg == msg_type) {
+      DRIVER_INFO(*cur_node_,
+          "%s publish use PointCloud2 format", topic_name.c_str());
+      return cur_node_->create_publisher<PointCloud2>(topic_name, queue_size);
+    } else if (kLivoxCustomMsg == msg_type) {
+      DRIVER_INFO(*cur_node_,
+          "%s publish use livox custom format", topic_name.c_str());
+      return cur_node_->create_publisher<CustomMsg>(topic_name, queue_size);
+    }
+#if 0
+    else if (kPclPxyziMsg == msg_type)  {
+      DRIVER_INFO(*cur_node_,
+          "%s publish use pcl PointXYZI format", topic_name.c_str());
+      return cur_node_->create_publisher<PointCloud>(topic_name, queue_size);
+    }
+#endif
+    else if (kLivoxImuMsg == msg_type)  {
+      DRIVER_INFO(*cur_node_,
+          "%s publish use imu format", topic_name.c_str());
+      return cur_node_->create_publisher<ImuMsg>(topic_name,
+          queue_size);
+    } else {
+      PublisherPtr null_publisher(nullptr);
+      return null_publisher;
+    }
+}
+#endif
+
+#ifdef BUILDING_ROS1
+PublisherPtr Lddc::GetCurrentPublisher(uint8_t index) {
+  ros::Publisher **pub = nullptr;
+  uint32_t queue_size = kMinEthPacketQueueSize;
+
+  if (use_multi_topic_) {
+    pub = &private_pub_[index];
+    queue_size = queue_size / 8; // queue size is 4 for only one lidar
+  } else {
+    pub = &global_pub_;
+    queue_size = queue_size * 8; // shared queue size is 256, for all lidars
+  }
+
+  if (*pub == nullptr) {
+    char name_str[48];
+    memset(name_str, 0, sizeof(name_str));
+    if (use_multi_topic_) {
+      std::string ip_string = IpNumToString(lds_->lidars_[index].handle);
+      snprintf(name_str, sizeof(name_str), "livox/lidar_%s",
+               ReplacePeriodByUnderline(ip_string).c_str());
+      DRIVER_INFO(*cur_node_, "Support multi topics.");
+    } else {
+      DRIVER_INFO(*cur_node_, "Support only one topic.");
+      snprintf(name_str, sizeof(name_str), "livox/lidar");
+    }
+
+    *pub = new ros::Publisher;
+    if (kPointCloud2Msg == transfer_format_) {
+      **pub =
+          cur_node_->GetNode().advertise<sensor_msgs::PointCloud2>(name_str, queue_size);
+      DRIVER_INFO(*cur_node_,
+          "%s publish use PointCloud2 format, set ROS publisher queue size %d",
+          name_str, queue_size);
+    } else if (kLivoxCustomMsg == transfer_format_) {
+      **pub = cur_node_->GetNode().advertise<livox_ros_driver2::CustomMsg>(name_str,
+                                                                queue_size);
+      DRIVER_INFO(*cur_node_,
+          "%s publish use livox custom format, set ROS publisher queue size %d",
+          name_str, queue_size);
+    } else if (kPclPxyziMsg == transfer_format_) {
+      **pub = cur_node_->GetNode().advertise<PointCloud>(name_str, queue_size);
+      DRIVER_INFO(*cur_node_,
+          "%s publish use pcl PointXYZI format, set ROS publisher queue "
+          "size %d",
+          name_str, queue_size);
+    }
+  }
+
+  return *pub;
+}
+
+PublisherPtr Lddc::GetCurrentImuPublisher(uint8_t handle) {
+  ros::Publisher **pub = nullptr;
+  uint32_t queue_size = kMinEthPacketQueueSize;
+
+  if (use_multi_topic_) {
+    pub = &private_imu_pub_[handle];
+    queue_size = queue_size * 2; // queue size is 64 for only one lidar
+  } else {
+    pub = &global_imu_pub_;
+    queue_size = queue_size * 8; // shared queue size is 256, for all lidars
+  }
+
+  if (*pub == nullptr) {
+    char name_str[48];
+    memset(name_str, 0, sizeof(name_str));
+    if (use_multi_topic_) {
+      DRIVER_INFO(*cur_node_, "Support multi topics.");
+      std::string ip_string = IpNumToString(lds_->lidars_[handle].handle);
+      snprintf(name_str, sizeof(name_str), "livox/imu_%s",
+               ReplacePeriodByUnderline(ip_string).c_str());
+    } else {
+      DRIVER_INFO(*cur_node_, "Support only one topic.");
+      snprintf(name_str, sizeof(name_str), "livox/imu");
+    }
+
+    *pub = new ros::Publisher;
+    **pub = cur_node_->GetNode().advertise<sensor_msgs::Imu>(name_str, queue_size);
+    DRIVER_INFO(*cur_node_, "%s publish imu data, set ROS publisher queue size %d", name_str,
+             queue_size);
+  }
+
+  return *pub;
+}
+#elif defined BUILDING_ROS2
+std::shared_ptr<rclcpp::PublisherBase> Lddc::GetCurrentPublisher(uint8_t handle) {
+  uint32_t queue_size = kMinEthPacketQueueSize;
+  if (use_multi_topic_) {
+    if (!private_pub_[handle]) {
+      char name_str[48];
+      memset(name_str, 0, sizeof(name_str));
+
+      std::string ip_string = IpNumToString(lds_->lidars_[handle].handle);
+      snprintf(name_str, sizeof(name_str), "livox/lidar_%s",
+          ReplacePeriodByUnderline(ip_string).c_str());
+      std::string topic_name(name_str);
+      queue_size = queue_size * 2; // queue size is 64 for only one lidar
+      private_pub_[handle] = CreatePublisher(transfer_format_, topic_name, queue_size);
+    }
+    return private_pub_[handle];
+  } else {
+    if (!global_pub_) {
+      std::string topic_name("livox/lidar");
+      queue_size = queue_size * 8; // shared queue size is 256, for all lidars
+      global_pub_ = CreatePublisher(transfer_format_, topic_name, queue_size);
+    }
+    return global_pub_;
+  }
+}
+
+std::shared_ptr<rclcpp::PublisherBase> Lddc::GetCurrentImuPublisher(uint8_t handle) {
+  uint32_t queue_size = kMinEthPacketQueueSize;
+  if (use_multi_topic_) {
+    if (!private_imu_pub_[handle]) {
+      char name_str[48];
+      memset(name_str, 0, sizeof(name_str));
+      std::string ip_string = IpNumToString(lds_->lidars_[handle].handle);
+      snprintf(name_str, sizeof(name_str), "livox/imu_%s",
+          ReplacePeriodByUnderline(ip_string).c_str());
+      std::string topic_name(name_str);
+      queue_size = queue_size * 2; // queue size is 64 for only one lidar
+      private_imu_pub_[handle] = CreatePublisher(kLivoxImuMsg, topic_name,
+          queue_size);
+    }
+    return private_imu_pub_[handle];
+  } else {
+    if (!global_imu_pub_) {
+      std::string topic_name("livox/imu");
+      queue_size = queue_size * 8; // shared queue size is 256, for all lidars
+      global_imu_pub_ = CreatePublisher(kLivoxImuMsg, topic_name, queue_size);
+    }
+    return global_imu_pub_;
+  }
+}
+#endif
+
+void Lddc::CreateBagFile(const std::string &file_name) {
+#ifdef BUILDING_ROS1
+  if (!bag_) {
+    bag_ = new rosbag::Bag;
+    bag_->open(file_name, rosbag::bagmode::Write);
+    DRIVER_INFO(*cur_node_, "Create bag file :%s!", file_name.c_str());
+  }
+#endif
+}
+
+}  // namespace livox_ros
diff --git a/livox_ros_driver2/src/lddc.h b/livox_ros_driver2/src/lddc.h
new file mode 100644
index 0000000..6d146d1
--- /dev/null
+++ b/livox_ros_driver2/src/lddc.h
@@ -0,0 +1,163 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER2_LDDC_H_
+#define LIVOX_ROS_DRIVER2_LDDC_H_
+
+#include "include/livox_ros_driver2.h"
+
+#include "driver_node.h"
+#include "lds.h"
+
+namespace livox_ros {
+
+/** Send pointcloud message Data to ros subscriber or save them in rosbag file */
+typedef enum {
+  kOutputToRos = 0,
+  kOutputToRosBagFile = 1,
+} DestinationOfMessageOutput;
+
+/** The message type of transfer */
+typedef enum {
+  kPointCloud2Msg = 0,
+  kLivoxCustomMsg = 1,
+  kPclPxyziMsg = 2,
+  kLivoxImuMsg = 3,
+} TransferType;
+
+/** Type-Definitions based on ROS versions */
+#ifdef BUILDING_ROS1
+using Publisher = ros::Publisher;
+using PublisherPtr = ros::Publisher*;
+using PointCloud2 = sensor_msgs::PointCloud2;
+using PointField = sensor_msgs::PointField;
+using CustomMsg = livox_ros_driver2::CustomMsg;
+using CustomPoint = livox_ros_driver2::CustomPoint;
+using ImuMsg = sensor_msgs::Imu;
+#elif defined BUILDING_ROS2
+template <typename MessageT> using Publisher = rclcpp::Publisher<MessageT>;
+using PublisherPtr = std::shared_ptr<rclcpp::PublisherBase>;
+using PointCloud2 = sensor_msgs::msg::PointCloud2;
+using PointField = sensor_msgs::msg::PointField;
+using CustomMsg = livox_ros_driver2::msg::CustomMsg;
+using CustomPoint = livox_ros_driver2::msg::CustomPoint;
+using ImuMsg = sensor_msgs::msg::Imu;
+#endif
+
+using PointCloud = pcl::PointCloud<pcl::PointXYZI>;
+
+class DriverNode;
+
+class Lddc final {
+ public:
+#ifdef BUILDING_ROS1
+  Lddc(int format, int multi_topic, int data_src, int output_type, double frq,
+      std::string &frame_id, bool lidar_bag, bool imu_bag);
+#elif defined BUILDING_ROS2
+  Lddc(int format, int multi_topic, int data_src, int output_type, double frq,
+      std::string &frame_id);
+#endif
+  ~Lddc();
+
+  int RegisterLds(Lds *lds);
+  void DistributePointCloudData(void);
+  void DistributeImuData(void);
+  void CreateBagFile(const std::string &file_name);
+  void PrepareExit(void);
+
+  uint8_t GetTransferFormat(void) { return transfer_format_; }
+  uint8_t IsMultiTopic(void) { return use_multi_topic_; }
+  void SetRosNode(livox_ros::DriverNode *node) { cur_node_ = node; }
+
+  // void SetRosPub(ros::Publisher *pub) { global_pub_ = pub; };  // NOT USED
+  void SetPublishFrq(uint32_t frq) { publish_frq_ = frq; }
+
+ public:
+  Lds *lds_;
+
+ private:
+  void PollingLidarPointCloudData(uint8_t index, LidarDevice *lidar);
+  void PollingLidarImuData(uint8_t index, LidarDevice *lidar);
+
+  void PublishPointcloud2(LidarDataQueue *queue, uint8_t index);
+  void PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index);
+  void PublishPclMsg(LidarDataQueue *queue, uint8_t index);
+
+  void PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index);
+
+  void InitPointcloud2MsgHeader(PointCloud2& cloud);
+  void InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint64_t& timestamp);
+  void PublishPointcloud2Data(const uint8_t index, uint64_t timestamp, const PointCloud2& cloud);
+
+  void InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index);
+  void FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg);
+  void PublishCustomPointData(const CustomMsg& livox_msg, const uint8_t index);
+
+  void InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp);
+  void FillPointsToPclMsg(const StoragePacket& pkg, PointCloud& pcl_msg);
+  void PublishPclData(const uint8_t index, const uint64_t timestamp, const PointCloud& cloud);
+
+  void InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp);
+
+  void FillPointsToPclMsg(PointCloud& pcl_msg, LivoxPointXyzrtlt* src_point, uint32_t num);
+  void FillPointsToCustomMsg(CustomMsg& livox_msg, LivoxPointXyzrtlt* src_point, uint32_t num,
+      uint32_t offset_time, uint32_t point_interval, uint32_t echo_num);
+
+#ifdef BUILDING_ROS2
+  PublisherPtr CreatePublisher(uint8_t msg_type, std::string &topic_name, uint32_t queue_size);
+#endif
+
+  PublisherPtr GetCurrentPublisher(uint8_t index);
+  PublisherPtr GetCurrentImuPublisher(uint8_t index);
+
+ private:
+  uint8_t transfer_format_;
+  uint8_t use_multi_topic_;
+  uint8_t data_src_;
+  uint8_t output_type_;
+  double publish_frq_;
+  uint32_t publish_period_ns_;
+  std::string frame_id_;
+
+#ifdef BUILDING_ROS1
+  bool enable_lidar_bag_;
+  bool enable_imu_bag_;
+  PublisherPtr private_pub_[kMaxSourceLidar];
+  PublisherPtr global_pub_;
+  PublisherPtr private_imu_pub_[kMaxSourceLidar];
+  PublisherPtr global_imu_pub_;
+  rosbag::Bag *bag_;
+#elif defined BUILDING_ROS2
+  PublisherPtr private_pub_[kMaxSourceLidar];
+  PublisherPtr global_pub_;
+  PublisherPtr private_imu_pub_[kMaxSourceLidar];
+  PublisherPtr global_imu_pub_;
+#endif
+
+  livox_ros::DriverNode *cur_node_;
+};
+
+}  // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER2_LDDC_H_
diff --git a/livox_ros_driver2/src/lds.cpp b/livox_ros_driver2/src/lds.cpp
new file mode 100644
index 0000000..ca98462
--- /dev/null
+++ b/livox_ros_driver2/src/lds.cpp
@@ -0,0 +1,200 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <time.h>
+#include <chrono>
+#include <algorithm>
+
+#include "lds.h"
+#include "comm/ldq.h"
+
+namespace livox_ros {
+
+CacheIndex Lds::cache_index_;
+
+/* Member function --------------------------------------------------------- */
+Lds::Lds(const double publish_freq, const uint8_t data_src)
+    : lidar_count_(kMaxSourceLidar),
+      pcd_semaphore_(0),
+      imu_semaphore_(0),
+      publish_freq_(publish_freq),
+      data_src_(data_src),
+      request_exit_(false) {
+  ResetLds(data_src_);
+}
+
+Lds::~Lds() {
+  lidar_count_ = 0;
+  ResetLds(0);
+  printf("lds destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
+}
+
+void Lds::ResetLidar(LidarDevice *lidar, uint8_t data_src) {
+  //cache_index_.ResetIndex(lidar);
+  DeInitQueue(&lidar->data);
+  lidar->imu_data.Clear();
+
+  lidar->data_src = data_src;
+  lidar->connect_state = kConnectStateOff;
+}
+
+void Lds::SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src) {
+  lidar->data_src = data_src;
+}
+
+void Lds::ResetLds(uint8_t data_src) {
+  lidar_count_ = kMaxSourceLidar;
+  for (uint32_t i = 0; i < kMaxSourceLidar; i++) {
+    ResetLidar(&lidars_[i], data_src);
+  }
+}
+
+void Lds::RequestExit() {
+  request_exit_ = true;
+}
+
+bool Lds::IsAllQueueEmpty() {
+  for (int i = 0; i < lidar_count_; i++) {
+    if (!QueueIsEmpty(&lidars_[i].data)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+bool Lds::IsAllQueueReadStop() {
+  for (int i = 0; i < lidar_count_; i++) {
+    uint32_t data_size = QueueUsedSize(&lidars_[i].data);
+    if (data_size) {
+      return false;
+    }
+  }
+  return true;
+}
+
+void Lds::StorageImuData(ImuData* imu_data) {
+  uint32_t device_num = 0;
+  if (imu_data->lidar_type == kLivoxLidarType) {
+    device_num = imu_data->handle;
+  } else {
+    printf("Storage imu data failed, unknown lidar type:%u.\n", imu_data->lidar_type);
+    return;
+  }
+
+  uint8_t index = 0;
+  int ret = cache_index_.GetIndex(imu_data->lidar_type, device_num, index);
+  if (ret != 0) {
+    printf("Storage point data failed, can not get index, lidar type:%u, device_num:%u.\n", imu_data->lidar_type, device_num);
+    return;
+  }
+
+  LidarDevice *p_lidar = &lidars_[index];
+  LidarImuDataQueue* imu_queue = &p_lidar->imu_data;
+  imu_queue->Push(imu_data);
+  if (!imu_queue->Empty()) {
+    if (imu_semaphore_.GetCount() <= 0) {
+      imu_semaphore_.Signal();
+    }
+  }
+}
+
+void Lds::StorageLvxPointData(PointFrame* frame) {
+  if (frame == nullptr) {
+    return;
+  }
+
+  uint8_t lidar_number = frame->lidar_num;
+  for (uint i = 0; i < lidar_number; ++i) {
+    PointPacket& lidar_point = frame->lidar_point[i];
+
+    uint64_t base_time = frame->base_time[i];
+    uint8_t index = 0;
+    int8_t ret = cache_index_.LvxGetIndex(lidar_point.lidar_type, lidar_point.handle, index);
+    if (ret != 0) {
+      printf("Storage lvx point data failed, lidar type:%u, device num:%u.\n", lidar_point.lidar_type, lidar_point.handle);
+      continue;
+    }
+
+    lidars_[index].connect_state = kConnectStateSampling;
+
+    PushLidarData(&lidar_point, index, base_time);
+  }
+}
+
+void Lds::StoragePointData(PointFrame* frame) {
+  if (frame == nullptr) {
+    return;
+  }
+
+  uint8_t lidar_number = frame->lidar_num;
+  for (uint i = 0; i < lidar_number; ++i) {
+    PointPacket& lidar_point = frame->lidar_point[i];
+    //printf("StoragePointData, lidar_type:%u, point_num:%lu.\n", lidar_point.lidar_type, lidar_point.points_num);
+
+    uint64_t base_time = frame->base_time[i];
+
+    uint8_t index = 0;
+    int8_t ret = cache_index_.GetIndex(lidar_point.lidar_type, lidar_point.handle, index);
+    if (ret != 0) {
+      printf("Storage point data failed, lidar type:%u, handle:%u.\n", lidar_point.lidar_type, lidar_point.handle);
+      continue;
+    }
+    PushLidarData(&lidar_point, index, base_time);
+  }
+}
+
+void Lds::PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint64_t base_time) {
+  if (lidar_data == nullptr) {
+    return;
+  }
+
+  LidarDevice *p_lidar = &lidars_[index];
+  LidarDataQueue *queue = &p_lidar->data;
+
+  if (nullptr == queue->storage_packet) {
+    uint32_t queue_size = CalculatePacketQueueSize(publish_freq_);
+    InitQueue(queue, queue_size);
+    printf("Lidar[%u] storage queue size: %u\n", index, queue_size);
+  }
+
+  if (!QueueIsFull(queue)) {
+    QueuePushAny(queue, (uint8_t *)lidar_data, base_time);
+    if (!QueueIsEmpty(queue)) {
+      if (pcd_semaphore_.GetCount() <= 0) {
+        pcd_semaphore_.Signal();
+      }
+    }
+  } else {
+    if (pcd_semaphore_.GetCount() <= 0) {
+        pcd_semaphore_.Signal();
+    }
+  }
+}
+
+void Lds::PrepareExit(void) {}
+
+}  // namespace livox_ros
diff --git a/livox_ros_driver2/src/lds.h b/livox_ros_driver2/src/lds.h
new file mode 100644
index 0000000..c61eeea
--- /dev/null
+++ b/livox_ros_driver2/src/lds.h
@@ -0,0 +1,83 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// 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.
+//
+
+// livox lidar data source
+
+#ifndef LIVOX_ROS_DRIVER_LDS_H_
+#define LIVOX_ROS_DRIVER_LDS_H_
+
+#include <map>
+
+#include "comm/semaphore.h"
+#include "comm/comm.h"
+#include "comm/cache_index.h"
+
+namespace livox_ros {
+/**
+ * Lidar data source abstract.
+ */
+class Lds {
+ public:
+  Lds(const double publish_freq, const uint8_t data_src);
+  virtual ~Lds();
+
+  void StorageImuData(ImuData* imu_data);
+  void StoragePointData(PointFrame* frame);
+  void StorageLvxPointData(PointFrame* frame);
+
+  int8_t GetHandle(const uint8_t lidar_type, const PointPacket* lidar_point);
+  void PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint64_t base_time);
+
+  static void ResetLidar(LidarDevice *lidar, uint8_t data_src);
+  static void SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src);
+  void ResetLds(uint8_t data_src);
+
+  void RequestExit();
+
+  bool IsAllQueueEmpty();
+  bool IsAllQueueReadStop();
+
+  void CleanRequestExit() { request_exit_ = false; }
+  bool IsRequestExit() { return request_exit_; }
+  virtual void PrepareExit(void);
+
+  // get publishing frequency
+  double GetLdsFrequency() { return publish_freq_; }
+
+ public:
+  uint8_t lidar_count_;                 /**< Lidar access handle. */
+  LidarDevice lidars_[kMaxSourceLidar]; /**< The index is the handle */
+  Semaphore pcd_semaphore_;
+  Semaphore imu_semaphore_;
+  static CacheIndex cache_index_;
+ protected:
+  double publish_freq_;
+  uint8_t data_src_;
+ private:
+  volatile bool request_exit_;
+};
+
+}  // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER_LDS_H_
diff --git a/livox_ros_driver2/src/lds_lidar.cpp b/livox_ros_driver2/src/lds_lidar.cpp
new file mode 100644
index 0000000..6c739b0
--- /dev/null
+++ b/livox_ros_driver2/src/lds_lidar.cpp
@@ -0,0 +1,214 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "lds_lidar.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <memory>
+#include <mutex>
+#include <thread>
+
+#ifdef WIN32
+#include <winsock2.h>
+#include <ws2def.h>
+#pragma comment(lib, "Ws2_32.lib")
+#else
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#endif // WIN32
+
+#include "comm/comm.h"
+#include "comm/pub_handler.h"
+
+#include "parse_cfg_file/parse_cfg_file.h"
+#include "parse_cfg_file/parse_livox_lidar_cfg.h"
+
+#include "call_back/lidar_common_callback.h"
+#include "call_back/livox_lidar_callback.h"
+
+using namespace std;
+
+namespace livox_ros {
+
+/** Const varible ------------------------------------------------------------*/
+/** For callback use only */
+LdsLidar *g_lds_ldiar = nullptr;
+
+/** Global function for common use -------------------------------------------*/
+
+/** Lds lidar function -------------------------------------------------------*/
+LdsLidar::LdsLidar(double publish_freq)
+    : Lds(publish_freq, kSourceRawLidar), 
+      auto_connect_mode_(true),
+      whitelist_count_(0),
+      is_initialized_(false) {
+  memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
+  ResetLdsLidar();
+}
+
+LdsLidar::~LdsLidar() {}
+
+void LdsLidar::ResetLdsLidar(void) { ResetLds(kSourceRawLidar); }
+
+
+
+bool LdsLidar::InitLdsLidar(const std::string& path_name) {
+  if (is_initialized_) {
+    printf("Lds is already inited!\n");
+    return false;
+  }
+
+  if (g_lds_ldiar == nullptr) {
+    g_lds_ldiar = this;
+  }
+
+  path_ = path_name;
+  if (!InitLidars()) {
+    return false;
+  }
+  SetLidarPubHandle();
+  if (!Start()) {
+    return false;
+  }
+  is_initialized_ = true;
+  return true;
+}
+
+bool LdsLidar::InitLidars() {
+  if (!ParseSummaryConfig()) {
+    return false;
+  }
+  std::cout << "config lidar type: " << static_cast<int>(lidar_summary_info_.lidar_type) << std::endl;
+
+  if (lidar_summary_info_.lidar_type & kLivoxLidarType) {
+    if (!InitLivoxLidar()) {
+      return false;
+    }
+  }
+  return true;
+}
+
+
+bool LdsLidar::Start() {
+  if (lidar_summary_info_.lidar_type & kLivoxLidarType) {
+    if (!LivoxLidarStart()) {
+      return false;
+    }
+  }
+  return true;
+}
+
+bool LdsLidar::ParseSummaryConfig() {
+  return ParseCfgFile(path_).ParseSummaryInfo(lidar_summary_info_);
+}
+
+bool LdsLidar::InitLivoxLidar() {
+#ifdef BUILDING_ROS2
+  DisableLivoxSdkConsoleLogger();
+#endif
+
+  // parse user config
+  LivoxLidarConfigParser parser(path_);
+  std::vector<UserLivoxLidarConfig> user_configs;
+  if (!parser.Parse(user_configs)) {
+    std::cout << "failed to parse user-defined config" << std::endl;
+  }
+
+  // SDK initialization
+  if (!LivoxLidarSdkInit(path_.c_str())) {
+    std::cout << "Failed to init livox lidar sdk." << std::endl;
+    return false;
+  }
+
+  // fill in lidar devices
+  for (auto& config : user_configs) {
+    uint8_t index = 0;
+    int8_t ret = g_lds_ldiar->cache_index_.GetFreeIndex(kLivoxLidarType, config.handle, index);
+    if (ret != 0) {
+      std::cout << "failed to get free index, lidar ip: " << IpNumToString(config.handle) << std::endl;
+      continue;
+    }
+    LidarDevice *p_lidar = &(g_lds_ldiar->lidars_[index]);
+    p_lidar->lidar_type = kLivoxLidarType;
+    p_lidar->livox_config = config;
+    p_lidar->handle = config.handle;
+
+    LidarExtParameter lidar_param;
+    lidar_param.handle = config.handle;
+    lidar_param.lidar_type = kLivoxLidarType;
+    if (config.pcl_data_type == kLivoxLidarCartesianCoordinateLowData) {
+      // temporary resolution
+      lidar_param.param.roll  = config.extrinsic_param.roll;
+      lidar_param.param.pitch = config.extrinsic_param.pitch;
+      lidar_param.param.yaw   = config.extrinsic_param.yaw;
+      lidar_param.param.x     = config.extrinsic_param.x / 10;
+      lidar_param.param.y     = config.extrinsic_param.y / 10;
+      lidar_param.param.z     = config.extrinsic_param.z / 10;
+    } else {
+      lidar_param.param.roll  = config.extrinsic_param.roll;
+      lidar_param.param.pitch = config.extrinsic_param.pitch;
+      lidar_param.param.yaw   = config.extrinsic_param.yaw;
+      lidar_param.param.x     = config.extrinsic_param.x;
+      lidar_param.param.y     = config.extrinsic_param.y;
+      lidar_param.param.z     = config.extrinsic_param.z;
+    }
+    pub_handler().AddLidarsExtParam(lidar_param);
+  }
+
+  SetLivoxLidarInfoChangeCallback(LivoxLidarCallback::LidarInfoChangeCallback, g_lds_ldiar);
+  return true;
+}
+
+void LdsLidar::SetLidarPubHandle() {
+  pub_handler().SetPointCloudsCallback(LidarCommonCallback::OnLidarPointClounCb, g_lds_ldiar);
+  pub_handler().SetImuDataCallback(LidarCommonCallback::LidarImuDataCallback, g_lds_ldiar);
+
+  double publish_freq = Lds::GetLdsFrequency();
+  pub_handler().SetPointCloudConfig(publish_freq);
+}
+
+bool LdsLidar::LivoxLidarStart() {
+  return true;
+}
+
+int LdsLidar::DeInitLdsLidar(void) {
+  if (!is_initialized_) {
+    printf("LiDAR data source is not exit");
+    return -1;
+  }
+
+  if (lidar_summary_info_.lidar_type & kLivoxLidarType) {
+    LivoxLidarSdkUninit();
+    printf("Livox Lidar SDK Deinit completely!\n");
+  }
+
+  return 0;
+}
+
+void LdsLidar::PrepareExit(void) { DeInitLdsLidar(); }
+
+}  // namespace livox_ros
diff --git a/livox_ros_driver2/src/lds_lidar.h b/livox_ros_driver2/src/lds_lidar.h
new file mode 100644
index 0000000..b35c9c2
--- /dev/null
+++ b/livox_ros_driver2/src/lds_lidar.h
@@ -0,0 +1,95 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// 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.
+//
+
+/** Livox LiDAR data source, data from dependent lidar */
+
+#ifndef LIVOX_ROS_DRIVER_LDS_LIDAR_H_
+#define LIVOX_ROS_DRIVER_LDS_LIDAR_H_
+
+#include <memory>
+#include <mutex>
+#include <vector>
+
+#include "lds.h"
+#include "comm/comm.h"
+
+#include "livox_lidar_api.h"
+#include "livox_lidar_def.h"
+
+#include "rapidjson/document.h"
+
+namespace livox_ros {
+
+class LdsLidar final : public Lds {
+ public:
+  static LdsLidar *GetInstance(double publish_freq) {
+    printf("LdsLidar *GetInstance\n");
+    static LdsLidar lds_lidar(publish_freq);
+    return &lds_lidar;
+  }
+
+  bool InitLdsLidar(const std::string& path_name);
+  bool Start();
+
+  int DeInitLdsLidar(void);
+ private:
+  LdsLidar(double publish_freq);
+  LdsLidar(const LdsLidar &) = delete;
+  ~LdsLidar();
+  LdsLidar &operator=(const LdsLidar &) = delete;
+
+  bool ParseSummaryConfig();
+
+  bool InitLidars();
+  bool InitLivoxLidar();    // for new SDK
+
+  bool LivoxLidarStart();
+
+  void ResetLdsLidar(void);
+
+  void SetLidarPubHandle();
+
+	// auto connect mode
+	void EnableAutoConnectMode(void) { auto_connect_mode_ = true; }
+  void DisableAutoConnectMode(void) { auto_connect_mode_ = false; }
+  bool IsAutoConnectMode(void) { return auto_connect_mode_; }
+
+  virtual void PrepareExit(void);
+
+ public:
+  std::mutex config_mutex_;
+
+ private:
+  std::string path_;
+  LidarSummaryInfo lidar_summary_info_;
+
+  bool auto_connect_mode_;
+  uint32_t whitelist_count_;
+  volatile bool is_initialized_;
+  char broadcast_code_whitelist_[kMaxLidarCount][kBroadcastCodeSize];
+};
+
+}  // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER_LDS_LIDAR_H_
diff --git a/livox_ros_driver2/src/livox_ros_driver2.cpp b/livox_ros_driver2/src/livox_ros_driver2.cpp
new file mode 100644
index 0000000..6dda05f
--- /dev/null
+++ b/livox_ros_driver2/src/livox_ros_driver2.cpp
@@ -0,0 +1,235 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include <iostream>
+#include <chrono>
+#include <vector>
+#include <csignal>
+#include <thread>
+
+#include "include/livox_ros_driver2.h"
+#include "include/ros_headers.h"
+#include "driver_node.h"
+#include "lddc.h"
+#include "lds_lidar.h"
+
+using namespace livox_ros;
+
+#ifdef BUILDING_ROS1
+int main(int argc, char **argv) {
+  /** Ros related */
+  if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) {
+    ros::console::notifyLoggerLevelsChanged();
+  }
+
+  ros::init(argc, argv, "livox_lidar_publisher");
+
+  // ros::NodeHandle livox_node;
+  livox_ros::DriverNode livox_node;
+
+  DRIVER_INFO(livox_node, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING);
+
+  /** Init default system parameter */
+  int xfer_format = kPointCloud2Msg;
+  int multi_topic = 0;
+  int data_src = kSourceRawLidar;
+  double publish_freq  = 10.0; /* Hz */
+  int output_type      = kOutputToRos;
+  std::string frame_id = "livox_frame";
+  bool lidar_bag = true;
+  bool imu_bag   = false;
+
+  livox_node.GetNode().getParam("xfer_format", xfer_format);
+  livox_node.GetNode().getParam("multi_topic", multi_topic);
+  livox_node.GetNode().getParam("data_src", data_src);
+  livox_node.GetNode().getParam("publish_freq", publish_freq);
+  livox_node.GetNode().getParam("output_data_type", output_type);
+  livox_node.GetNode().getParam("frame_id", frame_id);
+  livox_node.GetNode().getParam("enable_lidar_bag", lidar_bag);
+  livox_node.GetNode().getParam("enable_imu_bag", imu_bag);
+
+  printf("data source:%u.\n", data_src);
+
+  if (publish_freq > 100.0) {
+    publish_freq = 100.0;
+  } else if (publish_freq < 0.5) {
+    publish_freq = 0.5;
+  } else {
+    publish_freq = publish_freq;
+  }
+
+  livox_node.future_ = livox_node.exit_signal_.get_future();
+
+  /** Lidar data distribute control and lidar data source set */
+  livox_node.lddc_ptr_ = std::make_unique<Lddc>(xfer_format, multi_topic, data_src, output_type,
+                        publish_freq, frame_id, lidar_bag, imu_bag);
+  livox_node.lddc_ptr_->SetRosNode(&livox_node);
+
+  if (data_src == kSourceRawLidar) {
+    DRIVER_INFO(livox_node, "Data Source is raw lidar.");
+
+    std::string user_config_path;
+    livox_node.getParam("user_config_path", user_config_path);
+    DRIVER_INFO(livox_node, "Config file : %s", user_config_path.c_str());
+
+    LdsLidar *read_lidar = LdsLidar::GetInstance(publish_freq);
+    livox_node.lddc_ptr_->RegisterLds(static_cast<Lds *>(read_lidar));
+
+    if ((read_lidar->InitLdsLidar(user_config_path))) {
+      DRIVER_INFO(livox_node, "Init lds lidar successfully!");
+    } else {
+      DRIVER_ERROR(livox_node, "Init lds lidar failed!");
+    }
+  } else {
+    DRIVER_ERROR(livox_node, "Invalid data src (%d), please check the launch file", data_src);
+  }
+
+  livox_node.pointclouddata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::PointCloudDataPollThread, &livox_node);
+  livox_node.imudata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::ImuDataPollThread, &livox_node);
+  while (ros::ok()) { usleep(10000); }
+
+  return 0;
+}
+
+#elif defined BUILDING_ROS2
+namespace livox_ros
+{
+DriverNode::DriverNode(const rclcpp::NodeOptions & node_options)
+: Node("livox_driver_node", node_options)
+{
+  DRIVER_INFO(*this, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING);
+
+  /** Init default system parameter */
+  int xfer_format = kPointCloud2Msg;
+  int multi_topic = 0;
+  int data_src = kSourceRawLidar;
+  double publish_freq = 10.0; /* Hz */
+  int output_type = kOutputToRos;
+  std::string frame_id;
+
+  this->declare_parameter("xfer_format", xfer_format);
+  this->declare_parameter("multi_topic", 0);
+  this->declare_parameter("data_src", data_src);
+  this->declare_parameter("publish_freq", 10.0);
+  this->declare_parameter("output_data_type", output_type);
+  this->declare_parameter("frame_id", "frame_default");
+  this->declare_parameter("user_config_path", "path_default");
+  this->declare_parameter("cmdline_input_bd_code", "000000000000001");
+  this->declare_parameter("lvx_file_path", "/home/livox/livox_test.lvx");
+
+  this->get_parameter("xfer_format", xfer_format);
+  this->get_parameter("multi_topic", multi_topic);
+  this->get_parameter("data_src", data_src);
+  this->get_parameter("publish_freq", publish_freq);
+  this->get_parameter("output_data_type", output_type);
+  this->get_parameter("frame_id", frame_id);
+
+  if (publish_freq > 100.0) {
+    publish_freq = 100.0;
+  } else if (publish_freq < 0.5) {
+    publish_freq = 0.5;
+  } else {
+    publish_freq = publish_freq;
+  }
+
+  future_ = exit_signal_.get_future();
+
+  /** Lidar data distribute control and lidar data source set */
+  lddc_ptr_ = std::make_unique<Lddc>(xfer_format, multi_topic, data_src, output_type, publish_freq, frame_id);
+  lddc_ptr_->SetRosNode(this);
+
+  if (data_src == kSourceRawLidar) {
+    DRIVER_INFO(*this, "Data Source is raw lidar.");
+
+    std::string user_config_path;
+    this->get_parameter("user_config_path", user_config_path);
+    DRIVER_INFO(*this, "Config file : %s", user_config_path.c_str());
+
+    std::string cmdline_bd_code;
+    this->get_parameter("cmdline_input_bd_code", cmdline_bd_code);
+
+    LdsLidar *read_lidar = LdsLidar::GetInstance(publish_freq);
+    lddc_ptr_->RegisterLds(static_cast<Lds *>(read_lidar));
+
+    if ((read_lidar->InitLdsLidar(user_config_path))) {
+      DRIVER_INFO(*this, "Init lds lidar success!");
+    } else {
+      DRIVER_ERROR(*this, "Init lds lidar fail!");
+    }
+  } else {
+    DRIVER_ERROR(*this, "Invalid data src (%d), please check the launch file", data_src);
+  }
+
+  pointclouddata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::PointCloudDataPollThread, this);
+  imudata_poll_thread_ = std::make_shared<std::thread>(&DriverNode::ImuDataPollThread, this);
+}
+
+}  // namespace livox_ros
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(livox_ros::DriverNode)
+
+#endif  // defined BUILDING_ROS2
+
+
+void DriverNode::PointCloudDataPollThread()
+{
+  std::future_status status;
+  std::this_thread::sleep_for(std::chrono::seconds(3));
+  do {
+    lddc_ptr_->DistributePointCloudData();
+    status = future_.wait_for(std::chrono::microseconds(0));
+  } while (status == std::future_status::timeout);
+}
+
+void DriverNode::ImuDataPollThread()
+{
+  std::future_status status;
+  std::this_thread::sleep_for(std::chrono::seconds(3));
+  do {
+    lddc_ptr_->DistributeImuData();
+    status = future_.wait_for(std::chrono::microseconds(0));
+  } while (status == std::future_status::timeout);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.cpp b/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.cpp
new file mode 100644
index 0000000..d967087
--- /dev/null
+++ b/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.cpp
@@ -0,0 +1,67 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "parse_cfg_file.h"
+
+#include <iostream>
+#include <cstdio>
+#include <arpa/inet.h>
+
+namespace livox_ros {
+
+ParseCfgFile::ParseCfgFile(const std::string& path) : path_(path) {}
+
+bool ParseCfgFile::ParseSummaryInfo(LidarSummaryInfo& lidar_summary_info) {
+  FILE* raw_file = std::fopen(path_.c_str(), "rb");
+  if (!raw_file) {
+    std::cout << "parse summary info failed, can not open file: " << path_ << std::endl;
+    return false;
+  }
+
+  char read_buffer[kMaxBufferSize];
+  rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer));
+  rapidjson::Document doc;
+  do {
+    if (doc.ParseStream(config_file).HasParseError()) {
+      break;
+    }
+    if (!doc.HasMember("lidar_summary_info") || !doc["lidar_summary_info"].IsObject()) {
+      break;
+    }
+    const rapidjson::Value &object = doc["lidar_summary_info"];
+    if (!object.HasMember("lidar_type") || !object["lidar_type"].IsUint()) {
+      break;
+    }
+    lidar_summary_info.lidar_type = static_cast<uint8_t>(object["lidar_type"].GetUint());
+    std::fclose(raw_file);
+    return true;
+  } while (false);
+
+  std::cout << "parse lidar type failed." << std::endl;
+  std::fclose(raw_file);
+  return false;
+}
+
+} // namespace livox_ros
+
diff --git a/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.h b/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.h
new file mode 100644
index 0000000..8d2db1c
--- /dev/null
+++ b/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.h
@@ -0,0 +1,52 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_
+#define LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_
+
+#include "../comm/comm.h"
+
+#include "rapidjson/document.h"
+#include "rapidjson/filereadstream.h"
+#include "rapidjson/stringbuffer.h"
+
+#include <string>
+#include <vector>
+
+namespace livox_ros {
+
+class ParseCfgFile {
+ public:
+  explicit ParseCfgFile(const std::string& path);
+  ~ParseCfgFile() {}
+
+  bool ParseSummaryInfo(LidarSummaryInfo& lidar_summary_info);
+  
+ private:
+  const std::string path_;
+};
+
+} // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_
diff --git a/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.cpp b/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.cpp
new file mode 100644
index 0000000..9360733
--- /dev/null
+++ b/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.cpp
@@ -0,0 +1,156 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "parse_livox_lidar_cfg.h"
+#include <iostream>
+
+namespace livox_ros {
+
+bool LivoxLidarConfigParser::Parse(std::vector<UserLivoxLidarConfig> &lidar_configs) {
+  FILE* raw_file = std::fopen(path_.c_str(), "rb");
+  if (!raw_file) {
+    std::cout << "failed to open config file: " << path_ << std::endl;
+    return false;
+  }
+
+  lidar_configs.clear();
+  char read_buffer[kMaxBufferSize];
+  rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer));
+  rapidjson::Document doc;
+
+  do {
+    if (doc.ParseStream(config_file).HasParseError()) {
+      std::cout << "failed to parse config jason" << std::endl;
+      break;
+    }
+    if (!doc.HasMember("lidar_configs") ||
+        !doc["lidar_configs"].IsArray() ||
+        0 == doc["lidar_configs"].Size()) {
+      std::cout << "there is no user-defined config" << std::endl;
+      break;
+    }
+    if (!ParseUserConfigs(doc, lidar_configs)) {
+      std::cout << "failed to parse basic configs" << std::endl;
+      break;
+    }
+    return true;
+  } while (false);
+
+  std::fclose(raw_file);
+  return false;
+}
+
+bool LivoxLidarConfigParser::ParseUserConfigs(const rapidjson::Document &doc,
+                                              std::vector<UserLivoxLidarConfig> &user_configs) {
+  const rapidjson::Value &lidar_configs = doc["lidar_configs"];
+  for (auto &config : lidar_configs.GetArray()) {
+    if (!config.HasMember("ip")) {
+      continue;
+    }
+    UserLivoxLidarConfig user_config;
+
+    // parse user configs
+    user_config.handle = IpStringToNum(std::string(config["ip"].GetString()));
+    if (!config.HasMember("pcl_data_type")) {
+      user_config.pcl_data_type = -1;
+    } else {
+      user_config.pcl_data_type = static_cast<int8_t>(config["pcl_data_type"].GetInt());
+    }
+    if (!config.HasMember("pattern_mode")) {
+      user_config.pattern_mode = -1;
+    } else {
+      user_config.pattern_mode = static_cast<int8_t>(config["pattern_mode"].GetInt());
+    }
+    if (!config.HasMember("blind_spot_set")) {
+      user_config.blind_spot_set = -1;
+    } else {
+      user_config.blind_spot_set = static_cast<int8_t>(config["blind_spot_set"].GetInt());
+    }
+    if (!config.HasMember("dual_emit_en")) {
+      user_config.dual_emit_en = -1;
+    } else {
+      user_config.dual_emit_en = static_cast<uint8_t>(config["dual_emit_en"].GetInt());
+    }
+    if (!config.HasMember("extrinsic_parameter")) {
+      memset(&user_config.extrinsic_param, 0, sizeof(user_config.extrinsic_param));
+    } else {
+      auto &value = config["extrinsic_parameter"];
+      if (!ParseExtrinsics(value, user_config.extrinsic_param)) {
+        memset(&user_config.extrinsic_param, 0, sizeof(user_config.extrinsic_param));
+        std::cout << "failed to parse extrinsic parameters, ip: "
+                  << IpNumToString(user_config.handle) << std::endl;
+      }
+    }
+    user_config.set_bits = 0;
+    user_config.get_bits = 0;
+
+    user_configs.push_back(user_config);
+  }
+
+  if (0 == user_configs.size()) {
+    std::cout << "no valid base configs" << std::endl;
+    return false;
+  }
+  std::cout << "successfully parse base config, counts: "
+            << user_configs.size() << std::endl;
+  return true;
+}
+
+bool LivoxLidarConfigParser::ParseExtrinsics(const rapidjson::Value &value,
+                                             ExtParameter &param) {
+  if (!value.HasMember("roll")) {
+    param.roll = 0.0f;
+  } else {
+    param.roll = static_cast<float>(value["roll"].GetFloat());
+  }
+  if (!value.HasMember("pitch")) {
+    param.pitch = 0.0f;
+  } else {
+    param.pitch = static_cast<float>(value["pitch"].GetFloat());
+  }
+  if (!value.HasMember("yaw")) {
+    param.yaw = 0.0f;
+  } else {
+    param.yaw = static_cast<float>(value["yaw"].GetFloat());
+  }
+  if (!value.HasMember("x")) {
+    param.x = 0;
+  } else {
+    param.x = static_cast<int32_t>(value["x"].GetInt());
+  }
+  if (!value.HasMember("y")) {
+    param.y = 0;
+  } else {
+    param.y = static_cast<int32_t>(value["y"].GetInt());
+  }
+  if (!value.HasMember("z")) {
+    param.z = 0;
+  } else {
+    param.z = static_cast<int32_t>(value["z"].GetInt());
+  }
+
+  return true;
+}
+
+} // namespace livox_ros
diff --git a/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.h b/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.h
new file mode 100644
index 0000000..8219f91
--- /dev/null
+++ b/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.h
@@ -0,0 +1,58 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_
+#define LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_
+
+#include "comm/comm.h"
+#include "livox_lidar_def.h"
+
+#include "rapidjson/document.h"
+#include "rapidjson/filereadstream.h"
+#include "rapidjson/stringbuffer.h"
+
+#include <iostream>
+#include <string>
+#include <vector>
+
+namespace livox_ros {
+  
+class LivoxLidarConfigParser {
+ public:
+  explicit LivoxLidarConfigParser(const std::string& path)  : path_(path) {}
+  ~LivoxLidarConfigParser() {}
+
+  bool Parse(std::vector<UserLivoxLidarConfig> &lidar_configs);
+
+ private:
+  bool ParseUserConfigs(const rapidjson::Document &doc,
+                         std::vector<UserLivoxLidarConfig> &user_configs);
+  bool ParseExtrinsics(const rapidjson::Value &value, ExtParameter &param);
+
+  const std::string path_;
+};
+
+} // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_
diff --git a/pcd2pgm/.clang-format b/pcd2pgm/.clang-format
new file mode 100644
index 0000000..aea802f
--- /dev/null
+++ b/pcd2pgm/.clang-format
@@ -0,0 +1,18 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+BraceWrapping:
+  AfterClass: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: false
diff --git a/pcd2pgm/.clang-tidy b/pcd2pgm/.clang-tidy
new file mode 100644
index 0000000..d805018
--- /dev/null
+++ b/pcd2pgm/.clang-tidy
@@ -0,0 +1,62 @@
+---
+Checks:          '-*,
+                  performance-*,
+                  -performance-unnecessary-value-param,
+                  llvm-namespace-comment,
+                  modernize-redundant-void-arg,
+                  modernize-use-nullptr,
+                  modernize-use-default,
+                  modernize-use-override,
+                  modernize-loop-convert,
+                  modernize-make-shared,
+                  modernize-make-unique,
+                  misc-unused-parameters,
+                  readability-named-parameter,
+                  readability-redundant-smartptr-get,
+                  readability-redundant-string-cstr,
+                  readability-simplify-boolean-expr,
+                  readability-container-size-empty,
+                  readability-identifier-naming,
+                  '
+HeaderFilterRegex: ''
+CheckOptions:
+  - key:             llvm-namespace-comment.ShortNamespaceLines
+    value:           '10'
+  - key:             llvm-namespace-comment.SpacesBeforeComments
+    value:           '2'
+  - key:             misc-unused-parameters.StrictMode
+    value:           '1'
+  - key:             readability-braces-around-statements.ShortStatementLines
+    value:           '2'
+  # type names
+  - key:             readability-identifier-naming.ClassCase
+    value:           CamelCase
+  - key:             readability-identifier-naming.EnumCase
+    value:           CamelCase
+  - key:             readability-identifier-naming.UnionCase
+    value:           CamelCase
+  # method names
+  - key:             readability-identifier-naming.MethodCase
+    value:           camelBack
+  # variable names
+  - key:             readability-identifier-naming.VariableCase
+    value:           lower_case
+  # class member names
+  - key:             readability-identifier-naming.PrivateMemberCase
+    value:           lower_case
+  - key:             readability-identifier-naming.PrivateMemberSuffix
+    value:           '_'
+  - key:             readability-identifier-naming.ProtectedMemberCase
+    value:           lower_case
+  - key:             readability-identifier-naming.ProtectedMemberSuffix
+    value:           '_'
+  # const static or global variables are UPPER_CASE
+  - key:             readability-identifier-naming.EnumConstantCase
+    value:           UPPER_CASE
+  - key:             readability-identifier-naming.StaticConstantCase
+    value:           UPPER_CASE
+  - key:             readability-identifier-naming.ClassConstantCase
+    value:           UPPER_CASE
+  - key:             readability-identifier-naming.GlobalVariableCase
+    value:           UPPER_CASE
+...
diff --git a/pcd2pgm/.docs/pcd.png b/pcd2pgm/.docs/pcd.png
new file mode 100644
index 0000000..2ac39fa
Binary files /dev/null and b/pcd2pgm/.docs/pcd.png differ
diff --git a/pcd2pgm/.docs/pgm.png b/pcd2pgm/.docs/pgm.png
new file mode 100644
index 0000000..d2f6f5b
Binary files /dev/null and b/pcd2pgm/.docs/pgm.png differ
diff --git a/pcd2pgm/.gitignore b/pcd2pgm/.gitignore
new file mode 100644
index 0000000..b0a66c2
--- /dev/null
+++ b/pcd2pgm/.gitignore
@@ -0,0 +1,12 @@
+devel/
+build/
+install/
+log/
+
+.vscode/
+.cache/
+__pycache__/
+.catkin_workspace
+*.gv
+*.pdf
+*.pcd
diff --git a/pcd2pgm/.pre-commit-config.yaml b/pcd2pgm/.pre-commit-config.yaml
new file mode 100644
index 0000000..2badd49
--- /dev/null
+++ b/pcd2pgm/.pre-commit-config.yaml
@@ -0,0 +1,127 @@
+# To use:
+#
+#     pre-commit run -a
+#
+# Or:
+#
+#     pre-commit install  # (runs every time you commit in git)
+#
+# To update this file:
+#
+#     pre-commit autoupdate
+#
+# See https://github.com/pre-commit/pre-commit
+
+repos:
+  # Standard hooks
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v5.0.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-ast
+      - id: check-case-conflict
+      - id: check-docstring-first
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: check-xml
+      - id: check-yaml
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+        exclude_types: [rst]
+      - id: fix-byte-order-marker
+
+  # Python hooks
+  - repo: https://github.com/asottile/pyupgrade
+    rev: v3.19.1
+    hooks:
+    -   id: pyupgrade
+        args: [--py36-plus]
+
+  - repo: https://github.com/astral-sh/ruff-pre-commit
+    rev: v0.8.4
+    hooks:
+      - id: ruff
+        args: [ --fix ]
+      - id: ruff-format
+
+  # CPP hooks
+  - repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v14.0.3
+    hooks:
+      - id: clang-format
+        args: ['-fallback-style=none', '-i']
+
+  - repo: local
+    hooks:
+      - id: ament_cppcheck
+        name: ament_cppcheck
+        description: Static code analysis of C/C++ files.
+        entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+
+  - repo: local
+    hooks:
+      - id: ament_cpplint
+        name: ament_cpplint
+        description: Static code analysis of C/C++ files.
+        entry: ament_cpplint
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+        args: ["--linelength=100", "--filter=-whitespace/newline"]
+
+  # Cmake hooks
+  - repo: local
+    hooks:
+      - id: ament_lint_cmake
+        name: ament_lint_cmake
+        description: Check format of CMakeLists.txt files.
+        entry: ament_lint_cmake
+        language: system
+        files: CMakeLists\.txt$
+
+  # Copyright
+  - repo: local
+    hooks:
+      - id: ament_copyright
+        name: ament_copyright
+        description: Check if copyright notice is available in all files.
+        entry: ament_copyright
+        language: system
+
+  # Docs - RestructuredText hooks
+  - repo: https://github.com/PyCQA/doc8
+    rev: v1.1.2
+    hooks:
+      - id: doc8
+        args: ['--max-line-length=100', '--ignore=D001']
+        exclude: CHANGELOG\.rst$
+
+  - repo: https://github.com/pre-commit/pygrep-hooks
+    rev: v1.10.0
+    hooks:
+      - id: rst-backticks
+        exclude: CHANGELOG\.rst$
+      - id: rst-directive-colons
+      - id: rst-inline-touching-normal
+
+  # Spellcheck in comments and docs
+  # skipping of *.svg files is not working...
+  - repo: https://github.com/codespell-project/codespell
+    rev: v2.3.0
+    hooks:
+      - id: codespell
+        args: ['--write-changes']
+        exclude: CHANGELOG\.rst|\.(svg|pyc)$
+
+  - repo: https://github.com/python-jsonschema/check-jsonschema
+    rev: 0.30.0
+    hooks:
+      - id: check-github-workflows
+        args: ["--verbose"]
+      - id: check-github-actions
+        args: ["--verbose"]
+      - id: check-dependabot
+        args: ["--verbose"]
diff --git a/pcd2pgm/CMakeLists.txt b/pcd2pgm/CMakeLists.txt
new file mode 100644
index 0000000..f07d404
--- /dev/null
+++ b/pcd2pgm/CMakeLists.txt
@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.8)
+project(pcd2pgm)
+
+## Use C++14
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## Export compile commands for clangd
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+#######################
+## Find dependencies ##
+#######################
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+
+###########
+## Build ##
+###########
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN pcd2pgm::Pcd2PgmNode
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+#############
+## Testing ##
+#############
+
+if(BUILD_TESTING)
+set(ament_cmake_clang_format_CONFIG_FILE "${CMAKE_SOURCE_DIR}/.clang-format")
+find_package(ament_lint_auto REQUIRED)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE
+    ament_cmake_uncrustify
+    ament_cmake_flake8
+  )
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+#############
+## Install ##
+#############
+
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  launch
+  rviz
+)
diff --git a/pcd2pgm/CONTRIBUTING.md b/pcd2pgm/CONTRIBUTING.md
new file mode 100644
index 0000000..331a805
--- /dev/null
+++ b/pcd2pgm/CONTRIBUTING.md
@@ -0,0 +1,63 @@
+# Contributing Guidelines
+
+Thank you for your interest in contributing to `pcd2pgm`.
+Whether it's a bug report, new feature, correction, or additional
+documentation, we greatly value feedback and contributions from our community.
+
+Please read through this document before submitting any issues or pull requests to ensure we have all the necessary
+information to effectively respond to your bug report or contribution.
+
+## Reporting Bugs/Feature Requests
+
+We welcome you to use the GitHub issue tracker to report bugs or suggest features.
+
+When filing an issue, please check [existing open][issues], or [recently closed][closed-issues], issues to make sure
+ somebody else hasn't already reported the issue.
+Please try to include as much information as you can. Details like these are incredibly useful:
+
+* A reproducible test case or series of steps
+* The version of our code being used
+* Any modifications you've made relevant to the bug
+* Anything unusual about your environment or deployment
+
+## Contributing via Pull Requests
+
+To send us a pull request, please:
+
+1. Fork the repository.
+2. Modify the source; please focus on the specific change you are contributing.
+  If you also reformat all the code, it will be hard for us to focus on your change.
+3. Ensure local tests pass. (`colcon test` and `pre-commit run` (requires you to install pre-commit by `pip3 install pre-commit`)
+4. Commit to your fork using clear commit messages.
+5. Send a pull request, answering any default questions in the pull request interface.
+6. Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation.
+
+GitHub provides additional documentation on [forking a repository](https://help.github.com/articles/fork-a-repo/) and
+[creating a pull request](https://help.github.com/articles/creating-a-pull-request/).
+
+## Finding contributions to work on
+
+Looking at the existing issues is a great way to find something to contribute on.
+As this project, by default, uses the default GitHub issue labels
+  (enhancement/bug/duplicate/help wanted/invalid/question/wontfix), looking at any ['help wanted'][help-wanted] issues
+  is a great place to start.
+
+## Licensing
+
+Any contribution that you make to this repository will
+be under the Apache 2 License, as dictated by that
+[license](http://www.apache.org/licenses/LICENSE-2.0.html):
+
+~~~
+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.
+~~~
+
+[issues]:https://github.com/LihanChen2004/pcd2pgm/issues
+[closed-issues]:https://github.com/LihanChen2004/pcd2pgm/issues?utf8=%E2%9C%93&q=is%3Aissue%20is%3Aclosed%20
+[help-wanted]:https://github.com/LihanChen2004/pcd2pgm/issues?q=is%3Aopen+is%3Aissue+label%3A%22help+wanted%22
diff --git a/pcd2pgm/LICENSE b/pcd2pgm/LICENSE
new file mode 100644
index 0000000..d645695
--- /dev/null
+++ b/pcd2pgm/LICENSE
@@ -0,0 +1,202 @@
+
+                                 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 [yyyy] [name of copyright owner]
+
+   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.
diff --git a/pcd2pgm/README.md b/pcd2pgm/README.md
new file mode 100644
index 0000000..4c2b978
--- /dev/null
+++ b/pcd2pgm/README.md
@@ -0,0 +1,79 @@
+# pcd2pgm
+
+基于 ROS2 和 PCL 库，用于将 `.pcd` 点云文件转换为用于 Navigation 的 `pgm` 栅格地图
+
+|pcd|pgm|
+|:-:|:-:|
+|![pcd](.docs/pcd.png)|![pgm](.docs/pgm.png)|
+
+## 1. Overview
+
+- 读取指定的 `.pcd` 文件
+
+- 使用 Pass Through 滤波器过滤点云
+
+- 使用 Radius Outlier 滤波器进一步处理点云
+
+- 将处理后的点云转换为占据栅格地图（Occupancy Grid Map）
+
+- 将转换后的地图发布到指定 ROS 话题上
+
+## 2. Quick Start
+
+### 2.1 Setup Environment
+
+- Ubuntu 22.04
+- ROS: [Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html)
+
+### 2.2 Create Workspace
+
+```bash
+mkdir -p ~/ros_ws
+cd ~/ros_ws
+```
+
+```bash
+git clone https://github.com/LihanChen2004/pcd2pgm.git
+```
+
+### 2.3 Build
+
+```bash
+rosdep install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
+```
+
+```bash
+colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=release
+```
+
+### 2.4 Running
+
+启动 pcd2pgm 节点，可在 RViz 中预览滤波后点云和栅格地图：
+
+```sh
+ros2 launch pcd2pgm pcd2pgm_launch.py
+```
+
+保存栅格地图：
+
+```sh
+ros2 run nav2_map_server map_saver_cli -f <YOUR_MAP_NAME>
+```
+
+## 三. Node Parameters
+
+可以通过修改 `pcd2pgm/pcd2pgm.yaml` 文件来配置节点的参数。
+
+  ```yaml
+  pcd2pgm:
+    ros__parameters:
+      pcd_file: /home/lihanchen/NAVIGATION_WS/pcd2pgm/rmuc_2025.pcd   # pcd 文件所在目录
+      odom_to_lidar_odom: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]              # [x, y, z, r, p, y] 里程计到激光雷达的坐标变换（用于变换点云）
+      flag_pass_through: false                                        # 是否使用 Pass Through 滤波器
+      map_resolution: 0.05                                            # 地图分辨率
+      map_topic_name: map                                             # 发布地图的 ROS 话题名
+      thre_radius: 0.1                                                # Radius Outlier 滤波器半径
+      thre_z_max: 2.0                                                 # Z轴最大值（用于 Pass Through 滤波器）
+      thre_z_min: 0.1                                                 # Z轴最小值（用于 Pass Through 滤波器）
+      thres_point_count: 10                                           # 最小点数阈值（用于 Radius Outlier 滤波器）
+  ```
diff --git a/pcd2pgm/config/pcd2pgm.yaml b/pcd2pgm/config/pcd2pgm.yaml
new file mode 100644
index 0000000..df02def
--- /dev/null
+++ b/pcd2pgm/config/pcd2pgm.yaml
@@ -0,0 +1,12 @@
+pcd2pgm:
+  ros__parameters:
+    pcd_file: /home/elephant/agv_pro_ros2/src/point_lio_ros2/PCD/scans.pcd
+    odom_to_lidar_odom: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+    # odom_to_lidar_odom: [-0.16, 0.0, -0.18, -0.523599, 0.0, -1.5707963]
+    flag_pass_through: false
+    map_resolution: 0.04
+    map_topic_name: map
+    thre_radius: 0.1
+    thre_z_max: 1.3
+    thre_z_min: 0.3
+    thres_point_count: 10
diff --git a/pcd2pgm/include/pcd2pgm/pcd2pgm.hpp b/pcd2pgm/include/pcd2pgm/pcd2pgm.hpp
new file mode 100644
index 0000000..a35f515
--- /dev/null
+++ b/pcd2pgm/include/pcd2pgm/pcd2pgm.hpp
@@ -0,0 +1,72 @@
+// Copyright 2025 Lihan Chen
+//
+// 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.
+
+#ifndef PCD2PGM__PCD2PGM_HPP_
+#define PCD2PGM__PCD2PGM_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "pcl/filters/passthrough.h"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+
+namespace pcd2pgm
+{
+class Pcd2PgmNode : public rclcpp::Node
+{
+public:
+  explicit Pcd2PgmNode(const rclcpp::NodeOptions & options);
+
+private:
+  void declareParameters();
+
+  void getParameters();
+
+  void passThroughFilter(double thre_low, double thre_high, bool flag_in);
+
+  void radiusOutlierFilter(
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd_cloud0, double radius, int thre_count);
+
+  void setMapTopicMsg(
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, nav_msgs::msg::OccupancyGrid & msg);
+
+  void publishCallback();
+
+  void applyTransform();
+
+  float thre_z_min_;
+  float thre_z_max_;
+  float thre_radius_;
+  bool flag_pass_through_;
+  float map_resolution_;
+  int thres_point_count_;
+  std::string pcd_file_;
+  std::string map_topic_name_;
+  std::vector<double> odom_to_lidar_odom_;
+
+  std::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> pcd_cloud_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_pass_through_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_radius_;
+  nav_msgs::msg::OccupancyGrid map_topic_msg_;
+
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pcd_publisher_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+}  // namespace pcd2pgm
+
+#endif  // PCD2PGM__PCD2PGM_HPP_
diff --git a/pcd2pgm/launch/pcd2pgm_launch.py b/pcd2pgm/launch/pcd2pgm_launch.py
new file mode 100644
index 0000000..f378ccf
--- /dev/null
+++ b/pcd2pgm/launch/pcd2pgm_launch.py
@@ -0,0 +1,88 @@
+# Copyright 2025 Lihan Chen
+#
+# 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    SetEnvironmentVariable,
+)
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory("pcd2pgm")
+
+    # Create the launch configuration variables``
+    params_file = LaunchConfiguration("params_file")
+    rviz_config_file = LaunchConfiguration("rviz_config_file")
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        "RCUTILS_LOGGING_BUFFERED_STREAM", "1"
+    )
+
+    colorized_output_envvar = SetEnvironmentVariable("RCUTILS_COLORIZED_OUTPUT", "1")
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        "params_file",
+        default_value=os.path.join(bringup_dir, "config", "pcd2pgm.yaml"),
+        description="Full path to the ROS2 parameters file to use for all launched nodes",
+    )
+
+    declare_rviz_config_file_cmd = DeclareLaunchArgument(
+        "rviz_config_file",
+        default_value=os.path.join(bringup_dir, "rviz", "pcd2pgm.rviz"),
+        description="Full path to the RVIZ config file to use",
+    )
+
+    start_pcd2pgm_cmd = Node(
+        package="pcd2pgm",
+        executable="pcd2pgm_node",
+        name="pcd2pgm",
+        output="screen",
+        parameters=[params_file],
+    )
+
+    start_rviz_cmd = Node(
+        package="rviz2",
+        executable="rviz2",
+        arguments=["-d", rviz_config_file],
+        output="screen",
+        remappings=[
+            ("/tf", "tf"),
+            ("/tf_static", "tf_static"),
+        ],
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+    ld.add_action(colorized_output_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_rviz_config_file_cmd)
+
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(start_pcd2pgm_cmd)
+    ld.add_action(start_rviz_cmd)
+
+    return ld
diff --git a/pcd2pgm/package.xml b/pcd2pgm/package.xml
new file mode 100644
index 0000000..f0d2f7b
--- /dev/null
+++ b/pcd2pgm/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>pcd2pgm</name>
+  <version>0.0.0</version>
+  <description>Convert .pcd files to .pgm grid maps for navigation.</description>
+  <maintainer email="lihanchen2004@163.com">LihanChen</maintainer>
+  <license>Apache-2.0</license>
+  <author email="lihanchen2004@163.com">LihanChen</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>pcl_ros</depend>
+  <depend>pcl_conversions</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_clang_format</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/pcd2pgm/rviz/pcd2pgm.rviz b/pcd2pgm/rviz/pcd2pgm.rviz
new file mode 100644
index 0000000..2f2d96f
--- /dev/null
+++ b/pcd2pgm/rviz/pcd2pgm.rviz
@@ -0,0 +1,194 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+      Splitter Ratio: 0.5
+    Tree Height: 716
+  - 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: cloud_after_radius
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_default_plugins/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map_updates
+      Use Timestamp: false
+      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: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: cloud_after_radius
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /pcd_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    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: 17.899526596069336
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.028435707092285156
+        Y: 0.07508468627929688
+        Z: 0.047394752502441406
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6574800610542297
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 2.4183316230773926
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 982
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f70000033afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000033a0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000022f000000b60000000000000000fb0000001800440065006200750067002000430061006d00650072006100000002b8000000f00000000000000000000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004c20000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004380000003efc0100000002fb0000000800540069006d00650000000000000004380000058100fffffffb0000000800540069006d00650100000000000004500000000000000000000004c90000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1740
+  X: 1233
+  Y: 54
diff --git a/pcd2pgm/src/pcd2pgm.cpp b/pcd2pgm/src/pcd2pgm.cpp
new file mode 100644
index 0000000..666190d
--- /dev/null
+++ b/pcd2pgm/src/pcd2pgm.cpp
@@ -0,0 +1,187 @@
+// Copyright 2025 Lihan Chen
+//
+// 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.
+
+#include "pcd2pgm/pcd2pgm.hpp"
+
+#include "pcl/common/transforms.h"
+#include "pcl/filters/radius_outlier_removal.h"
+#include "pcl/io/pcd_io.h"
+#include "pcl_conversions/pcl_conversions.h"
+
+namespace pcd2pgm
+{
+Pcd2PgmNode::Pcd2PgmNode(const rclcpp::NodeOptions & options) : Node("pcd2pgm", options)
+{
+  declareParameters();
+  getParameters();
+
+  rclcpp::QoS map_qos(10);
+  map_qos.transient_local();
+  map_qos.reliable();
+  map_qos.keep_last(1);
+
+  pcd_cloud_ = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  map_publisher_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(map_topic_name_, map_qos);
+  pcd_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("pcd_cloud", 10);
+
+  if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_file_, *pcd_cloud_) == -1) {
+    RCLCPP_ERROR(get_logger(), "Couldn't read file: %s", pcd_file_.c_str());
+    return;
+  }
+
+  RCLCPP_INFO(get_logger(), "Initial point cloud size: %lu", pcd_cloud_->points.size());
+
+  applyTransform();
+
+  passThroughFilter(thre_z_min_, thre_z_max_, flag_pass_through_);
+  radiusOutlierFilter(cloud_after_pass_through_, thre_radius_, thres_point_count_);
+  setMapTopicMsg(cloud_after_radius_, map_topic_msg_);
+
+  timer_ =
+    create_wall_timer(std::chrono::seconds(1), std::bind(&Pcd2PgmNode::publishCallback, this));
+}
+
+void Pcd2PgmNode::publishCallback()
+{
+  sensor_msgs::msg::PointCloud2 output;
+  pcl::toROSMsg(*cloud_after_radius_, output);
+  output.header.frame_id = "map";
+  pcd_publisher_->publish(output);
+  map_publisher_->publish(map_topic_msg_);
+}
+
+void Pcd2PgmNode::declareParameters()
+{
+  declare_parameter("pcd_file", "");
+  declare_parameter("thre_z_min", 0.5);
+  declare_parameter("thre_z_max", 2.0);
+  declare_parameter("flag_pass_through", false);
+  declare_parameter("thre_radius", 0.5);
+  declare_parameter("map_resolution", 0.05);
+  declare_parameter("thres_point_count", 10);
+  declare_parameter("map_topic_name", "map");
+  declare_parameter(
+    "odom_to_lidar_odom", std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0});  // 新增的参数
+}
+
+void Pcd2PgmNode::getParameters()
+{
+  get_parameter("pcd_file", pcd_file_);
+  get_parameter("thre_z_min", thre_z_min_);
+  get_parameter("thre_z_max", thre_z_max_);
+  get_parameter("flag_pass_through", flag_pass_through_);
+  get_parameter("thre_radius", thre_radius_);
+  get_parameter("map_resolution", map_resolution_);
+  get_parameter("thres_point_count", thres_point_count_);
+  get_parameter("map_topic_name", map_topic_name_);
+  get_parameter("odom_to_lidar_odom", odom_to_lidar_odom_);  // 获取新的参数
+}
+
+void Pcd2PgmNode::passThroughFilter(double thre_low, double thre_high, bool flag_in)
+{
+  auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  pcl::PassThrough<pcl::PointXYZ> passthrough;
+  passthrough.setInputCloud(pcd_cloud_);
+  passthrough.setFilterFieldName("z");
+  passthrough.setFilterLimits(thre_low, thre_high);
+  passthrough.setNegative(flag_in);
+  passthrough.filter(*filtered_cloud);
+
+  cloud_after_pass_through_ = filtered_cloud;
+  RCLCPP_INFO(
+    get_logger(), "After PassThrough filtering: %lu points",
+    cloud_after_pass_through_->points.size());
+}
+
+void Pcd2PgmNode::radiusOutlierFilter(
+  const pcl::PointCloud<pcl::PointXYZ>::Ptr & input_cloud, double radius, int thre_count)
+{
+  auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  pcl::RadiusOutlierRemoval<pcl::PointXYZ> radius_outlier;
+  radius_outlier.setInputCloud(input_cloud);
+  radius_outlier.setRadiusSearch(radius);
+  radius_outlier.setMinNeighborsInRadius(thre_count);
+  radius_outlier.filter(*filtered_cloud);
+
+  cloud_after_radius_ = filtered_cloud;
+  RCLCPP_INFO(
+    get_logger(), "After RadiusOutlier filtering: %lu points", cloud_after_radius_->points.size());
+}
+
+void Pcd2PgmNode::setMapTopicMsg(
+  const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, nav_msgs::msg::OccupancyGrid & msg)
+{
+  msg.header.stamp = now();
+  msg.header.frame_id = "map";
+
+  msg.info.map_load_time = now();
+  msg.info.resolution = map_resolution_;
+
+  double x_min = std::numeric_limits<double>::max();
+  double x_max = std::numeric_limits<double>::lowest();
+  double y_min = std::numeric_limits<double>::max();
+  double y_max = std::numeric_limits<double>::lowest();
+
+  if (cloud->points.empty()) {
+    RCLCPP_WARN(get_logger(), "Point cloud is empty!");
+    return;
+  }
+
+  for (const auto & point : cloud->points) {
+    x_min = std::min(x_min, static_cast<double>(point.x));
+    x_max = std::max(x_max, static_cast<double>(point.x));
+    y_min = std::min(y_min, static_cast<double>(point.y));
+    y_max = std::max(y_max, static_cast<double>(point.y));
+  }
+
+  msg.info.origin.position.x = x_min;
+  msg.info.origin.position.y = y_min;
+  msg.info.origin.position.z = 0.0;
+  msg.info.origin.orientation.x = 0.0;
+  msg.info.origin.orientation.y = 0.0;
+  msg.info.origin.orientation.z = 0.0;
+  msg.info.origin.orientation.w = 1.0;
+
+  msg.info.width = std::ceil((x_max - x_min) / map_resolution_);
+  msg.info.height = std::ceil((y_max - y_min) / map_resolution_);
+  msg.data.assign(msg.info.width * msg.info.height, 0);
+
+  for (const auto & point : cloud->points) {
+    int i = std::floor((point.x - x_min) / map_resolution_);
+    int j = std::floor((point.y - y_min) / map_resolution_);
+
+    if (i >= 0 && i < msg.info.width && j >= 0 && j < msg.info.height) {
+      msg.data[i + j * msg.info.width] = 100;
+    }
+  }
+
+  RCLCPP_INFO(get_logger(), "Map data size: %lu", msg.data.size());
+}
+
+void Pcd2PgmNode::applyTransform()
+{
+  Eigen::Affine3f transform = Eigen::Affine3f::Identity();
+
+  transform.translation() << odom_to_lidar_odom_[0], odom_to_lidar_odom_[1], odom_to_lidar_odom_[2];
+  transform.rotate(Eigen::AngleAxisf(odom_to_lidar_odom_[3], Eigen::Vector3f::UnitX()));
+  transform.rotate(Eigen::AngleAxisf(odom_to_lidar_odom_[4], Eigen::Vector3f::UnitY()));
+  transform.rotate(Eigen::AngleAxisf(odom_to_lidar_odom_[5], Eigen::Vector3f::UnitZ()));
+
+  pcl::transformPointCloud(*pcd_cloud_, *pcd_cloud_, transform.inverse());
+}
+
+}  // namespace pcd2pgm
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(pcd2pgm::Pcd2PgmNode)
diff --git a/point_lio_ros2/.gitignore b/point_lio_ros2/.gitignore
new file mode 100644
index 0000000..f239d34
--- /dev/null
+++ b/point_lio_ros2/.gitignore
@@ -0,0 +1,4 @@
+/.idea/
+/cmake-build-debug/
+*.pcd
+/Log/pos_log.csv
diff --git a/point_lio_ros2/.gitmodules b/point_lio_ros2/.gitmodules
new file mode 100644
index 0000000..dd6379e
--- /dev/null
+++ b/point_lio_ros2/.gitmodules
@@ -0,0 +1,8 @@
+[submodule "ikd-Tree"]
+	path = ikd-Tree
+	url = https://github.com/hku-mars/ikd-Tree.git
+	branch = fast_lio
+[submodule "include/IKFoM"]
+	path = include/IKFoM
+	url = https://github.com/hku-mars/IKFoM.git
+	branch = toolkit
diff --git a/point_lio_ros2/CMakeLists.txt b/point_lio_ros2/CMakeLists.txt
new file mode 100644
index 0000000..75e0b30
--- /dev/null
+++ b/point_lio_ros2/CMakeLists.txt
@@ -0,0 +1,112 @@
+cmake_minimum_required(VERSION 3.5)
+project(point_lio)
+
+# Use C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
+
+# Compiler settings for performance and threading
+add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
+add_compile_options(-O3 -pthread -fexceptions)
+
+# Processor count and definitions
+message(STATUS "Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}")
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" )
+  include(ProcessorCount)
+  ProcessorCount(N)
+  message(STATUS "Processor number:  ${N}")
+  if(N GREATER 5)
+    add_definitions(-DMP_EN)
+    add_definitions(-DMP_PROC_NUM=4)
+    message(STATUS "core for MP:  3")
+  elseif(N GREATER 3)
+    math(EXPR PROC_NUM "${N} - 2")
+    add_definitions(-DMP_EN)
+    add_definitions(-DMP_PROC_NUM="${PROC_NUM}")
+    message(STATUS "core for MP:  ${PROC_NUM}")
+  else()
+    add_definitions(-DMP_PROC_NUM=1)
+  endif()
+else()
+  add_definitions(-DMP_PROC_NUM=1)
+endif()
+
+# Find packages
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(visualization_msgs REQUIRED)
+# find_package(livox_ros_driver2 REQUIRED)
+
+find_package(Eigen3 REQUIRED)
+
+
+# OpenMP
+find_package(OpenMP QUIET)
+if(OPENMP_FOUND)
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+endif()
+
+find_package(PythonLibs REQUIRED) # Consider using Pybind11 for ROS2
+find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
+
+include_directories(
+        include
+        ${EIGEN3_INCLUDE_DIR}
+        ${PYTHON_INCLUDE_DIRS}
+)
+
+# Declare a ROS2 executable
+add_executable(pointlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp)
+ament_target_dependencies(pointlio_mapping
+        rclcpp
+        rclpy
+        geometry_msgs
+        nav_msgs
+        sensor_msgs
+        pcl_ros
+        pcl_conversions
+        tf2_ros
+        visualization_msgs
+        # livox_ros_driver2
+)
+target_link_libraries(pointlio_mapping ${PYTHON_LIBRARIES})
+target_include_directories(pointlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})
+
+# Install the executable
+install(TARGETS
+        pointlio_mapping
+        DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+        DIRECTORY config launch rviz_cfg
+        DESTINATION share/${PROJECT_NAME}
+)
+
+# Export dependencies
+ament_export_dependencies(rclcpp
+  rclpy
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  pcl_ros
+  pcl_conversions
+  tf2_ros
+  visualization_msgs
+  # livox_ros_driver2
+  Eigen3
+)
+
+ament_package()
diff --git a/point_lio_ros2/LICENSE b/point_lio_ros2/LICENSE
new file mode 100644
index 0000000..d159169
--- /dev/null
+++ b/point_lio_ros2/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.
diff --git a/point_lio_ros2/Log/guide.md b/point_lio_ros2/Log/guide.md
new file mode 100644
index 0000000..883b3d4
--- /dev/null
+++ b/point_lio_ros2/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.
diff --git a/point_lio_ros2/Log/imu.txt b/point_lio_ros2/Log/imu.txt
new file mode 100644
index 0000000..e69de29
diff --git a/point_lio_ros2/Log/imu_pbp.txt b/point_lio_ros2/Log/imu_pbp.txt
new file mode 100644
index 0000000..e69de29
diff --git a/point_lio_ros2/Log/mat_out.txt b/point_lio_ros2/Log/mat_out.txt
new file mode 100644
index 0000000..e69de29
diff --git a/point_lio_ros2/Log/plot.py b/point_lio_ros2/Log/plot.py
new file mode 100644
index 0000000..b5d1c1a
--- /dev/null
+++ b/point_lio_ros2/Log/plot.py
@@ -0,0 +1,46 @@
+# import matplotlib
+# matplotlib.use('Agg')
+import numpy as np
+import matplotlib.pyplot as plt
+
+a_out=np.loadtxt('mat_out.txt')
+#######for normal#######
+fig, axs = plt.subplots(3,2)
+lab_out = ['', 'out-x', 'out-y', 'out-z']
+plot_ind = range(7,10)
+time=a_out[:,0]
+axs[0,0].set_title('Attitude')
+axs[1,0].set_title('Translation')
+axs[2,0].set_title('Velocity')
+axs[0,1].set_title('bg')
+axs[1,1].set_title('ba')
+axs[2,1].set_title('Gravity')
+for i in range(1,4):
+	for j in range(6):
+	    axs[j%3, j//3].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
+	for j in range(6):
+		axs[j%3, j//3].grid()
+		axs[j%3, j//3].legend()
+plt.grid()
+    #######for normal#######
+
+
+#### Draw IMU data
+#fig, axs = plt.subplots(2)
+#imu=np.loadtxt('imu_pbp.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()
+
+plt.show()
diff --git a/point_lio_ros2/Log/plot_imu.py b/point_lio_ros2/Log/plot_imu.py
new file mode 100644
index 0000000..0177273
--- /dev/null
+++ b/point_lio_ros2/Log/plot_imu.py
@@ -0,0 +1,125 @@
+# import matplotlib
+# matplotlib.use('Agg')
+import numpy as np
+import matplotlib.pyplot as plt
+
+#### Draw IMU data
+fig, axs = plt.subplots(2)
+imu=np.loadtxt('imu_pbp.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()
+
+#fig, axs = plt.subplots(5)
+#axs[0].set_title('miss')
+#axs[1].set_title('miss')
+#axs[2].set_title('miss')
+#axs[3].set_title('miss')
+#axs[4].set_title('miss')
+#len_time1 = np.arange(0,1977)
+#len_time2 = np.arange(1977, 3954)
+#len_time3 = np.arange(3954,5931)
+#len_time4 = np.arange(5931,7908)
+#len_time5 = np.arange(7908,9885)
+    #if i==1:
+#axs[0].plot(len_time1, time[0:1977],'.-', label='check')
+#axs[1].plot(len_time2, time[1977:3954],'.-', label='check')
+#axs[2].plot(len_time3, time[3954:5931],'.-', label='check')
+#axs[3].plot(len_time4, time[5931:7908],'.-', label='check')
+#axs[4].plot(len_time5, time[7908:9885],'.-', label='check')
+    #axs[i].set_xlim(386,389)
+#axs[0].grid()
+#axs[0].legend()
+#axs[1].grid()
+#axs[1].legend()
+#axs[2].grid()
+#axs[2].legend()
+#axs[3].grid()
+#axs[3].legend()
+#axs[4].grid()
+#axs[4].legend()
+#plt.grid()
+
+#fig, axs = plt.subplots(5)
+#axs[0].set_title('miss')
+#axs[1].set_title('miss')
+#axs[2].set_title('miss')
+#axs[3].set_title('miss')
+#axs[4].set_title('miss')
+#len_time1 = np.arange(9885,9885+1977)
+#len_time2 = np.arange(9885+1977,9885+3954)
+#len_time3 = np.arange(9885+3954,9885+5931)
+#len_time4 = np.arange(9885+5931,9885+7908)
+#len_time5 = np.arange(9885+7908,9885+9885)
+    #if i==1:
+#axs[0].plot(len_time1, time[9885+0:9885+1977],'.-', label='check')
+#axs[1].plot(len_time2, time[9885+1977:9885+3954],'.-', label='check')
+#axs[2].plot(len_time3, time[9885+3954:9885+5931],'.-', label='check')
+#axs[3].plot(len_time4, time[9885+5931:9885+7908],'.-', label='check')
+#axs[4].plot(len_time5, time[9885+7908:9885+9885],'.-', label='check')
+    #axs[i].set_xlim(386,389)
+#axs[0].grid()
+#axs[0].legend()
+#axs[1].grid()
+#axs[1].legend()
+#axs[2].grid()
+#axs[2].legend()
+#axs[3].grid()
+#axs[3].legend()
+#axs[4].grid()
+#axs[4].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()
diff --git a/point_lio_ros2/Log/plot_out.py b/point_lio_ros2/Log/plot_out.py
new file mode 100644
index 0000000..8dd80cb
--- /dev/null
+++ b/point_lio_ros2/Log/plot_out.py
@@ -0,0 +1,178 @@
+# import matplotlib
+# matplotlib.use('Agg')
+import numpy as np
+import matplotlib.pyplot as plt
+
+# a_pre=np.loadtxt('mat_pre.txt')
+a_out=np.loadtxt('mat_out.txt')
+if((a_out.shape[1] != 19) & (a_out.shape[1] != 20)):
+    ######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)
+    time=a_out[:,0]
+    axs[0,0].set_title('Attitude')
+    axs[1,0].set_title('Translation')
+    axs[2,0].set_title('Velocity')
+    axs[3,0].set_title('Angular velocity')
+    axs[0,1].set_title('Acceleration')
+    axs[1,1].set_title('Gravity')
+    axs[2,1].set_title('bg')
+    axs[3,1].set_title('ba')
+    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#######
+else:
+    #######for normal#######
+    fig, axs = plt.subplots(3,2)
+    lab_pre = ['', 'pre-x', 'pre-y', 'pre-z']
+    lab_out = ['', 'out-x', 'out-y', 'out-z']
+    plot_ind = range(7,10)
+    time=a_out[:,0]
+    time1 = a_pre[:,0]
+    axs[0,0].set_title('Attitude')
+    axs[1,0].set_title('Translation')
+    axs[2,0].set_title('Velocity')
+    axs[0,1].set_title('bg')
+    axs[1,1].set_title('ba')
+    axs[2,1].set_title('Gravity')
+    for i in range(1,4):
+        for j in range(6):
+            axs[j%3, j/3].plot(time1, a_pre[:,i+j*3],'.-', label=lab_pre[i])
+            axs[j%3, j/3].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
+    for j in range(6):
+        # axs[j].set_xlim(386,389)
+        axs[j%3, j//3].grid()
+        axs[j%3, j//3].legend()
+    plt.grid()
+    #######for normal#######
+
+
+#### Draw IMU data
+fig, axs = plt.subplots(2)
+imu=np.loadtxt('imu_pbp.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()
+
+#fig, axs = plt.subplots(5)
+#axs[0].set_title('miss')
+#axs[1].set_title('miss')
+#axs[2].set_title('miss')
+#axs[3].set_title('miss')
+#axs[4].set_title('miss')
+#len_time1 = np.arange(0,1977)
+#len_time2 = np.arange(1977, 3954)
+#len_time3 = np.arange(3954,5931)
+#len_time4 = np.arange(5931,7908)
+#len_time5 = np.arange(7908,9885)
+    #if i==1:
+#axs[0].plot(len_time1, time[0:1977],'.-', label='check')
+#axs[1].plot(len_time2, time[1977:3954],'.-', label='check')
+#axs[2].plot(len_time3, time[3954:5931],'.-', label='check')
+#axs[3].plot(len_time4, time[5931:7908],'.-', label='check')
+#axs[4].plot(len_time5, time[7908:9885],'.-', label='check')
+    #axs[i].set_xlim(386,389)
+#axs[0].grid()
+#axs[0].legend()
+#axs[1].grid()
+#axs[1].legend()
+#axs[2].grid()
+#axs[2].legend()
+#axs[3].grid()
+#axs[3].legend()
+#axs[4].grid()
+#axs[4].legend()
+#plt.grid()
+
+#fig, axs = plt.subplots(5)
+#axs[0].set_title('miss')
+#axs[1].set_title('miss')
+#axs[2].set_title('miss')
+#axs[3].set_title('miss')
+#axs[4].set_title('miss')
+#len_time1 = np.arange(9885,9885+1977)
+#len_time2 = np.arange(9885+1977,9885+3954)
+#len_time3 = np.arange(9885+3954,9885+5931)
+#len_time4 = np.arange(9885+5931,9885+7908)
+#len_time5 = np.arange(9885+7908,9885+9885)
+    #if i==1:
+#axs[0].plot(len_time1, time[9885+0:9885+1977],'.-', label='check')
+#axs[1].plot(len_time2, time[9885+1977:9885+3954],'.-', label='check')
+#axs[2].plot(len_time3, time[9885+3954:9885+5931],'.-', label='check')
+#axs[3].plot(len_time4, time[9885+5931:9885+7908],'.-', label='check')
+#axs[4].plot(len_time5, time[9885+7908:9885+9885],'.-', label='check')
+    #axs[i].set_xlim(386,389)
+#axs[0].grid()
+#axs[0].legend()
+#axs[1].grid()
+#axs[1].legend()
+#axs[2].grid()
+#axs[2].legend()
+#axs[3].grid()
+#axs[3].legend()
+#axs[4].grid()
+#axs[4].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()
diff --git a/point_lio_ros2/Log/pos_log.txt b/point_lio_ros2/Log/pos_log.txt
new file mode 100644
index 0000000..e69de29
diff --git a/point_lio_ros2/PCD/temp.txt b/point_lio_ros2/PCD/temp.txt
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/point_lio_ros2/PCD/temp.txt
@@ -0,0 +1 @@
+
diff --git a/point_lio_ros2/README.md b/point_lio_ros2/README.md
new file mode 100644
index 0000000..4e7c83b
--- /dev/null
+++ b/point_lio_ros2/README.md
@@ -0,0 +1,230 @@
+# Point-LIO-ROS2 (with Unitree Unilidar L1/L2 support)
+## Point-LIO: Robust High-Bandwidth Lidar-Inertial Odometry
+
+*(Pay attention to modifying the parameters for IMU in .yaml file, according to the IMU you use.)*
+
+ROS2 port of [Point-LIO](https://github.com/hku-mars/Point-LIO) with support for the Unitree Unilidar L1/L2 as implemented on the ROS1 [point_lio_unilidar](https://github.com/unitreerobotics/point_lio_unilidar) package.
+
+## 1. Introduction
+
+`Point-LIO` is a robust and high-bandwidth lidar inertial odometry (LIO) with the capability to provide accurate, high-frequency odometry and reliable mapping under severe vibrations and aggressive motions. If you need further information about the `Point-LIO` algorithm, you can refer to their official website and paper:
+- <https://github.com/hku-mars/Point-LIO>
+- [Point‐LIO: Robust High‐Bandwidth Light Detection and Ranging Inertial Odometry](https://onlinelibrary.wiley.com/doi/epdf/10.1002/aisy.202200459)
+
+<div align="center">
+    <div align="center">
+        <img src="https://github.com/hku-mars/Point-LIO/raw/master/image/toc4.png" width = 75% >
+    </div>
+    <font color=#a0a0a0 size=2>The framework and key points of the Point-LIO.</font>
+</div>
+
+<br>
+
+The codes of this repo are contributed by:
+[Dongjiao He (贺东娇)](https://github.com/Joanna-HE) and [Wei Xu (徐威)](https://github.com/XW-HKU) as well as [Daniel Florea](https://github.com/dfloreaa) for the ROS2 port and Unitree LiDAR support.
+
+**Important notes:**
+
+A. Please make sure the IMU and LiDAR are **Synchronized**, that's important.
+
+B. Please obtain the saturation values of your used IMU (i.e., accelerator and gyroscope), and the units of the accelerator of your used IMU, then modify the .yaml file according to those settings, including values of 'satu_acc', 'satu_gyro', 'acc_norm'. That's improtant.
+
+C. 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 because Point-LIO processes at the sampling time of each LiDAR point.
+
+D. We recommend to set the **extrinsic_est_en** to false if the extrinsic is given. As for the extrinsic initiallization, please refer to our recent work: [**Robust and Online LiDAR-inertial Initialization**](https://github.com/hku-mars/LiDAR_IMU_Init).
+
+E. If a high odometry output frequency without downsample is required, set `publish_odometry_without_downsample` as true. Then the warning message of tf `"TF_REPEATED_DATA"` will pop up in the terminal window, because the time interval between two publish odometery is too small. The following command could be used to suppress this warning to a smaller frequency:
+
+in your `catkin_ws/src`,
+
+```bash
+git clone --branch throttle-tf-repeated-data-error git@github.com:BadgerTechnologies/geometry2.git
+```
+
+Then rebuild, source `setup.bash`, run and then it should be reduced down to once every 10 seconds. If 10 seconds is still too much log output then change the ros::Duration(10.0) to 10000 seconds or whatever you like.
+
+F. If you want to use Point-LIO without imu, set the `"imu_en"` as false, and provide a predefined value of gavity in `"gravity_init"` as true as possible in the yaml file, and keep the `"use_imu_as_input"` as 0.
+
+## **2. Videos**
+An set of accompaning videos are available on **YouTube**.
+### 2.1 Point-LIO original demo
+Original video demostration, straight from the original repo
+<div align="center">
+    <a href="https://youtu.be/oS83xUs42Uw" target="_blank"><img src="https://github.com/hku-mars/Point-LIO/raw/master/image/final.png" width=60% /></a>
+</div>
+
+### 2.2 L1 LiDAR
+Official demo by Unitree using their [`point_lio_unilidar`](https://github.com/unitreerobotics/point_lio_unilidar) implementation for the L1 LiDAR:
+<div align="center">
+    <a href="https://oss-global-cdn.unitree.com/static/c0bd0ac7d1e147e7a7eaf909f1fc214f.mp4" target="_blank"><img src="https://github.com/unitreerobotics/point_lio_unilidar/raw/main/doc/video.png" width=60% /></a>
+</div>
+
+### 2.3 L2 LiDAR
+Official demo by Unitree using their [`point_lio_unilidar`](https://github.com/unitreerobotics/point_lio_unilidar) implementation for the L2 LiDAR:
+<div align="center">
+    <a href="https://youtu.be/juAfGrg2xBg?si=IVTWM9shEmHsKKJ_" target="_blank"><img src="https://github.com/unitreerobotics/point_lio_unilidar/raw/main/doc/l2-demo-video-bilibili.png" width=60% /></a>
+</div>
+
+## **3. Prerequisites**
+
+### **3.1 Ubuntu and [ROS](https://www.ros.org/)**
+We tested our code on Ubuntu 22.04 with Humble. Other versions may have problems of environments to support the Point-LIO, try to avoid using Point-LIO in those systems.
+
+Additional ROS package is required:
+
+- For ROS2 Humble:
+    ```bash
+    sudo apt-get install ros-humble-pcl-ros
+    sudo apt-get install ros-humble-pcl-conversions
+    sudo apt-get install ros-humble-visualization-msgs
+    ```
+
+### **3.2 Eigen**
+Following the official [Eigen installation](eigen.tuxfamily.org/index.php?title=Main_Page), or directly install Eigen by:
+```bash
+sudo apt-get install libeigen3-dev
+```
+
+### **3.3 `livox_ros_driver2`**
+Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2).
+
+*Remarks:*
+- Since the Point-LIO supports Livox serials LiDAR, so the **livox_ros_driver2** must be installed and **sourced** before run any Point-LIO launch file.
+- How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/install/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_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).
+
+### 3.4 `unilidar_sdk`
+
+For using lidar `L1`, you should download and build [unilidar_sdk](https://github.com/unitreerobotics/unilidar_sdk) follwing these steps:
+
+```bash
+git clone https://github.com/unitreerobotics/unilidar_sdk.git
+
+cd unilidar_sdk/unitree_lidar_ros2
+
+colcon build
+```
+
+### 3.5 `unilidar_sdk2`
+
+For using lidar `L2`, you should download and build [unilidar_sdk2](https://github.com/unitreerobotics/unilidar_sdk2) follwing these steps:
+
+```bash
+git clone https://github.com/unitreerobotics/unilidar_sdk2.git
+
+cd unilidar_sdk/unitree_lidar_ros2
+
+colcon build
+```
+
+## 4. Build
+Clone the repository and colcon build:
+
+```bash
+    mkdir -p catkin_point_lio_unilidar/src
+    cd catkin_point_lio_unilidar/src
+    git clone https://github.com/dfloreaa/point_lio_ros2.git
+    cd ..
+    colcon build --symlink-install
+    source install/setup.bash
+```
+- Remember to source the livox_ros_driver before build (follow 3.3 `livox_ros_driver2`)
+- If you want to use a custom build of PCL, add the following line to ~/.bashrc
+```export PCL_ROOT={CUSTOM_PCL_PATH}```
+
+## 5. Directly run
+
+### 5.1 For Avia
+Connect to your PC to Livox Avia LiDAR by following  [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then
+```
+    cd ~/$Point_LIO_ROS_DIR$
+    source install/setup.bash
+    ros2 launch point_lio mapping_avia.launch.py
+    ros2 launch livox_ros_driver msg_HAP_launch.py
+```
+- For livox serials, Point-LIO only support the data collected by the ``` msg_HAP_launch.py ``` since only its ``` livox_ros_driver/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for Point-LIO. ``` livox_lidar.launch.py ``` can not produce it right now.
+- If you want to change the frame rate, please modify the **publish_freq** parameter in the [msg_HAP_launch.py](https://github.com/Livox-SDK/livox_ros_driver2/blob/master/launch_ROS2/msg_HAP_launch.py) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage.
+
+### 5.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 Point-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.
+5. Saturation value of IMU's accelerator and gyroscope: ```satu_acc```, ```satu_gyro```
+6. The norm of IMU's acceleration according to unit of acceleration messages: ``` acc_norm ```
+
+### 5.3 For Velodyne or Ouster (Velodyne as an example)
+
+Step A: Setup before run
+
+Edit ``` config/velodyne.yaml ``` to set the below parameters:
+
+1. LiDAR point cloud topic name: ``` lid_topic ```
+2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine)
+3. Set the parameter ```timestamp_unit``` based on the unit of **time** (Velodyne) or **t** (Ouster) field in PoindCloud2 rostopic
+4. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ```
+5. Translational extrinsic: ``` extrinsic_T ```
+6. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)
+- The extrinsic parameters in Point-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame).
+7. Saturation value of IMU's accelerator and gyroscope: ```satu_acc```, ```satu_gyro```
+8. The norm of IMU's acceleration according to unit of acceleration messages: ``` acc_norm ```
+
+Step B: Run below
+```
+    cd ~/$Point_LIO_ROS_DIR$
+    source install/setup.bash
+    ros2 launch point_lio mapping_velody16.launch.py
+```
+
+Step C: Run LiDAR's ros driver or play rosbag.
+
+### 5.4 For Unitree LiDAR (L1 as an example)
+
+Step A: Run below
+```
+    cd ~/catkin_point_lio_unilidar
+    source install/setup.bash
+    ros2 launch point_lio mapping_unilidar_l1.py
+```
+
+Step B: Run LiDAR's ros driver or play rosbag.
+
+### 5.5 PCD file save
+
+Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` Point-LIO/PCD/scans.pcd ``` after the Point-LIO is terminated. ```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
+```
+
+# **6. Examples**
+
+The example datasets could be downloaded through [onedrive](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/hdj65822_connect_hku_hk/EmRJYy4ZfAlMiIJ786ogCPoBcGQ2BAchuXjE5oJQjrQu0Q?e=igu44W). Pay attention that if you want to test on racing_drone.bag, [0.0, 9.810, 0.0] should be input in 'mapping/gravity_init' in avia.yaml, and set the 'start_in_aggressive_motion' as true in the yaml. Because this bag start from a high speed motion. And for PULSAR.bag, we change the measuring range of the gyroscope of the built-in IMU to 17.5 rad/s. Therefore, when you test on this bag, please change 'satu_gyro' to 17.5 in avia.yaml.
+
+## **6.1. Example-1: SLAM on datasets with aggressive motions where IMU is saturated**
+<div align="center">
+<img src="https://github.com/hku-mars/Point-LIO/raw/master/image/example1.gif"  width="40%" />
+<img src="https://github.com/hku-mars/Point-LIO/raw/master/image/example2.gif"  width="54%" />
+</div>
+
+## **6.2. Example-2: Application on FPV and PULSAR**
+<div align="center">
+<img src="https://github.com/hku-mars/Point-LIO/raw/master/image/example3.gif"  width="58%" />
+<img src="https://github.com/hku-mars/Point-LIO/raw/master/image/example4.gif"  width="35%" />
+</div>
+
+PULSAR is a self-rotating UAV actuated by only one motor, [PULSAR](https://github.com/hku-mars/PULSAR)
+
+## 7. Contact us
+If you have any questions about this work, please feel free to contact me <dflorea@uc.cl> via email.
diff --git a/point_lio_ros2/config/avia.yaml b/point_lio_ros2/config/avia.yaml
new file mode 100644
index 0000000..68a4da5
--- /dev/null
+++ b/point_lio_ros2/config/avia.yaml
@@ -0,0 +1,59 @@
+/**:
+    ros__parameters:
+        common:
+            lid_topic: "/livox/lidar"
+            imu_topic: "/livox/imu"
+            con_frame: false # true: if you need to combine several LiDAR frames into one
+            con_frame_num: 1 # the number of frames combined
+            cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+            cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
+            time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+            # the timestamp of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value
+
+        preprocess:
+            lidar_type: 1
+            scan_line: 6
+            timestamp_unit: 1           # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 1.0
+
+        mapping:
+            imu_en: true
+            start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
+            extrinsic_est_en: false # for aggressive motion, set this variable false
+            imu_time_inte: 0.005 # = 1 / frequency of IMU
+            satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
+            satu_gyro: 17.5 # the saturation value of IMU's angular velocity. not related to the units (default = 35 rad/s)
+            acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
+            lidar_meas_cov: 0.001 # 0.001; 0.01
+            acc_cov_output: 500.0
+            gyr_cov_output: 1000.0
+            b_acc_cov: 0.0001
+            b_gyr_cov: 0.0001
+            imu_meas_acc_cov: 0.1 #0.1 # 0.1
+            imu_meas_omg_cov: 0.1 #0.01 # 0.1
+            gyr_cov_input: 0.01 # for IMU as input model
+            acc_cov_input: 0.1 # for IMU as input model
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
+            match_s: 81.0
+            fov_degree: 90.0
+            det_range: 450.0
+            gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
+            gravity: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # gravity to be aligned
+            gravity_init: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
+            extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
+            extrinsic_R: [ 1.0, 0.0, 0.0,
+                           0.0, 1.0, 0.0,
+                           0.0, 0.0, 1.0 ]
+
+        odometry:
+            publish_odometry_without_downsample: false
+
+        publish:
+            path_en: true                 # false: close the path output
+            scan_publish_en: true         # false: close all the point cloud output
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: false
+            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.
\ No newline at end of file
diff --git a/point_lio_ros2/config/horizon.yaml b/point_lio_ros2/config/horizon.yaml
new file mode 100644
index 0000000..b5320f6
--- /dev/null
+++ b/point_lio_ros2/config/horizon.yaml
@@ -0,0 +1,59 @@
+/**:
+    ros__parameters:
+        common:
+            lid_topic: "/livox/lidar"
+            imu_topic: "/livox/imu"
+            con_frame: false # true: if you need to combine several LiDAR frames into one
+            con_frame_num: 1 # the number of frames combined
+            cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+            cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
+            time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme),
+            # the timestamp of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value
+
+        preprocess:
+            lidar_type: 1
+            scan_line: 6
+            timestamp_unit: 1   # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 4.0
+
+        mapping:
+            imu_en: true
+            start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
+            extrinsic_est_en: false # for aggressive motion, set this variable false
+            imu_time_inte: 0.005 # = 1 / frequency of IMU
+            satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
+            satu_gyro: 35.0 # the saturation value of IMU's angular velocity. not related to the units
+            acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
+            lidar_meas_cov: 0.01 # 0.001
+            acc_cov_output: 500.0
+            gyr_cov_output: 1000.0
+            b_acc_cov: 0.0001
+            b_gyr_cov: 0.0001
+            imu_meas_acc_cov: 0.01 #0.1 # 2
+            imu_meas_omg_cov: 0.01 #0.1 # 2
+            gyr_cov_input: 0.01 # for IMU as input model
+            acc_cov_input: 0.1 # for IMU as input model
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
+            match_s: 81.0
+            fov_degree: 100.0
+            det_range: 260.0
+            gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
+            gravity: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # gravity to be aligned
+            gravity_init: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
+            extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]
+            extrinsic_R: [ 1.0, 0.0, 0.0,
+                           0.0, 1.0, 0.0,
+                           0.0, 0.0, 1.0 ]
+
+        odometry:
+            publish_odometry_without_downsample: false
+
+        publish:
+            path_en: true                 # false: close the path output
+            scan_publish_en: true         # false: close all the point cloud output
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: false
+            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.
\ No newline at end of file
diff --git a/point_lio_ros2/config/mid360.yaml b/point_lio_ros2/config/mid360.yaml
new file mode 100644
index 0000000..0e38af0
--- /dev/null
+++ b/point_lio_ros2/config/mid360.yaml
@@ -0,0 +1,59 @@
+/**:
+    ros__parameters:
+        common:
+            lid_topic: "/livox/lidar"
+            imu_topic: "/livox/imu"
+            con_frame: false # true: if you need to combine several LiDAR frames into one
+            con_frame_num: 1 # the number of frames combined
+            cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+            cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
+            time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme),
+            # the timestamp of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value
+
+        preprocess:
+            lidar_type: 1      # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR
+            scan_line: 4
+            timestamp_unit: 3   # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 0.5
+
+        mapping:
+            imu_en: true
+            start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
+            extrinsic_est_en: false # for aggressive motion, set this variable false
+            imu_time_inte: 0.005 # = 1 / frequency of IMU
+            satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units
+            satu_gyro: 35.0 # the saturation value of IMU's angular velocity. not related to the units
+            acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
+            lidar_meas_cov: 0.01 # 0.001
+            acc_cov_output: 500.0
+            gyr_cov_output: 1000.0
+            b_acc_cov: 0.0001
+            b_gyr_cov: 0.0001
+            imu_meas_acc_cov: 0.01 #0.1 # 2
+            imu_meas_omg_cov: 0.01 #0.1 # 2
+            gyr_cov_input: 0.01 # for IMU as input model
+            acc_cov_input: 0.1 # for IMU as input model
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
+            match_s: 81.0
+            fov_degree: 360.0
+            det_range: 100.0
+            gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
+            gravity: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # gravity to be aligned
+            gravity_init: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
+            extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
+            extrinsic_R: [ 1.0, 0.0, 0.0,
+                           0.0, 1.0, 0.0,
+                           0.0, 0.0, 1.0 ]
+
+        odometry:
+            publish_odometry_without_downsample: false
+
+        publish:
+            path_en: true                 # false: close the path output
+            scan_publish_en: true         # false: close all the point cloud output
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: false
+            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.
\ No newline at end of file
diff --git a/point_lio_ros2/config/ouster64.yaml b/point_lio_ros2/config/ouster64.yaml
new file mode 100644
index 0000000..612d8aa
--- /dev/null
+++ b/point_lio_ros2/config/ouster64.yaml
@@ -0,0 +1,59 @@
+/**:
+    ros__parameters:
+        common:
+            lid_topic: "/os_cloud_node/points"
+            imu_topic: "/os_cloud_node/imu"
+            con_frame: false # true: if you need to combine several LiDAR frames into one
+            con_frame_num: 1 # the number of frames combined
+            cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+            cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
+            time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+            # the timestamp of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value
+
+        preprocess:
+            lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR
+            scan_line: 32 # 32 #velodyne 6 avia
+            timestamp_unit: 3   # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 0.20
+
+        mapping:
+            imu_en: true
+            start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
+            extrinsic_est_en: false # for aggressive motion, set this variable false
+            imu_time_inte: 0.01 # = 1 / frequency of IMU
+            satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
+            satu_gyro: 35.0 # the saturation value of IMU's angular velocity. not related to the units
+            acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
+            lidar_meas_cov: 0.1 # 0.01
+            acc_cov_output: 500.0
+            gyr_cov_output: 1000.0
+            b_acc_cov: 0.0001
+            b_gyr_cov: 0.0001
+            imu_meas_acc_cov: 0.1 #0.1 # 2
+            imu_meas_omg_cov: 0.1 #0.1 # 2
+            gyr_cov_input: 0.01 # for IMU as input model
+            acc_cov_input: 0.1 # for IMU as input model
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
+            match_s: 81.0
+            fov_degree: 180.0
+            det_range: 150.0
+            gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
+            gravity: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # gravity to be aligned
+            gravity_init: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
+            extrinsic_T: [ 0.0, 0.0, 0.0 ]
+            extrinsic_R: [ 1.0, 0.0, 0.0,
+                           0.0, 1.0, 0.0,
+                           0.0, 0.0, 1.0 ]
+
+        odometry:
+            publish_odometry_without_downsample: false
+
+        publish:
+            path_en: true                 # false: close the path output
+            scan_publish_en: true         # false: close all the point cloud output
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: false
+            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.
\ No newline at end of file
diff --git a/point_lio_ros2/config/unilidar_l1.yaml b/point_lio_ros2/config/unilidar_l1.yaml
new file mode 100644
index 0000000..3d868c5
--- /dev/null
+++ b/point_lio_ros2/config/unilidar_l1.yaml
@@ -0,0 +1,61 @@
+/**:
+    ros__parameters:
+        common:
+            lid_topic: "/unilidar/cloud" 
+            imu_topic: "/unilidar/imu" 
+            con_frame: false # true: if you need to combine several LiDAR frames into one
+            con_frame_num: 1 # the number of frames combined
+            cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+            cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency   
+            time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+                                        # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value
+
+        preprocess:
+            lidar_type: 5
+            scan_line: 18
+            timestamp_unit: 0           # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 0.5
+
+        mapping:
+            imu_en: true
+            start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
+            extrinsic_est_en: false # for aggressive motion, set this variable false
+            imu_time_inte: 0.004 # = 1 / frequency of IMU
+            satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
+            satu_gyro: 35.0 # the saturation value of IMU's angular velocity. not related to the units
+            acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
+            lidar_meas_cov: 0.01 # 0.001
+            acc_cov_output: 500.0
+            gyr_cov_output: 1000.0
+            b_acc_cov: 0.0001 
+            b_gyr_cov: 0.0001 
+            imu_meas_acc_cov: 0.1 #0.1 # 2
+            imu_meas_omg_cov: 0.1 #0.1 # 2 
+            gyr_cov_input: 0.01 # for IMU as input model
+            acc_cov_input: 0.1 # for IMU as input model
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
+            match_s: 81.0
+            fov_degree: 180.0
+            det_range: 100.0
+            gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
+            gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned
+            gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
+            
+            # transform from imu to lidar
+            extrinsic_T: [ 0.007698, 0.014655, -0.00667] # ulhk # [-0.5, 1.4, 1.5] # utbm
+            extrinsic_R: [ 1.0, 0.0, 0.0,
+                          0.0, 1.0, 0.0,
+                          0.0, 0.0, 1.0 ] # ulhk 4 utbm 3
+
+        odometry: 
+            publish_odometry_without_downsample: true  
+
+        publish:
+            path_en: true                 # false: close the path output
+            scan_publish_en: true         # false: close all the point cloud output
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: true       # save map to pcd file
+            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.
\ No newline at end of file
diff --git a/point_lio_ros2/config/unilidar_l2.yaml b/point_lio_ros2/config/unilidar_l2.yaml
new file mode 100644
index 0000000..18485bc
--- /dev/null
+++ b/point_lio_ros2/config/unilidar_l2.yaml
@@ -0,0 +1,61 @@
+/**:
+    ros__parameters:
+        common:
+            lid_topic: "/unilidar/cloud" 
+            imu_topic: "/unilidar/imu" 
+            con_frame: false # true: if you need to combine several LiDAR frames into one
+            con_frame_num: 1 # the number of frames combined
+            cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+            cut_frame_time_interval: 0.2 # should be integral fraction of 1 / LiDAR frequency
+            time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+                                        # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value
+
+        preprocess:
+            lidar_type: 5
+            scan_line: 18
+            timestamp_unit: 0           # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 1.0
+
+        mapping:
+            imu_en: true
+            start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
+            extrinsic_est_en: false # for aggressive motion, set this variable false
+            imu_time_inte: 0.005 # = 1 / frequency of IMU
+            satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
+            satu_gyro: 35.0 # the saturation value of IMU's angular velocity. not related to the units
+            acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
+            lidar_meas_cov: 0.05
+            acc_cov_output: 500.0
+            gyr_cov_output: 1000.0
+            b_acc_cov: 0.0001 
+            b_gyr_cov: 0.0001 
+            imu_meas_acc_cov: 0.2
+            imu_meas_omg_cov: 0.2
+            gyr_cov_input: 0.05
+            acc_cov_input: 0.2
+            plane_thr: 0.2 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
+            match_s: 50.0
+            fov_degree: 180.0
+            det_range: 20.0
+            gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
+            gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned
+            gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
+            
+            # transform from imu to lidar
+            extrinsic_T: [ 0.007698, 0.014655, -0.00667] # ulhk # [-0.5, 1.4, 1.5] # utbm
+            extrinsic_R: [ 1.0, 0.0, 0.0,
+                          0.0, 1.0, 0.0,
+                          0.0, 0.0, 1.0 ] # ulhk 4 utbm 3
+
+        odometry: 
+            publish_odometry_without_downsample: false
+
+        publish:
+            path_en: true                 # false: close the path output
+            scan_publish_en: true         # false: close all the point cloud output
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: true       # save map to pcd file
+            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.
diff --git a/point_lio_ros2/config/velody16.yaml b/point_lio_ros2/config/velody16.yaml
new file mode 100644
index 0000000..50ed7f3
--- /dev/null
+++ b/point_lio_ros2/config/velody16.yaml
@@ -0,0 +1,65 @@
+/**:
+    ros__parameters:
+        common:
+            lid_topic: "/velodyne_points"
+            imu_topic: "/imu/data"
+            con_frame: false # true: if you need to combine several LiDAR frames into one
+            con_frame_num: 1 # the number of frames combined
+            cut_frame: false # true: if you need to cut one LiDAR frame into several subframes
+            cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency
+            time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)
+            # the timestamp of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value
+
+        preprocess:
+            lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR
+            scan_line: 32
+            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:
+            imu_en: true
+            start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init
+            extrinsic_est_en: false # for aggressive motion, set this variable false
+            imu_time_inte: 0.01 # = 1 / frequency of IMU
+            satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units
+            satu_gyro: 35.0 # the saturation value of IMU's angular velocity. not related to the units
+            acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
+            lidar_meas_cov: 0.01 # 0.001
+            acc_cov_output: 500.0
+            gyr_cov_output: 1000.0
+            b_acc_cov: 0.0001
+            b_gyr_cov: 0.0001
+            imu_meas_acc_cov: 0.1 #0.1 # 2
+            imu_meas_omg_cov: 0.1 #0.1 # 2
+            gyr_cov_input: 0.01 # for IMU as input model
+            acc_cov_input: 0.1 # for IMU as input model
+            plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
+            match_s: 81.0
+            fov_degree: 180.0
+            det_range: 100.0
+            gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below
+            gravity: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # gravity to be aligned
+            gravity_init: [ 0.0, 0.0, -9.810 ] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state
+            extrinsic_T: [ 0.0, 0.0, 0.28 ] # ulhk # [-0.5, 1.4, 1.5] # utbm
+            # extrinsic_R: [ 0, 1, 0,
+            #                -1, 0, 0,
+            #                0, 0, 1 ] # ulhk 5 6
+            # extrinsic_R: [ 0, -1, 0,
+            #                 1, 0, 0,
+            #                 0, 0, 1 ] # utbm 1, 2
+            extrinsic_R: [ 1.0, 0.0, 0.0,
+                           0.0, 1.0, 0.0,
+                           0.0, 0.0, 1.0 ] # ulhk 4 utbm 3
+
+        odometry:
+            publish_odometry_without_downsample: false
+
+        publish:
+            path_en: true                 # false: close the path output
+            scan_publish_en: true         # false: close all the point cloud output
+            scan_bodyframe_pub_en: false  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: false
+            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.
\ No newline at end of file
diff --git a/point_lio_ros2/include/FOV_Checker/FOV_Checker.cpp b/point_lio_ros2/include/FOV_Checker/FOV_Checker.cpp
new file mode 100644
index 0000000..69032bf
--- /dev/null
+++ b/point_lio_ros2/include/FOV_Checker/FOV_Checker.cpp
@@ -0,0 +1,472 @@
+#include "FOV_Checker.h"
+
+FOV_Checker::FOV_Checker(){
+    // fp = fopen("/home/ecstasy/catkin_ws/fov_data.csv","w");
+    // fprintf(fp,"cur_pose_x,cur_pose_y,cur_pose_z,axis_x,axis_y,axis_z,theta,depth\n");
+    // fclose(fp);
+}
+
+FOV_Checker::~FOV_Checker(){
+
+}
+
+void FOV_Checker::Set_Env(BoxPointType env_param){
+    env = env_param;
+}
+
+void FOV_Checker::Set_BoxLength(double box_len_param){
+    box_length = box_len_param;
+}
+
+void round_v3d(Eigen::Vector3d &vec, int decimal){
+    double tmp;
+    int t;
+    for (int i = 0; i < 3; i++){
+        t = pow(10,decimal);
+        tmp = round(vec(i)*t);
+        vec(i) = tmp/t;
+    }
+    return;
+}
+
+void FOV_Checker::check_fov(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, vector<BoxPointType> &boxes){
+    round_v3d(cur_pose,4);
+    round_v3d(axis,3);
+    axis = axis/axis.norm();
+    // fp = fopen("/home/ecstasy/catkin_ws/fov_data.csv","a");
+    // fprintf(fp,"%f,%f,%f,%f,%f,%f,%0.4f,%0.1f,",cur_pose(0),cur_pose(1),cur_pose(2),axis(0),axis(1),axis(2),theta,depth);
+    // fclose(fp);
+    // cout << "cur_pose: " << cur_pose.transpose() << endl;
+    // cout<< "axis: " << axis.transpose() << endl;
+    // cout<< "theta: " << theta << " depth: " << depth << endl;
+    // cout<< "env: " << env.vertex_min[0] << " " << env.vertex_max[0] << endl;
+    double axis_angle[6], min_angle, gap, plane_u_min, plane_u_max;
+    Eigen::Vector3d plane_w, plane_u, plane_v, center_point, start_point, box_p;
+    Eigen::Vector3d box_p_min, box_p_max;
+    int i, j, k, index, maxn, start_i, max_uN, max_vN, max_ulogN, u_min, u_max;
+    bool flag = false, box_found = false;
+    boxes.clear();
+    BoxPointType box;
+    axis_angle[0] = acos(axis(0));
+    axis_angle[1] = acos(axis(1));
+    axis_angle[2] = acos(axis(2));
+    axis_angle[3] = acos(-axis(0));
+    axis_angle[4] = acos(-axis(1));
+    axis_angle[5] = acos(-axis(2));
+    index = 1;
+    min_angle = axis_angle[0];
+    for (i=1;i<6;i++){
+        if (axis_angle[i]<min_angle){
+            min_angle = axis_angle[i];
+            index = i+1;
+        }
+    }
+    switch (index){
+        case 1:
+            // YZ plane
+            plane_w = Eigen::Vector3d(1,0,0); 
+            plane_u = Eigen::Vector3d(0,1,0); 
+            plane_v = Eigen::Vector3d(0,0,1);
+            gap = floor(cur_pose(0)/box_length + 0.5 + eps_value) * box_length + 0.5 * box_length - cur_pose(0);
+            maxn = ceil((env.vertex_max[0]-cur_pose(0))/box_length) +1;
+            start_i = 0;
+            break;
+        case 2:
+            // XZ plane
+            plane_w = Eigen::Vector3d(0,1,0);             
+            plane_u = Eigen::Vector3d(1,0,0); 
+            plane_v = Eigen::Vector3d(0,0,1);
+            gap = floor(cur_pose(1)/box_length + 0.5 + eps_value) * box_length + 0.5 * box_length - cur_pose(1);
+            maxn = ceil((env.vertex_max[1]-cur_pose(1))/box_length) +1;
+            start_i = 0;
+            break;      
+        case 3:
+            // XY plane
+            plane_w = Eigen::Vector3d(0,0,1);            
+            plane_u = Eigen::Vector3d(1,0,0); 
+            plane_v = Eigen::Vector3d(0,1,0);
+            gap = floor(cur_pose(2)/box_length + 0.5 + eps_value) * box_length + 0.5 * box_length - cur_pose(2);
+            maxn = ceil((env.vertex_max[2]-cur_pose(2))/box_length) +1;
+            start_i = 0;
+            break;          
+        case 4:
+            // YZ plane
+            plane_w = Eigen::Vector3d(1,0,0); 
+            plane_u = Eigen::Vector3d(0,1,0); 
+            plane_v = Eigen::Vector3d(0,0,1);
+            gap = ceil(cur_pose(0)/box_length - 0.5 - eps_value) * box_length - 0.5 * box_length - cur_pose(0);
+            maxn = ceil((cur_pose(0)-env.vertex_min[0])/box_length) +1;
+            start_i = 1;
+            break;
+        case 5:
+            // XZ plane
+            plane_w = Eigen::Vector3d(0,1,0);             
+            plane_u = Eigen::Vector3d(1,0,0); 
+            plane_v = Eigen::Vector3d(0,0,1);
+            gap = ceil(cur_pose(1)/box_length - 0.5 - eps_value) * box_length - 0.5 * box_length - cur_pose(1);
+            maxn = ceil((cur_pose(1)-env.vertex_min[1])/box_length) +1;
+            start_i = 1;   
+            break;         
+        case 6:
+            // XY plane
+            plane_w = Eigen::Vector3d(1,0,0); 
+            plane_u = Eigen::Vector3d(0,1,0); 
+            plane_v = Eigen::Vector3d(0,0,1);
+            gap = ceil(cur_pose(2)/box_length - 0.5 - eps_value) * box_length - 0.5 * box_length - cur_pose(2); 
+            maxn = ceil((cur_pose(2)-env.vertex_min[2])/box_length) +1;
+            start_i = 1;
+            break;   
+        default:
+            // YZ plane
+            plane_w = Eigen::Vector3d(1,0,0); 
+            plane_u = Eigen::Vector3d(0,1,0); 
+            plane_v = Eigen::Vector3d(0,0,1);
+            gap = ceil(cur_pose(0)/box_length + 0.5 + eps_value) * box_length + 0.5 * box_length - cur_pose(0);
+            maxn = ceil((env.vertex_max[0]-cur_pose(0))/box_length) +1;
+            start_i = 0;
+            break;
+    }
+    for (i=start_i; i<=maxn; i++){
+        center_point = cur_pose + (abs(gap) + (i-1) * box_length)/cos(min_angle) * axis;
+        if (index == 1 || index == 4){
+            start_point = Eigen::Vector3d(center_point(0),floor(center_point(1)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length,floor(center_point(2)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length);           
+            max_uN = ceil((env.vertex_max[1]-env.vertex_min[1])/box_length);
+            max_ulogN = floor(log2(max_uN));
+            max_vN = ceil((env.vertex_max[2]-env.vertex_min[2])/box_length);
+            plane_u_min = env.vertex_min[1];
+            plane_u_max = env.vertex_max[1];
+        } else {
+            if (index == 2 || index == 5){                            
+                start_point = Eigen::Vector3d(floor(center_point(0)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length, center_point(1), floor(center_point(2)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length);                
+                max_uN = ceil((env.vertex_max[0]-env.vertex_min[0])/box_length);
+                max_ulogN = floor(log2(max_uN));
+                max_vN = ceil((env.vertex_max[2]-env.vertex_min[2])/box_length);
+                plane_u_min = env.vertex_min[0];
+                plane_u_max = env.vertex_max[0];                
+            } else {                            
+                start_point = Eigen::Vector3d(floor(center_point(0)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length, floor(center_point(1)/box_length + 0.5 + eps_value)*box_length - 0.5 * box_length, center_point(2));               
+                max_uN = ceil((env.vertex_max[1]-env.vertex_min[1])/box_length);
+                max_ulogN = floor(log2(max_uN));
+                max_vN = ceil((env.vertex_max[2]-env.vertex_min[2])/box_length);
+                plane_u_min = env.vertex_min[1];
+                plane_u_max = env.vertex_max[1];                
+            }
+        }
+        flag = false;    
+        for (j = 1; j <= max_vN; j++){
+            k = max_ulogN;
+            u_min = 0;
+            box_p_min = start_point.cwiseProduct(plane_w + plane_v) + plane_u * plane_u_min + plane_v * box_length * (j-1);  
+            box_p_max = plane_u * plane_u_max + start_point.cwiseProduct(plane_w + plane_v) + plane_v * box_length * j +  plane_w * box_length;    
+            //printf("---- UPSIDE (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box_p_min[0],box_p_min[1],box_p_min[2],box_p_max[0],box_p_max[1],box_p_max[2]);
+
+            while (k>=0){
+                box_p = box_p_min + plane_u * box_length * (u_min + pow(2,k)) + plane_v * box_length + plane_w * box_length;
+                box.vertex_min[0] = box_p_min(0);
+                box.vertex_min[1] = box_p_min(1);
+                box.vertex_min[2] = box_p_min(2);
+                box.vertex_max[0] = box_p(0);
+                box.vertex_max[1] = box_p(1); 
+                box.vertex_max[2] = box_p(2);  
+                if (!check_box(cur_pose, axis, theta, depth, box)) u_min = u_min + pow(2,k);
+                k = k-1;
+            }
+            k = max_ulogN;
+            u_max = 0;
+            while (k>=0){
+                box_p = box_p_max - plane_u * box_length * (u_max + pow(2,k)) - plane_v * box_length - plane_w * box_length;
+                box.vertex_min[0] = box_p(0);
+                box.vertex_min[1] = box_p(1);
+                box.vertex_min[2] = box_p(2);
+                box.vertex_max[0] = box_p_max(0);
+                box.vertex_max[1] = box_p_max(1); 
+                box.vertex_max[2] = box_p_max(2); 
+                if (!check_box(cur_pose, axis, theta, depth, box)) u_max = u_max + pow(2,k);
+
+                k = k-1;
+            }           
+            u_max = max(0, max_uN - u_max - 1);           
+            box_found = false;
+            //printf("---- u_min -> u_max: %d->%d\n",u_min,u_max);            
+            for (k = u_min; k <= u_max; k++){
+                box_p = box_p_min  + plane_u * box_length * k;
+                box.vertex_min[0] = box_p(0);
+                box.vertex_min[1] = box_p(1);
+                box.vertex_min[2] = box_p(2);
+                box.vertex_max[0] = box_p(0) + box_length;
+                box.vertex_max[1] = box_p(1) + box_length; 
+                box.vertex_max[2] = box_p(2) + box_length;
+                if (check_box_in_env(box)){
+                    //printf("---- FOUND: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]);
+                    box_found = true;
+                    boxes.push_back(box); 
+                }
+            }   
+            if (box_found) { 
+                flag = true; 
+            } else {
+                if (j>1) break;
+            }
+        }
+        for (j = 1; j <= max_vN; j++){
+            k = max_ulogN;
+            u_min = 0;
+            box_p_min = start_point.cwiseProduct(plane_w + plane_v) + plane_u * plane_u_min - plane_v * box_length * j;  
+            box_p_max = plane_u * plane_u_max + start_point.cwiseProduct(plane_w + plane_v) - plane_v * box_length * (j-1) +  plane_w * box_length;  
+            //printf("---- DOWNSIDE (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box_p_min[0],box_p_min[1],box_p_min[2],box_p_max[0],box_p_max[1],box_p_max[2]);
+
+            while (k>=0){   
+                box_p = box_p_min + plane_u * box_length * (u_min + pow(2,k)) + plane_v * box_length + plane_w * box_length;
+                box.vertex_min[0] = box_p_min(0);
+                box.vertex_min[1] = box_p_min(1);
+                box.vertex_min[2] = box_p_min(2);
+                box.vertex_max[0] = box_p(0);
+                box.vertex_max[1] = box_p(1); 
+                box.vertex_max[2] = box_p(2);  
+                if (!check_box(cur_pose, axis, theta, depth, box)) u_min = u_min + pow(2,k);
+                k = k-1;
+            }
+            k = max_ulogN;
+            u_max = 0;
+            while (k>=0){
+                box_p = box_p_max - plane_u * box_length * (u_max + pow(2,k)) - plane_v * box_length - plane_w * box_length;
+                box.vertex_min[0] = box_p(0);
+                box.vertex_min[1] = box_p(1);
+                box.vertex_min[2] = box_p(2);
+                box.vertex_max[0] = box_p_max(0);
+                box.vertex_max[1] = box_p_max(1); 
+                box.vertex_max[2] = box_p_max(2); 
+                if (!check_box(cur_pose, axis, theta, depth, box)) {
+                    u_max = u_max + pow(2,k);
+                    // printf("-------- Not Included: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]);
+                }
+
+                k = k-1;
+            }
+            u_max = max(0, max_uN - u_max - 1);
+            //printf("---- u_min -> u_max: %d->%d\n",u_min,u_max);
+            box_found = 0;
+            for (k = u_min; k <= u_max; k++){
+                box_p = box_p_min  + plane_u * box_length * k;
+                box.vertex_min[0] = box_p(0);
+                box.vertex_min[1] = box_p(1);
+                box.vertex_min[2] = box_p(2);
+                box.vertex_max[0] = box_p(0) + box_length;
+                box.vertex_max[1] = box_p(1) + box_length; 
+                box.vertex_max[2] = box_p(2) + box_length;
+                if (check_box_in_env(box)){
+                    //printf("---- FOUND: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]);
+                    box_found = 1;
+                    boxes.push_back(box); 
+                }
+            }
+            if (box_found) { 
+                flag = true; 
+            } else {
+                if (j>1) break;
+            }
+        }        
+        if (!flag && i>0) break;
+    }
+}
+
+bool FOV_Checker::check_box(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, const BoxPointType box){
+    Eigen::Vector3d vertex[8];
+    bool s;
+    vertex[0] = Eigen::Vector3d(box.vertex_min[0], box.vertex_min[1], box.vertex_min[2]);
+    vertex[1] = Eigen::Vector3d(box.vertex_min[0], box.vertex_min[1], box.vertex_max[2]);
+    vertex[2] = Eigen::Vector3d(box.vertex_min[0], box.vertex_max[1], box.vertex_min[2]);
+    vertex[3] = Eigen::Vector3d(box.vertex_min[0], box.vertex_max[1], box.vertex_max[2]);
+    vertex[4] = Eigen::Vector3d(box.vertex_max[0], box.vertex_min[1], box.vertex_min[2]);
+    vertex[5] = Eigen::Vector3d(box.vertex_max[0], box.vertex_min[1], box.vertex_max[2]);
+    vertex[6] = Eigen::Vector3d(box.vertex_max[0], box.vertex_max[1], box.vertex_min[2]);
+    vertex[7] = Eigen::Vector3d(box.vertex_max[0], box.vertex_max[1], box.vertex_max[2]);
+    for (int i = 0; i < 8; i++){
+        if (check_point(cur_pose, axis, theta, depth, vertex[i])){
+            return true;
+        } 
+    }
+    Eigen::Vector3d center_point = (vertex[7]+vertex[0])/2.0;
+    if (check_point(cur_pose, axis, theta, depth, center_point)){
+        return true;
+    }
+    PlaneType plane[6];
+    plane[0].p[0] = vertex[0]; 
+    plane[0].p[1] = vertex[2]; 
+    plane[0].p[2] = vertex[1]; 
+    plane[0].p[3] = vertex[3];
+
+    plane[1].p[0] = vertex[0]; 
+    plane[1].p[1] = vertex[4]; 
+    plane[1].p[2] = vertex[2]; 
+    plane[1].p[3] = vertex[6];
+
+    plane[2].p[0] = vertex[0]; 
+    plane[2].p[1] = vertex[4]; 
+    plane[2].p[2] = vertex[1]; 
+    plane[2].p[3] = vertex[5];
+
+    plane[3].p[0] = vertex[4]; 
+    plane[3].p[1] = vertex[6]; 
+    plane[3].p[2] = vertex[5]; 
+    plane[3].p[3] = vertex[7];
+
+    plane[4].p[0] = vertex[2]; 
+    plane[4].p[1] = vertex[6]; 
+    plane[4].p[2] = vertex[3]; 
+    plane[4].p[3] = vertex[7];
+
+    plane[5].p[0] = vertex[1]; 
+    plane[5].p[1] = vertex[5]; 
+    plane[5].p[2] = vertex[3]; 
+    plane[5].p[3] = vertex[7];  
+    if (check_surface(cur_pose, axis, theta, depth, plane[0]) || check_surface(cur_pose, axis, theta, depth, plane[1]) || check_surface(cur_pose, axis, theta, depth, plane[2]) || check_surface(cur_pose, axis, theta, depth, plane[3]) || check_surface(cur_pose, axis, theta, depth, plane[4]) || check_surface(cur_pose, axis, theta, depth, plane[5]))
+        s = 1;
+    else
+        s = 0;
+    return s;
+}
+
+bool FOV_Checker::check_surface(Eigen::Vector3d cur_pose, Eigen::Vector3d axis,  double theta, double depth, PlaneType plane){
+    Eigen::Vector3d plane_p, plane_u, plane_v, plane_w, pc, p, vec;
+    bool s;
+    double t, vec_dot_u, vec_dot_v;
+    plane_p = plane.p[0];
+    plane_u = plane.p[1] - plane_p;
+    plane_v = plane.p[2] - plane_p;
+    if (check_line(cur_pose, axis, theta, depth, plane_p, plane_u) || check_line(cur_pose, axis, theta, depth, plane_p, plane_v) || check_line(cur_pose, axis, theta, depth, plane_p + plane_u, plane_v) || check_line(cur_pose, axis, theta, depth, plane_p + plane_v, plane_u)){
+        s = 1;
+        return s;
+    }
+    pc = plane_p + (plane.p[3]-plane.p[0])/2;
+    if (check_point(cur_pose, axis, theta, depth, pc)){
+        s = 1;
+        return s;
+    }
+    plane_w = plane_u.cross(plane_v);
+    p = plane_p - cur_pose;
+    t = (p.dot(plane_w))/(axis.dot(plane_w));
+    vec = cur_pose + t * axis - plane_p;
+    vec_dot_u = vec.dot(plane_u)/plane_u.norm();
+    vec_dot_v = vec.dot(plane_v)/plane_v.norm();
+    if (t>=-eps_value && t<=depth && vec_dot_u>=-eps_value && vec_dot_u<=plane_u.norm() && vec_dot_v>=-eps_value && vec_dot_v <= plane_v.norm())
+        s = 1;
+    else
+        s = 0;
+    return s;
+}
+
+bool FOV_Checker::check_line(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d line_p, Eigen::Vector3d line_vec){
+    Eigen::Vector3d p, vec_1, vec_2;
+    double xl, yl, zl, xn, yn, zn, dot_1, dot_2, ln, pn, pl, l2, p2;
+    double A, B, C, delta, t1, t2;
+    bool s;
+    p = line_p - cur_pose;
+    xl = line_vec(0); yl = line_vec(1); zl = line_vec(2);
+    xn = axis(0); yn = axis(1); zn = axis(2);
+    vec_1 = line_p - cur_pose;
+    vec_2 = line_p + line_vec - cur_pose;
+    dot_1 = vec_1.dot(axis);
+    dot_2 = vec_2.dot(axis);
+    //printf("xl yl zl: %0.4f, %0.4f, %0.4f\n", xl, yl, zl);
+    //printf("xn yn zn: %0.4f, %0.4f, %0.4f\n", xn, yn, zn);
+    //printf("dot_1, dot_2, %0.4f, %0.4f\n",dot_1, dot_2);
+    if ((dot_1<0 && dot_2<0) || (dot_1>depth && dot_2>depth)){
+        s = false;
+        return s;
+    }
+    ln = xl*xn+yl*yn+zl*zn;
+    pn = p(0)*xn+p(1)*yn+p(2)*zn;
+    pl = p(0)*xl+p(1)*yl+p(2)*zl;
+    l2 = xl*xl+yl*yl+zl*zl;
+    p2 = p.norm()*p.norm();
+    //printf("ln: %0.4f\n",ln);
+    //printf("pn:%0.4f\n",pn);
+    //printf("pl:%0.4f\n",pl);
+    //printf("l2:%0.4f\n",l2);
+    //printf("p2:%0.4f\n",p2);
+    //printf("theta, cos(theta):%0.4f %0.4f\n",theta,cos(theta));              
+    A = ln * ln - l2 * cos(theta) * cos(theta);
+    B = 2 * pn * ln - 2 * cos(theta) * cos(theta)*pl;
+    C = pn * pn - p2 * cos(theta) * cos(theta);
+    //printf("A:%0.4f, B:%0.4f, C:%0.4f\n", A,B,C);
+    if (!(fabs(A)<=eps_value)){
+        delta = B*B - 4*A*C;
+        //printf("delta: %0.4f\n",delta);
+        if (delta <= eps_value){
+            if (A < -eps_value){
+                s = false;
+                return s;
+            }
+            else{
+                s = true;
+                return s;
+            }
+        } else {
+            double sqrt_delta = sqrt(delta);
+            t1 = (-B - sqrt_delta)/(2*A);
+            t2 = (-B + sqrt_delta)/(2*A);
+            if (t1>t2) swap(t1,t2);
+            //printf("t1,t2: %0.4f,%0.4f\n",t1,t2); 
+            // printf("%d\n",check_point(cur_pose, axis, theta, depth, line_p + line_vec * t1));       
+            if ((t1>=-eps_value && t1<=1+eps_value) && check_point(cur_pose, axis, theta, depth, line_p + line_vec * t1)){
+                s = true;
+                return s;
+            }
+            // printf("%d\n",check_point(cur_pose, axis, theta, depth, line_p + line_vec * t2));
+            if ((t2>=-eps_value && t2<=1+eps_value) && check_point(cur_pose, axis, theta, depth, line_p + line_vec * t2)){
+                s = true;
+                return s;
+            }
+            if (A>-eps_value && (t2<eps_value || t1>1-eps_value)){
+                s = true;
+                return s;
+            }
+            if (A<eps_value && t1<eps_value && t2>1-eps_value){
+                s = true;
+                return s;
+            }
+            s = false;
+        }
+    } else{
+        if (!(fabs(B)<=eps_value)){
+            s = (B>-eps_value && -C/B<=1+eps_value) || (B<eps_value && -C/B>=-eps_value);
+            return s;
+        }
+        else {
+            s = C>=-eps_value;
+            return s;
+        }
+    }
+    return false;
+}
+
+bool FOV_Checker::check_point(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d point){
+    Eigen::Vector3d vec;
+    double proj_len;
+    bool s;
+    vec = point-cur_pose;
+    if (vec.transpose()*vec < 0.4 * box_length * box_length){
+        return true;
+    }
+    proj_len = vec.dot(axis);
+    if (proj_len > depth){
+        s = false;
+        return s;
+    }
+    //printf("acos: %0.4f\n",acos(proj_len/vec.norm()));
+    if (fabs(vec.norm()) <= 1e-4 || acos(proj_len/vec.norm()) <= theta + 0.0175)
+        s = true;
+    else
+        s = false;
+    return s;
+}
+
+bool FOV_Checker::check_box_in_env(BoxPointType box){
+    if (box.vertex_min[0] >= env.vertex_min[0]-eps_value && box.vertex_min[1] >= env.vertex_min[1]-eps_value && box.vertex_min[2] >= env.vertex_min[2]-eps_value && box.vertex_max[0]<= env.vertex_max[0]+eps_value && box.vertex_max[1]<= env.vertex_max[1]+eps_value && box.vertex_max[2]<= env.vertex_max[2]+eps_value){
+        return true; 
+    } else {
+        return false;
+    }
+}
+
diff --git a/point_lio_ros2/include/FOV_Checker/FOV_Checker.h b/point_lio_ros2/include/FOV_Checker/FOV_Checker.h
new file mode 100644
index 0000000..e7db390
--- /dev/null
+++ b/point_lio_ros2/include/FOV_Checker/FOV_Checker.h
@@ -0,0 +1,33 @@
+// Include Files
+#pragma once
+#include <math.h>
+#include <cmath>
+#include "ikd-Tree/ikd_Tree.h"
+#include <Eigen/Core>
+#include <algorithm>
+
+#define eps_value 1e-6
+
+struct PlaneType{
+    Eigen::Vector3d p[4];
+};
+
+class FOV_Checker{
+public:
+    FOV_Checker();
+    ~FOV_Checker();
+    void Set_Env(BoxPointType env_param);
+    void Set_BoxLength(double box_len_param);
+    void check_fov(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, vector<BoxPointType> &boxes);
+    bool check_box(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, const BoxPointType box);
+    bool check_line(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d line_p, Eigen::Vector3d line_vec);
+    bool check_surface(Eigen::Vector3d cur_pose, Eigen::Vector3d axis,  double theta, double depth, PlaneType plane);
+    bool check_point(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d point);
+    bool check_box_in_env(BoxPointType box);    
+private:
+    BoxPointType env;
+    double box_length;
+    FILE *fp;
+
+};
+
diff --git a/point_lio_ros2/include/IKFoM/.gitignore b/point_lio_ros2/include/IKFoM/.gitignore
new file mode 100644
index 0000000..259148f
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/.gitignore
@@ -0,0 +1,32 @@
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp
new file mode 100644
index 0000000..742e117
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp
@@ -0,0 +1,390 @@
+/*
+ *  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 ESEKFOM_EKF_HPP
+#define ESEKFOM_EKF_HPP
+
+
+#include <vector>
+#include <cstdlib>
+
+#include <boost/bind/bind.hpp>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <Eigen/Dense>
+#include <Eigen/Eigen>
+#include <Eigen/Sparse>
+
+#include "../mtk/types/vect.hpp"
+#include "../mtk/types/SOn.hpp"
+#include "../mtk/types/S2.hpp"
+#include "../mtk/types/SEn.hpp"
+#include "../mtk/startIdx.hpp"
+#include "../mtk/build_manifold.hpp"
+#include "util.hpp"
+
+namespace esekfom {
+
+using namespace Eigen;
+
+template<typename T>
+struct dyn_share_modified
+{
+	bool valid;
+	bool converge;
+	T M_Noise;
+	Eigen::Matrix<T, Eigen::Dynamic, 1> z;
+	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_x;
+	Eigen::Matrix<T, 6, 1> z_IMU;
+	Eigen::Matrix<T, 6, 1> R_IMU;
+	bool satu_check[6];
+};
+
+template<typename state, int process_noise_dof, typename input = state, typename measurement=state, int measurement_noise_dof=0>
+class esekf{
+
+	typedef esekf self;
+	enum{
+		n = state::DOF, m = state::DIM, l = measurement::DOF
+	};
+
+public:
+	
+	typedef typename state::scalar scalar_type;
+	typedef Matrix<scalar_type, n, n> cov;
+	typedef Matrix<scalar_type, m, n> cov_;
+	typedef SparseMatrix<scalar_type> spMt;
+	typedef Matrix<scalar_type, n, 1> vectorized_state;
+	typedef Matrix<scalar_type, m, 1> flatted_state;
+	typedef flatted_state processModel(state &, const input &);
+	typedef Eigen::Matrix<scalar_type, m, n> processMatrix1(state &, const input &);
+	typedef Eigen::Matrix<scalar_type, m, process_noise_dof> processMatrix2(state &, const input &);
+	typedef Eigen::Matrix<scalar_type, process_noise_dof, process_noise_dof> processnoisecovariance;
+
+	typedef void measurementModel_dyn_share_modified(state &, dyn_share_modified<scalar_type> &);
+	typedef Eigen::Matrix<scalar_type ,l, n> measurementMatrix1(state &);
+	typedef Eigen::Matrix<scalar_type , Eigen::Dynamic, n> measurementMatrix1_dyn(state &);
+	typedef Eigen::Matrix<scalar_type ,l, measurement_noise_dof> measurementMatrix2(state &);
+	typedef Eigen::Matrix<scalar_type ,Eigen::Dynamic, Eigen::Dynamic> measurementMatrix2_dyn(state &);
+	typedef Eigen::Matrix<scalar_type, measurement_noise_dof, measurement_noise_dof> measurementnoisecovariance;
+	typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> measurementnoisecovariance_dyn;
+
+	esekf(const state &x = state(),
+		const cov  &P = cov::Identity()): x_(x), P_(P){};
+
+	void init_dyn_share_modified(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified h_dyn_share_in)
+	{
+		f = f_in;
+		f_x = f_x_in;
+		// f_w = f_w_in;
+		h_dyn_share_modified_1 = h_dyn_share_in;
+		maximum_iter = 1;
+		x_.build_S2_state();
+		x_.build_SO3_state();
+		x_.build_vect_state();
+		x_.build_SEN_state();
+	}
+	
+	void init_dyn_share_modified_2h(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified h_dyn_share_in1, measurementModel_dyn_share_modified h_dyn_share_in2)
+	{
+		f = f_in;
+		f_x = f_x_in;
+		// f_w = f_w_in;
+		h_dyn_share_modified_1 = h_dyn_share_in1;
+		h_dyn_share_modified_2 = h_dyn_share_in2;
+		maximum_iter = 1;
+		x_.build_S2_state();
+		x_.build_SO3_state();
+		x_.build_vect_state();
+		x_.build_SEN_state();
+	}
+
+	// iterated error state EKF propogation
+	void predict(double &dt, processnoisecovariance &Q, const input &i_in, bool predict_state, bool prop_cov){
+		if (predict_state)
+		{
+			flatted_state f_ = f(x_, i_in);
+			x_.oplus(f_, dt);
+		}
+
+		if (prop_cov)
+		{
+			flatted_state f_ = f(x_, i_in);
+			// state x_before = x_;
+
+			cov_ f_x_ = f_x(x_, i_in);
+			cov f_x_final;
+			F_x1 = cov::Identity();
+			for (std::vector<std::pair<std::pair<int, int>, int> >::iterator it = x_.vect_state.begin(); it != x_.vect_state.end(); it++) {
+				int idx = (*it).first.first;
+				int dim = (*it).first.second;
+				int dof = (*it).second;
+				for(int i = 0; i < n; i++){
+					for(int j=0; j<dof; j++)
+					{f_x_final(idx+j, i) = f_x_(dim+j, i);}	
+				}
+			}
+
+			Matrix<scalar_type, 3, 3> res_temp_SO3;
+			MTK::vect<3, scalar_type> seg_SO3;
+			for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+				int idx = (*it).first;
+				int dim = (*it).second;
+				for(int i = 0; i < 3; i++){
+					seg_SO3(i) = -1 * f_(dim + i) * dt;
+				}
+				MTK::SO3<scalar_type> res;
+				res.w() = MTK::exp<scalar_type, 3>(res.vec(), seg_SO3, scalar_type(1/2));
+				F_x1.template block<3, 3>(idx, idx) = res.normalized().toRotationMatrix();		
+				res_temp_SO3 = MTK::A_matrix(seg_SO3);
+				for(int i = 0; i < n; i++){
+					f_x_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_. template block<3, 1>(dim, i));	
+				}
+			}
+	
+			F_x1 += f_x_final * dt;
+			P_ = F_x1 * P_ * (F_x1).transpose() + Q * (dt * dt);
+		}
+	}
+
+	bool update_iterated_dyn_share_modified() {
+		dyn_share_modified<scalar_type> dyn_share;
+		state x_propagated = x_;
+		int dof_Measurement;
+		double m_noise;
+		for(int i=0; i<maximum_iter; i++)
+		{
+			dyn_share.valid = true;
+			h_dyn_share_modified_1(x_, dyn_share);
+			if(! dyn_share.valid)
+			{
+				return false;
+				// continue;
+			}
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> z = dyn_share.z;
+			// Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R = dyn_share.R; 
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x = dyn_share.h_x;
+			// Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v = dyn_share.h_v;
+			dof_Measurement = h_x.rows();
+			m_noise = dyn_share.M_Noise;
+			// dof_Measurement_noise = dyn_share.R.rows();
+			// vectorized_state dx, dx_new;
+			// x_.boxminus(dx, x_propagated);
+			// dx_new = dx;
+			// P_ = P_propagated;
+
+			Matrix<scalar_type, n, Eigen::Dynamic> PHT;
+			Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> HPHT;
+			Matrix<scalar_type, n, Eigen::Dynamic> K_;
+			// if(n > dof_Measurement)
+			{
+				PHT = P_. template block<n, 12>(0, 0) * h_x.transpose();
+				HPHT = h_x * PHT.topRows(12);
+				for (int m = 0; m < dof_Measurement; m++)
+				{
+					HPHT(m, m) += m_noise;
+				}
+				K_= PHT*HPHT.inverse();
+			}
+			Matrix<scalar_type, n, 1> dx_ = K_ * z; // - h) + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new; 
+			// state x_before = x_;
+
+			x_.boxplus(dx_);
+			dyn_share.converge = true;
+			
+			// L_ = P_;
+			// Matrix<scalar_type, 3, 3> res_temp_SO3;
+			// MTK::vect<3, scalar_type> seg_SO3;
+			// for(typename std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
+			// 	int idx = (*it).first;
+			// 	for(int i = 0; i < 3; i++){
+			// 		seg_SO3(i) = dx_(i + idx);
+			// 	}
+			// 	res_temp_SO3 = A_matrix(seg_SO3).transpose();
+			// 	for(int i = 0; i < n; i++){
+			// 		L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); 
+			// 	}
+			// 	{
+			// 		for(int i = 0; i < dof_Measurement; i++){
+			// 			K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
+			// 		}
+			// 	}
+			// 	for(int i = 0; i < n; i++){
+			// 		L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+			// 		// P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+			// 	}
+			// 	for(int i = 0; i < n; i++){
+			// 		P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
+			// 	}
+			// }
+			// Matrix<scalar_type, 2, 2> res_temp_S2;
+			// MTK::vect<2, scalar_type> seg_S2;
+			// for(typename std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
+			// 	int idx = (*it).first;
+		
+			// 	for(int i = 0; i < 2; i++){
+			// 		seg_S2(i) = dx_(i + idx);
+			// 	}
+		
+			// 	Eigen::Matrix<scalar_type, 2, 3> Nx;
+			// 	Eigen::Matrix<scalar_type, 3, 2> Mx;
+			// 	x_.S2_Nx_yy(Nx, idx);
+			// 	x_propagated.S2_Mx(Mx, seg_S2, idx);
+			// 	res_temp_S2 = Nx * Mx; 
+	
+			// 	for(int i = 0; i < n; i++){
+			// 		L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); 
+			// 	}
+				
+			// 	{
+			// 		for(int i = 0; i < dof_Measurement; i++){
+			// 			K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
+			// 		}
+			// 	}
+			// 	for(int i = 0; i < n; i++){
+			// 		L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+			// 	}
+			// 	for(int i = 0; i < n; i++){
+			// 		P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
+			// 	}
+			// }
+			// if(n > dof_Measurement)
+			{
+				P_ = P_ - K_*h_x*P_. template block<12, n>(0, 0);
+			}
+		}
+		return true;
+	}
+	
+	void update_iterated_dyn_share_IMU() {
+		
+		dyn_share_modified<scalar_type> dyn_share;
+		for(int i=0; i<maximum_iter; i++)
+		{
+			dyn_share.valid = true;
+			h_dyn_share_modified_2(x_, dyn_share);
+
+			Matrix<scalar_type, 6, 1> z = dyn_share.z_IMU;
+
+			Matrix<double, 30, 6> PHT;
+            Matrix<double, 6, 30> HP;
+            Matrix<double, 6, 6> HPHT;
+			PHT.setZero();
+			HP.setZero();
+			HPHT.setZero();
+			for (int l_ = 0; l_ < 6; l_++)
+			{
+				if (!dyn_share.satu_check[l_])
+				{
+					PHT.col(l_) = P_.col(15+l_) + P_.col(24+l_);
+					HP.row(l_) = P_.row(15+l_) + P_.row(24+l_);
+				}
+			}
+			for (int l_ = 0; l_ < 6; l_++)
+			{
+				if (!dyn_share.satu_check[l_])
+				{
+					HPHT.col(l_) = HP.col(15+l_) + HP.col(24+l_);
+				}
+				HPHT(l_, l_) += dyn_share.R_IMU(l_); //, l);
+			}
+        	Eigen::Matrix<double, 30, 6> K = PHT * HPHT.inverse(); 
+                                    
+            Matrix<scalar_type, n, 1> dx_ = K * z; 
+
+            P_ -= K * HP;
+			x_.boxplus(dx_);
+		}
+		return;
+	}
+	
+	void change_x(state &input_state)
+	{
+		x_ = input_state;
+
+		if((!x_.vect_state.size())&&(!x_.SO3_state.size())&&(!x_.S2_state.size())&&(!x_.SEN_state.size()))
+		{
+			x_.build_S2_state();
+			x_.build_SO3_state();
+			x_.build_vect_state();
+			x_.build_SEN_state();
+		}
+	}
+
+	void change_P(cov &input_cov)
+	{
+		P_ = input_cov;
+	}
+
+	const state& get_x() const {
+		return x_;
+	}
+	const cov& get_P() const {
+		return P_;
+	}
+	state x_;
+private:
+	measurement m_;
+	cov P_;
+	spMt l_;
+	spMt f_x_1;
+	spMt f_x_2;
+	cov F_x1 = cov::Identity();
+	cov F_x2 = cov::Identity();
+	cov L_ = cov::Identity();
+
+	processModel *f;
+	processMatrix1 *f_x;
+	processMatrix2 *f_w;
+
+	measurementMatrix1 *h_x;
+	measurementMatrix2 *h_v;
+
+	measurementMatrix1_dyn *h_x_dyn;
+	measurementMatrix2_dyn *h_v_dyn;
+
+	measurementModel_dyn_share_modified *h_dyn_share_modified_1;
+
+	measurementModel_dyn_share_modified *h_dyn_share_modified_2;
+
+	int maximum_iter = 0;
+	scalar_type limit[n];
+	
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} // namespace esekfom
+
+#endif //  ESEKFOM_EKF_HPP
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp
new file mode 100644
index 0000000..ab39fc4
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/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__
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp
new file mode 100644
index 0000000..115ec7e
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp
@@ -0,0 +1,248 @@
+// 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_HAT(              type, id) if(id.IDX == idx){id.hat(vec, res);}
+#define MTK_JACOB_RIGHT_INV(     type, id) if(id.IDX == idx){id.Jacob_right_inv(vec, res);}
+#define MTK_JACOB_RIGHT(         type, id) if(id.IDX == idx){id.Jacob_right(vec, res);}
+#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_SEN_state(        type, id) if(id.TYP == 4){(SEN_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));}
+
+#define MTK_SUBVARLIST(seq, S2state, SO3state, SENstate) \
+BOOST_PP_FOR_1( \
+		( \
+				BOOST_PP_SEQ_SIZE(seq), \
+				BOOST_PP_SEQ_HEAD(seq), \
+				BOOST_PP_SEQ_TAIL(seq) (~), \
+				0,\
+				0,\
+				S2state,\
+				SO3state,\
+				SENstate ),\
+		MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT)
+
+#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \
+	MTK::SubManifold<type, dof, dim> id; 
+#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state, SENstate) \
+	MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \
+	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, SENstate) \
+	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, SENstate)) 
+
+#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, h) a
+
+#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state
+#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state, SENstate) ( \
+		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,\
+		SENstate )
+
+#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;\
+	std::vector<std::pair<std::pair<int, int>, int> > SEN_state;\
+	MTK_SUBVARLIST(entries, S2_state, SO3_state, SEN_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 build_SEN_state(){\
+		MTK_TRANSFORM(MTK_SEN_state, entries)\
+	}\
+	void Lie_hat(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
+		MTK_TRANSFORM(MTK_HAT, entries)\
+	}\
+	void Lie_Jacob_Right_Inv(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
+		MTK_TRANSFORM(MTK_JACOB_RIGHT_INV, entries)\
+	}\
+	void Lie_Jacob_Right(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\
+		MTK_TRANSFORM(MTK_JACOB_RIGHT, 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_*/
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp
new file mode 100644
index 0000000..a1b13de
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/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_ */
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp
new file mode 100644
index 0000000..a8770e4
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/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 and must not be nan/-nan");
+	
+	// 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_ */
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp
new file mode 100644
index 0000000..5025071
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/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_ */
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp
new file mode 100644
index 0000000..4dc2958
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/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_
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp
new file mode 100644
index 0000000..730e664
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp
@@ -0,0 +1,326 @@
+// 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.normalized().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.normalized().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 hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	
+	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.normalized().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.normalized() * 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_*/
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp
new file mode 100644
index 0000000..2270136
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp
@@ -0,0 +1,334 @@
+// 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/SEn.hpp
+ * @brief Standard Orthogonal Groups i.e.\ rotatation groups.
+ */
+#ifndef SEN_H_
+#define SEN_H_
+
+#include <Eigen/Geometry>
+
+#include "SOn.hpp"
+#include "vect.hpp"
+#include "../src/mtkmath.hpp"
+
+
+namespace MTK {
+
+
+/**
+ * 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 num_of_vec_plus1 = 6, int dim_of_mat = 4, int Options = Eigen::AutoAlign>
+struct SEN {
+	enum {DOF = num_of_vec_plus1, DIM = num_of_vec_plus1, TYP = 4}; 
+	typedef _scalar scalar;
+	typedef Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> base;
+	typedef SO3<scalar> SO3_type;
+	// typedef Eigen::Quaternion<scalar, Options> base;
+	// typedef Eigen::Quaternion<scalar> Quaternion;
+	typedef vect<DIM, scalar, Options> vect_type;
+	SO3_type SO3_data;
+	base mat;
+		
+	/**
+	 * Construct from real part and three imaginary parts.
+	 * Quaternion is normalized after construction.
+	 */
+	// SEN(const base& src) : mat(src) {
+	// 	// base::normalize();
+	// }
+	
+	/**
+	 * Construct from Eigen::Quaternion.
+	 * @note Non-normalized input may result result in spurious behavior.
+	 */
+	SEN(const base& src = base::Identity()) : mat(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) {
+		SEN delta = exp(vec, scale); // ?
+		mat = mat * delta.mat;
+	}
+	void boxminus(MTK::vectview<scalar, DOF> res, const SEN<scalar,num_of_vec_plus1,dim_of_mat, Options>& other) const {
+		base error_mat = other.mat.inverse() * mat;
+		res = log(error_mat);
+	}
+	//}
+
+	void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
+		SEN delta = exp(vec, scale);
+		mat = mat * delta.mat;
+	}
+
+	// void hat(MTK::vectview<const scalar, DOF>& v, Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> &res) {
+	void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		res = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Zero();
+		Eigen::Matrix<scalar, 3, 3> psi;
+		psi << 0, -v[2], v[1],
+			v[2], 0, -v[0],
+			-v[1], v[0], 0;
+		res.block<3, 3>(0, 0) = psi;
+		for(int i = 3; i < v.size() / 3 + 2; i++)
+		{
+			res.block<3, 1>(0, i) = v.segment<3>(i + (i-3)*3);
+		}
+		// return res;
+	}
+
+	// void Jacob_right_inv(MTK::vectview<const scalar, DOF> vec, Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> & res){
+	void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){
+		res = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Zero();
+    	Eigen::Matrix<scalar, 3, 3> M_v;
+    	Eigen::VectorXd vec_psi, vec_ro;
+    	Eigen::MatrixXd jac_v;
+		Eigen::MatrixXd hat_v, hat_ro;
+		vec_psi = vec.segment<3>(0);
+		// Eigen::Matrix<scalar, 3, 1> ;
+		SO3_data.hat(vec_psi, hat_v);
+		SO3_data.Jacob_right_inv(vec_psi, jac_v);
+		double norm = vec_psi.norm();
+		for(int i = 0; i < vec.size() / 3; i++)
+		{
+			res.block<3, 3>(i*3, i*3) = jac_v;
+		}
+		for(int i = 1; i < vec.size() / 3; i++)
+		{
+			vec_ro = vec.segment<3>(i * 3);
+			SO3_data.hat(vec_ro, hat_ro);
+			if(norm > MTK::tolerance<scalar>())
+			{
+				res.block<3,3>(i*3, 0) = 0.5 * hat_ro + (1 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2))/norm/norm * (hat_ro * hat_v + hat_v * hat_ro) + ((2 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2)) / 2 / norm / norm / norm / norm - 1 / 8 / norm / norm / std::sin(norm / 2) / std::sin(norm / 2)) * hat_v * (hat_ro * hat_v + hat_v * hat_ro) * hat_v; 
+			}
+			else
+			{
+				res.block<3,3>(i*3, 0) = 0.5 * hat_ro;
+			}
+
+		}
+    	// return res;
+	}
+
+	// void Jacob_right(MTK::vectview<const scalar, DOF> & vec, Eigen::Matrix<scalar, dim_of_mat, dim_of_mat> &res){
+	void Jacob_right(Eigen::VectorXd& vec, Eigen::MatrixXd &res){
+		res = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Zero();
+		Eigen::MatrixXd hat_v, hat_ro;
+    	Eigen::VectorXd vec_psi, vec_ro;
+		Eigen::MatrixXd jac_v;
+		vec_psi = vec.segment<3>(0);
+		// Eigen::Matrix<scalar, 3, 1> ;
+		SO3_data.hat(vec_psi, hat_v);
+		SO3_data.Jacob_right(vec_psi, jac_v);
+		// double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
+		// double norm = std::sqrt(squaredNorm);
+		double norm = vec_psi.norm();
+		for(int i = 0; i < vec.size() / 3; i++)
+		{
+			res.block<3, 3>(i*3, i*3) = jac_v;
+		}
+		for(int i = 1; i < vec.size() / 3; i++)
+		{
+			vec_ro = vec.segment<3>(i * 3);
+			SO3_data.hat(vec_ro, hat_ro);
+			if(norm > MTK::tolerance<scalar>())
+			{
+				res.block<3,3>(i*3, 0) = -1 * jac_v * (0.5 * hat_ro + (1 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2))/norm/norm * (hat_ro * hat_v + hat_v * hat_ro) + ((2 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2)) / 2 / norm / norm / norm / norm - 1 / 8 / norm / norm / std::sin(norm / 2) / std::sin(norm / 2)) * hat_v * (hat_ro * hat_v + hat_v * hat_ro) * hat_v) * jac_v; 
+			}
+			else
+			{
+				res.block<3,3>(i*3, 0) = -0.5 * jac_v * hat_ro * jac_v;
+			}
+
+		}
+		// return res;
+	}
+
+	void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
+	{
+		std::cerr << "wrong idx for S2" << std::endl;
+		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 SEN<scalar, DOF, dim_of_mat, Options>& q){
+		for(int i=0; i<dim_of_mat; i++)
+		{
+			for(int j = 0; j < dim_of_mat; j++)
+			{
+				os << q.mat(i, j) << " ";
+			}			
+		}
+		return os;
+	}
+
+	friend std::istream& operator>>(std::istream &is, SEN<scalar, DOF, dim_of_mat, Options>& q){
+		// vect<dim_of_mat * dim_of_mat,scalar> coeffs;
+		for(int i=0; i<dim_of_mat; i++)
+		{
+			for(int j = 0; j < dim_of_mat; j++)
+			{
+				is >> q.mat(i, j);
+			}			
+		}
+		// is >> q.mat;
+		// 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 SEN exp(const Eigen::Matrix<scalar, DOF, 1>& dvec, scalar scale = 1){
+		SEN res;
+		res.mat = Eigen::Matrix<scalar, dim_of_mat, dim_of_mat>::Identity();
+		Eigen::Matrix<scalar, 3, 3> exp_; //, jac;
+		Eigen::MatrixXd jac;
+		Eigen::Matrix<scalar, 3, 1> psi;
+		Eigen::VectorXd minus_psi;
+		psi = dvec.template block<3,1>(0, 0);
+		minus_psi = -psi;
+		SO3_type SO3_temp;
+		exp_ = SO3_type::exp(psi);
+		SO3_temp.Jacob_right(minus_psi, jac);
+		res.mat.template block<3,3>(0, 0) = exp_;
+		for(int i = 3; i < DOF / 3 + 2; i++)
+		{
+			res.mat.template block<3, 1>(0, i) = jac * dvec.template block<3,1>(i + (i-3)*3,0);
+		}
+		return res;
+	}
+	/**
+	 * Calculate the inverse of @c exp.
+	 * Only guarantees that <code>exp(log(x)) == x </code>
+	 */
+	static Eigen::Matrix<scalar, DOF, 1> log(base &orient){
+		Eigen::Matrix<scalar, DOF, 1> res;
+		Eigen::Matrix<scalar, 3, 1> psi;
+		Eigen::VectorXd minus_psi;
+		Eigen::Matrix<scalar, 3, 3> mat_psi;
+		Eigen::MatrixXd jac;
+		mat_psi = orient.template block<3, 3>(0, 0);
+		SO3_type SO3_temp;
+		SO3_type exp_psi(mat_psi);
+		psi = SO3_type::log(exp_psi);
+		minus_psi = -psi;
+		SO3_temp.Jacob_right_inv(minus_psi, jac);
+		for(int i = 3; i < dim_of_mat; i++)
+		{
+			res.template block<3,1>(i + (i-3)*3,0) = jac * orient.template block<3, 1>(0, i);
+		}
+		return res;
+	}
+};
+
+
+}  // namespace MTK
+
+#endif /*SON_H_*/
+
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp
new file mode 100644
index 0000000..78f9eec
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp
@@ -0,0 +1,365 @@
+// 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();
+	}
+	void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+};
+
+
+/**
+ * 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 hat(MTK::vectview<const scalar, DOF>& v, Eigen::Matrix<scalar, 3, 3> &res) {
+	void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		// Eigen::Matrix<scalar, 3, 3> res;
+		res << 0, -v[2], v[1],
+			v[2], 0, -v[0],
+			-v[1], v[0], 0;
+		// return res;
+	}
+
+	// void Jacob_right_inv(MTK::vectview<const scalar, DOF> vec, Eigen::Matrix<scalar, 3, 3> & res){
+	void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){
+    	Eigen::MatrixXd hat_v;
+		hat(vec, hat_v);
+    	if(vec.norm() > MTK::tolerance<scalar>())
+    	{
+        	res = Eigen::Matrix<scalar, 3, 3>::Identity() + 0.5 * hat_v + (1 - vec.norm() * std::cos(vec.norm() / 2) / 2 / std::sin(vec.norm() / 2)) * hat_v * hat_v / vec.squaredNorm();
+    	}
+    	else
+    	{
+        	res = Eigen::Matrix<scalar, 3, 3>::Identity();
+    	}
+    	// return res;
+	}
+
+	// void Jacob_right(MTK::vectview<const scalar, DOF> & v, Eigen::Matrix<scalar, 3, 3> &res){
+	void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res){
+		Eigen::MatrixXd hat_v;
+		hat(v, hat_v);
+		double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
+		double norm = std::sqrt(squaredNorm);
+		if(norm < MTK::tolerance<scalar>()){
+			res = Eigen::Matrix<scalar, 3, 3>::Identity();
+		}
+		else{
+			res = Eigen::Matrix<scalar, 3, 3>::Identity() - (1 - std::cos(norm)) / squaredNorm * hat_v + (1 - std::sin(norm) / norm) / squaredNorm * hat_v * hat_v;
+		}
+		// return res;
+	}
+
+
+	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_*/
+
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp
new file mode 100644
index 0000000..d75e6d7
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp
@@ -0,0 +1,511 @@
+// 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 hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+
+	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 hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+
+	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;
+	}
+
+	void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+};
+
+/**
+ * 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 hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+
+	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 hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+	void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) {
+		std::cout << "wrong idx" << std::endl;
+	}
+
+	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_*/
diff --git a/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp b/point_lio_ros2/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp
new file mode 100644
index 0000000..b6643f1
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/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_ */
diff --git a/point_lio_ros2/include/IKFoM/LICENSE b/point_lio_ros2/include/IKFoM/LICENSE
new file mode 100644
index 0000000..d159169
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/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.
diff --git a/point_lio_ros2/include/IKFoM/README.md b/point_lio_ros2/include/IKFoM/README.md
new file mode 100644
index 0000000..53a7dc5
--- /dev/null
+++ b/point_lio_ros2/include/IKFoM/README.md
@@ -0,0 +1,489 @@
+## IKFoM 
+**IKFoM** (Iterated Kalman Filters on Manifolds) is a computationally efficient and convenient toolkit for deploying iterated Kalman filters on various robotic systems, especially systems operating on high-dimension manifold. It implements a manifold-embedding Kalman filter which separates the manifold structures from system descriptions and is able to be used by only defining the system in a canonical form and calling the respective steps accordingly. The current implementation supports the full iterated Kalman filtering for systems on manifold <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbb{R}^m\times&space;SO(3)\times\cdots\times&space;SO(3)\times\mathbb{S}^2\times\cdots\times\mathbb{S}^2" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbb{R}^m\times&space;SO(3)\times\cdots\times&space;SO(3)\times\mathbb{S}^2\times\cdots\times\mathbb{S}^2" title="\mathbb{R}^m\times SO(3)\times\cdots\times SO(3)\times\mathbb{S}^2\times\cdots\times\mathbb{S}^2" /></a> and any of its sub-manifolds, and it is extendable to other types of manifold when necessary.
+
+
+**Developers**
+
+[Dongjiao He](https://github.com/Joanna-HE)
+
+**Our related video**: https://youtu.be/sz_ZlDkl6fA
+
+## 1. Prerequisites
+
+### 1.1. **Eigen && Boost**
+Eigen  >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page).
+
+Boost >= 1.65.
+
+## 2. Usage when the measurement is of constant dimension and type.
+Clone the repository:
+
+```
+    git clone https://github.com/hku-mars/IKFoM.git
+```
+
+1. include the necessary head file:
+```
+#include<esekfom/esekfom.hpp>
+```
+2. Select and instantiate the primitive manifolds:
+```
+    typedef MTK::SO3<double> SO3; // scalar type of variable: double
+    typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
+    typedef MTK::S2<double, 98, 10, 1> S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
+```
+3. Build system state, input and measurement as compound manifolds which are composed of the primitive manifolds:
+``` 
+MTK_BUILD_MANIFOLD(state, // name of compound manifold: state
+((vect3, pos)) // ((primitive manifold type, name of variable))
+((vect3, vel))
+((SO3, rot))
+((vect3, bg))
+((vect3, ba))
+((S2, grav))
+((SO3, offset_R_L_I))
+((vect3, offset_T_L_I)) 
+);
+```
+4. Implement the vector field <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" title="\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)" /></a> that is defined as <a href="https://latex.codecogs.com/svg.image?\mathbf{x}_{k&plus;1}&space;=&space;\mathbf{x}_k\oplus\Delta&space;t\mathbf{f}(\mathbf{x}_k,&space;\mathbf{u}_k,&space;\mathbf{w}_k);\hat{\mathbf{x}}_{k&plus;1}&space;=&space;\hat{\mathbf{x}}_k\oplus\Delta&space;t\mathbf{f}(\hat{\mathbf{x}}_k,&space;\mathbf{u}_k,&space;\mathbf{0})"><see here>, and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{u}, \mathbf{0}\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)}{\partial\mathbf{w}}" /></a>, where w=0 could be left out:
+```
+Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
+	Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
+	res(0) = s.vel[0];
+	res(1) = s.vel[1];
+	res(2) = s.vel[2];
+	return res;
+}
+Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
+	Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
+	cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
+	return cov;
+}
+Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
+	Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
+	cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
+	return cov;
+}
+```
+Those functions would be called during the ekf state predict
+
+5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
+```
+measurement h(state &s, bool &valid) // the iteration stops before convergence whenever the user set valid as false
+{
+	if (condition){ valid = false; 
+	} // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied.
+	measurement h_;
+	h_.position = s.pos;
+	return h_;
+}
+Eigen::Matrix<double, measurement_dof, state_dof> dh_dx(state &s) {} 
+Eigen::Matrix<double, measurement_dof, measurement_noise_dof> dh_dv(state &s) {}
+```
+Those functions would be called during the ekf state update
+
+6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
+
+(1) initial state and covariance:
+```
+state init_state;
+esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov init_P;
+esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf(init_state,init_P);
+```
+(2) default state and covariance:
+```
+esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf;
+```
+where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof**.
+
+7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
+```
+double epsi[state_dof] = {0.001};
+fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
+kf.init(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi);
+```
+8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
+```
+kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
+```
+9. Once a measurement **z** is received, an iterated update is executed:
+```
+kf.update_iterated(z, R); // measurement noise covariance: R, an Eigen matrix
+```
+*Remarks(1):*
+- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
+
+5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
+```
+measurement h_share(state &s, esekfom::share_datastruct<state, measurement, measurement_noise_dof> &share_data) 
+{
+	if(share_data.converge) {} // this value is true means iteration is converged 
+	if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified
+	share_data.h_x = H_x; // H_x is the result matrix of the first differentiation 
+	share_data.h_v = H_v; // H_v is the result matrix of the second differentiation
+	share_data.R = R; // R is the measurement noise covariance
+	share_data.z = z; // z is the obtained measurement 
+
+	measurement h_;
+	h_.position = s.pos;
+	return h_;
+}
+```
+This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function
+
+6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
+
+(1) initial state and covariance:
+```
+state init_state;
+esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov init_P;
+esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf(init_state,init_P);
+```
+(2) default state and covariance:
+```
+esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof> kf;
+```
+7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
+```
+double epsi[state_dof] = {0.001};
+fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
+kf.init_share(f, df_dx, df_dw, h_share, Maximum_iter, epsi);
+```
+8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
+```
+kf.predict(dt, Q, in); // process noise covariance: Q
+```
+9. Once a measurement **z** is received, an iterated update is executed:
+```
+kf.update_iterated_share();
+```
+
+*Remarks(2):*
+- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**:
+```
+state set_x;
+kf.change_x(set_x);
+esekfom::esekf<state, process_noise_dof, input, measurement, measurement_noise_dof>::cov set_P;
+kf.change_P(set_P);
+```
+
+## 3. Usage when the measurement is an Eigen vector of changing dimension.
+
+Clone the repository:
+
+```
+    git clone https://github.com/hku-mars/IKFoM.git
+```
+
+1. include the necessary head file:
+```
+#include<esekfom/esekfom.hpp>
+```
+2. Select and instantiate the primitive manifolds:
+```
+    typedef MTK::SO3<double> SO3; // scalar type of variable: double
+    typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
+    typedef MTK::S2<double, 98, 10, 1> S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
+```
+3. Build system state and input as compound manifolds which are composed of the primitive manifolds:
+``` 
+MTK_BUILD_MANIFOLD(state, // name of compound manifold: state
+((vect3, pos)) // ((primitive manifold type, name of variable))
+((vect3, vel))
+((SO3, rot))
+((vect3, bg))
+((vect3, ba))
+((S2, grav))
+((SO3, offset_R_L_I))
+((vect3, offset_T_L_I)) 
+);
+```
+4. Implement the vector field <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" title="\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)" /></a> that is defined as <a href="https://latex.codecogs.com/svg.image?\mathbf{x}_{k&plus;1}&space;=&space;\mathbf{x}_k\oplus\Delta&space;t\mathbf{f}(\mathbf{x}_k,&space;\mathbf{u}_k,&space;\mathbf{w}_k);\hat{\mathbf{x}}_{k&plus;1}&space;=&space;\hat{\mathbf{x}}_k\oplus\Delta&space;t\mathbf{f}(\hat{\mathbf{x}}_k,&space;\mathbf{u}_k,&space;\mathbf{0})"> <see here>, and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{u}, \mathbf{0}\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)}{\partial\mathbf{w}}" /></a>, where w=0 could be left out:
+```
+Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
+	Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
+	res(0) = s.vel[0];
+	res(1) = s.vel[1];
+	res(2) = s.vel[2];
+	return res;
+}
+Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
+	Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
+	cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
+	return cov;
+}
+Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
+	Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
+	cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
+	return cov;
+}
+```
+Those functions would be called during ekf state predict
+
+5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
+```
+Eigen::Matrix<double, Eigen::Dynamic, 1> h(state &s, bool &valid) //the iteration stops before convergence when valid is false {
+	if (condition){ valid = false; 
+	} // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied.
+	Eigen::Matrix<double, Eigen::Dynamic, 1> h_;
+	h_(0) = s.pos[0];
+	return h_;
+}
+Eigen::Matrix<double, Eigen::Dynamic, state_dof> dh_dx(state &s) {} 
+Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> dh_dv(state &s) {}
+```
+Those functions would be called during ekf state update
+
+6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
+
+(1) initial state and covariance:
+```
+state init_state;
+esekfom::esekf<state, process_noise_dof, input>::cov init_P;
+esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
+```
+(2) default state and covariance:
+```
+esekfom::esekf<state, process_noise_dof, input> kf;
+```
+where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof**
+
+7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
+```
+double epsi[state_dof] = {0.001};
+fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
+kf.init_dyn(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi);
+```
+8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
+```
+kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
+```
+9. Once a measurement **z** is received, an iterated update is executed:
+```
+kf.update_iterated_dyn(z, R); // measurement noise covariance: R, an Eigen matrix
+```
+*Remarks(1):*
+- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
+5. Implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
+```
+Eigen::Matrix<double, Eigen::Dynamic, 1> h_dyn_share(state &s, esekfom::dyn_share_datastruct<double> &dyn_share_data) 
+{
+	if(dyn_share_data.converge) {} // this value is true means iteration is converged 
+	if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified
+	dyn_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation
+	dyn_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation 
+	dyn_share_data.R = R; // R is the measurement noise covariance
+	dyn_share_data.z = z; // z is the obtained measurement 
+
+	Eigen::Matrix<double, Eigen::Dynamic, 1> h_;
+	h_(0) = s.pos[0];
+	return h_;
+}
+This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function
+```
+6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
+(1) initial state and covariance:
+```
+state init_state;
+esekfom::esekf<state, process_noise_dof, input>::cov init_P;
+esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
+```
+(2) default state and covariance:
+```
+esekfom::esekf<state, process_noise_dof, input> kf;
+```
+7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
+```
+double epsi[state_dof] = {0.001};
+fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
+kf.init_dyn_share(f, df_dx, df_dw, h_dyn_share, Maximum_iter, epsi);
+```
+8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
+```
+kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix
+```
+9. Once a measurement **z** is received, an iterated update is executed:
+```
+kf.update_iterated_dyn_share();
+```
+
+*Remarks(2):*
+- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**:
+```
+state set_x;
+kf.change_x(set_x);
+esekfom::esekf<state, process_noise_dof, input>::cov set_P;
+kf.change_P(set_P);
+```
+
+## 4. Usage when the measurement is a changing manifold during the run time.
+
+Clone the repository:
+
+```
+    git clone https://github.com/hku-mars/IKFoM.git
+```
+
+1. include the necessary head file:
+```
+#include<esekfom/esekfom.hpp>
+```
+2. Select and instantiate the primitive manifolds:
+```
+    typedef MTK::SO3<double> SO3; // scalar type of variable: double
+    typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3
+    typedef MTK::S2<double, 98, 10, 1> S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1
+```
+3. Build system state and input as compound manifolds which are composed of the primitive manifolds:
+``` 
+MTK_BUILD_MANIFOLD(state, // name of compound manifold: state
+((vect3, pos)) // ((primitive manifold type, name of variable))
+((vect3, vel))
+((SO3, rot))
+((vect3, bg))
+((vect3, ba))
+((S2, grav))
+((SO3, offset_R_L_I))
+((vect3, offset_T_L_I)) 
+);
+```
+4. Implement the vector field <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)" title="\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)" /></a> that is defined as <a href="https://latex.codecogs.com/svg.image?\mathbf{x}_{k&plus;1}&space;=&space;\mathbf{x}_k\oplus\Delta&space;t\mathbf{f}(\mathbf{x}_k,&space;\mathbf{u}_k,&space;\mathbf{w}_k);\hat{\mathbf{x}}_{k&plus;1}&space;=&space;\hat{\mathbf{x}}_k\oplus\Delta&space;t\mathbf{f}(\hat{\mathbf{x}}_k,&space;\mathbf{u}_k,&space;\mathbf{0})"> <see here>, and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{u},&space;\mathbf{0}\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{u}, \mathbf{0}\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\mathbf{f}\left(\mathbf{x},&space;\mathbf{u},&space;\mathbf{w}\right)}{\partial\mathbf{w}}" title="\frac{\partial\mathbf{f}\left(\mathbf{x}, \mathbf{u}, \mathbf{w}\right)}{\partial\mathbf{w}}" /></a>, where w=0 could be left out:
+```
+Eigen::Matrix<double, state_length, 1> f(state &s, const input &i) {
+	Eigen::Matrix<double, state_length, 1> res = Eigen::Matrix<double, state_length, 1>::Zero();
+	res(0) = s.vel[0];
+	res(1) = s.vel[1];
+	res(2) = s.vel[2];
+	return res;
+}
+Eigen::Matrix<double, state_length, state_dof> df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 {
+	Eigen::Matrix<double, state_length, state_dof> cov = Eigen::Matrix<double, state_length, state_dof>::Zero();
+	cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
+	return cov;
+}
+Eigen::Matrix<double, state_length, process_noise_dof> df_dw(state &s, const input &i) {
+	Eigen::Matrix<double, state_length, process_noise_dof> cov = Eigen::Matrix<double, state_length, process_noise_dof>::Zero();
+	cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
+	return cov;
+}
+```
+Those functions would be called during ekf state predict
+
+5. Implement the differentiation of the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
+```
+Eigen::Matrix<double, Eigen::Dynamic, state_dof> dh_dx(state &s, bool &valid) {} //the iteration stops before convergence when valid is false
+Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> dh_dv(state &s, bool &valid) {}
+```
+Those functions would be called during ekf state update
+
+6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
+
+(1) initial state and covariance:
+```
+state init_state;
+esekfom::esekf<state, process_noise_dof, input>::cov init_P;
+esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
+```
+(2)
+```
+esekfom::esekf<state, process_noise_dof, input> kf;
+```
+Where **process_noise_dof** is the dimension of process noise, of type of std int
+
+7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
+```
+double epsi[state_dof] = {0.001};
+fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
+kf.init_dyn_runtime(f, df_dx, df_dw, dh_dx, dh_dv, Maximum_iter, epsi);
+```
+8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
+```
+kf.predict(dt, Q, in); // process noise covariance: Q
+```
+9. Once a measurement **z** is received, build system measurement as compound manifolds following step 3 and implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> :
+```
+measurement h(state &s, bool &valid) //the iteration stops before convergence when valid is false
+{
+	if (condition) valid = false; // the update iteration could be stopped when the condition other than convergence is satisfied
+	measurement h_;
+	h_.pos = s.pos;
+	return h_;
+}
+``` 
+then an iterated update is executed:
+```
+kf.update_iterated_dyn_runtime(z, R, h); // measurement noise covariance: R, an Eigen matrix
+```
+*Remarks(1):*
+- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows.
+5. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance.
+
+(1) initial state and covariance:
+```
+state init_state;
+esekfom::esekf<state, process_noise_dof, input>::cov init_P;
+esekfom::esekf<state, process_noise_dof, input> kf(init_state,init_P);
+```
+(2) default state and covariance:
+```
+esekfom::esekf<state, process_noise_dof, input> kf;
+```
+6. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object:
+```
+double epsi[state_dof] = {0.001};
+fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged
+kf.init_dyn_runtime_share(f, df_dx, df_dw, Maximum_iter, epsi);
+```
+7. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed:
+```
+kf.predict(dt, Q, in); // process noise covariance: Q. an Eigen matrix
+```
+8. Once a measurement **z** is received, build system measurement as compound manifolds following step 3 and implement the output equation <a href="https://www.codecogs.com/eqnedit.php?latex=\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)" title="\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)" /></a> and its differentiation <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x},&space;\mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}\boxplus\delta\mathbf{x}, \mathbf{0}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\delta\mathbf{x}}" /></a>, <a href="https://www.codecogs.com/eqnedit.php?latex=\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{\partial\left(\mathbf{h}\left(\mathbf{x},&space;\mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" title="\frac{\partial\left(\mathbf{h}\left(\mathbf{x}, \mathbf{v}\right)\boxminus\mathbf{h}\left(\mathbf{x},\mathbf{0}\right)\right)}{\partial\mathbf{v}}" /></a>:
+```
+measurement h_dyn_runtime_share(state &s, esekfom::dyn_runtime_share_datastruct<double> &dyn_runtime_share_data) 
+{
+	if(dyn_runtime_share_data.converge) {} // this value is true means iteration is converged 
+	if(condition) dyn_runtime_share_data.valid = false; // the iteration stops before convergence when this value is false, if conditions other than convergence is satisfied
+	dyn_runtime_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation 
+	dyn_runtime_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation
+	dyn_runtime_share_data.R = R; // R is the measurement noise covariance
+	
+	measurement h_;
+	h_.pos = s.pos;
+	return h_;
+}
+```
+This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function
+
+then an iterated update is executed:
+```
+kf.update_iterated_dyn_runtime_share(z, h_dyn_runtime_share);
+```
+
+*Remarks(2):*
+- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**:
+```
+state set_x;
+kf.change_x(set_x);
+esekfom::esekf<state, process_noise_dof, input>::cov set_P;
+kf.change_P(set_P);
+```
+
+## 5. Run the sample
+Clone the repository:
+
+```
+    git clone https://github.com/hku-mars/IKFoM.git
+```
+In the **Samples** file folder, there is the scource code that applys the **IKFoM** on the original source code from [FAST LIO](https://github.com/hku-mars/FAST_LIO). Please follow the README.md shown in that repository excepting the step **2. Build**, which is modified as:
+```
+cd ~/catkin_ws/src
+cp -r ~/IKFoM/Samples/FAST_LIO-stable FAST_LIO-stable
+cd ..
+catkin_make
+source devel/setup.bash
+```
+
+## 6.Acknowledgments
+Thanks for C. Hertzberg,  R.  Wagner,  U.  Frese,  and  L.  Schroder.  Integratinggeneric   sensor   fusion   algorithms   with   sound   state   representationsthrough  encapsulation  of  manifolds.
+
diff --git a/point_lio_ros2/include/common_lib.h b/point_lio_ros2/include/common_lib.h
new file mode 100644
index 0000000..a3ef8e5
--- /dev/null
+++ b/point_lio_ros2/include/common_lib.h
@@ -0,0 +1,189 @@
+#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 <sensor_msgs/msg/imu.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <deque>
+using namespace std;
+using namespace Eigen;
+
+#define PI_M (3.14159265358)
+#define G_m_s2 (9.81)         // Gravity 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   (0.0001)
+#define NUM_MATCH_POINTS    (5)
+#define MAX_MEAS_DIM        (10000)
+
+#define VEC_FROM_ARRAY(v)        v[0],v[1],v[2]
+#define VEC_FROM_ARRAY_SIX(v)    v[0],v[1],v[2],v[3],v[4],v[5]
+#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 pcl::PointXYZINormal PointType;
+typedef pcl::PointXYZRGB     PointTypeRGB;
+typedef pcl::PointCloud<PointType>    PointCloudXYZI;
+typedef pcl::PointCloud<PointTypeRGB> PointCloudXYZRGB;
+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>
+
+const M3D Eye3d(M3D::Identity());
+const M3F Eye3f(M3F::Identity());
+const V3D Zero3d(0, 0, 0);
+const V3F Zero3f(0, 0, 0);
+
+struct MeasureGroup     // Lidar data and imu dates for the curent process
+{
+    MeasureGroup()
+    {
+        lidar_beg_time = 0.0;
+        lidar_last_time = 0.0;
+        this->lidar.reset(new PointCloudXYZI());
+    };
+    double lidar_beg_time;
+    double lidar_last_time;
+    PointCloudXYZI::Ptr lidar;
+    deque<sensor_msgs::msg::Imu::ConstSharedPtr> imu{};
+};
+
+template <typename T>
+T calc_dist(PointType p1, PointType p2){
+    T 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>
+T calc_dist(Eigen::Vector3d p1, PointType p2){
+    T d = (p1(0) - p2.x) * (p1(0) - p2.x) + (p1(1) - p2.y) * (p1(1) - p2.y) + (p1(2) - p2.z) * (p1(2) - p2.z);
+    return d;
+}
+
+template<typename T>
+std::vector<int> time_compressing(const PointCloudXYZI::Ptr &point_cloud)
+{
+  int points_size = point_cloud->points.size();
+  int j = 0;
+  std::vector<int> time_seq;
+  // time_seq.clear();
+  time_seq.reserve(points_size);
+  for(int i = 0; i < points_size - 1; i++)
+  {
+    j++;
+    if (point_cloud->points[i+1].curvature > point_cloud->points[i].curvature)
+    {
+      time_seq.emplace_back(j);
+      j = 0;
+    }
+  }
+  if (j == 0)
+  {
+    time_seq.emplace_back(1);
+  }
+  else
+  {
+    time_seq.emplace_back(j+1);
+  }
+  return time_seq;
+}
+
+/* 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;
+}
+
+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;
+}
+
+inline double get_time_sec(const builtin_interfaces::msg::Time &time)
+{
+  return rclcpp::Time(time).seconds();
+}
+
+inline 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
\ No newline at end of file
diff --git a/point_lio_ros2/include/ikd-Tree/README.md b/point_lio_ros2/include/ikd-Tree/README.md
new file mode 100644
index 0000000..e113a91
--- /dev/null
+++ b/point_lio_ros2/include/ikd-Tree/README.md
@@ -0,0 +1,2 @@
+# ikd-Tree
+ikd-Tree is an incremental k-d tree for robotic applications.
diff --git a/point_lio_ros2/include/ikd-Tree/ikd_Tree.cpp b/point_lio_ros2/include/ikd-Tree/ikd_Tree.cpp
new file mode 100644
index 0000000..e8c4e86
--- /dev/null
+++ b/point_lio_ros2/include/ikd-Tree/ikd_Tree.cpp
@@ -0,0 +1,1728 @@
+#include "ikd_Tree.h"
+
+/*
+Description: ikd-Tree: an incremental k-d tree for robotic applications 
+Author: Yixi Cai
+email: yixicai@connect.hku.hk
+*/
+
+template <typename PointType>
+KD_TREE<PointType>::KD_TREE(float delete_param, float balance_param, float box_length)
+{
+    delete_criterion_param = delete_param;
+    balance_criterion_param = balance_param;
+    downsample_size = box_length;
+    Rebuild_Logger.clear();
+    termination_flag = false;
+    start_thread();
+}
+
+template <typename PointType>
+KD_TREE<PointType>::~KD_TREE()
+{
+    stop_thread();
+    Delete_Storage_Disabled = true;
+    delete_tree_nodes(&Root_Node);
+    PointVector().swap(PCL_Storage);
+    Rebuild_Logger.clear();
+}
+
+
+
+template <typename PointType>
+void KD_TREE<PointType>::InitializeKDTree(float delete_param, float balance_param, float box_length)
+{
+    Set_delete_criterion_param(delete_param);
+    Set_balance_criterion_param(balance_param);
+    set_downsample_param(box_length);
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::InitTreeNode(KD_TREE_NODE *root)
+{
+    root->point.x = 0.0f;
+    root->point.y = 0.0f;
+    root->point.z = 0.0f;
+    root->node_range_x[0] = 0.0f;
+    root->node_range_x[1] = 0.0f;
+    root->node_range_y[0] = 0.0f;
+    root->node_range_y[1] = 0.0f;
+    root->node_range_z[0] = 0.0f;
+    root->node_range_z[1] = 0.0f;
+    root->radius_sq = 0.0f;
+    root->division_axis = 0;
+    root->father_ptr = nullptr;
+    root->left_son_ptr = nullptr;
+    root->right_son_ptr = nullptr;
+    root->TreeSize = 0;
+    root->invalid_point_num = 0;
+    root->down_del_num = 0;
+    root->point_deleted = false;
+    root->tree_deleted = false;
+    root->need_push_down_to_left = false;
+    root->need_push_down_to_right = false;
+    root->point_downsample_deleted = false;
+    root->working_flag = false;
+    pthread_mutex_init(&(root->push_down_mutex_lock), NULL);
+}
+
+template <typename PointType>
+int KD_TREE<PointType>::size()
+{
+    int s = 0;
+    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+    {
+        if (Root_Node != nullptr)
+        {
+            return Root_Node->TreeSize;
+        }
+        else
+        {
+            return 0;
+        }
+    }
+    else
+    {
+        if (!pthread_mutex_trylock(&working_flag_mutex))
+        {
+            s = Root_Node->TreeSize;
+            pthread_mutex_unlock(&working_flag_mutex);
+            return s;
+        }
+        else
+        {
+            return Treesize_tmp;
+        }
+    }
+}
+
+template <typename PointType>
+BoxPointType KD_TREE<PointType>::tree_range()
+{
+    BoxPointType range;
+    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+    {
+        if (Root_Node != nullptr)
+        {
+            range.vertex_min[0] = Root_Node->node_range_x[0];
+            range.vertex_min[1] = Root_Node->node_range_y[0];
+            range.vertex_min[2] = Root_Node->node_range_z[0];
+            range.vertex_max[0] = Root_Node->node_range_x[1];
+            range.vertex_max[1] = Root_Node->node_range_y[1];
+            range.vertex_max[2] = Root_Node->node_range_z[1];
+        }
+        else
+        {
+            memset(&range, 0, sizeof(range));
+        }
+    }
+    else
+    {
+        if (!pthread_mutex_trylock(&working_flag_mutex))
+        {
+            range.vertex_min[0] = Root_Node->node_range_x[0];
+            range.vertex_min[1] = Root_Node->node_range_y[0];
+            range.vertex_min[2] = Root_Node->node_range_z[0];
+            range.vertex_max[0] = Root_Node->node_range_x[1];
+            range.vertex_max[1] = Root_Node->node_range_y[1];
+            range.vertex_max[2] = Root_Node->node_range_z[1];
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+        else
+        {
+            memset(&range, 0, sizeof(range));
+        }
+    }
+    return range;
+}
+
+template <typename PointType>
+int KD_TREE<PointType>::validnum()
+{
+    int s = 0;
+    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+    {
+        if (Root_Node != nullptr)
+            return (Root_Node->TreeSize - Root_Node->invalid_point_num);
+        else
+            return 0;
+    }
+    else
+    {
+        if (!pthread_mutex_trylock(&working_flag_mutex))
+        {
+            s = Root_Node->TreeSize - Root_Node->invalid_point_num;
+            pthread_mutex_unlock(&working_flag_mutex);
+            return s;
+        }
+        else
+        {
+            return -1;
+        }
+    }
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::root_alpha(float &alpha_bal, float &alpha_del)
+{
+    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+    {
+        alpha_bal = Root_Node->alpha_bal;
+        alpha_del = Root_Node->alpha_del;
+        return;
+    }
+    else
+    {
+        if (!pthread_mutex_trylock(&working_flag_mutex))
+        {
+            alpha_bal = Root_Node->alpha_bal;
+            alpha_del = Root_Node->alpha_del;
+            pthread_mutex_unlock(&working_flag_mutex);
+            return;
+        }
+        else
+        {
+            alpha_bal = alpha_bal_tmp;
+            alpha_del = alpha_del_tmp;
+            return;
+        }
+    }
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::start_thread()
+{
+    pthread_mutex_init(&termination_flag_mutex_lock, NULL);
+    pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL);
+    pthread_mutex_init(&rebuild_logger_mutex_lock, NULL);
+    pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL);
+    pthread_mutex_init(&working_flag_mutex, NULL);
+    pthread_mutex_init(&search_flag_mutex, NULL);
+    pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void *)this);
+    printf("Multi thread started \n");
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::stop_thread()
+{
+    pthread_mutex_lock(&termination_flag_mutex_lock);
+    termination_flag = true;
+    pthread_mutex_unlock(&termination_flag_mutex_lock);
+    if (rebuild_thread)
+        pthread_join(rebuild_thread, NULL);
+    pthread_mutex_destroy(&termination_flag_mutex_lock);
+    pthread_mutex_destroy(&rebuild_logger_mutex_lock);
+    pthread_mutex_destroy(&rebuild_ptr_mutex_lock);
+    pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock);
+    pthread_mutex_destroy(&working_flag_mutex);
+    pthread_mutex_destroy(&search_flag_mutex);
+}
+
+template <typename PointType>
+void *KD_TREE<PointType>::multi_thread_ptr(void *arg)
+{
+    KD_TREE *handle = (KD_TREE *)arg;
+    handle->multi_thread_rebuild();
+    return nullptr;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::multi_thread_rebuild()
+{
+    bool terminated = false;
+    KD_TREE_NODE *father_ptr, **new_node_ptr;
+    pthread_mutex_lock(&termination_flag_mutex_lock);
+    terminated = termination_flag;
+    pthread_mutex_unlock(&termination_flag_mutex_lock);
+    while (!terminated)
+    {
+        pthread_mutex_lock(&rebuild_ptr_mutex_lock);
+        pthread_mutex_lock(&working_flag_mutex);
+        if (Rebuild_Ptr != nullptr)
+        {
+            /* Traverse and copy */
+            if (!Rebuild_Logger.empty())
+            {
+                printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n");
+            }
+            rebuild_flag = true;
+            if (*Rebuild_Ptr == Root_Node)
+            {
+                Treesize_tmp = Root_Node->TreeSize;
+                Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num;
+                alpha_bal_tmp = Root_Node->alpha_bal;
+                alpha_del_tmp = Root_Node->alpha_del;
+            }
+            KD_TREE_NODE *old_root_node = (*Rebuild_Ptr);
+            father_ptr = (*Rebuild_Ptr)->father_ptr;
+            PointVector().swap(Rebuild_PCL_Storage);
+            // Lock Search
+            pthread_mutex_lock(&search_flag_mutex);
+            while (search_mutex_counter != 0)
+            {
+                pthread_mutex_unlock(&search_flag_mutex);
+                usleep(1);
+                pthread_mutex_lock(&search_flag_mutex);
+            }
+            search_mutex_counter = -1;
+            pthread_mutex_unlock(&search_flag_mutex);
+            // Lock deleted points cache
+            pthread_mutex_lock(&points_deleted_rebuild_mutex_lock);
+            flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC);
+            // Unlock deleted points cache
+            pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock);
+            // Unlock Search
+            pthread_mutex_lock(&search_flag_mutex);
+            search_mutex_counter = 0;
+            pthread_mutex_unlock(&search_flag_mutex);
+            pthread_mutex_unlock(&working_flag_mutex);
+            /* Rebuild and update missed operations*/
+            Operation_Logger_Type Operation;
+            KD_TREE_NODE *new_root_node = nullptr;
+            if (int(Rebuild_PCL_Storage.size()) > 0)
+            {
+                BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size() - 1, Rebuild_PCL_Storage);
+                // Rebuild has been done. Updates the blocked operations into the new tree
+                pthread_mutex_lock(&working_flag_mutex);
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                int tmp_counter = 0;
+                while (!Rebuild_Logger.empty())
+                {
+                    Operation = Rebuild_Logger.front();
+                    max_queue_size = max(max_queue_size, Rebuild_Logger.size());
+                    Rebuild_Logger.pop();
+                    pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+                    pthread_mutex_unlock(&working_flag_mutex);
+                    run_operation(&new_root_node, Operation);
+                    tmp_counter++;
+                    if (tmp_counter % 10 == 0)
+                        usleep(1);
+                    pthread_mutex_lock(&working_flag_mutex);
+                    pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                }
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            /* Replace to original tree*/
+            // pthread_mutex_lock(&working_flag_mutex);
+            pthread_mutex_lock(&search_flag_mutex);
+            while (search_mutex_counter != 0)
+            {
+                pthread_mutex_unlock(&search_flag_mutex);
+                usleep(1);
+                pthread_mutex_lock(&search_flag_mutex);
+            }
+            search_mutex_counter = -1;
+            pthread_mutex_unlock(&search_flag_mutex);
+            if (father_ptr->left_son_ptr == *Rebuild_Ptr)
+            {
+                father_ptr->left_son_ptr = new_root_node;
+            }
+            else if (father_ptr->right_son_ptr == *Rebuild_Ptr)
+            {
+                father_ptr->right_son_ptr = new_root_node;
+            }
+            else
+            {
+                throw "Error: Father ptr incompatible with current node\n";
+            }
+            if (new_root_node != nullptr)
+                new_root_node->father_ptr = father_ptr;
+            (*Rebuild_Ptr) = new_root_node;
+            int valid_old = old_root_node->TreeSize - old_root_node->invalid_point_num;
+            int valid_new = new_root_node->TreeSize - new_root_node->invalid_point_num;
+            if (father_ptr == STATIC_ROOT_NODE)
+                Root_Node = STATIC_ROOT_NODE->left_son_ptr;
+            KD_TREE_NODE *update_root = *Rebuild_Ptr;
+            while (update_root != nullptr && update_root != Root_Node)
+            {
+                update_root = update_root->father_ptr;
+                if (update_root->working_flag)
+                    break;
+                if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left)
+                    break;
+                if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right)
+                    break;
+                Update(update_root);
+            }
+            pthread_mutex_lock(&search_flag_mutex);
+            search_mutex_counter = 0;
+            pthread_mutex_unlock(&search_flag_mutex);
+            Rebuild_Ptr = nullptr;
+            pthread_mutex_unlock(&working_flag_mutex);
+            rebuild_flag = false;
+            /* Delete discarded tree nodes */
+            delete_tree_nodes(&old_root_node);
+        }
+        else
+        {
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+        pthread_mutex_unlock(&rebuild_ptr_mutex_lock);
+        pthread_mutex_lock(&termination_flag_mutex_lock);
+        terminated = termination_flag;
+        pthread_mutex_unlock(&termination_flag_mutex_lock);
+        usleep(100);
+    }
+    printf("Rebuild thread terminated normally\n");
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation)
+{
+    switch (operation.op)
+    {
+    case ADD_POINT:
+        Add_by_point(root, operation.point, false, (*root)->division_axis);
+        break;
+    case ADD_BOX:
+        Add_by_range(root, operation.boxpoint, false);
+        break;
+    case DELETE_POINT:
+        Delete_by_point(root, operation.point, false);
+        break;
+    case DELETE_BOX:
+        Delete_by_range(root, operation.boxpoint, false, false);
+        break;
+    case DOWNSAMPLE_DELETE:
+        Delete_by_range(root, operation.boxpoint, false, true);
+        break;
+    case PUSH_DOWN:
+        (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted;
+        (*root)->point_downsample_deleted |= operation.tree_downsample_deleted;
+        (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted;
+        (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted;
+        if (operation.tree_downsample_deleted)
+            (*root)->down_del_num = (*root)->TreeSize;
+        if (operation.tree_deleted)
+            (*root)->invalid_point_num = (*root)->TreeSize;
+        else
+            (*root)->invalid_point_num = (*root)->down_del_num;
+        (*root)->need_push_down_to_left = true;
+        (*root)->need_push_down_to_right = true;
+        break;
+    default:
+        break;
+    }
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Build(PointVector point_cloud)
+{
+    if (Root_Node != nullptr)
+    {
+        delete_tree_nodes(&Root_Node);
+    }
+    if (point_cloud.size() == 0)
+        return;
+    STATIC_ROOT_NODE = new KD_TREE_NODE;
+    InitTreeNode(STATIC_ROOT_NODE);
+    BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size() - 1, point_cloud);
+    Update(STATIC_ROOT_NODE);
+    STATIC_ROOT_NODE->TreeSize = 0;
+    Root_Node = STATIC_ROOT_NODE->left_son_ptr;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector<float> &Point_Distance, float max_dist)
+{
+    MANUAL_HEAP q(2 * k_nearest);
+    q.clear();
+    vector<float>().swap(Point_Distance);
+    if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+    {
+        Search(Root_Node, k_nearest, point, q, max_dist);
+    }
+    else
+    {
+        pthread_mutex_lock(&search_flag_mutex);
+        while (search_mutex_counter == -1)
+        {
+            pthread_mutex_unlock(&search_flag_mutex);
+            usleep(1);
+            pthread_mutex_lock(&search_flag_mutex);
+        }
+        search_mutex_counter += 1;
+        pthread_mutex_unlock(&search_flag_mutex);
+        Search(Root_Node, k_nearest, point, q, max_dist);
+        pthread_mutex_lock(&search_flag_mutex);
+        search_mutex_counter -= 1;
+        pthread_mutex_unlock(&search_flag_mutex);
+    }
+    int k_found = min(k_nearest, int(q.size()));
+    PointVector().swap(Nearest_Points);
+    vector<float>().swap(Point_Distance);
+    for (int i = 0; i < k_found; i++)
+    {
+        Nearest_Points.insert(Nearest_Points.begin(), q.top().point);
+        Point_Distance.insert(Point_Distance.begin(), q.top().dist);
+        q.pop();
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage)
+{
+    Storage.clear();
+    Search_by_range(Root_Node, Box_of_Point, Storage);
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Radius_Search(PointType point, const float radius, PointVector &Storage)
+{
+    Storage.clear();
+    Search_by_radius(Root_Node, point, radius, Storage);
+}
+
+template <typename PointType>
+int KD_TREE<PointType>::Add_Points(PointVector &PointToAdd, bool downsample_on)
+{
+    int NewPointSize = PointToAdd.size();
+    int tree_size = size();
+    BoxPointType Box_of_Point;
+    PointType downsample_result, mid_point;
+    bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH;
+    float min_dist, tmp_dist;
+    int tmp_counter = 0;
+    for (int i = 0; i < PointToAdd.size(); i++)
+    {
+        if (downsample_switch)
+        {
+            Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x / downsample_size) * downsample_size;
+            Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0] + downsample_size;
+            Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y / downsample_size) * downsample_size;
+            Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1] + downsample_size;
+            Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z / downsample_size) * downsample_size;
+            Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2] + downsample_size;
+            mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0] - Box_of_Point.vertex_min[0]) / 2.0;
+            mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1] - Box_of_Point.vertex_min[1]) / 2.0;
+            mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2] - Box_of_Point.vertex_min[2]) / 2.0;
+            PointVector().swap(Downsample_Storage);
+            Search_by_range(Root_Node, Box_of_Point, Downsample_Storage);
+            min_dist = calc_dist(PointToAdd[i], mid_point);
+            downsample_result = PointToAdd[i];
+            for (int index = 0; index < Downsample_Storage.size(); index++)
+            {
+                tmp_dist = calc_dist(Downsample_Storage[index], mid_point);
+                if (tmp_dist < min_dist)
+                {
+                    min_dist = tmp_dist;
+                    downsample_result = Downsample_Storage[index];
+                }
+            }
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+            {
+                if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result))
+                {
+                    if (Downsample_Storage.size() > 0)
+                        Delete_by_range(&Root_Node, Box_of_Point, true, true);
+                    Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis);
+                    tmp_counter++;
+                }
+            }
+            else
+            {
+                if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result))
+                {
+                    Operation_Logger_Type operation_delete, operation;
+                    operation_delete.boxpoint = Box_of_Point;
+                    operation_delete.op = DOWNSAMPLE_DELETE;
+                    operation.point = downsample_result;
+                    operation.op = ADD_POINT;
+                    pthread_mutex_lock(&working_flag_mutex);
+                    if (Downsample_Storage.size() > 0)
+                        Delete_by_range(&Root_Node, Box_of_Point, false, true);
+                    Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis);
+                    tmp_counter++;
+                    if (rebuild_flag)
+                    {
+                        pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                        if (Downsample_Storage.size() > 0)
+                            Rebuild_Logger.push(operation_delete);
+                        Rebuild_Logger.push(operation);
+                        pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+                    }
+                    pthread_mutex_unlock(&working_flag_mutex);
+                };
+            }
+        }
+        else
+        {
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+            {
+                Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis);
+            }
+            else
+            {
+                Operation_Logger_Type operation;
+                operation.point = PointToAdd[i];
+                operation.op = ADD_POINT;
+                pthread_mutex_lock(&working_flag_mutex);
+                Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis);
+                if (rebuild_flag)
+                {
+                    pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                    Rebuild_Logger.push(operation);
+                    pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+                }
+                pthread_mutex_unlock(&working_flag_mutex);
+            }
+        }
+    }
+    return tmp_counter;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Add_Point_Boxes(vector<BoxPointType> &BoxPoints)
+{
+    for (int i = 0; i < BoxPoints.size(); i++)
+    {
+        if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+        {
+            Add_by_range(&Root_Node, BoxPoints[i], true);
+        }
+        else
+        {
+            Operation_Logger_Type operation;
+            operation.boxpoint = BoxPoints[i];
+            operation.op = ADD_BOX;
+            pthread_mutex_lock(&working_flag_mutex);
+            Add_by_range(&Root_Node, BoxPoints[i], false);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(operation);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Delete_Points(PointVector &PointToDel)
+{
+    for (int i = 0; i < PointToDel.size(); i++)
+    {
+        if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+        {
+            Delete_by_point(&Root_Node, PointToDel[i], true);
+        }
+        else
+        {
+            Operation_Logger_Type operation;
+            operation.point = PointToDel[i];
+            operation.op = DELETE_POINT;
+            pthread_mutex_lock(&working_flag_mutex);
+            Delete_by_point(&Root_Node, PointToDel[i], false);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(operation);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    return;
+}
+
+template <typename PointType>
+int KD_TREE<PointType>::Delete_Point_Boxes(vector<BoxPointType> &BoxPoints)
+{
+    int tmp_counter = 0;
+    for (int i = 0; i < BoxPoints.size(); i++)
+    {
+        if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node)
+        {
+            tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], true, false);
+        }
+        else
+        {
+            Operation_Logger_Type operation;
+            operation.boxpoint = BoxPoints[i];
+            operation.op = DELETE_BOX;
+            pthread_mutex_lock(&working_flag_mutex);
+            tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], false, false);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(operation);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    return tmp_counter;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::acquire_removed_points(PointVector &removed_points)
+{
+    pthread_mutex_lock(&points_deleted_rebuild_mutex_lock);
+    for (int i = 0; i < Points_deleted.size(); i++)
+    {
+        removed_points.push_back(Points_deleted[i]);
+    }
+    for (int i = 0; i < Multithread_Points_deleted.size(); i++)
+    {
+        removed_points.push_back(Multithread_Points_deleted[i]);
+    }
+    Points_deleted.clear();
+    Multithread_Points_deleted.clear();
+    pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock);
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage)
+{
+    if (l > r)
+        return;
+    *root = new KD_TREE_NODE;
+    InitTreeNode(*root);
+    int mid = (l + r) >> 1;
+    int div_axis = 0;
+    int i;
+    // Find the best division Axis
+    float min_value[3] = {INFINITY, INFINITY, INFINITY};
+    float max_value[3] = {-INFINITY, -INFINITY, -INFINITY};
+    float dim_range[3] = {0, 0, 0};
+    for (i = l; i <= r; i++)
+    {
+        min_value[0] = min(min_value[0], Storage[i].x);
+        min_value[1] = min(min_value[1], Storage[i].y);
+        min_value[2] = min(min_value[2], Storage[i].z);
+        max_value[0] = max(max_value[0], Storage[i].x);
+        max_value[1] = max(max_value[1], Storage[i].y);
+        max_value[2] = max(max_value[2], Storage[i].z);
+    }
+    // Select the longest dimension as division axis
+    for (i = 0; i < 3; i++)
+        dim_range[i] = max_value[i] - min_value[i];
+    for (i = 1; i < 3; i++)
+        if (dim_range[i] > dim_range[div_axis])
+            div_axis = i;
+    // Divide by the division axis and recursively build.
+
+    (*root)->division_axis = div_axis;
+    switch (div_axis)
+    {
+    case 0:
+        nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x);
+        break;
+    case 1:
+        nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_y);
+        break;
+    case 2:
+        nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_z);
+        break;
+    default:
+        nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x);
+        break;
+    }
+    (*root)->point = Storage[mid];
+    KD_TREE_NODE *left_son = nullptr, *right_son = nullptr;
+    BuildTree(&left_son, l, mid - 1, Storage);
+    BuildTree(&right_son, mid + 1, r, Storage);
+    (*root)->left_son_ptr = left_son;
+    (*root)->right_son_ptr = right_son;
+    Update((*root));
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Rebuild(KD_TREE_NODE **root)
+{
+    KD_TREE_NODE *father_ptr;
+    if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num)
+    {
+        if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock))
+        {
+            if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize))
+            {
+                Rebuild_Ptr = root;
+            }
+            pthread_mutex_unlock(&rebuild_ptr_mutex_lock);
+        }
+    }
+    else
+    {
+        father_ptr = (*root)->father_ptr;
+        int size_rec = (*root)->TreeSize;
+        PCL_Storage.clear();
+        flatten(*root, PCL_Storage, DELETE_POINTS_REC);
+        delete_tree_nodes(root);
+        BuildTree(root, 0, PCL_Storage.size() - 1, PCL_Storage);
+        if (*root != nullptr)
+            (*root)->father_ptr = father_ptr;
+        if (*root == Root_Node)
+            STATIC_ROOT_NODE->left_son_ptr = *root;
+    }
+    return;
+}
+
+template <typename PointType>
+int KD_TREE<PointType>::Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample)
+{
+    if ((*root) == nullptr || (*root)->tree_deleted)
+        return 0;
+    (*root)->working_flag = true;
+    Push_Down(*root);
+    int tmp_counter = 0;
+    if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1])
+        return 0;
+    if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1])
+        return 0;
+    if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1])
+        return 0;
+    if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1])
+    {
+        (*root)->tree_deleted = true;
+        (*root)->point_deleted = true;
+        (*root)->need_push_down_to_left = true;
+        (*root)->need_push_down_to_right = true;
+        tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num;
+        (*root)->invalid_point_num = (*root)->TreeSize;
+        if (is_downsample)
+        {
+            (*root)->tree_downsample_deleted = true;
+            (*root)->point_downsample_deleted = true;
+            (*root)->down_del_num = (*root)->TreeSize;
+        }
+        return tmp_counter;
+    }
+    if (!(*root)->point_deleted && boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z)
+    {
+        (*root)->point_deleted = true;
+        tmp_counter += 1;
+        if (is_downsample)
+            (*root)->point_downsample_deleted = true;
+    }
+    Operation_Logger_Type delete_box_log;
+    struct timespec Timeout;
+    if (is_downsample)
+        delete_box_log.op = DOWNSAMPLE_DELETE;
+    else
+        delete_box_log.op = DELETE_BOX;
+    delete_box_log.boxpoint = boxpoint;
+    if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)
+    {
+        tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample);
+    }
+    else
+    {
+        pthread_mutex_lock(&working_flag_mutex);
+        tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample);
+        if (rebuild_flag)
+        {
+            pthread_mutex_lock(&rebuild_logger_mutex_lock);
+            Rebuild_Logger.push(delete_box_log);
+            pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+        }
+        pthread_mutex_unlock(&working_flag_mutex);
+    }
+    if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)
+    {
+        tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample);
+    }
+    else
+    {
+        pthread_mutex_lock(&working_flag_mutex);
+        tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample);
+        if (rebuild_flag)
+        {
+            pthread_mutex_lock(&rebuild_logger_mutex_lock);
+            Rebuild_Logger.push(delete_box_log);
+            pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+        }
+        pthread_mutex_unlock(&working_flag_mutex);
+    }
+    Update(*root);
+    if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)
+        Rebuild_Ptr = nullptr;
+    bool need_rebuild = allow_rebuild & Criterion_Check((*root));
+    if (need_rebuild)
+        Rebuild(root);
+    if ((*root) != nullptr)
+        (*root)->working_flag = false;
+    return tmp_counter;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild)
+{
+    if ((*root) == nullptr || (*root)->tree_deleted)
+        return;
+    (*root)->working_flag = true;
+    Push_Down(*root);
+    if (same_point((*root)->point, point) && !(*root)->point_deleted)
+    {
+        (*root)->point_deleted = true;
+        (*root)->invalid_point_num += 1;
+        if ((*root)->invalid_point_num == (*root)->TreeSize)
+            (*root)->tree_deleted = true;
+        return;
+    }
+    Operation_Logger_Type delete_log;
+    struct timespec Timeout;
+    delete_log.op = DELETE_POINT;
+    delete_log.point = point;
+    if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z))
+    {
+        if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)
+        {
+            Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild);
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            Delete_by_point(&(*root)->left_son_ptr, point, false);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(delete_log);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    else
+    {
+        if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)
+        {
+            Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild);
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            Delete_by_point(&(*root)->right_son_ptr, point, false);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(delete_log);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    Update(*root);
+    if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)
+        Rebuild_Ptr = nullptr;
+    bool need_rebuild = allow_rebuild & Criterion_Check((*root));
+    if (need_rebuild)
+        Rebuild(root);
+    if ((*root) != nullptr)
+        (*root)->working_flag = false;
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild)
+{
+    if ((*root) == nullptr)
+        return;
+    (*root)->working_flag = true;
+    Push_Down(*root);
+    if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1])
+        return;
+    if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1])
+        return;
+    if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1])
+        return;
+    if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1])
+    {
+        (*root)->tree_deleted = false || (*root)->tree_downsample_deleted;
+        (*root)->point_deleted = false || (*root)->point_downsample_deleted;
+        (*root)->need_push_down_to_left = true;
+        (*root)->need_push_down_to_right = true;
+        (*root)->invalid_point_num = (*root)->down_del_num;
+        return;
+    }
+    if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z)
+    {
+        (*root)->point_deleted = (*root)->point_downsample_deleted;
+    }
+    Operation_Logger_Type add_box_log;
+    struct timespec Timeout;
+    add_box_log.op = ADD_BOX;
+    add_box_log.boxpoint = boxpoint;
+    if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)
+    {
+        Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild);
+    }
+    else
+    {
+        pthread_mutex_lock(&working_flag_mutex);
+        Add_by_range(&((*root)->left_son_ptr), boxpoint, false);
+        if (rebuild_flag)
+        {
+            pthread_mutex_lock(&rebuild_logger_mutex_lock);
+            Rebuild_Logger.push(add_box_log);
+            pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+        }
+        pthread_mutex_unlock(&working_flag_mutex);
+    }
+    if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)
+    {
+        Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild);
+    }
+    else
+    {
+        pthread_mutex_lock(&working_flag_mutex);
+        Add_by_range(&((*root)->right_son_ptr), boxpoint, false);
+        if (rebuild_flag)
+        {
+            pthread_mutex_lock(&rebuild_logger_mutex_lock);
+            Rebuild_Logger.push(add_box_log);
+            pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+        }
+        pthread_mutex_unlock(&working_flag_mutex);
+    }
+    Update(*root);
+    if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)
+        Rebuild_Ptr = nullptr;
+    bool need_rebuild = allow_rebuild & Criterion_Check((*root));
+    if (need_rebuild)
+        Rebuild(root);
+    if ((*root) != nullptr)
+        (*root)->working_flag = false;
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis)
+{
+    if (*root == nullptr)
+    {
+        *root = new KD_TREE_NODE;
+        InitTreeNode(*root);
+        (*root)->point = point;
+        (*root)->division_axis = (father_axis + 1) % 3;
+        Update(*root);
+        return;
+    }
+    (*root)->working_flag = true;
+    Operation_Logger_Type add_log;
+    struct timespec Timeout;
+    add_log.op = ADD_POINT;
+    add_log.point = point;
+    Push_Down(*root);
+    if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z))
+    {
+        if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr)
+        {
+            Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis);
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            Add_by_point(&(*root)->left_son_ptr, point, false, (*root)->division_axis);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(add_log);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    else
+    {
+        if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr)
+        {
+            Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild, (*root)->division_axis);
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            Add_by_point(&(*root)->right_son_ptr, point, false, (*root)->division_axis);
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(add_log);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    Update(*root);
+    if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num)
+        Rebuild_Ptr = nullptr;
+    bool need_rebuild = allow_rebuild & Criterion_Check((*root));
+    if (need_rebuild)
+        Rebuild(root);
+    if ((*root) != nullptr)
+        (*root)->working_flag = false;
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist)
+{
+    if (root == nullptr || root->tree_deleted)
+        return;
+    float cur_dist = calc_box_dist(root, point);
+    float max_dist_sqr = max_dist * max_dist;
+    if (cur_dist > max_dist_sqr)
+        return;
+    int retval;
+    if (root->need_push_down_to_left || root->need_push_down_to_right)
+    {
+        retval = pthread_mutex_trylock(&(root->push_down_mutex_lock));
+        if (retval == 0)
+        {
+            Push_Down(root);
+            pthread_mutex_unlock(&(root->push_down_mutex_lock));
+        }
+        else
+        {
+            pthread_mutex_lock(&(root->push_down_mutex_lock));
+            pthread_mutex_unlock(&(root->push_down_mutex_lock));
+        }
+    }
+    if (!root->point_deleted)
+    {
+        float dist = calc_dist(point, root->point);
+        if (dist <= max_dist_sqr && (q.size() < k_nearest || dist < q.top().dist))
+        {
+            if (q.size() >= k_nearest)
+                q.pop();
+            PointType_CMP current_point{root->point, dist};
+            q.push(current_point);
+        }
+    }
+    int cur_search_counter;
+    float dist_left_node = calc_box_dist(root->left_son_ptr, point);
+    float dist_right_node = calc_box_dist(root->right_son_ptr, point);
+    if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist)
+    {
+        if (dist_left_node <= dist_right_node)
+        {
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)
+            {
+                Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+            }
+            else
+            {
+                pthread_mutex_lock(&search_flag_mutex);
+                while (search_mutex_counter == -1)
+                {
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    usleep(1);
+                    pthread_mutex_lock(&search_flag_mutex);
+                }
+                search_mutex_counter += 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+                Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+                pthread_mutex_lock(&search_flag_mutex);
+                search_mutex_counter -= 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+            }
+            if (q.size() < k_nearest || dist_right_node < q.top().dist)
+            {
+                if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)
+                {
+                    Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+                }
+                else
+                {
+                    pthread_mutex_lock(&search_flag_mutex);
+                    while (search_mutex_counter == -1)
+                    {
+                        pthread_mutex_unlock(&search_flag_mutex);
+                        usleep(1);
+                        pthread_mutex_lock(&search_flag_mutex);
+                    }
+                    search_mutex_counter += 1;
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+                    pthread_mutex_lock(&search_flag_mutex);
+                    search_mutex_counter -= 1;
+                    pthread_mutex_unlock(&search_flag_mutex);
+                }
+            }
+        }
+        else
+        {
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)
+            {
+                Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+            }
+            else
+            {
+                pthread_mutex_lock(&search_flag_mutex);
+                while (search_mutex_counter == -1)
+                {
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    usleep(1);
+                    pthread_mutex_lock(&search_flag_mutex);
+                }
+                search_mutex_counter += 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+                Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+                pthread_mutex_lock(&search_flag_mutex);
+                search_mutex_counter -= 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+            }
+            if (q.size() < k_nearest || dist_left_node < q.top().dist)
+            {
+                if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)
+                {
+                    Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+                }
+                else
+                {
+                    pthread_mutex_lock(&search_flag_mutex);
+                    while (search_mutex_counter == -1)
+                    {
+                        pthread_mutex_unlock(&search_flag_mutex);
+                        usleep(1);
+                        pthread_mutex_lock(&search_flag_mutex);
+                    }
+                    search_mutex_counter += 1;
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+                    pthread_mutex_lock(&search_flag_mutex);
+                    search_mutex_counter -= 1;
+                    pthread_mutex_unlock(&search_flag_mutex);
+                }
+            }
+        }
+    }
+    else
+    {
+        if (dist_left_node < q.top().dist)
+        {
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)
+            {
+                Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+            }
+            else
+            {
+                pthread_mutex_lock(&search_flag_mutex);
+                while (search_mutex_counter == -1)
+                {
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    usleep(1);
+                    pthread_mutex_lock(&search_flag_mutex);
+                }
+                search_mutex_counter += 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+                Search(root->left_son_ptr, k_nearest, point, q, max_dist);
+                pthread_mutex_lock(&search_flag_mutex);
+                search_mutex_counter -= 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+            }
+        }
+        if (dist_right_node < q.top().dist)
+        {
+            if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)
+            {
+                Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+            }
+            else
+            {
+                pthread_mutex_lock(&search_flag_mutex);
+                while (search_mutex_counter == -1)
+                {
+                    pthread_mutex_unlock(&search_flag_mutex);
+                    usleep(1);
+                    pthread_mutex_lock(&search_flag_mutex);
+                }
+                search_mutex_counter += 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+                Search(root->right_son_ptr, k_nearest, point, q, max_dist);
+                pthread_mutex_lock(&search_flag_mutex);
+                search_mutex_counter -= 1;
+                pthread_mutex_unlock(&search_flag_mutex);
+            }
+        }
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage)
+{
+    if (root == nullptr)
+        return;
+    Push_Down(root);
+    if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1])
+        return;
+    if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1])
+        return;
+    if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1])
+        return;
+    if (boxpoint.vertex_min[0] <= root->node_range_x[0] && boxpoint.vertex_max[0] > root->node_range_x[1] && boxpoint.vertex_min[1] <= root->node_range_y[0] && boxpoint.vertex_max[1] > root->node_range_y[1] && boxpoint.vertex_min[2] <= root->node_range_z[0] && boxpoint.vertex_max[2] > root->node_range_z[1])
+    {
+        flatten(root, Storage, NOT_RECORD);
+        return;
+    }
+    if (boxpoint.vertex_min[0] <= root->point.x && boxpoint.vertex_max[0] > root->point.x && boxpoint.vertex_min[1] <= root->point.y && boxpoint.vertex_max[1] > root->point.y && boxpoint.vertex_min[2] <= root->point.z && boxpoint.vertex_max[2] > root->point.z)
+    {
+        if (!root->point_deleted)
+            Storage.push_back(root->point);
+    }
+    if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr)
+    {
+        Search_by_range(root->left_son_ptr, boxpoint, Storage);
+    }
+    else
+    {
+        pthread_mutex_lock(&search_flag_mutex);
+        Search_by_range(root->left_son_ptr, boxpoint, Storage);
+        pthread_mutex_unlock(&search_flag_mutex);
+    }
+    if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr)
+    {
+        Search_by_range(root->right_son_ptr, boxpoint, Storage);
+    }
+    else
+    {
+        pthread_mutex_lock(&search_flag_mutex);
+        Search_by_range(root->right_son_ptr, boxpoint, Storage);
+        pthread_mutex_unlock(&search_flag_mutex);
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage)
+{
+    if (root == nullptr)
+        return;
+    Push_Down(root);
+    PointType range_center;
+    range_center.x = (root->node_range_x[0] + root->node_range_x[1]) * 0.5;
+    range_center.y = (root->node_range_y[0] + root->node_range_y[1]) * 0.5;
+    range_center.z = (root->node_range_z[0] + root->node_range_z[1]) * 0.5;
+    float dist = sqrt(calc_dist(range_center, point));
+    if (dist > radius + sqrt(root->radius_sq)) return;
+    if (dist <= radius - sqrt(root->radius_sq)) 
+    {
+        flatten(root, Storage, NOT_RECORD);
+        return;
+    }
+    if (!root->point_deleted && calc_dist(root->point, point) <= radius * radius){
+        Storage.push_back(root->point);
+    }
+    if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr)
+    {
+        Search_by_radius(root->left_son_ptr, point, radius, Storage);
+    }
+    else
+    {
+        pthread_mutex_lock(&search_flag_mutex);
+        Search_by_radius(root->left_son_ptr, point, radius, Storage);
+        pthread_mutex_unlock(&search_flag_mutex);
+    }
+    if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr)
+    {
+        Search_by_radius(root->right_son_ptr, point, radius, Storage);
+    }
+    else
+    {
+        pthread_mutex_lock(&search_flag_mutex);
+        Search_by_radius(root->right_son_ptr, point, radius, Storage);
+        pthread_mutex_unlock(&search_flag_mutex);
+    }    
+    return;
+}
+
+template <typename PointType>
+bool KD_TREE<PointType>::Criterion_Check(KD_TREE_NODE *root)
+{
+    if (root->TreeSize <= Minimal_Unbalanced_Tree_Size)
+    {
+        return false;
+    }
+    float balance_evaluation = 0.0f;
+    float delete_evaluation = 0.0f;
+    KD_TREE_NODE *son_ptr = root->left_son_ptr;
+    if (son_ptr == nullptr)
+        son_ptr = root->right_son_ptr;
+    delete_evaluation = float(root->invalid_point_num) / root->TreeSize;
+    balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize - 1);
+    if (delete_evaluation > delete_criterion_param)
+    {
+        return true;
+    }
+    if (balance_evaluation > balance_criterion_param || balance_evaluation < 1 - balance_criterion_param)
+    {
+        return true;
+    }
+    return false;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Push_Down(KD_TREE_NODE *root)
+{
+    if (root == nullptr)
+        return;
+    Operation_Logger_Type operation;
+    operation.op = PUSH_DOWN;
+    operation.tree_deleted = root->tree_deleted;
+    operation.tree_downsample_deleted = root->tree_downsample_deleted;
+    if (root->need_push_down_to_left && root->left_son_ptr != nullptr)
+    {
+        if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr)
+        {
+            root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
+            root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
+            root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted;
+            root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted;
+            if (root->tree_downsample_deleted)
+                root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize;
+            if (root->tree_deleted)
+                root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize;
+            else
+                root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num;
+            root->left_son_ptr->need_push_down_to_left = true;
+            root->left_son_ptr->need_push_down_to_right = true;
+            root->need_push_down_to_left = false;
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
+            root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
+            root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted;
+            root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted;
+            if (root->tree_downsample_deleted)
+                root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize;
+            if (root->tree_deleted)
+                root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize;
+            else
+                root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num;
+            root->left_son_ptr->need_push_down_to_left = true;
+            root->left_son_ptr->need_push_down_to_right = true;
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(operation);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            root->need_push_down_to_left = false;
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    if (root->need_push_down_to_right && root->right_son_ptr != nullptr)
+    {
+        if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr)
+        {
+            root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
+            root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
+            root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted;
+            root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted;
+            if (root->tree_downsample_deleted)
+                root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize;
+            if (root->tree_deleted)
+                root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize;
+            else
+                root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num;
+            root->right_son_ptr->need_push_down_to_left = true;
+            root->right_son_ptr->need_push_down_to_right = true;
+            root->need_push_down_to_right = false;
+        }
+        else
+        {
+            pthread_mutex_lock(&working_flag_mutex);
+            root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted;
+            root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted;
+            root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted;
+            root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted;
+            if (root->tree_downsample_deleted)
+                root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize;
+            if (root->tree_deleted)
+                root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize;
+            else
+                root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num;
+            root->right_son_ptr->need_push_down_to_left = true;
+            root->right_son_ptr->need_push_down_to_right = true;
+            if (rebuild_flag)
+            {
+                pthread_mutex_lock(&rebuild_logger_mutex_lock);
+                Rebuild_Logger.push(operation);
+                pthread_mutex_unlock(&rebuild_logger_mutex_lock);
+            }
+            root->need_push_down_to_right = false;
+            pthread_mutex_unlock(&working_flag_mutex);
+        }
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::Update(KD_TREE_NODE *root)
+{
+    KD_TREE_NODE *left_son_ptr = root->left_son_ptr;
+    KD_TREE_NODE *right_son_ptr = root->right_son_ptr;
+    float tmp_range_x[2] = {INFINITY, -INFINITY};
+    float tmp_range_y[2] = {INFINITY, -INFINITY};
+    float tmp_range_z[2] = {INFINITY, -INFINITY};
+    // Update Tree Size
+    if (left_son_ptr != nullptr && right_son_ptr != nullptr)
+    {
+        root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1;
+        root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0);
+        root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0);
+        root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;
+        root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted;
+        if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted))
+        {
+            tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0], right_son_ptr->node_range_x[0]), root->point.x);
+            tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1], right_son_ptr->node_range_x[1]), root->point.x);
+            tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0], right_son_ptr->node_range_y[0]), root->point.y);
+            tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1], right_son_ptr->node_range_y[1]), root->point.y);
+            tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0], right_son_ptr->node_range_z[0]), root->point.z);
+            tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1], right_son_ptr->node_range_z[1]), root->point.z);
+        }
+        else
+        {
+            if (!left_son_ptr->tree_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]);
+                tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]);
+                tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]);
+                tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]);
+                tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]);
+                tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]);
+            }
+            if (!right_son_ptr->tree_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]);
+                tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]);
+                tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]);
+                tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]);
+                tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]);
+                tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]);
+            }
+            if (!root->point_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], root->point.x);
+                tmp_range_x[1] = max(tmp_range_x[1], root->point.x);
+                tmp_range_y[0] = min(tmp_range_y[0], root->point.y);
+                tmp_range_y[1] = max(tmp_range_y[1], root->point.y);
+                tmp_range_z[0] = min(tmp_range_z[0], root->point.z);
+                tmp_range_z[1] = max(tmp_range_z[1], root->point.z);
+            }
+        }
+    }
+    else if (left_son_ptr != nullptr)
+    {
+        root->TreeSize = left_son_ptr->TreeSize + 1;
+        root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0);
+        root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0);
+        root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;
+        root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted;
+        if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted))
+        {
+            tmp_range_x[0] = min(left_son_ptr->node_range_x[0], root->point.x);
+            tmp_range_x[1] = max(left_son_ptr->node_range_x[1], root->point.x);
+            tmp_range_y[0] = min(left_son_ptr->node_range_y[0], root->point.y);
+            tmp_range_y[1] = max(left_son_ptr->node_range_y[1], root->point.y);
+            tmp_range_z[0] = min(left_son_ptr->node_range_z[0], root->point.z);
+            tmp_range_z[1] = max(left_son_ptr->node_range_z[1], root->point.z);
+        }
+        else
+        {
+            if (!left_son_ptr->tree_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]);
+                tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]);
+                tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]);
+                tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]);
+                tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]);
+                tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]);
+            }
+            if (!root->point_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], root->point.x);
+                tmp_range_x[1] = max(tmp_range_x[1], root->point.x);
+                tmp_range_y[0] = min(tmp_range_y[0], root->point.y);
+                tmp_range_y[1] = max(tmp_range_y[1], root->point.y);
+                tmp_range_z[0] = min(tmp_range_z[0], root->point.z);
+                tmp_range_z[1] = max(tmp_range_z[1], root->point.z);
+            }
+        }
+    }
+    else if (right_son_ptr != nullptr)
+    {
+        root->TreeSize = right_son_ptr->TreeSize + 1;
+        root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0);
+        root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0);
+        root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted;
+        root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted;
+        if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted))
+        {
+            tmp_range_x[0] = min(right_son_ptr->node_range_x[0], root->point.x);
+            tmp_range_x[1] = max(right_son_ptr->node_range_x[1], root->point.x);
+            tmp_range_y[0] = min(right_son_ptr->node_range_y[0], root->point.y);
+            tmp_range_y[1] = max(right_son_ptr->node_range_y[1], root->point.y);
+            tmp_range_z[0] = min(right_son_ptr->node_range_z[0], root->point.z);
+            tmp_range_z[1] = max(right_son_ptr->node_range_z[1], root->point.z);
+        }
+        else
+        {
+            if (!right_son_ptr->tree_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]);
+                tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]);
+                tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]);
+                tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]);
+                tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]);
+                tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]);
+            }
+            if (!root->point_deleted)
+            {
+                tmp_range_x[0] = min(tmp_range_x[0], root->point.x);
+                tmp_range_x[1] = max(tmp_range_x[1], root->point.x);
+                tmp_range_y[0] = min(tmp_range_y[0], root->point.y);
+                tmp_range_y[1] = max(tmp_range_y[1], root->point.y);
+                tmp_range_z[0] = min(tmp_range_z[0], root->point.z);
+                tmp_range_z[1] = max(tmp_range_z[1], root->point.z);
+            }
+        }
+    }
+    else
+    {
+        root->TreeSize = 1;
+        root->invalid_point_num = (root->point_deleted ? 1 : 0);
+        root->down_del_num = (root->point_downsample_deleted ? 1 : 0);
+        root->tree_downsample_deleted = root->point_downsample_deleted;
+        root->tree_deleted = root->point_deleted;
+        tmp_range_x[0] = root->point.x;
+        tmp_range_x[1] = root->point.x;
+        tmp_range_y[0] = root->point.y;
+        tmp_range_y[1] = root->point.y;
+        tmp_range_z[0] = root->point.z;
+        tmp_range_z[1] = root->point.z;
+    }
+    memcpy(root->node_range_x, tmp_range_x, sizeof(tmp_range_x));
+    memcpy(root->node_range_y, tmp_range_y, sizeof(tmp_range_y));
+    memcpy(root->node_range_z, tmp_range_z, sizeof(tmp_range_z));
+    float x_L = (root->node_range_x[1] - root->node_range_x[0]) * 0.5;
+    float y_L = (root->node_range_y[1] - root->node_range_y[0]) * 0.5;
+    float z_L = (root->node_range_z[1] - root->node_range_z[0]) * 0.5;
+    root->radius_sq = x_L*x_L + y_L * y_L + z_L * z_L;
+    if (left_son_ptr != nullptr)
+        left_son_ptr->father_ptr = root;
+    if (right_son_ptr != nullptr)
+        right_son_ptr->father_ptr = root;
+    if (root == Root_Node && root->TreeSize > 3)
+    {
+        KD_TREE_NODE *son_ptr = root->left_son_ptr;
+        if (son_ptr == nullptr)
+            son_ptr = root->right_son_ptr;
+        float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize - 1);
+        root->alpha_del = float(root->invalid_point_num) / root->TreeSize;
+        root->alpha_bal = (tmp_bal >= 0.5 - EPSS) ? tmp_bal : 1 - tmp_bal;
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type)
+{
+    if (root == nullptr)
+        return;
+    Push_Down(root);
+    if (!root->point_deleted)
+    {
+        Storage.push_back(root->point);
+    }
+    flatten(root->left_son_ptr, Storage, storage_type);
+    flatten(root->right_son_ptr, Storage, storage_type);
+    switch (storage_type)
+    {
+    case NOT_RECORD:
+        break;
+    case DELETE_POINTS_REC:
+        if (root->point_deleted && !root->point_downsample_deleted)
+        {
+            Points_deleted.push_back(root->point);
+        }
+        break;
+    case MULTI_THREAD_REC:
+        if (root->point_deleted && !root->point_downsample_deleted)
+        {
+            Multithread_Points_deleted.push_back(root->point);
+        }
+        break;
+    default:
+        break;
+    }
+    return;
+}
+
+template <typename PointType>
+void KD_TREE<PointType>::delete_tree_nodes(KD_TREE_NODE **root)
+{
+    if (*root == nullptr)
+        return;
+    Push_Down(*root);
+    delete_tree_nodes(&(*root)->left_son_ptr);
+    delete_tree_nodes(&(*root)->right_son_ptr);
+
+    pthread_mutex_destroy(&(*root)->push_down_mutex_lock);
+    delete *root;
+    *root = nullptr;
+
+    return;
+}
+
+template <typename PointType>
+bool KD_TREE<PointType>::same_point(PointType a, PointType b)
+{
+    return (fabs(a.x - b.x) < EPSS && fabs(a.y - b.y) < EPSS && fabs(a.z - b.z) < EPSS);
+}
+
+template <typename PointType>
+float KD_TREE<PointType>::calc_dist(PointType a, PointType b)
+{
+    float dist = 0.0f;
+    dist = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z);
+    return dist;
+}
+
+template <typename PointType>
+float KD_TREE<PointType>::calc_box_dist(KD_TREE_NODE *node, PointType point)
+{
+    if (node == nullptr)
+        return INFINITY;
+    float min_dist = 0.0;
+    if (point.x < node->node_range_x[0])
+        min_dist += (point.x - node->node_range_x[0]) * (point.x - node->node_range_x[0]);
+    if (point.x > node->node_range_x[1])
+        min_dist += (point.x - node->node_range_x[1]) * (point.x - node->node_range_x[1]);
+    if (point.y < node->node_range_y[0])
+        min_dist += (point.y - node->node_range_y[0]) * (point.y - node->node_range_y[0]);
+    if (point.y > node->node_range_y[1])
+        min_dist += (point.y - node->node_range_y[1]) * (point.y - node->node_range_y[1]);
+    if (point.z < node->node_range_z[0])
+        min_dist += (point.z - node->node_range_z[0]) * (point.z - node->node_range_z[0]);
+    if (point.z > node->node_range_z[1])
+        min_dist += (point.z - node->node_range_z[1]) * (point.z - node->node_range_z[1]);
+    return min_dist;
+}
+template <typename PointType>
+bool KD_TREE<PointType>::point_cmp_x(PointType a, PointType b) { return a.x < b.x; }
+template <typename PointType>
+bool KD_TREE<PointType>::point_cmp_y(PointType a, PointType b) { return a.y < b.y; }
+template <typename PointType>
+bool KD_TREE<PointType>::point_cmp_z(PointType a, PointType b) { return a.z < b.z; }
+
+// Manual heap
+
+
+
+// manual queue
+
+
+// Manual Instatiations
+template class KD_TREE<pcl::PointXYZ>;
+template class KD_TREE<pcl::PointXYZI>;
+template class KD_TREE<pcl::PointXYZINormal>;
+
diff --git a/point_lio_ros2/include/ikd-Tree/ikd_Tree.h b/point_lio_ros2/include/ikd-Tree/ikd_Tree.h
new file mode 100644
index 0000000..d4b302e
--- /dev/null
+++ b/point_lio_ros2/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);
diff --git a/point_lio_ros2/include/so3_math.h b/point_lio_ros2/include/so3_math.h
new file mode 100644
index 0000000..d289aed
--- /dev/null
+++ b/point_lio_ros2/include/so3_math.h
@@ -0,0 +1,113 @@
+#ifndef SO3_MATH_H
+#define SO3_MATH_H
+
+#include <math.h>
+#include <Eigen/Core>
+
+// #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> 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
diff --git a/point_lio_ros2/launch/correct_odom_unilidar_l1.launch.py b/point_lio_ros2/launch/correct_odom_unilidar_l1.launch.py
new file mode 100644
index 0000000..934d187
--- /dev/null
+++ b/point_lio_ros2/launch/correct_odom_unilidar_l1.launch.py
@@ -0,0 +1,48 @@
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'config', 'unilidar_l1.yaml'
+        ]),
+        {
+            'use_imu_as_input': False,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 1,  # Options: 1, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.1,  # Options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.1,  # Options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 1000.0,  # Option: 1000
+            'runtime_pos_log_enable': False,  # Option: True
+            'odom_only': True, # Option: False
+            'odom_header_frame_id': "odom",     # Default: "camera_init"
+            'odom_child_frame_id': "base_link", # Default: "aft_mapped"
+        }
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        # prefix='gdb -ex run --args'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        laser_mapping_node,
+    ])
+
+    return ld
diff --git a/point_lio_ros2/launch/correct_odom_unilidar_l2.launch.py b/point_lio_ros2/launch/correct_odom_unilidar_l2.launch.py
new file mode 100644
index 0000000..0d6501e
--- /dev/null
+++ b/point_lio_ros2/launch/correct_odom_unilidar_l2.launch.py
@@ -0,0 +1,48 @@
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'config', 'unilidar_l2.yaml'
+        ]),
+        {
+            'use_imu_as_input': False,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 1,  # Options: 1, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.1,  # Options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.1,  # Options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 1000.0,  # Option: 1000
+            'runtime_pos_log_enable': False,  # Option: True
+            'odom_only': True, # Option: False
+            'odom_header_frame_id': "odom",     # Default: "camera_init"
+            'odom_child_frame_id': "base_link", # Default: "aft_mapped"
+        }
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        # prefix='gdb -ex run --args'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        laser_mapping_node,
+    ])
+
+    return ld
diff --git a/point_lio_ros2/launch/gdb_debug_example.launch.py b/point_lio_ros2/launch/gdb_debug_example.launch.py
new file mode 100644
index 0000000..a452b76
--- /dev/null
+++ b/point_lio_ros2/launch/gdb_debug_example.launch.py
@@ -0,0 +1,68 @@
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare the RViz argument
+    rviz_arg = DeclareLaunchArgument(
+        'rviz', default_value='true',
+        description='Flag to launch RViz.')
+
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        {
+            'use_imu_as_input': False,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 1,  # Options: 1, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.3,  # Options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.2,  # Options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 1000.0,  # Option: 1000, 2000
+            'runtime_pos_log_enable': False  # Option: True
+        },
+        # PathJoinSubstitution([
+        #     FindPackageShare('point_lio'),
+        #     'config', 'horizon.yaml'
+        # ])
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        prefix='gdb -ex run --args'
+    )
+
+    # Conditional RViz node launch
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz',
+        arguments=['-d', PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'rviz_cfg', 'loam_livox.rviz'
+        ])],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+        prefix='nice'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        rviz_arg,
+        laser_mapping_node,
+        GroupAction(
+            actions=[rviz_node],
+            condition=IfCondition(LaunchConfiguration('rviz'))
+        ),
+    ])
+
+    return ld
diff --git a/point_lio_ros2/launch/mapping_avia.launch.py b/point_lio_ros2/launch/mapping_avia.launch.py
new file mode 100644
index 0000000..dc067f7
--- /dev/null
+++ b/point_lio_ros2/launch/mapping_avia.launch.py
@@ -0,0 +1,68 @@
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare the RViz argument
+    rviz_arg = DeclareLaunchArgument(
+        'rviz', default_value='true',
+        description='Flag to launch RViz.')
+
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'config', 'avia.yaml'
+        ]),
+        {
+            'use_imu_as_input': False,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 1,  # options: 4, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.3,  # options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.2,  # options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 2000.0,  # option: 1000
+            'runtime_pos_log_enable': False,  # option: True
+        },
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        # prefix='gdb -ex run --args'
+    )
+
+    # Conditional RViz node launch
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz',
+        arguments=['-d', PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'rviz_cfg', 'loam_livox.rviz'
+        ])],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+        prefix='nice'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        rviz_arg,
+        laser_mapping_node,
+        GroupAction(
+            actions=[rviz_node],
+            condition=IfCondition(LaunchConfiguration('rviz'))
+        ),
+    ])
+
+    return ld
diff --git a/point_lio_ros2/launch/mapping_horizon.launch.py b/point_lio_ros2/launch/mapping_horizon.launch.py
new file mode 100644
index 0000000..8b3374b
--- /dev/null
+++ b/point_lio_ros2/launch/mapping_horizon.launch.py
@@ -0,0 +1,68 @@
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare the RViz argument
+    rviz_arg = DeclareLaunchArgument(
+        'rviz', default_value='true',
+        description='Flag to launch RViz.')
+
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'config', 'horizon.yaml'
+        ]),
+        {
+            'use_imu_as_input': False,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 3,  # Options: 1, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.5,  # Options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.5,  # Options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 1000.0,  # Option: 1000
+            'runtime_pos_log_enable': False,  # Option: True
+        }
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        # prefix='gdb -ex run --args'
+    )
+
+    # Conditional RViz node launch
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz',
+        arguments=['-d', PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'rviz_cfg', 'loam_livox.rviz'
+        ])],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+        prefix='nice'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        rviz_arg,
+        laser_mapping_node,
+        GroupAction(
+            actions=[rviz_node],
+            condition=IfCondition(LaunchConfiguration('rviz'))
+        ),
+    ])
+
+    return ld
diff --git a/point_lio_ros2/launch/mapping_mid360.launch.py b/point_lio_ros2/launch/mapping_mid360.launch.py
new file mode 100644
index 0000000..dbf6ee4
--- /dev/null
+++ b/point_lio_ros2/launch/mapping_mid360.launch.py
@@ -0,0 +1,68 @@
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare the RViz argument
+    rviz_arg = DeclareLaunchArgument(
+        'rviz', default_value='true',
+        description='Flag to launch RViz.')
+
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'config', 'mid360.yaml'
+        ]),
+        {
+            'use_imu_as_input': False,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 3,  # Options: 1, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.5,  # Options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.5,  # Options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 1000.0,  # Option: 1000
+            'runtime_pos_log_enable': False,  # Option: True
+        }
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        # prefix='gdb -ex run --args'
+    )
+
+    # Conditional RViz node launch
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz',
+        arguments=['-d', PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'rviz_cfg', 'loam_livox.rviz'
+        ])],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+        prefix='nice'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        rviz_arg,
+        laser_mapping_node,
+        GroupAction(
+            actions=[rviz_node],
+            condition=IfCondition(LaunchConfiguration('rviz'))
+        ),
+    ])
+
+    return ld
diff --git a/point_lio_ros2/launch/mapping_ouster64.launch.py b/point_lio_ros2/launch/mapping_ouster64.launch.py
new file mode 100644
index 0000000..5577693
--- /dev/null
+++ b/point_lio_ros2/launch/mapping_ouster64.launch.py
@@ -0,0 +1,68 @@
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare the RViz argument
+    rviz_arg = DeclareLaunchArgument(
+        'rviz', default_value='true',
+        description='Flag to launch RViz.')
+
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'config', 'ouster64.yaml'
+        ]),
+        {
+            'use_imu_as_input': False,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 4,  # Options: 4, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.5,  # Options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.5,  # Options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 1000.0,  # Option: 1000 (changed from 2000)
+            'runtime_pos_log_enable': False,  # Option: True
+        }
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        # prefix='gdb -ex run --args'
+    )
+
+    # Conditional RViz node launch
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz',
+        arguments=['-d', PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'rviz_cfg', 'loam_livox.rviz'
+        ])],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+        prefix='nice'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        rviz_arg,
+        laser_mapping_node,
+        GroupAction(
+            actions=[rviz_node],
+            condition=IfCondition(LaunchConfiguration('rviz'))
+        ),
+    ])
+
+    return ld
diff --git a/point_lio_ros2/launch/mapping_unilidar_l1.launch.py b/point_lio_ros2/launch/mapping_unilidar_l1.launch.py
new file mode 100644
index 0000000..14aa43f
--- /dev/null
+++ b/point_lio_ros2/launch/mapping_unilidar_l1.launch.py
@@ -0,0 +1,68 @@
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare the RViz argument
+    rviz_arg = DeclareLaunchArgument(
+        'rviz', default_value='true',
+        description='Flag to launch RViz.')
+
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'config', 'unilidar_l1.yaml'
+        ]),
+        {
+            'use_imu_as_input': False,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 1,  # Options: 1, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.1,  # Options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.1,  # Options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 1000.0,  # Option: 1000
+            'runtime_pos_log_enable': False,  # Option: True
+        }
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        # prefix='gdb -ex run --args'
+    )
+
+    # Conditional RViz node launch
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz',
+        arguments=['-d', PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'rviz_cfg', 'loam_livox.rviz'
+        ])],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+        prefix='nice'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        rviz_arg,
+        laser_mapping_node,
+        GroupAction(
+            actions=[rviz_node],
+            condition=IfCondition(LaunchConfiguration('rviz'))
+        ),
+    ])
+
+    return ld
diff --git a/point_lio_ros2/launch/mapping_unilidar_l2.launch.py b/point_lio_ros2/launch/mapping_unilidar_l2.launch.py
new file mode 100644
index 0000000..a2db249
--- /dev/null
+++ b/point_lio_ros2/launch/mapping_unilidar_l2.launch.py
@@ -0,0 +1,70 @@
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare the RViz argument
+    rviz_arg = DeclareLaunchArgument(
+        'rviz', default_value='true',
+        description='Flag to launch RViz.')
+
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'config', 'unilidar_l2.yaml'
+        ]),
+        {
+            'use_imu_as_input': True,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 3,  # Options: 1, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.1,  # Options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.1,  # Options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 500.0,  # Option: 1000
+            'runtime_pos_log_enable': False,  # Option: True
+        }
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        # prefix='gdb -ex run --args'
+    )
+
+    # Conditional RViz node launch
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz',
+        arguments=['-d', PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'rviz_cfg', 'loam_livox.rviz'
+        ])],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+        prefix='nice'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        rviz_arg,
+        laser_mapping_node,
+        GroupAction(
+            actions=[rviz_node],
+            condition=IfCondition(LaunchConfiguration('rviz'))
+        ),
+    ])
+
+    return ld
+
+
diff --git a/point_lio_ros2/launch/mapping_velody16.launch.py b/point_lio_ros2/launch/mapping_velody16.launch.py
new file mode 100644
index 0000000..21cd90b
--- /dev/null
+++ b/point_lio_ros2/launch/mapping_velody16.launch.py
@@ -0,0 +1,68 @@
+from launch import LaunchDescription
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare the RViz argument
+    rviz_arg = DeclareLaunchArgument(
+        'rviz', default_value='true',
+        description='Flag to launch RViz.')
+
+    # Node parameters, including those from the YAML configuration file
+    laser_mapping_params = [
+        PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'config', 'velody16.yaml'
+        ]),
+        {
+            'use_imu_as_input': False,  # Change to True to use IMU as input of Point-LIO
+            'prop_at_freq_of_imu': True,
+            'check_satu': True,
+            'init_map_size': 10,
+            'point_filter_num': 4,  # Options: 4, 3
+            'space_down_sample': True,
+            'filter_size_surf': 0.5,  # Options: 0.5, 0.3, 0.2, 0.15, 0.1
+            'filter_size_map': 0.5,  # Options: 0.5, 0.3, 0.15, 0.1
+            'cube_side_length': 1000.0,  # Option: 1000 (changed from 2000)
+            'runtime_pos_log_enable': False,  # Option: True
+        }
+    ]
+
+    # Node definition for laserMapping with Point-LIO
+    laser_mapping_node = Node(
+        package='point_lio',
+        executable='pointlio_mapping',
+        name='laserMapping',
+        output='screen',
+        parameters=laser_mapping_params,
+        # prefix='gdb -ex run --args'
+    )
+
+    # Conditional RViz node launch
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz',
+        arguments=['-d', PathJoinSubstitution([
+            FindPackageShare('point_lio'),
+            'rviz_cfg', 'loam_livox.rviz'
+        ])],
+        condition=IfCondition(LaunchConfiguration('rviz')),
+        prefix='nice'
+    )
+
+    # Assemble the launch description
+    ld = LaunchDescription([
+        rviz_arg,
+        laser_mapping_node,
+        GroupAction(
+            actions=[rviz_node],
+            condition=IfCondition(LaunchConfiguration('rviz'))
+        ),
+    ])
+
+    return ld
diff --git a/point_lio_ros2/package.xml b/point_lio_ros2/package.xml
new file mode 100644
index 0000000..01ba0ff
--- /dev/null
+++ b/point_lio_ros2/package.xml
@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>point_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="dflorea@uc.cl">Daniel Florea</maintainer>
+
+  <license>BSD</license>
+
+  <author email="dflorea@uc.cl">Daniel Florea</author>
+
+  <!-- ROS2 uses ament as build system -->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <!-- Dependencies are now also categorized as build, build_export, and exec_depend -->
+  <depend>rclcpp</depend> <!-- roscpp in ROS1 is replaced by rclcpp in ROS2 -->
+  <depend>rclpy</depend>  <!-- rospy is replaced by rclpy in ROS2 -->
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>tf2_ros</depend> <!-- tf in ROS1 is replaced by tf2 in ROS2 -->
+  <depend>pcl_ros</depend> <!-- For PCL support -->
+  <depend>pcl_conversions</depend> <!-- For PCL support -->
+  <depend>visualization_msgs</depend> <!-- For visualization_msgs support -->
+  <!-- <depend>livox_ros_driver2</depend> -->
+
+  <!-- test_depend remains the same, but make sure the testing tools you use are compatible with ROS2 -->
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/point_lio_ros2/rviz_cfg/loam_livox.rviz b/point_lio_ros2/rviz_cfg/loam_livox.rviz
new file mode 100644
index 0000000..b6f0986
--- /dev/null
+++ b/point_lio_ros2/rviz_cfg/loam_livox.rviz
@@ -0,0 +1,326 @@
+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
+        aft_mapped:
+          Value: true
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+        camera_init:
+          Value: true
+        camera_link:
+          Value: true
+        gyro_link:
+          Value: true
+        laser:
+          Value: true
+        left_front_link:
+          Value: true
+        left_wheel_link:
+          Value: true
+        map:
+          Value: true
+        odom_combined:
+          Value: true
+        right_front_link:
+          Value: true
+        right_wheel_link:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        camera_init:
+          aft_mapped:
+            {}
+      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: 1.8584054708480835
+        Min Value: -0.10289539396762848
+        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.009999999776482582
+      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.019999999552965164
+      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: 2.036320447921753
+        Min Value: -0.09378375858068466
+        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.019999999552965164
+      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: 28.403661727905273
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -2.2604103088378906
+        Y: -0.3224470913410187
+        Z: 0.9725424647331238
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7497965693473816
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 1.7503817081451416
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ad0000003efc0100000002fb0000000800540069006d00650100000000000005ad000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000451000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1453
+  X: 2404
+  Y: 218
diff --git a/point_lio_ros2/src/Estimator.cpp b/point_lio_ros2/src/Estimator.cpp
new file mode 100644
index 0000000..912e98b
--- /dev/null
+++ b/point_lio_ros2/src/Estimator.cpp
@@ -0,0 +1,435 @@
+// #include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp>
+#include "Estimator.h"
+
+PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1));
+std::vector<int> time_seq;
+PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI());
+PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI());
+std::vector<V3D> pbody_list;
+std::vector<PointVector> Nearest_Points;
+KD_TREE<PointType> ikdtree;
+std::vector<float> pointSearchSqDis(NUM_MATCH_POINTS);
+bool point_selected_surf[100000] = {0};
+std::vector<M3D> crossmat_list;
+int effct_feat_num = 0;
+int k;
+int idx;
+esekfom::esekf<state_input, 24, input_ikfom> kf_input;
+esekfom::esekf<state_output, 30, input_ikfom> kf_output;
+state_input state_in;
+state_output state_out;
+input_ikfom input_in;
+V3D angvel_avr, acc_avr;
+
+V3D Lidar_T_wrt_IMU(Zero3d);
+M3D Lidar_R_wrt_IMU(Eye3d);
+
+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;
+
+Eigen::Matrix<double, 24, 24> process_noise_cov_input()
+{
+	Eigen::Matrix<double, 24, 24> cov;
+	cov.setZero();
+	cov.block<3, 3>(3, 3).diagonal() << gyr_cov_input, gyr_cov_input, gyr_cov_input;
+	cov.block<3, 3>(12, 12).diagonal() << acc_cov_input, acc_cov_input, acc_cov_input;
+	cov.block<3, 3>(15, 15).diagonal() << b_gyr_cov, b_gyr_cov, b_gyr_cov;
+	cov.block<3, 3>(18, 18).diagonal() << b_acc_cov, b_acc_cov, b_acc_cov;
+	// MTK::get_cov<process_noise_input>::type cov = MTK::get_cov<process_noise_input>::type::Zero();
+	// MTK::setDiagonal<process_noise_input, vect3, 0>(cov, &process_noise_input::ng, gyr_cov_input);// 0.03
+	// MTK::setDiagonal<process_noise_input, vect3, 3>(cov, &process_noise_input::na, acc_cov_input); // *dt 0.01 0.01 * dt * dt 0.05
+	// MTK::setDiagonal<process_noise_input, vect3, 6>(cov, &process_noise_input::nbg, b_gyr_cov); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01
+	// MTK::setDiagonal<process_noise_input, vect3, 9>(cov, &process_noise_input::nba, b_acc_cov);   //0.001 0.05 0.0001/out 0.01
+	return cov;
+}
+
+Eigen::Matrix<double, 30, 30> process_noise_cov_output()
+{
+	Eigen::Matrix<double, 30, 30> cov;
+	cov.setZero();
+	cov.block<3, 3>(12, 12).diagonal() << vel_cov, vel_cov, vel_cov;
+	cov.block<3, 3>(15, 15).diagonal() << gyr_cov_output, gyr_cov_output, gyr_cov_output;
+	cov.block<3, 3>(18, 18).diagonal() << acc_cov_output, acc_cov_output, acc_cov_output;
+	cov.block<3, 3>(24, 24).diagonal() << b_gyr_cov, b_gyr_cov, b_gyr_cov;
+	cov.block<3, 3>(27, 27).diagonal() << b_acc_cov, b_acc_cov, b_acc_cov;
+	// MTK::get_cov<process_noise_output>::type cov = MTK::get_cov<process_noise_output>::type::Zero();
+	// MTK::setDiagonal<process_noise_output, vect3, 0>(cov, &process_noise_output::vel, vel_cov);// 0.03
+	// MTK::setDiagonal<process_noise_output, vect3, 3>(cov, &process_noise_output::ng, gyr_cov_output); // *dt 0.01 0.01 * dt * dt 0.05
+	// MTK::setDiagonal<process_noise_output, vect3, 6>(cov, &process_noise_output::na, acc_cov_output); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01
+	// MTK::setDiagonal<process_noise_output, vect3, 9>(cov, &process_noise_output::nbg, b_gyr_cov);   //0.001 0.05 0.0001/out 0.01
+	// MTK::setDiagonal<process_noise_output, vect3, 12>(cov, &process_noise_output::nba, b_acc_cov);   //0.001 0.05 0.0001/out 0.01
+	return cov;
+}
+
+Eigen::Matrix<double, 24, 1> get_f_input(state_input &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.normalized() * (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.gravity[i]; 
+	}
+	return res;
+}
+
+Eigen::Matrix<double, 30, 1> get_f_output(state_output &s, const input_ikfom &in)
+{
+	Eigen::Matrix<double, 30, 1> res = Eigen::Matrix<double, 30, 1>::Zero();
+	vect3 a_inertial = s.rot.normalized() * s.acc; 
+	for(int i = 0; i < 3; i++ ){
+		res(i) = s.vel[i];
+		res(i + 3) = s.omg[i]; 
+		res(i + 12) = a_inertial[i] + s.gravity[i]; 
+	}
+	return res;
+}
+
+Eigen::Matrix<double, 24, 24> df_dx_input(state_input &s, const input_ikfom &in)
+{
+	Eigen::Matrix<double, 24, 24> cov = Eigen::Matrix<double, 24, 24>::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.normalized().toRotationMatrix()*MTK::hat(acc_);
+	cov.template block<3, 3>(12, 18) = -s.rot.normalized().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, 3>(12, 21) = Eigen::Matrix3d::Identity(); // grav_matrix; 
+	cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); 
+	return cov;
+}
+
+// Eigen::Matrix<double, 24, 12> df_dw_input(state_input &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.normalized().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;
+// }
+
+Eigen::Matrix<double, 30, 30> df_dx_output(state_output &s, const input_ikfom &in)
+{
+	Eigen::Matrix<double, 30, 30> cov = Eigen::Matrix<double, 30, 30>::Zero();
+	cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
+	cov.template block<3, 3>(12, 3) = -s.rot.normalized().toRotationMatrix()*MTK::hat(s.acc);
+	cov.template block<3, 3>(12, 18) = s.rot.normalized().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, 3>(12, 21) = Eigen::Matrix3d::Identity(); // grav_matrix; 
+	cov.template block<3, 3>(3, 15) = Eigen::Matrix3d::Identity(); 
+	return cov;
+}
+
+// Eigen::Matrix<double, 30, 15> df_dw_output(state_output &s)
+// {
+// 	Eigen::Matrix<double, 30, 15> cov = Eigen::Matrix<double, 30, 15>::Zero();
+// 	cov.template block<3, 3>(12, 0) = Eigen::Matrix3d::Identity();
+// 	cov.template block<3, 3>(15, 3) = Eigen::Matrix3d::Identity();
+// 	cov.template block<3, 3>(18, 6) = Eigen::Matrix3d::Identity();
+// 	cov.template block<3, 3>(24, 9) = Eigen::Matrix3d::Identity();
+// 	cov.template block<3, 3>(27, 12) = 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);
+	return euler_ang;
+}
+
+void h_model_input(state_input &s, esekfom::dyn_share_modified<double> &ekfom_data)
+{
+	bool match_in_map = false;
+	VF(4) pabcd;
+	pabcd.setZero();
+	normvec->resize(time_seq[k]);
+	int effect_num_k = 0;
+	for (int j = 0; j < time_seq[k]; j++)
+	{
+		PointType &point_body_j  = feats_down_body->points[idx+j+1];
+		PointType &point_world_j = feats_down_world->points[idx+j+1];
+		pointBodyToWorld(&point_body_j, &point_world_j); 
+		V3D p_body = pbody_list[idx+j+1];
+		V3D p_world;
+		p_world << point_world_j.x, point_world_j.y, point_world_j.z;
+		
+		{
+			auto &points_near = Nearest_Points[idx+j+1];
+			
+			ikdtree.Nearest_Search(point_world_j, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 2.236); //1.0); //, 3.0); // 2.236;
+			
+			if ((points_near.size() < NUM_MATCH_POINTS) || pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5) // 5)
+			{
+				point_selected_surf[idx+j+1] = false;
+			}
+			else
+			{
+				point_selected_surf[idx+j+1] = false;
+				if (esti_plane(pabcd, points_near, plane_thr)) //(planeValid)
+				{
+					float pd2 = pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3);
+					
+					if (p_body.norm() > match_s * pd2 * pd2)
+					{
+						point_selected_surf[idx+j+1] = true;
+						normvec->points[j].x = pabcd(0);
+						normvec->points[j].y = pabcd(1);
+						normvec->points[j].z = pabcd(2);
+						normvec->points[j].intensity = pabcd(3);
+						effect_num_k ++;
+					}
+				}  
+			}
+		}
+	}
+	if (effect_num_k == 0) 
+	{
+		ekfom_data.valid = false;
+		return;
+	}
+	ekfom_data.M_Noise = laser_point_cov;
+	ekfom_data.h_x = Eigen::MatrixXd::Zero(effect_num_k, 12);
+	ekfom_data.z.resize(effect_num_k);
+	int m = 0;
+	for (int j = 0; j < time_seq[k]; j++)
+	{
+		if(point_selected_surf[idx+j+1])
+		{
+			V3D norm_vec(normvec->points[j].x, normvec->points[j].y, normvec->points[j].z);
+			
+			if (extrinsic_est_en)
+			{
+				V3D p_body = pbody_list[idx+j+1];
+				M3D p_crossmat, p_imu_crossmat;
+				p_crossmat << SKEW_SYM_MATRX(p_body);
+				V3D point_imu = s.offset_R_L_I.normalized() * p_body + s.offset_T_L_I;
+				p_imu_crossmat << SKEW_SYM_MATRX(point_imu);
+				V3D C(s.rot.conjugate().normalized() * norm_vec);
+				V3D A(p_imu_crossmat * C);
+				V3D B(p_crossmat * s.offset_R_L_I.conjugate().normalized() * C);
+				ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
+			}
+			else
+			{   
+				M3D point_crossmat = crossmat_list[idx+j+1];
+				V3D C(s.rot.conjugate().normalized() * norm_vec);
+				V3D A(point_crossmat * C);
+				ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+			}
+			ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity;
+			m++;
+		}
+	}
+	effct_feat_num += effect_num_k;
+}
+
+void h_model_output(state_output &s, esekfom::dyn_share_modified<double> &ekfom_data)
+{
+	bool match_in_map = false;
+	VF(4) pabcd;
+	pabcd.setZero();
+	
+	normvec->resize(time_seq[k]);
+	int effect_num_k = 0;
+	for (int j = 0; j < time_seq[k]; j++)
+	{
+		PointType &point_body_j  = feats_down_body->points[idx+j+1];
+		PointType &point_world_j = feats_down_world->points[idx+j+1];
+		pointBodyToWorld(&point_body_j, &point_world_j); 
+		V3D p_body = pbody_list[idx+j+1];
+		V3D p_world;
+		p_world << point_world_j.x, point_world_j.y, point_world_j.z;
+		{
+			auto &points_near = Nearest_Points[idx+j+1];
+			
+			ikdtree.Nearest_Search(point_world_j, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 2.236); 
+			
+			if ((points_near.size() < NUM_MATCH_POINTS) || pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5)
+			{
+				point_selected_surf[idx+j+1] = false;
+			}
+			else
+			{
+				point_selected_surf[idx+j+1] = false;
+				if (esti_plane(pabcd, points_near, plane_thr)) //(planeValid)
+				{
+					float pd2 = pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3);
+					
+					if (p_body.norm() > match_s * pd2 * pd2)
+					{
+						// point_selected_surf[i] = true;
+						point_selected_surf[idx+j+1] = true;
+						normvec->points[j].x = pabcd(0);
+						normvec->points[j].y = pabcd(1);
+						normvec->points[j].z = pabcd(2);
+						normvec->points[j].intensity = pabcd(3);
+						effect_num_k ++;
+					}
+				}  
+			}
+		}
+	}
+	if (effect_num_k == 0) 
+	{
+		ekfom_data.valid = false;
+		return;
+	}
+	ekfom_data.M_Noise = laser_point_cov;
+	ekfom_data.h_x = Eigen::MatrixXd::Zero(effect_num_k, 12);
+	ekfom_data.z.resize(effect_num_k);
+	int m = 0;
+	for (int j = 0; j < time_seq[k]; j++)
+	{
+		if(point_selected_surf[idx+j+1])
+		{
+			V3D norm_vec(normvec->points[j].x, normvec->points[j].y, normvec->points[j].z);
+			
+			if (extrinsic_est_en)
+			{
+				V3D p_body = pbody_list[idx+j+1];
+				M3D p_crossmat, p_imu_crossmat;
+				p_crossmat << SKEW_SYM_MATRX(p_body);
+				V3D point_imu = s.offset_R_L_I.normalized() * p_body + s.offset_T_L_I;
+				p_imu_crossmat << SKEW_SYM_MATRX(point_imu);
+				V3D C(s.rot.conjugate().normalized() * norm_vec);
+				V3D A(p_imu_crossmat * C);
+				V3D B(p_crossmat * s.offset_R_L_I.conjugate().normalized() * C);
+				ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
+			}
+			else
+			{   
+				M3D point_crossmat = crossmat_list[idx+j+1];
+				V3D C(s.rot.conjugate().normalized() * norm_vec);
+				V3D A(point_crossmat * C);
+				// V3D A(point_crossmat * state.rot_end.transpose() * norm_vec);
+				ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+			}
+			ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity;
+			m++;
+		}
+	}
+	effct_feat_num += effect_num_k;
+}
+
+void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified<double> &ekfom_data)
+{
+    std::memset(ekfom_data.satu_check, false, 6);
+	ekfom_data.z_IMU.block<3,1>(0, 0) = angvel_avr - s.omg - s.bg;
+	ekfom_data.z_IMU.block<3,1>(3, 0) = acc_avr * G_m_s2 / acc_norm - s.acc - s.ba;
+    ekfom_data.R_IMU << imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_acc_cov, imu_meas_acc_cov, imu_meas_acc_cov;
+	if(check_satu)
+	{
+		if(fabs(angvel_avr(0)) >= 0.99 * satu_gyro)
+		{
+			ekfom_data.satu_check[0] = true; 
+			ekfom_data.z_IMU(0) = 0.0;
+		}
+		
+		if(fabs(angvel_avr(1)) >= 0.99 * satu_gyro) 
+		{
+			ekfom_data.satu_check[1] = true;
+			ekfom_data.z_IMU(1) = 0.0;
+		}
+		
+		if(fabs(angvel_avr(2)) >= 0.99 * satu_gyro)
+		{
+			ekfom_data.satu_check[2] = true;
+			ekfom_data.z_IMU(2) = 0.0;
+		}
+		
+		if(fabs(acc_avr(0)) >= 0.99 * satu_acc)
+		{
+			ekfom_data.satu_check[3] = true;
+			ekfom_data.z_IMU(3) = 0.0;
+		}
+
+		if(fabs(acc_avr(1)) >= 0.99 * satu_acc) 
+		{
+			ekfom_data.satu_check[4] = true;
+			ekfom_data.z_IMU(4) = 0.0;
+		}
+
+		if(fabs(acc_avr(2)) >= 0.99 * satu_acc) 
+		{
+			ekfom_data.satu_check[5] = true;
+			ekfom_data.z_IMU(5) = 0.0;
+		}
+	}
+}
+
+void pointBodyToWorld(PointType const * const pi, PointType * const po)
+{    
+    V3D p_body(pi->x, pi->y, pi->z);
+    
+    V3D p_global;
+	if (extrinsic_est_en)
+	{	
+		if (!use_imu_as_input)
+		{
+			p_global = kf_output.x_.rot.normalized() * (kf_output.x_.offset_R_L_I.normalized() * p_body + kf_output.x_.offset_T_L_I) + kf_output.x_.pos;
+		}
+		else
+		{
+			p_global = kf_input.x_.rot.normalized() * (kf_input.x_.offset_R_L_I.normalized() * p_body + kf_input.x_.offset_T_L_I) + kf_input.x_.pos;
+		}
+	}
+	else
+	{
+		if (!use_imu_as_input)
+		{
+			p_global = kf_output.x_.rot.normalized() * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_output.x_.pos;
+		}
+		else
+		{
+			p_global = kf_input.x_.rot.normalized() * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_input.x_.pos;
+		}
+	}
+
+    po->x = p_global(0);
+    po->y = p_global(1);
+    po->z = p_global(2);
+    po->intensity = pi->intensity;
+}
+
+const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
\ No newline at end of file
diff --git a/point_lio_ros2/src/Estimator.h b/point_lio_ros2/src/Estimator.h
new file mode 100644
index 0000000..6e0c552
--- /dev/null
+++ b/point_lio_ros2/src/Estimator.h
@@ -0,0 +1,118 @@
+#ifndef Estimator_H
+#define Estimator_H
+
+#include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp>
+#include "common_lib.h"
+#include "parameters.h"
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include <ikd-Tree/ikd_Tree.h>
+#include <pcl/io/pcd_io.h>
+
+extern PointCloudXYZI::Ptr normvec; //(new PointCloudXYZI(100000, 1));
+extern std::vector<int> time_seq;
+extern PointCloudXYZI::Ptr feats_down_body; //(new PointCloudXYZI());
+extern PointCloudXYZI::Ptr feats_down_world; //(new PointCloudXYZI());
+extern std::vector<V3D> pbody_list;
+extern std::vector<PointVector> Nearest_Points;
+extern KD_TREE<PointType> ikdtree;
+extern std::vector<float> pointSearchSqDis;
+extern bool point_selected_surf[100000]; // = {0};
+extern std::vector<M3D> crossmat_list;
+extern int effct_feat_num;
+extern int k;
+extern int idx;
+extern V3D angvel_avr, acc_avr;
+
+extern V3D Lidar_T_wrt_IMU; //(Zero3d);
+extern M3D Lidar_R_wrt_IMU; //(Eye3d);
+
+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_input,
+((vect3, pos))
+((SO3, rot))
+((SO3, offset_R_L_I))
+((vect3, offset_T_L_I))
+((vect3, vel))
+((vect3, bg))
+((vect3, ba))
+((vect3, gravity))
+);
+
+MTK_BUILD_MANIFOLD(state_output,
+((vect3, pos))
+((SO3, rot))
+((SO3, offset_R_L_I))
+((vect3, offset_T_L_I))
+((vect3, vel))
+((vect3, omg))
+((vect3, acc))
+((vect3, gravity))
+((vect3, bg))
+((vect3, ba))
+);
+
+MTK_BUILD_MANIFOLD(input_ikfom,
+((vect3, acc))
+((vect3, gyro))
+);
+
+MTK_BUILD_MANIFOLD(process_noise_input,
+((vect3, ng))
+((vect3, na))
+((vect3, nbg))
+((vect3, nba))
+);
+
+MTK_BUILD_MANIFOLD(process_noise_output,
+((vect3, vel))
+((vect3, ng))
+((vect3, na))
+((vect3, nbg))
+((vect3, nba))
+);
+
+extern esekfom::esekf<state_input, 24, input_ikfom> kf_input;
+extern esekfom::esekf<state_output, 30, input_ikfom> kf_output;
+extern state_input state_in;
+extern state_output state_out;
+extern input_ikfom input_in;
+
+Eigen::Matrix<double, 24, 24> process_noise_cov_input();
+
+Eigen::Matrix<double, 30, 30> process_noise_cov_output();
+
+//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_input(state_input &s, const input_ikfom &in);
+
+Eigen::Matrix<double, 30, 1> get_f_output(state_output &s, const input_ikfom &in);
+
+Eigen::Matrix<double, 24, 24> df_dx_input(state_input &s, const input_ikfom &in);
+
+// Eigen::Matrix<double, 24, 12> df_dw_input(state_input &s, const input_ikfom &in);
+
+Eigen::Matrix<double, 30, 30> df_dx_output(state_output &s, const input_ikfom &in);
+
+// Eigen::Matrix<double, 30, 15> df_dw_output(state_output &s);
+
+vect3 SO3ToEuler(const SO3 &orient);
+
+void h_model_input(state_input &s, esekfom::dyn_share_modified<double> &ekfom_data);
+
+void h_model_output(state_output &s, esekfom::dyn_share_modified<double> &ekfom_data);
+
+void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified<double> &ekfom_data);
+
+void pointBodyToWorld(PointType const *const pi, PointType *const po);
+
+const bool time_list(PointType &x, PointType &y); // {return (x.curvature < y.curvature);};
+
+#endif
\ No newline at end of file
diff --git a/point_lio_ros2/src/IMU_Processing.hpp b/point_lio_ros2/src/IMU_Processing.hpp
new file mode 100644
index 0000000..898a451
--- /dev/null
+++ b/point_lio_ros2/src/IMU_Processing.hpp
@@ -0,0 +1,164 @@
+#include <cmath>
+#include <math.h>
+#include <deque>
+#include <mutex>
+#include <thread>
+#include <fstream>
+#include <csignal>
+#include <rclcpp/rclcpp.hpp>
+#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 <tf2_ros/transform_broadcaster.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>
+
+/// *************Preconfiguration
+
+#define MAX_INI_COUNT (100)
+
+/// *************IMU Process and undistortion
+class ImuProcess {
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    ImuProcess();
+
+    ~ImuProcess();
+
+    void Reset();
+
+    //void Reset(double start_timestamp, const sensor_msgs::msg::Imu::ConstSharedPtr &lastimu);
+
+    void Process(const MeasureGroup &meas, const PointCloudXYZI::Ptr &pcl_un_);
+
+    void Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot);
+
+    ofstream fout_imu;
+    // double first_lidar_time;
+    int lidar_type;
+    bool imu_en;
+    V3D mean_acc, gravity_;
+    bool imu_need_init_ = true;
+    bool b_first_frame_ = true;
+    bool gravity_align_ = false;
+
+private:
+    void IMU_init(const MeasureGroup &meas, int &N);
+
+    V3D mean_gyr;
+    int init_iter_num = 1;
+    rclcpp::Logger logger;
+};
+
+ImuProcess::ImuProcess()
+        : b_first_frame_(true), imu_need_init_(true), gravity_align_(false),
+          logger(rclcpp::get_logger("laserMapping")) {
+    imu_en = true;
+    init_iter_num = 1;
+    mean_acc = V3D(0, 0, -1.0);
+    mean_gyr = V3D(0, 0, 0);
+}
+
+ImuProcess::~ImuProcess() {}
+
+void ImuProcess::Reset() {
+    RCLCPP_WARN(logger, "Reset ImuProcess");
+    mean_acc = V3D(0, 0, -1.0);
+    mean_gyr = V3D(0, 0, 0);
+    imu_need_init_ = true;
+    init_iter_num = 1;
+}
+
+void ImuProcess::IMU_init(const MeasureGroup &meas, int &N) {
+    /** 1. initializing the gravity, gyro bias, acc and gyro covariance
+     ** 2. normalize the acceleration measurenments to unit gravity **/
+    RCLCPP_INFO(logger, "IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100);
+    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;
+    }
+
+    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;
+
+        N++;
+    }
+}
+
+void ImuProcess::Process(const MeasureGroup &meas, const PointCloudXYZI::Ptr &cur_pcl_un_) {
+    if (imu_en) {
+        if (meas.imu.empty()) return;
+        assert(meas.lidar != nullptr);
+
+        if (imu_need_init_) {
+            /// The very first lidar frame
+            IMU_init(meas, init_iter_num);
+
+            imu_need_init_ = true;
+
+            if (init_iter_num > MAX_INI_COUNT) {
+                RCLCPP_INFO(logger, "IMU Initializing: %.1f %%", 100.0);
+                imu_need_init_ = false;
+                *cur_pcl_un_ = *(meas.lidar);
+            }
+            return;
+        }
+        if (!gravity_align_) gravity_align_ = true;
+        *cur_pcl_un_ = *(meas.lidar);
+        return;
+    } else {
+        if (!b_first_frame_) { if (!gravity_align_) gravity_align_ = true; }
+        else {
+            b_first_frame_ = false;
+            return;
+        }
+        *cur_pcl_un_ = *(meas.lidar);
+        return;
+    }
+}
+
+void ImuProcess::Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot) {
+    /** 1. initializing the gravity, gyro bias, acc and gyro covariance
+     ** 2. normalize the acceleration measurenments to unit gravity **/
+    // V3D tmp_gravity = - mean_acc / mean_acc.norm() * G_m_s2; // state_gravity;
+    M3D hat_grav;
+    hat_grav << 0.0, gravity_(2), -gravity_(1),
+            -gravity_(2), 0.0, gravity_(0),
+            gravity_(1), -gravity_(0), 0.0;
+    double align_norm = (hat_grav * tmp_gravity).norm() / tmp_gravity.norm() / gravity_.norm();
+    double align_cos = gravity_.transpose() * tmp_gravity;
+    align_cos = align_cos / gravity_.norm() / tmp_gravity.norm();
+    if (align_norm < 1e-6) {
+        if (align_cos > 1e-6) {
+            rot = Eye3d;
+        } else {
+            rot = -Eye3d;
+        }
+    } else {
+        V3D align_angle = hat_grav * tmp_gravity / (hat_grav * tmp_gravity).norm() * acos(align_cos);
+        rot = Exp(align_angle(0), align_angle(1), align_angle(2));
+    }
+}
\ No newline at end of file
diff --git a/point_lio_ros2/src/laserMapping.cpp b/point_lio_ros2/src/laserMapping.cpp
new file mode 100644
index 0000000..b6d5f4a
--- /dev/null
+++ b/point_lio_ros2/src/laserMapping.cpp
@@ -0,0 +1,1322 @@
+#include <omp.h>
+#include <mutex>
+#include <cmath>
+#include <thread>
+#include <fstream>
+#include <csignal>
+#include <Python.h>
+#include <so3_math.h>
+#include <rclcpp/rclcpp.hpp>
+#include <Eigen/Core>
+#include "IMU_Processing.hpp"
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/msg/vector3.hpp>
+// #include <livox_ros_driver2/msg/custom_msg.hpp>
+
+#include "parameters.h"
+#include "Estimator.h"
+
+
+#define MAXN                (720000)
+#define PUBFRAME_PERIOD     (20)
+
+const float MOV_THRESHOLD = 1.5f;
+
+mutex mtx_buffer;
+condition_variable sig_buffer;
+
+string root_dir = ROOT_DIR;
+
+int feats_down_size = 0, time_log_counter = 0, scan_count = 0, publish_count = 0;
+
+int frame_ct = 0;
+double time_update_last = 0.0, time_current = 0.0, time_predict_last_const = 0.0, t_last = 0.0;
+
+shared_ptr<ImuProcess> p_imu(new ImuProcess());
+bool init_map = false, flg_first_scan = true;
+PointCloudXYZI::Ptr ptr_con(new PointCloudXYZI());
+
+// Time Log Variables
+double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot11[MAXN];
+double match_time = 0, solve_time = 0, propag_time = 0, update_time = 0;
+
+bool lidar_pushed = false, flg_reset = false, flg_exit = false;
+
+vector<BoxPointType> cub_needrm;
+
+deque<PointCloudXYZI::Ptr> lidar_buffer;
+deque<double> time_buffer;
+deque<sensor_msgs::msg::Imu::ConstSharedPtr> imu_deque;
+
+//surf feature in map
+PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI());
+PointCloudXYZI::Ptr feats_down_body_space(new PointCloudXYZI());
+PointCloudXYZI::Ptr init_feats_world(new PointCloudXYZI());
+
+pcl::VoxelGrid<PointType> downSizeFilterSurf;
+pcl::VoxelGrid<PointType> downSizeFilterMap;
+
+V3D euler_cur;
+
+MeasureGroup Measures;
+
+sensor_msgs::msg::Imu imu_last, imu_next;
+sensor_msgs::msg::Imu::ConstSharedPtr imu_last_ptr;
+nav_msgs::msg::Path path;
+nav_msgs::msg::Odometry odomAftMapped;
+geometry_msgs::msg::PoseStamped msg_body_pose;
+
+auto logger = rclcpp::get_logger("laserMapping");
+
+void SigHandle(int sig) {
+    flg_exit = true;
+    RCLCPP_WARN(logger, "catch sig %d", sig);
+    sig_buffer.notify_all();
+}
+
+inline void dump_lio_state_to_log(FILE *fp) {
+    V3D rot_ang;
+    if (!use_imu_as_input) {
+        rot_ang = SO3ToEuler(kf_output.x_.rot);
+    } else {
+        rot_ang = SO3ToEuler(kf_input.x_.rot);
+    }
+
+    fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time);
+    fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2));                   // Angle
+    if (use_imu_as_input) {
+        fprintf(fp, "%lf %lf %lf ", kf_input.x_.pos(0), kf_input.x_.pos(1), kf_input.x_.pos(2)); // Pos  
+        fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0);                                        // omega  
+        fprintf(fp, "%lf %lf %lf ", kf_input.x_.vel(0), kf_input.x_.vel(1), kf_input.x_.vel(2)); // Vel  
+        fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0);                                        // Acc  
+        fprintf(fp, "%lf %lf %lf ", kf_input.x_.bg(0), kf_input.x_.bg(1), kf_input.x_.bg(2));    // Bias_g  
+        fprintf(fp, "%lf %lf %lf ", kf_input.x_.ba(0), kf_input.x_.ba(1), kf_input.x_.ba(2));    // Bias_a  
+        fprintf(fp, "%lf %lf %lf ", kf_input.x_.gravity(0), kf_input.x_.gravity(1), kf_input.x_.gravity(2)); // Bias_a  
+    } else {
+        fprintf(fp, "%lf %lf %lf ", kf_output.x_.pos(0), kf_output.x_.pos(1), kf_output.x_.pos(2)); // Pos  
+        fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0);                                        // omega  
+        fprintf(fp, "%lf %lf %lf ", kf_output.x_.vel(0), kf_output.x_.vel(1), kf_output.x_.vel(2)); // Vel  
+        fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0);                                        // Acc  
+        fprintf(fp, "%lf %lf %lf ", kf_output.x_.bg(0), kf_output.x_.bg(1), kf_output.x_.bg(2));    // Bias_g  
+        fprintf(fp, "%lf %lf %lf ", kf_output.x_.ba(0), kf_output.x_.ba(1), kf_output.x_.ba(2));    // Bias_a  
+        fprintf(fp, "%lf %lf %lf ", kf_output.x_.gravity(0), kf_output.x_.gravity(1),
+                kf_output.x_.gravity(2)); // Bias_a
+    }
+    fprintf(fp, "\r\n");
+    fflush(fp);
+}
+
+void pointBodyLidarToIMU(PointType const *const pi, PointType *const po) {
+    V3D p_body_lidar(pi->x, pi->y, pi->z);
+    V3D p_body_imu;
+    if (extrinsic_est_en) {
+        if (!use_imu_as_input) {
+            p_body_imu = kf_output.x_.offset_R_L_I.normalized() * p_body_lidar + kf_output.x_.offset_T_L_I;
+        } else {
+            p_body_imu = kf_input.x_.offset_R_L_I.normalized() * p_body_lidar + kf_input.x_.offset_T_L_I;
+        }
+    } else {
+        p_body_imu = Lidar_R_wrt_IMU * p_body_lidar + Lidar_T_wrt_IMU;
+    }
+    po->x = p_body_imu(0);
+    po->y = p_body_imu(1);
+    po->z = p_body_imu(2);
+    po->intensity = pi->intensity;
+}
+
+int points_cache_size = 0;
+
+void points_cache_collect() // seems for debug
+{
+    PointVector points_history;
+    ikdtree.acquire_removed_points(points_history);
+    points_cache_size = points_history.size();
+}
+
+BoxPointType LocalMap_Points;
+bool Localmap_Initialized = false;
+
+void lasermap_fov_segment() {
+    cub_needrm.shrink_to_fit();
+
+    V3D pos_LiD;
+    if (use_imu_as_input) {
+        pos_LiD = kf_input.x_.pos + kf_input.x_.rot.normalized() * Lidar_T_wrt_IMU;
+    } else {
+        pos_LiD = kf_output.x_.pos + kf_output.x_.rot.normalized() * Lidar_T_wrt_IMU;
+    }
+    if (!Localmap_Initialized) {
+        for (int i = 0; i < 3; i++) {
+            LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0;
+            LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0;
+        }
+        Localmap_Initialized = true;
+        return;
+    }
+    float dist_to_map_edge[3][2];
+    bool need_move = false;
+    for (int i = 0; i < 3; i++) {
+        dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]);
+        dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]);
+        if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE ||
+            dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE)
+            need_move = true;
+    }
+    if (!need_move) return;
+    BoxPointType New_LocalMap_Points, tmp_boxpoints;
+    New_LocalMap_Points = LocalMap_Points;
+    float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9,
+                         double(DET_RANGE * (MOV_THRESHOLD - 1)));
+    for (int i = 0; i < 3; i++) {
+        tmp_boxpoints = LocalMap_Points;
+        if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE) {
+            New_LocalMap_Points.vertex_max[i] -= mov_dist;
+            New_LocalMap_Points.vertex_min[i] -= mov_dist;
+            tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist;
+            cub_needrm.emplace_back(tmp_boxpoints);
+        } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) {
+            New_LocalMap_Points.vertex_max[i] += mov_dist;
+            New_LocalMap_Points.vertex_min[i] += mov_dist;
+            tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist;
+            cub_needrm.emplace_back(tmp_boxpoints);
+        }
+    }
+    LocalMap_Points = New_LocalMap_Points;
+
+    points_cache_collect();
+    if (cub_needrm.size() > 0) int kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm);
+}
+
+void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
+    mtx_buffer.lock();
+    scan_count++;
+    double preprocess_start_time = omp_get_wtime();
+    if (get_time_sec(msg->header.stamp) < last_timestamp_lidar) {
+        RCLCPP_ERROR(logger, "lidar loop back, clear buffer");
+        // lidar_buffer.shrink_to_fit();
+
+        mtx_buffer.unlock();
+        sig_buffer.notify_all();
+        return;
+    }
+
+    last_timestamp_lidar = msg->header.stamp.sec;
+
+    PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
+    PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI());
+    double time_div = get_time_sec(msg->header.stamp);
+    p_pre->process(msg, ptr);
+    if (cut_frame) {
+        sort(ptr->points.begin(), ptr->points.end(), time_list);
+
+        for (int i = 0; i < ptr->size(); i++) {
+            ptr_div->push_back(ptr->points[i]);
+            // cout << "check time:" << ptr->points[i].curvature << endl;
+            if (ptr->points[i].curvature / double(1000) + get_time_sec(msg->header.stamp) - time_div >
+                cut_frame_time_interval) {
+                if (ptr_div->size() < 1) continue;
+                PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI());
+                *ptr_div_i = *ptr_div;
+                lidar_buffer.push_back(ptr_div_i);
+                time_buffer.push_back(time_div);
+                time_div += ptr->points[i].curvature / double(1000);
+                ptr_div->clear();
+            }
+        }
+        if (!ptr_div->empty()) {
+            lidar_buffer.push_back(ptr_div);
+            // ptr_div->clear();
+            time_buffer.push_back(time_div);
+        }
+    } else if (con_frame) {
+        if (frame_ct == 0) {
+            time_con = last_timestamp_lidar; //get_time_sec(msg->header.stamp);
+        }
+        if (frame_ct < con_frame_num) {
+            for (int i = 0; i < ptr->size(); i++) {
+                ptr->points[i].curvature += (last_timestamp_lidar - time_con) * 1000;
+                ptr_con->push_back(ptr->points[i]);
+            }
+            frame_ct++;
+        } else {
+            PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI());
+            *ptr_con_i = *ptr_con;
+            lidar_buffer.push_back(ptr_con_i);
+            double time_con_i = time_con;
+            time_buffer.push_back(time_con_i);
+            ptr_con->clear();
+            frame_ct = 0;
+        }
+    } else {
+        lidar_buffer.emplace_back(ptr);
+        time_buffer.emplace_back(get_time_sec(msg->header.stamp));
+    }
+    s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time;
+    mtx_buffer.unlock();
+    sig_buffer.notify_all();
+}
+
+// void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::SharedPtr msg) {
+//     mtx_buffer.lock();
+//     double preprocess_start_time = omp_get_wtime();
+//     scan_count++;
+//     if (get_time_sec(msg->header.stamp) < last_timestamp_lidar) {
+//         RCLCPP_ERROR(logger, "lidar loop back, clear buffer");
+
+//         mtx_buffer.unlock();
+//         sig_buffer.notify_all();
+//         return;
+//     }
+
+//     last_timestamp_lidar = get_time_sec(msg->header.stamp);
+
+//     PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
+//     PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI());
+//     p_pre->process(msg, ptr);
+//     double time_div = get_time_sec(msg->header.stamp);
+//     if (cut_frame) {
+//         sort(ptr->points.begin(), ptr->points.end(), time_list);
+
+//         for (int i = 0; i < ptr->size(); i++) {
+//             ptr_div->push_back(ptr->points[i]);
+//             if (ptr->points[i].curvature / double(1000) + get_time_sec(msg->header.stamp) - time_div >
+//                 cut_frame_time_interval) {
+//                 if (ptr_div->size() < 1) continue;
+//                 PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI());
+//                 // cout << "ptr div num:" << ptr_div->size() << endl;
+//                 *ptr_div_i = *ptr_div;
+//                 // cout << "ptr div i num:" << ptr_div_i->size() << endl;
+//                 lidar_buffer.push_back(ptr_div_i);
+//                 time_buffer.push_back(time_div);
+//                 time_div += ptr->points[i].curvature / double(1000);
+//                 ptr_div->clear();
+//             }
+//         }
+//         if (!ptr_div->empty()) {
+//             lidar_buffer.push_back(ptr_div);
+//             // ptr_div->clear();
+//             time_buffer.push_back(time_div);
+//         }
+//     } else if (con_frame) {
+//         if (frame_ct == 0) {
+//             time_con = last_timestamp_lidar; //get_time_sec(msg->header.stamp);
+//         }
+//         if (frame_ct < con_frame_num) {
+//             for (int i = 0; i < ptr->size(); i++) {
+//                 ptr->points[i].curvature += (last_timestamp_lidar - time_con) * 1000;
+//                 ptr_con->push_back(ptr->points[i]);
+//             }
+//             frame_ct++;
+//         } else {
+//             PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI());
+//             *ptr_con_i = *ptr_con;
+//             double time_con_i = time_con;
+//             lidar_buffer.push_back(ptr_con_i);
+//             time_buffer.push_back(time_con_i);
+//             ptr_con->clear();
+//             frame_ct = 0;
+//         }
+//     } else {
+//         lidar_buffer.emplace_back(ptr);
+//         time_buffer.emplace_back(get_time_sec(msg->header.stamp));
+//     }
+//     s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time;
+//     mtx_buffer.unlock();
+//     sig_buffer.notify_all();
+// }
+
+void imu_cbk(const sensor_msgs::msg::Imu::SharedPtr msg_in) {
+    publish_count++;
+    sensor_msgs::msg::Imu::SharedPtr msg(new sensor_msgs::msg::Imu(*msg_in));
+
+    msg->header.stamp = get_ros_time(get_time_sec(msg_in->header.stamp) - time_lag_imu_to_lidar);
+    double timestamp = get_time_sec(msg->header.stamp);
+
+    mtx_buffer.lock();
+
+    if (timestamp < last_timestamp_imu) {
+        RCLCPP_ERROR(logger, "imu loop back, clear deque");
+        // imu_deque.shrink_to_fit();
+        mtx_buffer.unlock();
+        sig_buffer.notify_all();
+        return;
+    }
+
+    imu_deque.emplace_back(msg);
+    last_timestamp_imu = timestamp;
+    mtx_buffer.unlock();
+    sig_buffer.notify_all();
+}
+
+bool sync_packages(MeasureGroup &meas) {
+    if (!imu_en) {
+        if (!lidar_buffer.empty()) {
+            meas.lidar = lidar_buffer.front();
+            meas.lidar_beg_time = time_buffer.front();
+            time_buffer.pop_front();
+            lidar_buffer.pop_front();
+            if (meas.lidar->points.size() < 1) {
+                cout << "lose lidar" << std::endl;
+                return false;
+            }
+            double end_time = meas.lidar->points.back().curvature;
+            for (auto pt: meas.lidar->points) {
+                if (pt.curvature > end_time) {
+                    end_time = pt.curvature;
+                }
+            }
+            lidar_end_time = meas.lidar_beg_time + end_time / double(1000);
+            meas.lidar_last_time = lidar_end_time;
+            return true;
+        }
+        return false;
+    }
+
+    if (lidar_buffer.empty() || imu_deque.empty()) {
+        return false;
+    }
+
+    /*** push a lidar scan ***/
+    if (!lidar_pushed) {
+        meas.lidar = lidar_buffer.front();
+        if (meas.lidar->points.size() < 1) {
+            cout << "lose lidar" << endl;
+            lidar_buffer.pop_front();
+            time_buffer.pop_front();
+            return false;
+        }
+        meas.lidar_beg_time = time_buffer.front();
+        double end_time = meas.lidar->points.back().curvature;
+        for (auto pt: meas.lidar->points) {
+            if (pt.curvature > end_time) {
+                end_time = pt.curvature;
+            }
+        }
+        lidar_end_time = meas.lidar_beg_time + end_time / double(1000);
+
+        meas.lidar_last_time = lidar_end_time;
+        lidar_pushed = true;
+    }
+
+    if (last_timestamp_imu < lidar_end_time) {
+        return false;
+    }
+    /*** push imu data, and pop from imu buffer ***/
+    if (p_imu->imu_need_init_) {
+        double imu_time = get_time_sec(imu_deque.front()->header.stamp);
+        meas.imu.shrink_to_fit();
+        while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) {
+            imu_time = get_time_sec(imu_deque.front()->header.stamp);
+            if (imu_time > lidar_end_time) break;
+            meas.imu.emplace_back(imu_deque.front());
+            imu_last = imu_next;
+            imu_last_ptr = imu_deque.front();
+            imu_next = *(imu_deque.front());
+            imu_deque.pop_front();
+        }
+    } else if (!init_map) {
+        double imu_time = get_time_sec(imu_deque.front()->header.stamp);
+        meas.imu.shrink_to_fit();
+        meas.imu.emplace_back(imu_last_ptr);
+
+        while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) {
+            imu_time = get_time_sec(imu_deque.front()->header.stamp);
+            if (imu_time > lidar_end_time) break;
+            meas.imu.emplace_back(imu_deque.front());
+            imu_last = imu_next;
+            imu_last_ptr = imu_deque.front();
+            imu_next = *(imu_deque.front());
+            imu_deque.pop_front();
+        }
+    }
+
+    lidar_buffer.pop_front();
+    time_buffer.pop_front();
+    lidar_pushed = false;
+    return true;
+}
+
+int process_increments = 0;
+
+void map_incremental() {
+    PointVector PointToAdd;
+    PointVector PointNoNeedDownsample;
+    PointToAdd.reserve(feats_down_size);
+    PointNoNeedDownsample.reserve(feats_down_size);
+
+    for (int i = 0; i < feats_down_size; i++) {
+        if (!Nearest_Points[i].empty()) {
+            const PointVector &points_near = Nearest_Points[i];
+            bool need_add = true;
+            PointType downsample_result, mid_point;
+            mid_point.x = floor(feats_down_world->points[i].x / filter_size_map_min) * filter_size_map_min +
+                          0.5 * filter_size_map_min;
+            mid_point.y = floor(feats_down_world->points[i].y / filter_size_map_min) * filter_size_map_min +
+                          0.5 * filter_size_map_min;
+            mid_point.z = floor(feats_down_world->points[i].z / filter_size_map_min) * filter_size_map_min +
+                          0.5 * filter_size_map_min;
+            /* If the nearest points is definitely outside the downsample box */
+            if (fabs(points_near[0].x - mid_point.x) > 1.732 * filter_size_map_min ||
+                fabs(points_near[0].y - mid_point.y) > 1.732 * filter_size_map_min ||
+                fabs(points_near[0].z - mid_point.z) > 1.732 * filter_size_map_min) {
+                PointNoNeedDownsample.emplace_back(feats_down_world->points[i]);
+                continue;
+            }
+            /* Check if there is a point already in the downsample box */
+            float dist = calc_dist<float>(feats_down_world->points[i], mid_point);
+            for (int readd_i = 0; readd_i < points_near.size(); readd_i++) {
+                /* Those points which are outside the downsample box should not be considered. */
+                if (fabs(points_near[readd_i].x - mid_point.x) < 0.5 * filter_size_map_min &&
+                    fabs(points_near[readd_i].y - mid_point.y) < 0.5 * filter_size_map_min &&
+                    fabs(points_near[readd_i].z - mid_point.z) < 0.5 * filter_size_map_min) {
+                    need_add = false;
+                    break;
+                }
+            }
+            if (need_add) PointToAdd.emplace_back(feats_down_world->points[i]);
+        } else {
+            // PointToAdd.emplace_back(feats_down_world->points[i]);
+            PointNoNeedDownsample.emplace_back(feats_down_world->points[i]);
+        }
+    }
+    int add_point_size = ikdtree.Add_Points(PointToAdd, true);
+    ikdtree.Add_Points(PointNoNeedDownsample, false);
+}
+
+void publish_init_kdtree(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubLaserCloudFullRes) {
+    
+    if (odom_only) {return;}
+
+    int size_init_ikdtree = ikdtree.size();
+    PointCloudXYZI::Ptr laserCloudInit(new PointCloudXYZI(size_init_ikdtree, 1));
+
+    sensor_msgs::msg::PointCloud2 laserCloudmsg;
+    PointVector().swap(ikdtree.PCL_Storage);
+    ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
+
+    laserCloudInit->points = ikdtree.PCL_Storage;
+    pcl::toROSMsg(*laserCloudInit, laserCloudmsg);
+
+    laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
+    laserCloudmsg.header.frame_id = odom_header_frame_id;
+    if (!odom_only) {
+        pubLaserCloudFullRes->publish(laserCloudmsg);
+    }
+}
+
+PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1));
+PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI());
+
+void publish_frame_world(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubLaserCloudFullRes) {
+
+    if (odom_only) {return;}
+
+    if (scan_pub_en) {
+        PointCloudXYZI::Ptr laserCloudFullRes(feats_down_body);
+        int size = laserCloudFullRes->points.size();
+
+        PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1));
+
+        for (int i = 0; i < size; i++) {
+            // if (i % 3 == 0)
+            // {
+            laserCloudWorld->points[i].x = feats_down_world->points[i].x;
+            laserCloudWorld->points[i].y = feats_down_world->points[i].y;
+            laserCloudWorld->points[i].z = feats_down_world->points[i].z;
+            laserCloudWorld->points[i].intensity = feats_down_world->points[i].intensity; // feats_down_world->points[i].y; // 
+            // }
+        }
+        sensor_msgs::msg::PointCloud2 laserCloudmsg;
+        pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
+
+        laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
+        laserCloudmsg.header.frame_id = odom_header_frame_id;
+        pubLaserCloudFullRes->publish(laserCloudmsg);
+        publish_count -= PUBFRAME_PERIOD;
+    }
+
+    /**************** save map ****************/
+    /* 1. make sure you have enough memories
+    /* 2. noted that pcd save will influence the real-time performences **/
+    if (pcd_save_en) {
+        int size = feats_down_world->points.size();
+        PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1));
+
+        for (int i = 0; i < size; i++) {
+            laserCloudWorld->points[i].x = feats_down_world->points[i].x;
+            laserCloudWorld->points[i].y = feats_down_world->points[i].y;
+            laserCloudWorld->points[i].z = feats_down_world->points[i].z;
+            laserCloudWorld->points[i].intensity = feats_down_world->points[i].intensity;
+        }
+
+        *pcl_wait_save += *laserCloudWorld;
+
+        static int scan_wait_num = 0;
+        scan_wait_num++;
+        if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) {
+            pcd_index++;
+            string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd"));
+            pcl::PCDWriter pcd_writer;
+            cout << "current scan saved to /PCD/" << all_points_dir << endl;
+            pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
+            pcl_wait_save->clear();
+            scan_wait_num = 0;
+        }
+    }
+}
+
+void publish_frame_body(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubLaserCloudFull_body) {
+
+    if (odom_only) {return;}
+
+    int size = feats_undistort->points.size();
+    PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1));
+
+    for (int i = 0; i < size; i++) {
+        pointBodyLidarToIMU(&feats_undistort->points[i], \
+                            &laserCloudIMUBody->points[i]);
+    }
+
+    sensor_msgs::msg::PointCloud2 laserCloudmsg;
+    pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg);
+    laserCloudmsg.header.stamp = get_ros_time(lidar_end_time);
+    laserCloudmsg.header.frame_id = "body";
+    pubLaserCloudFull_body->publish(laserCloudmsg);
+    publish_count -= PUBFRAME_PERIOD;
+}
+
+template<typename T>
+void set_posestamp(T &out) {
+    if (!use_imu_as_input) {
+        out.position.x = kf_output.x_.pos(0);
+        out.position.y = kf_output.x_.pos(1);
+        out.position.z = kf_output.x_.pos(2);
+        out.orientation.x = kf_output.x_.rot.coeffs()[0];
+        out.orientation.y = kf_output.x_.rot.coeffs()[1];
+        out.orientation.z = kf_output.x_.rot.coeffs()[2];
+        out.orientation.w = kf_output.x_.rot.coeffs()[3];
+    } else {
+        out.position.x = kf_input.x_.pos(0);
+        out.position.y = kf_input.x_.pos(1);
+        out.position.z = kf_input.x_.pos(2);
+        out.orientation.x = kf_input.x_.rot.coeffs()[0];
+        out.orientation.y = kf_input.x_.rot.coeffs()[1];
+        out.orientation.z = kf_input.x_.rot.coeffs()[2];
+        out.orientation.w = kf_input.x_.rot.coeffs()[3];
+    }
+}
+
+template<typename T>
+void set_twist(T &out) {
+    if (!use_imu_as_input) {
+        out.linear.x = kf_output.x_.vel(0);
+        out.linear.y = kf_output.x_.vel(1);
+        out.linear.z = kf_output.x_.vel(2);
+        out.angular.x = kf_output.x_.omg(0);
+        out.angular.y = kf_output.x_.omg(1);
+        out.angular.z = kf_output.x_.omg(2);
+    } else {
+        out.linear.x = kf_input.x_.vel(0);
+        out.linear.y = kf_input.x_.vel(1);
+        out.linear.z = kf_input.x_.vel(2);
+        out.angular.x = imu_last.angular_velocity.x;
+        out.angular.y = imu_last.angular_velocity.y;
+        out.angular.z = imu_last.angular_velocity.z;
+    }
+}
+
+void publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr &pubOdomAftMapped,
+                      std::shared_ptr<tf2_ros::TransformBroadcaster> &tf_br) {
+
+    odomAftMapped.header.frame_id = odom_header_frame_id;
+    odomAftMapped.child_frame_id = odom_child_frame_id;
+
+    if (publish_odometry_without_downsample) {
+        odomAftMapped.header.stamp = get_ros_time(time_current);
+    } else {
+        odomAftMapped.header.stamp = get_ros_time(lidar_end_time);
+    }
+    set_posestamp(odomAftMapped.pose.pose);
+    set_twist(odomAftMapped.twist.twist);
+
+    if (odom_only){
+        Matrix3d cov = kf_output.get_P().block<3, 3>(0, 0);
+
+        // Get the position components (first 3x3)
+        for (int i = 0; i < 3; i++) {
+            for (int j = 0; j < 3; j++) {
+                odomAftMapped.pose.covariance[6 * i + j] = cov(i, j);
+            }
+        }
+
+        odomAftMapped.pose.covariance[21] = 0.0;    // Covariance for roll
+        odomAftMapped.pose.covariance[28] = 0.0;    // Covariance for pitch
+        odomAftMapped.pose.covariance[35] = 0.05;   // Covariance for yaw
+
+        odomAftMapped.twist.covariance[0] = 0.1;    // Covariance for linear velocity on x
+        odomAftMapped.twist.covariance[7] = 0.1;    // Covariance for linear velocity on y
+        odomAftMapped.twist.covariance[14] = 0.0;   // Covariance for linear velocity on z
+        odomAftMapped.twist.covariance[21] = 0.0;  // Covariance for angular velocity (roll)
+        odomAftMapped.twist.covariance[28] = 0.0;  // Covariance for angular velocity (pitch)
+        odomAftMapped.twist.covariance[35] = 0.05;  // Covariance for angular velocity (yaw)
+    }
+
+    pubOdomAftMapped->publish(odomAftMapped);
+
+    //static tf2_ros::TransformBroadcaster br = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
+    geometry_msgs::msg::TransformStamped transform;
+    transform.header.frame_id = odom_header_frame_id;
+    transform.child_frame_id = odom_child_frame_id;
+
+    transform.transform.translation.x = odomAftMapped.pose.pose.position.x;
+    transform.transform.translation.y = odomAftMapped.pose.pose.position.y;
+    transform.transform.translation.z = odomAftMapped.pose.pose.position.z;
+
+    transform.transform.rotation.w = odomAftMapped.pose.pose.orientation.w;
+    transform.transform.rotation.x = odomAftMapped.pose.pose.orientation.x;
+    transform.transform.rotation.y = odomAftMapped.pose.pose.orientation.y;
+    transform.transform.rotation.z = odomAftMapped.pose.pose.orientation.z;
+
+    transform.header.stamp = odomAftMapped.header.stamp;
+
+    tf_br->sendTransform(transform);
+}
+
+void publish_path(const rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr &pubPath) {
+
+    if (odom_only) {return;}
+
+    set_posestamp(msg_body_pose.pose);
+    // msg_body_pose.header.stamp = ros::Time::now();
+    msg_body_pose.header.stamp = get_ros_time(lidar_end_time);
+    msg_body_pose.header.frame_id = odom_header_frame_id;
+    static int jjj = 0;
+    jjj++;
+    // if (jjj % 2 == 0) // if path is too large, the rvis will crash
+    {
+        path.poses.emplace_back(msg_body_pose);
+        pubPath->publish(path);
+    }
+}
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    auto nh = std::make_shared<rclcpp::Node>("laserMapping");
+    readParameters(nh);
+    cout << "lidar_type: " << lidar_type << endl;
+
+    path.header.stamp = get_ros_time(lidar_end_time);
+    path.header.frame_id = odom_header_frame_id;
+
+    /*** variables definition for counting ***/
+    int frame_num = 0;
+    double aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_propag = 0;
+    std::time_t startTime, endTime;
+
+    /*** initialize variables ***/
+    double FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0);
+    double HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0);
+
+    memset(point_selected_surf, true, sizeof(point_selected_surf));
+    downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
+    downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min);
+    Lidar_T_wrt_IMU << VEC_FROM_ARRAY(extrinT);
+    Lidar_R_wrt_IMU << MAT_FROM_ARRAY(extrinR);
+    if (extrinsic_est_en) {
+        if (!use_imu_as_input) {
+            kf_output.x_.offset_R_L_I = Lidar_R_wrt_IMU;
+            kf_output.x_.offset_T_L_I = Lidar_T_wrt_IMU;
+        } else {
+            kf_input.x_.offset_R_L_I = Lidar_R_wrt_IMU;
+            kf_input.x_.offset_T_L_I = Lidar_T_wrt_IMU;
+        }
+    }
+    p_imu->lidar_type = p_pre->lidar_type = lidar_type;
+    p_imu->imu_en = imu_en;
+
+    kf_input.init_dyn_share_modified(get_f_input, df_dx_input, h_model_input);
+    kf_output.init_dyn_share_modified_2h(get_f_output, df_dx_output, h_model_output, h_model_IMU_output);
+    Eigen::Matrix<double, 24, 24> P_init = MD(24, 24)::Identity() * 0.01;
+    P_init.block<3, 3>(21, 21) = MD(3, 3)::Identity() * 0.0001;
+    P_init.block<6, 6>(15, 15) = MD(6, 6)::Identity() * 0.001;
+    P_init.block<6, 6>(6, 6) = MD(6, 6)::Identity() * 0.0001;
+    kf_input.change_P(P_init);
+    Eigen::Matrix<double, 30, 30> P_init_output = MD(30, 30)::Identity() * 0.01;
+    P_init_output.block<3, 3>(21, 21) = MD(3, 3)::Identity() * 0.0001;
+    P_init_output.block<6, 6>(6, 6) = MD(6, 6)::Identity() * 0.0001;
+    P_init_output.block<6, 6>(24, 24) = MD(6, 6)::Identity() * 0.001;
+    kf_input.change_P(P_init);
+    kf_output.change_P(P_init_output);
+    Eigen::Matrix<double, 24, 24> Q_input = process_noise_cov_input();
+    Eigen::Matrix<double, 30, 30> Q_output = process_noise_cov_output();
+    /*** debug record ***/
+    FILE *fp;
+    string pos_log_dir = root_dir + "/Log/pos_log.txt";
+    fp = fopen(pos_log_dir.c_str(), "w");
+
+    ofstream fout_out, fout_imu_pbp;
+    fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), ios::out);
+    fout_imu_pbp.open(DEBUG_FILE_DIR("imu_pbp.txt"), ios::out);
+    if (fout_out && fout_imu_pbp)
+        cout << "~~~~" << ROOT_DIR << " file opened" << endl;
+    else
+        cout << "~~~~" << ROOT_DIR << " doesn't exist" << endl;
+
+    /*** ROS subscribe initialization ***/
+    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_pcl;
+    // rclcpp::Subscription<livox_ros_driver2::msg::CustomMsg>::SharedPtr sub_pcl_livox_;
+    // if (p_pre->lidar_type == AVIA) {
+    //     sub_pcl_livox_ = nh->create_subscription<livox_ros_driver2::msg::CustomMsg>(lid_topic, 20, livox_pcl_cbk);
+    // } else {
+    sub_pcl = nh->create_subscription<sensor_msgs::msg::PointCloud2>(lid_topic, rclcpp::SensorDataQoS(), standard_pcl_cbk);
+    // }
+    auto sub_imu = nh->create_subscription<sensor_msgs::msg::Imu>(imu_topic, 200000, imu_cbk);
+
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes_body;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudEffect;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudMap;
+    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath;
+
+    if (!odom_only){
+        pubLaserCloudFullRes = nh->create_publisher<sensor_msgs::msg::PointCloud2>
+                ("/cloud_registered", 100000);
+        pubLaserCloudFullRes_body = nh->create_publisher<sensor_msgs::msg::PointCloud2>
+                ("/cloud_registered_body", 100000);
+        pubLaserCloudEffect = nh->create_publisher<sensor_msgs::msg::PointCloud2>
+                ("/cloud_effected", 100000);
+        pubLaserCloudMap = nh->create_publisher<sensor_msgs::msg::PointCloud2>
+                ("/Laser_map", 100000);
+        pubPath = nh->create_publisher<nav_msgs::msg::Path>
+                ("/path", 100000);
+    }
+
+    // Choose topic name depending on odom_only value
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped;
+    if (odom_only){
+        pubOdomAftMapped = nh->create_publisher<nav_msgs::msg::Odometry>
+                ("/odom_corrected", 100000);
+    } else {
+        pubOdomAftMapped = nh->create_publisher<nav_msgs::msg::Odometry>
+                ("/aft_mapped_to_init", 100000);
+    }
+
+    //auto plane_pub = nh->create_publisher<visualization_msgs::msg::Marker>
+    //        ("/planner_normal", 1000);
+    auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(nh);
+//------------------------------------------------------------------------------------------------------
+    signal(SIGINT, SigHandle);
+    rclcpp::Rate rate(5000);
+    while (rclcpp::ok()) {
+        if (flg_exit) break;
+        //ros::spinOnce();
+        rclcpp::executors::SingleThreadedExecutor executor;
+        executor.add_node(nh);
+        executor.spin_some(); // 处理当前可用的回调
+
+        if (sync_packages(Measures)) {
+            if (flg_first_scan) {
+                first_lidar_time = Measures.lidar_beg_time;
+                flg_first_scan = false;
+                cout << "first lidar time" << first_lidar_time << endl;
+            }
+
+            if (flg_reset) {
+                RCLCPP_WARN(logger, "reset when rosbag play back");
+                p_imu->Reset();
+                flg_reset = false;
+                continue;
+            }
+            double t0, t1, t2, t3, t4, t5, match_start, solve_start;
+            match_time = 0;
+            solve_time = 0;
+            propag_time = 0;
+            update_time = 0;
+            t0 = omp_get_wtime();
+
+            p_imu->Process(Measures, feats_undistort);
+
+            if (feats_undistort->empty() || feats_undistort == nullptr) {
+                continue;
+            }
+            if (imu_en) {
+                if (!p_imu->gravity_align_) {
+                    while (Measures.lidar_beg_time > get_time_sec(imu_next.header.stamp)) {
+                        imu_last = imu_next;
+                        imu_next = *(imu_deque.front());
+                        imu_deque.pop_front();
+                        // imu_deque.pop();
+                    }
+                    if (non_station_start) {
+                        state_in.gravity << VEC_FROM_ARRAY(gravity_init);
+                        state_out.gravity << VEC_FROM_ARRAY(gravity_init);
+                        state_out.acc << VEC_FROM_ARRAY(gravity_init);
+                        state_out.acc *= -1;
+                    } else {
+                        state_in.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm;
+                        state_out.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm;
+                        state_out.acc = p_imu->mean_acc * G_m_s2 / acc_norm;
+                    }
+                    if (gravity_align) {
+                        Eigen::Matrix3d rot_init;
+                        p_imu->gravity_ << VEC_FROM_ARRAY(gravity);
+                        p_imu->Set_init(state_in.gravity, rot_init);
+                        state_in.gravity = state_out.gravity = p_imu->gravity_;
+                        state_in.rot = state_out.rot = rot_init;
+                        state_in.rot.normalize();
+                        state_out.rot.normalize();
+                        state_out.acc = -rot_init.transpose() * state_out.gravity;
+                    }
+                    kf_input.change_x(state_in);
+                    kf_output.change_x(state_out);
+                }
+            } else {
+                if (!p_imu->gravity_align_) {
+                    state_in.gravity << VEC_FROM_ARRAY(gravity_init);
+                    state_out.gravity << VEC_FROM_ARRAY(gravity_init);
+                    state_out.acc << VEC_FROM_ARRAY(gravity_init);
+                    state_out.acc *= -1;
+                }
+            }
+            /*** Segment the map in lidar FOV ***/
+            lasermap_fov_segment();
+            /*** downsample the feature points in a scan ***/
+            t1 = omp_get_wtime();
+            if (space_down_sample) {
+                downSizeFilterSurf.setInputCloud(feats_undistort);
+                downSizeFilterSurf.filter(*feats_down_body);
+                sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list);
+            } else {
+                feats_down_body = Measures.lidar;
+                sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list);
+            }
+            time_seq = time_compressing<int>(feats_down_body);
+            feats_down_size = feats_down_body->points.size();
+
+            /*** initialize the map kdtree ***/
+            if (!init_map) {
+                if (ikdtree.Root_Node == nullptr) //
+                    // if(feats_down_size > 5)
+                {
+                    ikdtree.set_downsample_param(filter_size_map_min);
+                }
+
+                feats_down_world->resize(feats_down_size);
+                for (int i = 0; i < feats_down_size; i++) {
+                    pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
+                }
+                for (size_t i = 0; i < feats_down_world->size(); i++) {
+                    init_feats_world->points.emplace_back(feats_down_world->points[i]);
+                }
+                if (init_feats_world->size() < init_map_size) continue;
+                ikdtree.Build(init_feats_world->points);
+                init_map = true;
+                publish_init_kdtree(pubLaserCloudMap); //(pubLaserCloudFullRes);
+                continue;
+            }
+            /*** ICP and Kalman filter update ***/
+            normvec->resize(feats_down_size);
+            feats_down_world->resize(feats_down_size);
+
+            Nearest_Points.resize(feats_down_size);
+
+            t2 = omp_get_wtime();
+
+            /*** iterated state estimation ***/
+            crossmat_list.reserve(feats_down_size);
+            pbody_list.reserve(feats_down_size);
+            // pbody_ext_list.reserve(feats_down_size);
+
+            for (size_t i = 0; i < feats_down_body->size(); i++) {
+                V3D point_this(feats_down_body->points[i].x,
+                               feats_down_body->points[i].y,
+                               feats_down_body->points[i].z);
+                pbody_list[i] = point_this;
+                if (extrinsic_est_en) {
+                    if (!use_imu_as_input) {
+                        point_this = kf_output.x_.offset_R_L_I.normalized() * point_this + kf_output.x_.offset_T_L_I;
+                    } else {
+                        point_this = kf_input.x_.offset_R_L_I.normalized() * point_this + kf_input.x_.offset_T_L_I;
+                    }
+                } else {
+                    point_this = Lidar_R_wrt_IMU * point_this + Lidar_T_wrt_IMU;
+                }
+                M3D point_crossmat;
+                point_crossmat << SKEW_SYM_MATRX(point_this);
+                crossmat_list[i] = point_crossmat;
+            }
+
+            if (!use_imu_as_input) {
+                bool imu_upda_cov = false;
+                effct_feat_num = 0;
+                /**** point by point update ****/
+
+                double pcl_beg_time = Measures.lidar_beg_time;
+                idx = -1;
+                for (k = 0; k < time_seq.size(); k++) {
+                    PointType &point_body = feats_down_body->points[idx + time_seq[k]];
+
+                    time_current = point_body.curvature / 1000.0 + pcl_beg_time;
+
+                    if (is_first_frame) {
+                        if (imu_en) {
+                            while (time_current > get_time_sec(imu_next.header.stamp)) {
+                                imu_last = imu_next;
+                                imu_next = *(imu_deque.front());
+                                imu_deque.pop_front();
+                                // imu_deque.pop();
+                            }
+
+                            angvel_avr
+                                    << imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
+                            acc_avr
+                                    << imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
+                        }
+                        is_first_frame = false;
+                        imu_upda_cov = true;
+                        time_update_last = time_current;
+                        time_predict_last_const = time_current;
+                    }
+                    if (imu_en) {
+                        bool imu_comes = time_current > get_time_sec(imu_next.header.stamp);
+                        while (imu_comes) {
+                            imu_upda_cov = true;
+                            angvel_avr
+                                    << imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
+                            acc_avr
+                                    << imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z;
+
+                            /*** covariance update ***/
+                            imu_last = imu_next;
+                            imu_next = *(imu_deque.front());
+                            imu_deque.pop_front();
+                            double dt = get_time_sec(imu_last.header.stamp) - time_predict_last_const;
+                            kf_output.predict(dt, Q_output, input_in, true, false);
+                            time_predict_last_const = get_time_sec(imu_last.header.stamp); // big problem
+                            imu_comes = time_current > get_time_sec(imu_next.header.stamp);
+                            // if (!imu_comes)
+                            {
+                                double dt_cov = get_time_sec(imu_last.header.stamp) - time_update_last;
+
+                                if (dt_cov > 0.0) {
+                                    time_update_last = get_time_sec(imu_last.header.stamp);
+                                    double propag_imu_start = omp_get_wtime();
+
+                                    kf_output.predict(dt_cov, Q_output, input_in, false, true);
+
+                                    propag_time += omp_get_wtime() - propag_imu_start;
+                                    double solve_imu_start = omp_get_wtime();
+                                    kf_output.update_iterated_dyn_share_IMU();
+                                    solve_time += omp_get_wtime() - solve_imu_start;
+                                }
+                            }
+                        }
+                    }
+
+                    double dt = time_current - time_predict_last_const;
+                    double propag_state_start = omp_get_wtime();
+                    if (!prop_at_freq_of_imu) {
+                        double dt_cov = time_current - time_update_last;
+                        if (dt_cov > 0.0) {
+                            kf_output.predict(dt_cov, Q_output, input_in, false, true);
+                            time_update_last = time_current;
+                        }
+                    }
+                    kf_output.predict(dt, Q_output, input_in, true, false);
+                    propag_time += omp_get_wtime() - propag_state_start;
+                    time_predict_last_const = time_current;
+                    // if(k == 0)
+                    // {
+                    //     fout_imu_pbp << Measures.lidar_last_time - first_lidar_time << " " << imu_last.angular_velocity.x << " " << imu_last.angular_velocity.y << " " << imu_last.angular_velocity.z \
+                    //             << " " << imu_last.linear_acceleration.x << " " << imu_last.linear_acceleration.y << " " << imu_last.linear_acceleration.z << endl;
+                    // }
+
+                    double t_update_start = omp_get_wtime();
+
+                    if (feats_down_size < 1) {
+                        RCLCPP_WARN(logger, "No point, skip this scan!\n");
+                        idx += time_seq[k];
+                        continue;
+                    }
+                    if (!kf_output.update_iterated_dyn_share_modified()) {
+                        idx = idx + time_seq[k];
+                        continue;
+                    }
+
+                    if (prop_at_freq_of_imu) {
+                        double dt_cov = time_current - time_update_last;
+                        if (!imu_en && (dt_cov >= imu_time_inte)) // (point_cov_not_prop && imu_prop_cov)
+                        {
+                            double propag_cov_start = omp_get_wtime();
+                            kf_output.predict(dt_cov, Q_output, input_in, false, true);
+                            imu_upda_cov = false;
+                            time_update_last = time_current;
+                            propag_time += omp_get_wtime() - propag_cov_start;
+                        }
+                    }
+
+                    solve_start = omp_get_wtime();
+
+                    if (publish_odometry_without_downsample) {
+                        /******* Publish odometry *******/
+
+                        publish_odometry(pubOdomAftMapped, tf_broadcaster);
+                        if (runtime_pos_log) {
+                            state_out = kf_output.x_;
+                            euler_cur = SO3ToEuler(state_out.rot);
+                            fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " "
+                                     << euler_cur.transpose() << " " << state_out.pos.transpose() << " "
+                                     << state_out.vel.transpose() << " " << state_out.omg.transpose() << " "
+                                     << state_out.acc.transpose() << " " << state_out.gravity.transpose() << " "
+                                     << state_out.bg.transpose() << " " << state_out.ba.transpose() << " "
+                                     << feats_undistort->points.size() << endl;
+                        }
+                    }
+
+                    for (int j = 0; j < time_seq[k]; j++) {
+                        PointType &point_body_j = feats_down_body->points[idx + j + 1];
+                        PointType &point_world_j = feats_down_world->points[idx + j + 1];
+                        pointBodyToWorld(&point_body_j, &point_world_j);
+                    }
+
+                    solve_time += omp_get_wtime() - solve_start;
+
+                    update_time += omp_get_wtime() - t_update_start;
+                    idx += time_seq[k];
+                    // cout << "pbp output effect feat num:" << effct_feat_num << endl;
+                }
+            } else {
+                bool imu_prop_cov = false;
+                effct_feat_num = 0;
+
+                double pcl_beg_time = Measures.lidar_beg_time;
+                idx = -1;
+                for (k = 0; k < time_seq.size(); k++) {
+                    PointType &point_body = feats_down_body->points[idx + time_seq[k]];
+                    time_current = point_body.curvature / 1000.0 + pcl_beg_time;
+                    if (is_first_frame) {
+                        while (time_current > get_time_sec(imu_next.header.stamp)) {
+                            imu_last = imu_next;
+                            imu_next = *(imu_deque.front());
+                            imu_deque.pop_front();
+                            // imu_deque.pop();
+                        }
+                        imu_prop_cov = true;
+                        // imu_upda_cov = true;
+
+                        is_first_frame = false;
+                        t_last = time_current;
+                        time_update_last = time_current;
+                        // if(prop_at_freq_of_imu)
+                        {
+                            input_in.gyro << imu_last.angular_velocity.x,
+                                    imu_last.angular_velocity.y,
+                                    imu_last.angular_velocity.z;
+
+                            input_in.acc << imu_last.linear_acceleration.x,
+                                    imu_last.linear_acceleration.y,
+                                    imu_last.linear_acceleration.z;
+                            // angvel_avr<<0.5 * (imu_last.angular_velocity.x + imu_next.angular_velocity.x),
+                            //             0.5 * (imu_last.angular_velocity.y + imu_next.angular_velocity.y),
+                            //             0.5 * (imu_last.angular_velocity.z + imu_next.angular_velocity.z);
+
+                            // acc_avr   <<0.5 * (imu_last.linear_acceleration.x + imu_next.linear_acceleration.x),
+                            //             0.5 * (imu_last.linear_acceleration.y + imu_next.linear_acceleration.y),
+                            // 0.5 * (imu_last.linear_acceleration.z + imu_next.linear_acceleration.z);
+
+                            // angvel_avr -= state.bias_g;
+                            input_in.acc = input_in.acc * G_m_s2 / acc_norm;
+                        }
+                    }
+
+                    while (time_current > get_time_sec(imu_next.header.stamp)) // && !imu_deque.empty())
+                    {
+                        imu_last = imu_next;
+                        imu_next = *(imu_deque.front());
+                        imu_deque.pop_front();
+                        input_in.gyro
+                                << imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;
+                        input_in.acc
+                                << imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
+
+                        // angvel_avr<<0.5 * (imu_last.angular_velocity.x + imu_next.angular_velocity.x),
+                        //             0.5 * (imu_last.angular_velocity.y + imu_next.angular_velocity.y),
+                        //             0.5 * (imu_last.angular_velocity.z + imu_next.angular_velocity.z);
+
+                        // acc_avr   <<0.5 * (imu_last.linear_acceleration.x + imu_next.linear_acceleration.x),
+                        //             0.5 * (imu_last.linear_acceleration.y + imu_next.linear_acceleration.y),
+                        //             0.5 * (imu_last.linear_acceleration.z + imu_next.linear_acceleration.z);
+                        input_in.acc = input_in.acc * G_m_s2 / acc_norm;
+                        double dt = get_time_sec(imu_last.header.stamp) - t_last;
+
+                        // if(!prop_at_freq_of_imu)
+                        // {       
+                        double dt_cov = get_time_sec(imu_last.header.stamp) - time_update_last;
+                        if (dt_cov > 0.0) {
+                            kf_input.predict(dt_cov, Q_input, input_in, false, true);
+                            time_update_last = get_time_sec(imu_last.header.stamp); //time_current;
+                        }
+                        kf_input.predict(dt, Q_input, input_in, true, false);
+                        t_last = get_time_sec(imu_last.header.stamp);
+                        imu_prop_cov = true;
+                        // imu_upda_cov = true;
+                    }
+
+                    double dt = time_current - t_last;
+                    t_last = time_current;
+                    double propag_start = omp_get_wtime();
+
+                    if (!prop_at_freq_of_imu) {
+                        double dt_cov = time_current - time_update_last;
+                        if (dt_cov > 0.0) {
+                            kf_input.predict(dt_cov, Q_input, input_in, false, true);
+                            time_update_last = time_current;
+                        }
+                    }
+                    kf_input.predict(dt, Q_input, input_in, true, false);
+
+                    propag_time += omp_get_wtime() - propag_start;
+
+                    // if(k == 0)
+                    // {
+                    //     fout_imu_pbp << Measures.lidar_last_time - first_lidar_time << " " << imu_last.angular_velocity.x << " " << imu_last.angular_velocity.y << " " << imu_last.angular_velocity.z \
+                    //             << " " << imu_last.linear_acceleration.x << " " << imu_last.linear_acceleration.y << " " << imu_last.linear_acceleration.z << endl;
+                    // }
+
+                    double t_update_start = omp_get_wtime();
+
+                    if (feats_down_size < 1) {
+                        RCLCPP_WARN(logger, "No point, skip this scan!\n");
+
+                        idx += time_seq[k];
+                        continue;
+                    }
+                    if (!kf_input.update_iterated_dyn_share_modified()) {
+                        idx = idx + time_seq[k];
+                        continue;
+                    }
+
+                    solve_start = omp_get_wtime();
+
+                    // if(prop_at_freq_of_imu)
+                    // {
+                    //     double dt_cov = time_current - time_update_last;
+                    //     if ((imu_prop_cov && dt_cov > 0.0) || (dt_cov >= imu_time_inte * 1.2)) 
+                    //     {
+                    //         double propag_cov_start = omp_get_wtime();
+                    //         kf_input.predict(dt_cov, Q_input, input_in, false, true); 
+                    //         propag_time += omp_get_wtime() - propag_cov_start;
+                    //         time_update_last = time_current;
+                    //         imu_prop_cov = false;
+                    //     }
+                    // }
+                    if (publish_odometry_without_downsample) {
+                        /******* Publish odometry *******/
+
+                        publish_odometry(pubOdomAftMapped, tf_broadcaster);
+                        if (runtime_pos_log) {
+                            state_in = kf_input.x_;
+                            euler_cur = SO3ToEuler(state_in.rot);
+                            fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " "
+                                     << euler_cur.transpose() << " " << state_in.pos.transpose() << " "
+                                     << state_in.vel.transpose() << " " << state_in.bg.transpose() << " "
+                                     << state_in.ba.transpose() << " " << state_in.gravity.transpose() << " "
+                                     << feats_undistort->points.size() << endl;
+                        }
+                    }
+
+                    for (int j = 0; j < time_seq[k]; j++) {
+                        PointType &point_body_j = feats_down_body->points[idx + j + 1];
+                        PointType &point_world_j = feats_down_world->points[idx + j + 1];
+                        pointBodyToWorld(&point_body_j, &point_world_j);
+                    }
+                    solve_time += omp_get_wtime() - solve_start;
+
+                    update_time += omp_get_wtime() - t_update_start;
+                    idx = idx + time_seq[k];
+                }
+            }
+
+            /******* Publish odometry downsample *******/
+            if (!publish_odometry_without_downsample) {
+                publish_odometry(pubOdomAftMapped, tf_broadcaster);
+            }
+
+            /*** add the feature points to map kdtree ***/
+            t3 = omp_get_wtime();
+
+            if (feats_down_size > 4) {
+                map_incremental();
+            }
+
+            t5 = omp_get_wtime();
+            /******* Publish points *******/
+            if (path_en) publish_path(pubPath);
+            if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFullRes);
+            if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFullRes_body);
+
+            /*** Debug variables Logging ***/
+            if (runtime_pos_log) {
+                frame_num++;
+                aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num;
+                { aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + update_time / frame_num; }
+                aver_time_match = aver_time_match * (frame_num - 1) / frame_num + (match_time) / frame_num;
+                aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + solve_time / frame_num;
+                aver_time_propag = aver_time_propag * (frame_num - 1) / frame_num + propag_time / frame_num;
+                T1[time_log_counter] = Measures.lidar_beg_time;
+                s_plot[time_log_counter] = t5 - t0;
+                s_plot2[time_log_counter] = feats_undistort->points.size();
+                s_plot3[time_log_counter] = aver_time_consu;
+                time_log_counter++;
+                printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f  ave ICP: %0.6f  map incre: %0.6f ave total: %0.6f icp: %0.6f propogate: %0.6f \n",
+                       t1 - t0, aver_time_match, aver_time_solve, t3 - t1, t5 - t3, aver_time_consu, aver_time_icp,
+                       aver_time_propag);
+                if (!publish_odometry_without_downsample) {
+                    if (!use_imu_as_input) {
+                        state_out = kf_output.x_;
+                        euler_cur = SO3ToEuler(state_out.rot);
+                        fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " "
+                                 << euler_cur.transpose() << " " << state_out.pos.transpose() << " "
+                                 << state_out.vel.transpose() << " " << state_out.omg.transpose() << " "
+                                 << state_out.acc.transpose() << " " << state_out.gravity.transpose() << " "
+                                 << state_out.bg.transpose() << " " << state_out.ba.transpose() << " "
+                                 << feats_undistort->points.size() << endl;
+                    } else {
+                        state_in = kf_input.x_;
+                        euler_cur = SO3ToEuler(state_in.rot);
+                        fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " "
+                                 << euler_cur.transpose() << " " << state_in.pos.transpose() << " "
+                                 << state_in.vel.transpose() << " " << state_in.bg.transpose() << " "
+                                 << state_in.ba.transpose() << " " << state_in.gravity.transpose() << " "
+                                 << feats_undistort->points.size() << endl;
+                    }
+                }
+                dump_lio_state_to_log(fp);
+            }
+        }
+        rate.sleep();
+    }
+    //--------------------------save map-----------------------------------
+    /* 1. make sure you have enough memories
+       2. noted that pcd save will influence the real-time performences **/
+    if (pcl_wait_save->size() > 0 && pcd_save_en) {
+        string file_name = string("scans.pcd");
+        string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
+        pcl::PCDWriter pcd_writer;
+        pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
+    }
+    fout_out.close();
+    fout_imu_pbp.close();
+
+    return 0;
+}
diff --git a/point_lio_ros2/src/parameters.cpp b/point_lio_ros2/src/parameters.cpp
new file mode 100644
index 0000000..6ab2795
--- /dev/null
+++ b/point_lio_ros2/src/parameters.cpp
@@ -0,0 +1,157 @@
+#include "parameters.h"
+
+bool odom_only;
+std::string odom_header_frame_id, odom_child_frame_id;
+
+bool is_first_frame = true;
+double lidar_end_time = 0.0, first_lidar_time = 0.0, time_con = 0.0;
+double last_timestamp_lidar = -1.0, last_timestamp_imu = -1.0;
+int pcd_index = 0;
+
+std::string lid_topic, imu_topic;
+bool prop_at_freq_of_imu, check_satu, con_frame, cut_frame;
+bool use_imu_as_input, space_down_sample, publish_odometry_without_downsample;
+int init_map_size, con_frame_num;
+double match_s, satu_acc, satu_gyro, cut_frame_time_interval;
+float plane_thr;
+double filter_size_surf_min, filter_size_map_min, fov_deg;
+double cube_len;
+float DET_RANGE;
+bool imu_en, gravity_align, non_station_start;
+double imu_time_inte;
+double laser_point_cov, acc_norm;
+double vel_cov, acc_cov_input, gyr_cov_input;
+double gyr_cov_output, acc_cov_output, b_gyr_cov, b_acc_cov;
+double imu_meas_acc_cov, imu_meas_omg_cov;
+int lidar_type, pcd_save_interval;
+std::vector<double> gravity_init, gravity;
+std::vector<double> extrinT;
+std::vector<double> extrinR;
+bool runtime_pos_log, pcd_save_en, path_en, extrinsic_est_en = true;
+bool scan_pub_en, scan_body_pub_en;
+shared_ptr<Preprocess> p_pre;
+double time_lag_imu_to_lidar = 0.0;
+
+void readParameters(shared_ptr<rclcpp::Node> &nh) {
+    p_pre.reset(new Preprocess());
+
+    nh->declare_parameter<bool>("odom_only", false);
+    nh->declare_parameter<std::string>("odom_header_frame_id", "camera_init");
+    nh->declare_parameter<std::string>("odom_child_frame_id", "aft_mapped");
+
+    nh->declare_parameter<bool>("prop_at_freq_of_imu", true);
+    nh->declare_parameter<bool>("use_imu_as_input", true);
+    nh->declare_parameter<bool>("check_satu", true);
+    nh->declare_parameter<int>("init_map_size", 100);
+    nh->declare_parameter<bool>("space_down_sample", true);
+    nh->declare_parameter<double>("mapping.satu_acc", 3.0);
+    nh->declare_parameter<double>("mapping.satu_gyro", 35.0);
+    nh->declare_parameter<double>("mapping.acc_norm", 1.0);
+    nh->declare_parameter<float>("mapping.plane_thr", 0.05f);
+    nh->declare_parameter<int>("point_filter_num", 2);
+    nh->declare_parameter<std::string>("common.lid_topic", "/livox/lidar");
+    nh->declare_parameter<std::string>("common.imu_topic", "/livox/imu");
+    nh->declare_parameter<bool>("common.con_frame", false);
+    nh->declare_parameter<int>("common.con_frame_num", 1);
+    nh->declare_parameter<bool>("common.cut_frame", false);
+    nh->declare_parameter<double>("common.cut_frame_time_interval", 0.1);
+    nh->declare_parameter<double>("common.time_lag_imu_to_lidar", 0.0);
+    nh->declare_parameter<double>("filter_size_surf", 0.5);
+    nh->declare_parameter<double>("filter_size_map", 0.5);
+    nh->declare_parameter<double>("cube_side_length", 200);
+    nh->declare_parameter<float>("mapping.det_range", 300.f);
+    nh->declare_parameter<double>("mapping.fov_degree", 180);
+    nh->declare_parameter<bool>("mapping.imu_en", true);
+    nh->declare_parameter<bool>("mapping.start_in_aggressive_motion", false);
+    nh->declare_parameter<bool>("mapping.extrinsic_est_en", true);
+    nh->declare_parameter<double>("mapping.imu_time_inte", 0.005);
+    nh->declare_parameter<double>("mapping.lidar_meas_cov", 0.1);
+    nh->declare_parameter<double>("mapping.acc_cov_input", 0.1);
+    nh->declare_parameter<double>("mapping.vel_cov", 20);
+    nh->declare_parameter<double>("mapping.gyr_cov_input", 0.1);
+    nh->declare_parameter<double>("mapping.gyr_cov_output", 0.1);
+    nh->declare_parameter<double>("mapping.acc_cov_output", 0.1);
+    nh->declare_parameter<double>("mapping.b_gyr_cov", 0.0001);
+    nh->declare_parameter<double>("mapping.b_acc_cov", 0.0001);
+    nh->declare_parameter<double>("mapping.imu_meas_acc_cov", 0.1);
+    nh->declare_parameter<double>("mapping.imu_meas_omg_cov", 0.1);
+    nh->declare_parameter<double>("preprocess.blind", 1.0);
+    nh->declare_parameter<int>("preprocess.lidar_type", 1);
+    nh->declare_parameter<int>("preprocess.scan_line", 16);
+    nh->declare_parameter<int>("preprocess.scan_rate", 10);
+    nh->declare_parameter<int>("preprocess.timestamp_unit", 1);
+    nh->declare_parameter<double>("mapping.match_s", 81);
+    nh->declare_parameter<bool>("mapping.gravity_align", true);
+    nh->declare_parameter<std::vector<double>>("mapping.gravity", {0, 0, -9.810});
+    nh->declare_parameter<std::vector<double>>("mapping.gravity_init", {0, 0, -9.810});
+    nh->declare_parameter<std::vector<double>>("mapping.extrinsic_T", {0, 0, 0});
+    nh->declare_parameter<std::vector<double>>("mapping.extrinsic_R", {1, 0, 0, 0, 1, 0, 0, 0, 1});
+    nh->declare_parameter<bool>("odometry.publish_odometry_without_downsample", false);
+    nh->declare_parameter<bool>("publish.path_en", true);
+    nh->declare_parameter<bool>("publish.scan_publish_en", true);
+    nh->declare_parameter<bool>("publish.scan_bodyframe_pub_en", true);
+    nh->declare_parameter<bool>("runtime_pos_log_enable", false);
+    nh->declare_parameter<bool>("pcd_save.pcd_save_en", false);
+    nh->declare_parameter<int>("pcd_save.interval", -1);
+
+    // 使用get_parameter方法获取参数值
+    nh->get_parameter("odom_only", odom_only);
+    nh->get_parameter("odom_header_frame_id", odom_header_frame_id);
+    nh->get_parameter("odom_child_frame_id", odom_child_frame_id);
+
+    nh->get_parameter("prop_at_freq_of_imu", prop_at_freq_of_imu);
+    nh->get_parameter("use_imu_as_input", use_imu_as_input);
+    nh->get_parameter("check_satu", check_satu);
+    nh->get_parameter("init_map_size", init_map_size);
+    nh->get_parameter("space_down_sample", space_down_sample);
+    nh->get_parameter("mapping.satu_acc", satu_acc);
+    nh->get_parameter("mapping.satu_gyro", satu_gyro);
+    nh->get_parameter("mapping.acc_norm", acc_norm);
+    nh->get_parameter("mapping.plane_thr", plane_thr);
+    nh->get_parameter("point_filter_num", p_pre->point_filter_num);
+    nh->get_parameter("common.lid_topic", lid_topic);
+    nh->get_parameter("common.imu_topic", imu_topic);
+    nh->get_parameter("common.con_frame", con_frame);
+    nh->get_parameter("common.con_frame_num", con_frame_num);
+    nh->get_parameter("common.cut_frame", cut_frame);
+    nh->get_parameter("common.cut_frame_time_interval", cut_frame_time_interval);
+    nh->get_parameter("common.time_lag_imu_to_lidar", time_lag_imu_to_lidar);
+    nh->get_parameter("filter_size_surf", filter_size_surf_min);
+    nh->get_parameter("filter_size_map", filter_size_map_min);
+    nh->get_parameter("cube_side_length", cube_len);
+    nh->get_parameter("mapping.det_range", DET_RANGE);
+    nh->get_parameter("mapping.fov_degree", fov_deg);
+    nh->get_parameter("mapping.imu_en", imu_en);
+    nh->get_parameter("mapping.start_in_aggressive_motion", non_station_start);
+    nh->get_parameter("mapping.extrinsic_est_en", extrinsic_est_en);
+    nh->get_parameter("mapping.imu_time_inte", imu_time_inte);
+    nh->get_parameter("mapping.lidar_meas_cov", laser_point_cov);
+    nh->get_parameter("mapping.acc_cov_input", acc_cov_input);
+    nh->get_parameter("mapping.vel_cov", vel_cov);
+    nh->get_parameter("mapping.gyr_cov_input", gyr_cov_input);
+    nh->get_parameter("mapping.gyr_cov_output", gyr_cov_output);
+    nh->get_parameter("mapping.acc_cov_output", acc_cov_output);
+    nh->get_parameter("mapping.b_gyr_cov", b_gyr_cov);
+    nh->get_parameter("mapping.b_acc_cov", b_acc_cov);
+    nh->get_parameter("mapping.imu_meas_acc_cov", imu_meas_acc_cov);
+    nh->get_parameter("mapping.imu_meas_omg_cov", imu_meas_omg_cov);
+    nh->get_parameter("preprocess.blind", p_pre->blind);
+    nh->get_parameter("preprocess.lidar_type", lidar_type);
+    nh->get_parameter("preprocess.scan_line", p_pre->N_SCANS);
+    nh->get_parameter("preprocess.scan_rate", p_pre->SCAN_RATE);
+    nh->get_parameter("preprocess.timestamp_unit", p_pre->time_unit);
+    nh->get_parameter("mapping.match_s", match_s);
+    nh->get_parameter("mapping.gravity_align", gravity_align);
+    nh->get_parameter("mapping.gravity", gravity);
+    nh->get_parameter("mapping.gravity_init", gravity_init);
+    nh->get_parameter("mapping.extrinsic_T", extrinT);
+    nh->get_parameter("mapping.extrinsic_R", extrinR);
+    nh->get_parameter("odometry.publish_odometry_without_downsample", publish_odometry_without_downsample);
+    nh->get_parameter("publish.path_en", path_en);
+    nh->get_parameter("publish.scan_publish_en", scan_pub_en);
+    nh->get_parameter("publish.scan_bodyframe_pub_en", scan_body_pub_en);
+    nh->get_parameter("runtime_pos_log_enable", runtime_pos_log);
+    nh->get_parameter("pcd_save.pcd_save_en", pcd_save_en);
+    nh->get_parameter("pcd_save.interval", pcd_save_interval);
+}
+
diff --git a/point_lio_ros2/src/parameters.h b/point_lio_ros2/src/parameters.h
new file mode 100644
index 0000000..03e0c96
--- /dev/null
+++ b/point_lio_ros2/src/parameters.h
@@ -0,0 +1,46 @@
+// #ifndef PARAM_H
+// #define PARAM_H
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <Eigen/Eigen>
+#include <Eigen/Core>
+#include <cstring>
+#include <string>
+#include "preprocess.h"
+
+extern bool odom_only;
+extern std::string odom_header_frame_id;
+extern std::string odom_child_frame_id;
+
+extern bool is_first_frame;
+extern double lidar_end_time, first_lidar_time, time_con;
+extern double last_timestamp_lidar, last_timestamp_imu;
+extern int pcd_index;
+
+extern std::string lid_topic, imu_topic;
+extern bool prop_at_freq_of_imu, check_satu, con_frame, cut_frame;
+extern bool use_imu_as_input, space_down_sample;
+extern bool extrinsic_est_en, publish_odometry_without_downsample;
+extern int init_map_size, con_frame_num;
+extern double match_s, satu_acc, satu_gyro, cut_frame_time_interval;
+extern float plane_thr;
+extern double filter_size_surf_min, filter_size_map_min, fov_deg;
+extern double cube_len;
+extern float DET_RANGE;
+extern bool imu_en, gravity_align, non_station_start;
+extern double imu_time_inte;
+extern double laser_point_cov, acc_norm;
+extern double acc_cov_input, gyr_cov_input, vel_cov;
+extern double gyr_cov_output, acc_cov_output, b_gyr_cov, b_acc_cov;
+extern double imu_meas_acc_cov, imu_meas_omg_cov;
+extern int lidar_type, pcd_save_interval;
+extern std::vector<double> gravity_init, gravity;
+extern std::vector<double> extrinT;
+extern std::vector<double> extrinR;
+extern bool runtime_pos_log, pcd_save_en, path_en;
+extern bool scan_pub_en, scan_body_pub_en;
+extern shared_ptr<Preprocess> p_pre;
+extern double time_lag_imu_to_lidar;
+
+void readParameters(shared_ptr<rclcpp::Node> &nh);
diff --git a/point_lio_ros2/src/preprocess.cpp b/point_lio_ros2/src/preprocess.cpp
new file mode 100644
index 0000000..da017ac
--- /dev/null
+++ b/point_lio_ros2/src/preprocess.cpp
@@ -0,0 +1,732 @@
+#include "preprocess.h"
+
+#define RETURN0     0x00
+#define RETURN0AND1 0x10
+
+Preprocess::Preprocess()
+        : lidar_type(AVIA), blind(0.01), point_filter_num(1) {
+    inf_bound = 10;
+    N_SCANS = 6;
+    SCAN_RATE = 10;
+    group_size = 8;
+    disA = 0.01;
+    disA = 0.1; // B?
+    p2l_ratio = 225;
+    limit_maxmid = 6.25;
+    limit_midmin = 6.25;
+    limit_maxmin = 3.24;
+    jump_up_limit = 170.0;
+    jump_down_limit = 8.0;
+    cos160 = 160.0;
+    edgea = 2;
+    edgeb = 0.1;
+    smallp_intersect = 172.5;
+    smallp_ratio = 1.2;
+    given_offset_time = false;
+
+    jump_up_limit = cos(jump_up_limit / 180 * M_PI);
+    jump_down_limit = cos(jump_down_limit / 180 * M_PI);
+    cos160 = cos(cos160 / 180 * M_PI);
+    smallp_intersect = cos(smallp_intersect / 180 * M_PI);
+}
+
+Preprocess::~Preprocess() {}
+
+void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) {
+    lidar_type = lid_type;
+    blind = bld;
+    point_filter_num = pfilt_num;
+}
+
+// void Preprocess::process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) {
+//     avia_handler(msg);
+//     *pcl_out = pl_surf;
+// }
+
+void Preprocess::process(const sensor_msgs::msg::PointCloud2::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) {
+    switch (time_unit) {
+        case SEC:
+            time_unit_scale = 1.e3f;
+            break;
+        case MS:
+            time_unit_scale = 1.f;
+            break;
+        case US:
+            time_unit_scale = 1.e-3f;
+            break;
+        case NS:
+            time_unit_scale = 1.e-6f;
+            break;
+        default:
+            time_unit_scale = 1.f;
+            break;
+    }
+
+    switch (lidar_type) {
+        case OUST64:
+            oust64_handler(msg);
+            break;
+
+        case VELO16:
+            velodyne_handler(msg);
+            break;
+
+        case HESAIxt32:
+            hesai_handler(msg);
+            break;
+
+        case UNILIDAR:
+            unilidar_handler(msg);
+            break;
+
+        default:
+            printf("Error LiDAR Type");
+            break;
+    }
+    *pcl_out = pl_surf;
+}
+
+// void Preprocess::avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg) {
+//     pl_surf.clear();
+//     pl_corn.clear();
+//     pl_full.clear();
+//     double t1 = omp_get_wtime();
+//     int plsize = msg->point_num;
+
+//     pl_corn.reserve(plsize);
+//     pl_surf.reserve(plsize);
+//     pl_full.resize(plsize);
+
+//     uint valid_num = 0;
+
+//     for (uint i = 1; i < plsize; i++) {
+//         if ((msg->points[i].line < N_SCANS) &&
+//             ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) {
+//             valid_num++;
+//             if (valid_num % point_filter_num == 0) {
+//                 pl_full[i].x = msg->points[i].x;
+//                 pl_full[i].y = msg->points[i].y;
+//                 pl_full[i].z = msg->points[i].z;
+//                 pl_full[i].intensity = msg->points[i].reflectivity;
+//                 pl_full[i].curvature = msg->points[i].offset_time /
+//                                        float(1000000); // use curvature as time of each laser points, curvature unit: ms
+
+//                 if (i == 0) pl_full[i].curvature = fabs(pl_full[i].curvature) < 1.0 ? pl_full[i].curvature : 0.0;
+//                 else pl_full[i].curvature =
+//                              fabs(pl_full[i].curvature - pl_full[i - 1].curvature) < 1.0 ? pl_full[i].curvature :
+//                              pl_full[i - 1].curvature + 0.004166667f;
+
+//                 if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7)
+//                     || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7)
+//                     || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)
+//                        && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z >
+//                            (blind * blind))) {
+//                     pl_surf.push_back(pl_full[i]);
+//                 }
+//             }
+//         }
+//     }
+
+// }
+
+void Preprocess::oust64_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) {
+    pl_surf.clear();
+    pl_corn.clear();
+    pl_full.clear();
+    pcl::PointCloud<ouster_ros::Point> pl_orig;
+    pcl::fromROSMsg(*msg, pl_orig);
+    int plsize = pl_orig.size();
+    pl_corn.reserve(plsize);
+    pl_surf.reserve(plsize);
+
+
+    double time_stamp = rclcpp::Time(msg->header.stamp).seconds();
+    // cout << "===================================" << endl;
+    // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS);
+    for (int i = 0; i < pl_orig.points.size(); i++) {
+        if (i % point_filter_num != 0) continue;
+
+        double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y +
+                       pl_orig.points[i].z * pl_orig.points[i].z;
+
+        if (range < (blind * blind)) continue;
+
+        Eigen::Vector3d pt_vec;
+        PointType added_pt;
+        added_pt.x = pl_orig.points[i].x;
+        added_pt.y = pl_orig.points[i].y;
+        added_pt.z = pl_orig.points[i].z;
+        added_pt.intensity = pl_orig.points[i].intensity;
+        added_pt.normal_x = 0;
+        added_pt.normal_y = 0;
+        added_pt.normal_z = 0;
+        added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms
+
+        pl_surf.points.push_back(added_pt);
+    }
+
+    // pub_func(pl_surf, pub_full, msg->header.stamp);
+    // pub_func(pl_surf, pub_corn, msg->header.stamp);
+}
+
+void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) {
+    pl_surf.clear();
+    pl_corn.clear();
+    pl_full.clear();
+
+    pcl::PointCloud<velodyne_ros::Point> pl_orig;
+    pcl::fromROSMsg(*msg, pl_orig);
+    int plsize = pl_orig.points.size();
+    if (plsize == 0) return;
+    pl_surf.reserve(plsize);
+
+    /*** These variables only works when no point timestamps given ***/
+    double omega_l = 0.361 * SCAN_RATE;       // scan angular velocity
+    std::vector<bool> is_first(N_SCANS, true);
+    std::vector<double> yaw_fp(N_SCANS, 0.0);      // yaw of first scan point
+    std::vector<float> yaw_last(N_SCANS, 0.0);   // yaw of last scan point
+    std::vector<float> time_last(N_SCANS, 0.0);  // last offset time
+    /*****************************************************************/
+
+    if (pl_orig.points[plsize - 1].time > 0) {
+        given_offset_time = true;
+    } else {
+        given_offset_time = false;
+        double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
+        double yaw_end = yaw_first;
+        int layer_first = pl_orig.points[0].ring;
+        for (uint i = plsize - 1; i > 0; i--) {
+            if (pl_orig.points[i].ring == layer_first) {
+                yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
+                break;
+            }
+        }
+    }
+
+    for (int i = 0; i < plsize; i++) {
+        PointType added_pt;
+        // cout<<"!!!!!!"<<i<<" "<<plsize<<endl;
+
+        added_pt.normal_x = 0;
+        added_pt.normal_y = 0;
+        added_pt.normal_z = 0;
+        
+        added_pt.x = pl_orig.points[i].x;
+        added_pt.y = pl_orig.points[i].y;
+        added_pt.z = pl_orig.points[i].z;
+
+        added_pt.intensity = pl_orig.points[i].intensity;
+        added_pt.curvature = pl_orig.points[i].time * time_unit_scale;  // curvature unit: ms // cout<<added_pt.curvature<<endl;
+        if (!given_offset_time) {
+            int layer = pl_orig.points[i].ring;
+            double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
+
+            if (is_first[layer]) {
+                // printf("layer: %d; is first: %d", layer, is_first[layer]);
+                yaw_fp[layer] = yaw_angle;
+                is_first[layer] = false;
+                added_pt.curvature = 0.0;
+                yaw_last[layer] = yaw_angle;
+                time_last[layer] = added_pt.curvature;
+                continue;
+            }
+
+            // compute offset time
+            if (yaw_angle <= yaw_fp[layer]) {
+                added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
+            } else {
+                added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
+            }
+
+            if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l;
+
+            yaw_last[layer] = yaw_angle;
+            time_last[layer] = added_pt.curvature;
+        }
+
+        if (i % point_filter_num == 0) {
+            if (added_pt.x * added_pt.x
+                + added_pt.y * added_pt.y
+                + added_pt.z * added_pt.z > (blind * blind))
+            {
+                pl_surf.points.push_back(added_pt);
+            }
+        }
+    }
+
+}
+
+void Preprocess::unilidar_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg)
+{
+    pl_surf.clear();
+    pl_corn.clear();
+    pl_full.clear();
+
+    pcl::PointCloud<unilidar_ros::Point> pl_orig;
+    pcl::fromROSMsg(*msg, pl_orig);
+    int plsize = pl_orig.points.size();
+    if (plsize == 0) return;
+
+    pl_surf.reserve(plsize);
+
+    // std::cout << "plsize = " << plsize << ", given_offset_time = " << given_offset_time << std::endl;
+    int countElimnated = 0;
+    for (int i = 0; i < plsize; i++)
+    {
+      PointType added_pt;
+      
+      added_pt.normal_x = 0;
+      added_pt.normal_y = 0;
+      added_pt.normal_z = 0;
+
+      added_pt.x = pl_orig.points[i].x;
+      added_pt.y = pl_orig.points[i].y;
+      added_pt.z = pl_orig.points[i].z;
+      
+      added_pt.intensity = pl_orig.points[i].intensity;
+
+      added_pt.curvature = pl_orig.points[i].time * time_unit_scale; 
+
+      if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind))
+      {
+        pl_surf.points.push_back(added_pt);
+      }
+      else
+      {
+        countElimnated++;
+      }
+    }
+
+    // std::cout << "pl_surf.size() = " << pl_surf.size() << ", countElimnated = " << countElimnated << std::endl;
+    
+}
+
+void Preprocess::hesai_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) {
+    pl_surf.clear();
+    pl_corn.clear();
+    pl_full.clear();
+
+    pcl::PointCloud<hesai_ros::Point> pl_orig;
+    pcl::fromROSMsg(*msg, pl_orig);
+    int plsize = pl_orig.points.size();
+    if (plsize == 0) return;
+    pl_surf.reserve(plsize);
+
+    /*** These variables only works when no point timestamps given ***/
+    double omega_l = 0.361 * SCAN_RATE;       // scan angular velocity
+    std::vector<bool> is_first(N_SCANS, true);
+    std::vector<double> yaw_fp(N_SCANS, 0.0);      // yaw of first scan point
+    std::vector<float> yaw_last(N_SCANS, 0.0);   // yaw of last scan point
+    std::vector<float> time_last(N_SCANS, 0.0);  // last offset time
+    /*****************************************************************/
+
+    if (pl_orig.points[plsize - 1].timestamp > 0) {
+        given_offset_time = true;
+    } else {
+        given_offset_time = false;
+        double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
+        double yaw_end = yaw_first;
+        int layer_first = pl_orig.points[0].ring;
+        for (uint i = plsize - 1; i > 0; i--) {
+            if (pl_orig.points[i].ring == layer_first) {
+                yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
+                break;
+            }
+        }
+    }
+
+    double time_head = pl_orig.points[0].timestamp;
+
+    for (int i = 0; i < plsize; i++) {
+        PointType added_pt;
+        // cout<<"!!!!!!"<<i<<" "<<plsize<<endl;
+
+        added_pt.normal_x = 0;
+        added_pt.normal_y = 0;
+        added_pt.normal_z = 0;
+        added_pt.x = pl_orig.points[i].x;
+        added_pt.y = pl_orig.points[i].y;
+        added_pt.z = pl_orig.points[i].z;
+        added_pt.intensity = pl_orig.points[i].intensity;
+        added_pt.curvature = (pl_orig.points[i].timestamp - time_head) *
+                             1000.f; // time_unit_scale;  // curvature unit: ms // cout<<added_pt.curvature<<endl;
+        if (!given_offset_time) {
+            int layer = pl_orig.points[i].ring;
+            double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
+
+            if (is_first[layer]) {
+                // printf("layer: %d; is first: %d", layer, is_first[layer]);
+                yaw_fp[layer] = yaw_angle;
+                is_first[layer] = false;
+                added_pt.curvature = 0.0;
+                yaw_last[layer] = yaw_angle;
+                time_last[layer] = added_pt.curvature;
+                continue;
+            }
+
+            // compute offset time
+            if (yaw_angle <= yaw_fp[layer]) {
+                added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
+            } else {
+                added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
+            }
+
+            if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l;
+
+            yaw_last[layer] = yaw_angle;
+            time_last[layer] = added_pt.curvature;
+        }
+
+        if (i % point_filter_num == 0) {
+            if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind)) {
+                pl_surf.points.push_back(added_pt);
+            }
+        }
+    }
+
+}
+
+void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types) {
+    int plsize = pl.size();
+    int plsize2;
+    if (plsize == 0) {
+        printf("something wrong\n");
+        return;
+    }
+    uint head = 0;
+
+    while (types[head].range < blind) {
+        head++;
+    }
+
+    // Surf
+    plsize2 = (plsize > group_size) ? (plsize - group_size) : 0;
+
+    Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero());
+    Eigen::Vector3d last_direct(Eigen::Vector3d::Zero());
+
+    uint i_nex = 0, i2;
+    uint last_i = 0;
+    uint last_i_nex = 0;
+    int last_state = 0;
+    int plane_type;
+
+    for (uint i = head; i < plsize2; i++) {
+        if (types[i].range < blind) {
+            continue;
+        }
+
+        i2 = i;
+
+        plane_type = plane_judge(pl, types, i, i_nex, curr_direct);
+
+        if (plane_type == 1) {
+            for (uint j = i; j <= i_nex; j++) {
+                if (j != i && j != i_nex) {
+                    types[j].ftype = Real_Plane;
+                } else {
+                    types[j].ftype = Poss_Plane;
+                }
+            }
+
+            // if(last_state==1 && fabs(last_direct.sum())>0.5)
+            if (last_state == 1 && last_direct.norm() > 0.1) {
+                double mod = last_direct.transpose() * curr_direct;
+                if (mod > -0.707 && mod < 0.707) {
+                    types[i].ftype = Edge_Plane;
+                } else {
+                    types[i].ftype = Real_Plane;
+                }
+            }
+
+            i = i_nex - 1;
+            last_state = 1;
+        } else // if(plane_type == 2)
+        {
+            i = i_nex;
+            last_state = 0;
+        }
+
+        last_i = i2;
+        last_i_nex = i_nex;
+        last_direct = curr_direct;
+    }
+
+    plsize2 = plsize > 3 ? plsize - 3 : 0;
+    for (uint i = head + 3; i < plsize2; i++) {
+        if (types[i].range < blind || types[i].ftype >= Real_Plane) {
+            continue;
+        }
+
+        if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16) {
+            continue;
+        }
+
+        Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z);
+        Eigen::Vector3d vecs[2];
+
+        for (int j = 0; j < 2; j++) {
+            int m = -1;
+            if (j == 1) {
+                m = 1;
+            }
+
+            if (types[i + m].range < blind) {
+                if (types[i].range > inf_bound) {
+                    types[i].edj[j] = Nr_inf;
+                } else {
+                    types[i].edj[j] = Nr_blind;
+                }
+                continue;
+            }
+
+            vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z);
+            vecs[j] = vecs[j] - vec_a;
+
+            types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm();
+            if (types[i].angle[j] < jump_up_limit) {
+                types[i].edj[j] = Nr_180;
+            } else if (types[i].angle[j] > jump_down_limit) {
+                types[i].edj[j] = Nr_zero;
+            }
+        }
+
+        types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm();
+        if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 &&
+            types[i].dista > 4 * types[i - 1].dista) {
+            if (types[i].intersect > cos160) {
+                if (edge_jump_judge(pl, types, i, Prev)) {
+                    types[i].ftype = Edge_Jump;
+                }
+            }
+        } else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 &&
+                   types[i - 1].dista > 4 * types[i].dista) {
+            if (types[i].intersect > cos160) {
+                if (edge_jump_judge(pl, types, i, Next)) {
+                    types[i].ftype = Edge_Jump;
+                }
+            }
+        } else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf) {
+            if (edge_jump_judge(pl, types, i, Prev)) {
+                types[i].ftype = Edge_Jump;
+            }
+        } else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor) {
+            if (edge_jump_judge(pl, types, i, Next)) {
+                types[i].ftype = Edge_Jump;
+            }
+
+        } else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor) {
+            if (types[i].ftype == Nor) {
+                types[i].ftype = Wire;
+            }
+        }
+    }
+
+    plsize2 = plsize - 1;
+    double ratio;
+    for (uint i = head + 1; i < plsize2; i++) {
+        if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind) {
+            continue;
+        }
+
+        if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8) {
+            continue;
+        }
+
+        if (types[i].ftype == Nor) {
+            if (types[i - 1].dista > types[i].dista) {
+                ratio = types[i - 1].dista / types[i].dista;
+            } else {
+                ratio = types[i].dista / types[i - 1].dista;
+            }
+
+            if (types[i].intersect < smallp_intersect && ratio < smallp_ratio) {
+                if (types[i - 1].ftype == Nor) {
+                    types[i - 1].ftype = Real_Plane;
+                }
+                if (types[i + 1].ftype == Nor) {
+                    types[i + 1].ftype = Real_Plane;
+                }
+                types[i].ftype = Real_Plane;
+            }
+        }
+    }
+
+    int last_surface = -1;
+    for (uint j = head; j < plsize; j++) {
+        if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane) {
+            if (last_surface == -1) {
+                last_surface = j;
+            }
+
+            if (j == uint(last_surface + point_filter_num - 1)) {
+                PointType ap;
+                ap.x = pl[j].x;
+                ap.y = pl[j].y;
+                ap.z = pl[j].z;
+                ap.intensity = pl[j].intensity;
+                ap.curvature = pl[j].curvature;
+                pl_surf.push_back(ap);
+
+                last_surface = -1;
+            }
+        } else {
+            if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane) {
+                pl_corn.push_back(pl[j]);
+            }
+            if (last_surface != -1) {
+                PointType ap;
+                for (uint k = last_surface; k < j; k++) {
+                    ap.x += pl[k].x;
+                    ap.y += pl[k].y;
+                    ap.z += pl[k].z;
+                    ap.intensity += pl[k].intensity;
+                    ap.curvature += pl[k].curvature;
+                }
+                ap.x /= (j - last_surface);
+                ap.y /= (j - last_surface);
+                ap.z /= (j - last_surface);
+                ap.intensity /= (j - last_surface);
+                ap.curvature /= (j - last_surface);
+                pl_surf.push_back(ap);
+            }
+            last_surface = -1;
+        }
+    }
+}
+
+void Preprocess::pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct) {
+    pl.height = 1;
+    pl.width = pl.size();
+    sensor_msgs::msg::PointCloud2 output;
+    pcl::toROSMsg(pl, output);
+    output.header.frame_id = "livox";
+    output.header.stamp = ct;
+}
+
+int Preprocess::plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex,
+                            Eigen::Vector3d &curr_direct) {
+    double group_dis = disA * types[i_cur].range + disB;
+    group_dis = group_dis * group_dis;
+    // i_nex = i_cur;
+
+    double two_dis;
+    vector<double> disarr;
+    disarr.reserve(20);
+
+    for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++) {
+        if (types[i_nex].range < blind) {
+            curr_direct.setZero();
+            return 2;
+        }
+        disarr.push_back(types[i_nex].dista);
+    }
+
+    for (;;) {
+        if ((i_cur >= pl.size()) || (i_nex >= pl.size())) break;
+
+        if (types[i_nex].range < blind) {
+            curr_direct.setZero();
+            return 2;
+        }
+        vx = pl[i_nex].x - pl[i_cur].x;
+        vy = pl[i_nex].y - pl[i_cur].y;
+        vz = pl[i_nex].z - pl[i_cur].z;
+        two_dis = vx * vx + vy * vy + vz * vz;
+        if (two_dis >= group_dis) {
+            break;
+        }
+        disarr.push_back(types[i_nex].dista);
+        i_nex++;
+    }
+
+    double leng_wid = 0;
+    double v1[3], v2[3];
+    for (uint j = i_cur + 1; j < i_nex; j++) {
+        if ((j >= pl.size()) || (i_cur >= pl.size())) break;
+        v1[0] = pl[j].x - pl[i_cur].x;
+        v1[1] = pl[j].y - pl[i_cur].y;
+        v1[2] = pl[j].z - pl[i_cur].z;
+
+        v2[0] = v1[1] * vz - vy * v1[2];
+        v2[1] = v1[2] * vx - v1[0] * vz;
+        v2[2] = v1[0] * vy - vx * v1[1];
+
+        double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2];
+        if (lw > leng_wid) {
+            leng_wid = lw;
+        }
+    }
+
+
+    if ((two_dis * two_dis / leng_wid) < p2l_ratio) {
+        curr_direct.setZero();
+        return 0;
+    }
+
+    uint disarrsize = disarr.size();
+    for (uint j = 0; j < disarrsize - 1; j++) {
+        for (uint k = j + 1; k < disarrsize; k++) {
+            if (disarr[j] < disarr[k]) {
+                leng_wid = disarr[j];
+                disarr[j] = disarr[k];
+                disarr[k] = leng_wid;
+            }
+        }
+    }
+
+    if (disarr[disarr.size() - 2] < 1e-16) {
+        curr_direct.setZero();
+        return 0;
+    }
+
+    if (lidar_type == AVIA) {
+        double dismax_mid = disarr[0] / disarr[disarrsize / 2];
+        double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2];
+
+        if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin) {
+            curr_direct.setZero();
+            return 0;
+        }
+    } else {
+        double dismax_min = disarr[0] / disarr[disarrsize - 2];
+        if (dismax_min >= limit_maxmin) {
+            curr_direct.setZero();
+            return 0;
+        }
+    }
+
+    curr_direct << vx, vy, vz;
+    curr_direct.normalize();
+    return 1;
+}
+
+bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir) {
+    if (nor_dir == 0) {
+        if (types[i - 1].range < blind || types[i - 2].range < blind) {
+            return false;
+        }
+    } else if (nor_dir == 1) {
+        if (types[i + 1].range < blind || types[i + 2].range < blind) {
+            return false;
+        }
+    }
+    double d1 = types[i + nor_dir - 1].dista;
+    double d2 = types[i + 3 * nor_dir - 2].dista;
+    double d;
+
+    if (d1 < d2) {
+        d = d1;
+        d1 = d2;
+        d2 = d;
+    }
+
+    d1 = sqrt(d1);
+    d2 = sqrt(d2);
+
+    if (d1 > edgea * d2 || (d1 - d2) > edgeb) {
+        return false;
+    }
+
+    return true;
+}
diff --git a/point_lio_ros2/src/preprocess.h b/point_lio_ros2/src/preprocess.h
new file mode 100644
index 0000000..872104f
--- /dev/null
+++ b/point_lio_ros2/src/preprocess.h
@@ -0,0 +1,193 @@
+#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, HESAIxt32, UNILIDAR
+}; //{1, 2, 3, 4}
+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
+};
+
+const bool time_list_cut_frame(PointType &x, PointType &y);
+
+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)
+                                          (std::uint16_t, ring, ring)
+)
+
+/**
+ * @brief Unilidar Point Type
+ */
+namespace unilidar_ros {
+    struct EIGEN_ALIGN16 Point {
+        PCL_ADD_POINT4D
+        PCL_ADD_INTENSITY
+        std::uint16_t ring;
+        float time;
+
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    };
+}  // namespace unilidar_ros
+POINT_CLOUD_REGISTER_POINT_STRUCT(unilidar_ros::Point,
+                                    (float, x, x)
+                                    (float, y, y)
+                                    (float, z, z)
+                                    (float, intensity, intensity)
+                                    (std::uint16_t, ring, ring)
+                                    (float, time, time)
+                                    )
+
+namespace hesai_ros {
+    struct EIGEN_ALIGN16 Point {
+        PCL_ADD_POINT4D;
+        float intensity;
+        double timestamp;
+        uint16_t ring;
+
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    };
+}  // namespace velodyne_ros
+POINT_CLOUD_REGISTER_POINT_STRUCT(hesai_ros::Point,
+                                  (float, x, x)
+                                          (float, y, y)
+                                          (float, z, z)
+                                          (float, intensity, intensity)
+                                          (double, timestamp, timestamp)
+                                          (std::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)
+)
+
+class Preprocess {
+public:
+//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    Preprocess();
+
+    ~Preprocess();
+
+    // void process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
+
+    void process(const sensor_msgs::msg::PointCloud2::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
+
+    void set(bool feat_en, int lid_type, double bld, int pfilt_num);
+
+    // sensor_msgs::msg::PointCloud2::ConstSharedPtr 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 given_offset_time;
+    //ros::Publisher pub_full, pub_surf, pub_corn;
+
+
+private:
+    // void avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg);
+
+    void oust64_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg);
+
+    void velodyne_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg);
+
+    void unilidar_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg);
+
+    void hesai_handler(const sensor_msgs::msg::PointCloud2::SharedPtr &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;
+};
diff --git a/pointcloud_to_laserscan/CHANGELOG.rst b/pointcloud_to_laserscan/CHANGELOG.rst
new file mode 100644
index 0000000..54374c3
--- /dev/null
+++ b/pointcloud_to_laserscan/CHANGELOG.rst
@@ -0,0 +1,95 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package pointcloud_to_laserscan
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.0.1 (2021-10-19)
+------------------
+* Replace deprecated launch api (`#56 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/56>`_)
+* Fix linting errors in the code (`#52 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/52>`_)
+* Update README.md
+* Contributors: Chris Lalancette, Daisuke Nishimatsu, Paul Bovbel
+
+2.0.0 (2020-02-10)
+------------------
+* ROS 2 Migration (`#33 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/33>`_)
+  * ROS 2 Migration
+  Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
+* Contributors: Michel Hidalgo
+
+1.4.1 (2019-08-30)
+------------------
+* LaserScan to PointCloud node + nodelet (`#28 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/28>`_)
+* fix roslint (`#29 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/29>`_)
+* Merge pull request `#20 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/20>`_ from ros-perception/range_max_check
+  Add check for range_max
+* Add check for range_max
+* Contributors: Paul Bovbel, Rein Appeldoorn
+
+1.4.0 (2017-11-14)
+------------------
+* Added inf_epsilon parameter that determines the value added to max range when use_infs parameter is set to false
+* Merge pull request `#11 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/11>`_ from ros-perception/mikaelarguedas-patch-1
+  update to use non deprecated pluginlib macro
+* Migrate to package format 2; add roslint
+* Add sane parameter defaults; fixup lint warnings
+* update to use non deprecated pluginlib macro
+* Contributors: Mikael Arguedas, Paul Bovbel, Prasanna Kannappan
+
+1.3.1 (2017-04-26)
+------------------
+* Merge pull request `#4 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/4>`_ from yoshimalucky/fix-miscalculation-in-angle-increment
+  Fixed miscalculation in angle_increment in the launch files.
+* fixed miscalculation in angle_increment in the launchfiles.
+* Contributors: Paul Bovbel, yoshimalucky
+
+1.3.0 (2015-06-09)
+------------------
+* Fix pointcloud to laserscan transform tolerance issues
+* Move pointcloud_to_laserscan to new repository
+* Contributors: Paul Bovbel
+
+1.2.7 (2015-06-08)
+------------------
+
+* Cleanup pointcloud_to_laserscan launch files
+* Contributors: Paul Bovbel
+
+1.2.6 (2015-02-04)
+------------------
+* Fix default value for concurrency
+* Fix multithreaded lazy pub sub
+* Contributors: Paul Bovbel
+
+1.2.5 (2015-01-20)
+------------------
+* Switch to tf_sensor_msgs for transform
+* Set parameters in sample launch files to default
+* Add tolerance parameter
+* Contributors: Paul Bovbel
+
+1.2.4 (2015-01-15)
+------------------
+* Remove stray dependencies
+* Refactor with tf2 and message filters
+* Remove roslaunch check
+* Fix regressions
+* Refactor to allow debug messages from node and nodelet
+* Contributors: Paul Bovbel
+
+1.2.3 (2015-01-10)
+------------------
+* add launch tests
+* refactor naming and fix nodelet export
+* set default target frame to empty
+* clean up package.xml
+* Contributors: Paul Bovbel
+
+1.2.2 (2014-10-25)
+------------------
+* clean up package.xml
+* Fix header reference
+* Fix flow
+* Fix pointer assertion
+* Finalize pointcloud to laserscan
+* Initial pointcloud to laserscan commit
+* Contributors: Paul Bovbel
diff --git a/pointcloud_to_laserscan/CMakeLists.txt b/pointcloud_to_laserscan/CMakeLists.txt
new file mode 100644
index 0000000..b96516b
--- /dev/null
+++ b/pointcloud_to_laserscan/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+project(pointcloud_to_laserscan)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(laserscan_to_pointcloud SHARED
+  src/laserscan_to_pointcloud_node.cpp)
+
+rclcpp_components_register_node(laserscan_to_pointcloud
+  PLUGIN "pointcloud_to_laserscan::LaserScanToPointCloudNode"
+  EXECUTABLE laserscan_to_pointcloud_node)
+
+ament_auto_add_library(pointcloud_to_laserscan SHARED
+  src/pointcloud_to_laserscan_node.cpp)
+
+rclcpp_components_register_node(pointcloud_to_laserscan
+  PLUGIN "pointcloud_to_laserscan::PointCloudToLaserScanNode"
+  EXECUTABLE pointcloud_to_laserscan_node)
+
+ament_auto_add_executable(dummy_pointcloud_publisher
+  src/dummy_pointcloud_publisher.cpp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+)
diff --git a/pointcloud_to_laserscan/LICENSE b/pointcloud_to_laserscan/LICENSE
new file mode 100644
index 0000000..f05ef04
--- /dev/null
+++ b/pointcloud_to_laserscan/LICENSE
@@ -0,0 +1,12 @@
+Copyright (c) 2010-2012, Willow Garage, Inc.
+Copyright (c) 2019, Eurotec, Netherlands
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+2. 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.
+
+3. Neither the name of the copyright holder 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 HOLDER 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.
diff --git a/pointcloud_to_laserscan/README.md b/pointcloud_to_laserscan/README.md
new file mode 100644
index 0000000..dd8db4d
--- /dev/null
+++ b/pointcloud_to_laserscan/README.md
@@ -0,0 +1,49 @@
+# ROS 2 pointcloud <-> laserscan converters
+
+This is a ROS 2 package that provides components to convert `sensor_msgs/msg/PointCloud2` messages to `sensor_msgs/msg/LaserScan` messages and back.
+It is essentially a port of the original ROS 1 package.
+
+## pointcloud\_to\_laserscan::PointCloudToLaserScanNode
+
+This ROS 2 component projects `sensor_msgs/msg/PointCloud2` messages into `sensor_msgs/msg/LaserScan` messages.
+
+### Published Topics
+
+* `scan` (`sensor_msgs/msg/LaserScan`) - The output laser scan.
+
+### Subscribed Topics
+
+* `cloud_in` (`sensor_msgs/msg/PointCloud2`) - The input point cloud. No input will be processed if there isn't at least one subscriber to the `scan` topic.
+
+### Parameters
+
+* `min_height` (double, default: 2.2e-308) - The minimum height to sample in the point cloud in meters.
+* `max_height` (double, default: 1.8e+308) - The maximum height to sample in the point cloud in meters.
+* `angle_min` (double, default: -π) - The minimum scan angle in radians.
+* `angle_max` (double, default: π) - The maximum scan angle in radians.
+* `angle_increment` (double, default: π/180) - Resolution of laser scan in radians per ray.
+* `queue_size` (double, default: detected number of cores) - Input point cloud queue size.
+* `scan_time` (double, default: 1.0/30.0) - The scan rate in seconds. Only used to populate the scan_time field of the output laser scan message.
+* `range_min` (double, default: 0.0) - The minimum ranges to return in meters.
+* `range_max` (double, default: 1.8e+308) - The maximum ranges to return in meters.
+* `target_frame` (str, default: none) - If provided, transform the pointcloud into this frame before converting to a laser scan. Otherwise, laser scan will be generated in the same frame as the input point cloud.
+* `transform_tolerance` (double, default: 0.01) - Time tolerance for transform lookups. Only used if a `target_frame` is provided.
+* `use_inf` (boolean, default: true) - If disabled, report infinite range (no obstacle) as range_max + 1. Otherwise report infinite range as +inf.
+
+## pointcloud\_to\_laserscan::LaserScanToPointCloudNode
+
+This ROS 2 component re-publishes `sensor_msgs/msg/LaserScan` messages as `sensor_msgs/msg/PointCloud2` messages.
+
+### Published Topics
+
+* `cloud` (`sensor_msgs/msg/PointCloud2`) - The output point cloud.
+
+### Subscribed Topics
+
+* `scan_in` (`sensor_msgs/msg/LaserScan`) - The input laser scan. No input will be processed if there isn't at least one subscriber to the `cloud` topic.
+
+### Parameters
+
+* `queue_size` (double, default: detected number of cores) - Input laser scan queue size.
+* `target_frame` (str, default: none) - If provided, transform the pointcloud into this frame before converting to a laser scan. Otherwise, laser scan will be generated in the same frame as the input point cloud.
+* `transform_tolerance` (double, default: 0.01) - Time tolerance for transform lookups. Only used if a `target_frame` is provided.
diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/laserscan_to_pointcloud_node.hpp b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/laserscan_to_pointcloud_node.hpp
new file mode 100644
index 0000000..8948af9
--- /dev/null
+++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/laserscan_to_pointcloud_node.hpp
@@ -0,0 +1,98 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Eurotec, Netherlands
+ *  All rights reserved.
+ *
+ *  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 Willow Garage, Inc. 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.
+ *
+ *
+ */
+
+/*
+ * Author: Rein Appeldoorn
+ */
+
+#ifndef POINTCLOUD_TO_LASERSCAN__LASERSCAN_TO_POINTCLOUD_NODE_HPP_
+#define POINTCLOUD_TO_LASERSCAN__LASERSCAN_TO_POINTCLOUD_NODE_HPP_
+
+#include <atomic>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include "message_filters/subscriber.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/message_filter.h"
+#include "tf2_ros/transform_listener.h"
+
+#include "laser_geometry/laser_geometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+
+#include "pointcloud_to_laserscan/visibility_control.h"
+
+namespace pointcloud_to_laserscan
+{
+typedef tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan> MessageFilter;
+
+//! \brief The PointCloudToLaserScanNodelet class to process incoming laserscans into pointclouds.
+//!
+class LaserScanToPointCloudNode : public rclcpp::Node
+{
+public:
+  POINTCLOUD_TO_LASERSCAN_PUBLIC
+  explicit LaserScanToPointCloudNode(const rclcpp::NodeOptions & options);
+
+  ~LaserScanToPointCloudNode() override;
+
+private:
+  void scanCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg);
+
+  void subscriptionListenerThreadLoop();
+
+  std::unique_ptr<tf2_ros::Buffer> tf2_;
+  std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
+  message_filters::Subscriber<sensor_msgs::msg::LaserScan> sub_;
+  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_;
+  std::unique_ptr<MessageFilter> message_filter_;
+  std::thread subscription_listener_thread_;
+  std::atomic_bool alive_{true};
+
+  laser_geometry::LaserProjection projector_;
+
+  // ROS Parameters
+  int input_queue_size_;
+  std::string target_frame_;
+  double tolerance_;
+};
+
+}  // namespace pointcloud_to_laserscan
+
+#endif  // POINTCLOUD_TO_LASERSCAN__LASERSCAN_TO_POINTCLOUD_NODE_HPP_
diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_node.hpp b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_node.hpp
new file mode 100644
index 0000000..fe0aacd
--- /dev/null
+++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_node.hpp
@@ -0,0 +1,102 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  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 Willow Garage, Inc. 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.
+ *
+ *
+ */
+
+/*
+ * Author: Paul Bovbel
+ */
+
+#ifndef POINTCLOUD_TO_LASERSCAN__POINTCLOUD_TO_LASERSCAN_NODE_HPP_
+#define POINTCLOUD_TO_LASERSCAN__POINTCLOUD_TO_LASERSCAN_NODE_HPP_
+
+#include <atomic>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include "message_filters/subscriber.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/message_filter.h"
+#include "tf2_ros/transform_listener.h"
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+
+#include "pointcloud_to_laserscan/visibility_control.h"
+
+namespace pointcloud_to_laserscan
+{
+typedef tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2> MessageFilter;
+
+/**
+* Class to process incoming pointclouds into laserscans.
+* Some initial code was pulled from the defunct turtlebot pointcloud_to_laserscan implementation.
+*/
+class PointCloudToLaserScanNode : public rclcpp::Node
+{
+public:
+  POINTCLOUD_TO_LASERSCAN_PUBLIC
+  explicit PointCloudToLaserScanNode(const rclcpp::NodeOptions & options);
+
+  ~PointCloudToLaserScanNode() override;
+
+private:
+  void cloudCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg);
+
+  void subscriptionListenerThreadLoop();
+
+  std::unique_ptr<tf2_ros::Buffer> tf2_;
+  std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
+  message_filters::Subscriber<sensor_msgs::msg::PointCloud2> sub_;
+  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::LaserScan>> pub_;
+  std::unique_ptr<MessageFilter> message_filter_;
+
+  std::thread subscription_listener_thread_;
+  std::atomic_bool alive_{true};
+
+  // ROS Parameters
+  int input_queue_size_;
+  std::string target_frame_;
+  double tolerance_;
+  double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_,
+    range_max_;
+  bool use_inf_;
+  double inf_epsilon_;
+};
+
+}  // namespace pointcloud_to_laserscan
+
+#endif  // POINTCLOUD_TO_LASERSCAN__POINTCLOUD_TO_LASERSCAN_NODE_HPP_
diff --git a/pointcloud_to_laserscan/include/pointcloud_to_laserscan/visibility_control.h b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/visibility_control.h
new file mode 100644
index 0000000..7058490
--- /dev/null
+++ b/pointcloud_to_laserscan/include/pointcloud_to_laserscan/visibility_control.h
@@ -0,0 +1,80 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Eurotec, Netherlands
+ *  All rights reserved.
+ *
+ *  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 Willow Garage, Inc. 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 POINTCLOUD_TO_LASERSCAN__VISIBILITY_CONTROL_H_
+#define POINTCLOUD_TO_LASERSCAN__VISIBILITY_CONTROL_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define POINTCLOUD_TO_LASERSCAN_EXPORT __attribute__ ((dllexport))
+    #define POINTCLOUD_TO_LASERSCAN_IMPORT __attribute__ ((dllimport))
+  #else
+    #define POINTCLOUD_TO_LASERSCAN_EXPORT __declspec(dllexport)
+    #define POINTCLOUD_TO_LASERSCAN_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef POINTCLOUD_TO_LASERSCAN_BUILDING_DLL
+    #define POINTCLOUD_TO_LASERSCAN_PUBLIC POINTCLOUD_TO_LASERSCAN_EXPORT
+  #else
+    #define POINTCLOUD_TO_LASERSCAN_PUBLIC POINTCLOUD_TO_LASERSCAN_IMPORT
+  #endif
+  #define POINTCLOUD_TO_LASERSCAN_PUBLIC_TYPE POINTCLOUD_TO_LASERSCAN_PUBLIC
+  #define POINTCLOUD_TO_LASERSCAN_LOCAL
+#else
+  #define POINTCLOUD_TO_LASERSCAN_EXPORT __attribute__ ((visibility("default")))
+  #define POINTCLOUD_TO_LASERSCAN_IMPORT
+  #if __GNUC__ >= 4
+    #define POINTCLOUD_TO_LASERSCAN_PUBLIC __attribute__ ((visibility("default")))
+    #define POINTCLOUD_TO_LASERSCAN_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define POINTCLOUD_TO_LASERSCAN_PUBLIC
+    #define POINTCLOUD_TO_LASERSCAN_LOCAL
+  #endif
+  #define POINTCLOUD_TO_LASERSCAN_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // POINTCLOUD_TO_LASERSCAN__VISIBILITY_CONTROL_H_
diff --git a/pointcloud_to_laserscan/launch/pointcloud_to_laserscan_launch.py b/pointcloud_to_laserscan/launch/pointcloud_to_laserscan_launch.py
new file mode 100644
index 0000000..4be5e32
--- /dev/null
+++ b/pointcloud_to_laserscan/launch/pointcloud_to_laserscan_launch.py
@@ -0,0 +1,43 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+
+        DeclareLaunchArgument(
+            name='scanner', default_value='unilidar',
+            description='Namespace for Unitree LiDAR topics, used to organize topic names'
+        ),
+
+        # Make sure the Unitree LiDAR node is already running
+        # and publishing point cloud data to the /unilidar/cloud topic
+
+        # ========== Unitree PointCloud to LaserScan Converter ==========
+        # Convert Unitree LiDAR 3D point cloud data into 2D LaserScan data
+        Node(
+            package='pointcloud_to_laserscan', 
+            executable='pointcloud_to_laserscan_node',
+
+            remappings=[
+                ('cloud_in', '/unilidar/cloud'),  # Input: Unitree LiDAR point cloud topic
+                ('scan', '/scan')                 # Output: standard LaserScan topic
+            ],
+            parameters=[{
+                'min_height': 0.35,               # Minimum height: filter points below this height
+                'max_height': 0.45,               # Maximum height: keep obstacles, filter ceiling points
+                'angle_min': -3.14159,            # Minimum scan angle: -π radians (-180 degrees)
+                'angle_max': 3.14159,             # Maximum scan angle:  π radians (180 degrees)
+                'angle_increment': 0.00436,       # Angular resolution: π/720 radians (~0.25 degrees)
+                'scan_time': 0.1,                 # Scan time: 0.1s (10 Hz publishing rate, suitable for navigation)
+                'range_min': 0.1,                 # Minimum valid range: avoid self-reflections
+                'range_max': 100.0,               # Maximum valid range: Unitree LiDAR effective range
+                'use_inf': True,                  # Use infinity for missing readings (LaserScan standard)
+                'inf_epsilon': 1.0                # Epsilon value for infinity handling
+            }],
+            name='unitree_pointcloud_to_laserscan'  # Node name
+        )
+    ])
+
diff --git a/pointcloud_to_laserscan/launch/sample_laserscan_to_pointcloud_launch.py b/pointcloud_to_laserscan/launch/sample_laserscan_to_pointcloud_launch.py
new file mode 100644
index 0000000..2f711d7
--- /dev/null
+++ b/pointcloud_to_laserscan/launch/sample_laserscan_to_pointcloud_launch.py
@@ -0,0 +1,47 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import ExecuteProcess
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+import yaml
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            name='scanner', default_value='scanner',
+            description='Namespace for sample topics'
+        ),
+        ExecuteProcess(
+            cmd=[
+                'ros2', 'topic', 'pub', '-r', '10',
+                '--qos-profile', 'sensor_data',
+                [LaunchConfiguration(variable_name='scanner'), '/scan'],
+                'sensor_msgs/msg/LaserScan', yaml.dump({
+                    'header': {'frame_id': 'scan'}, 'angle_min': -1.0,
+                    'angle_max': 1.0, 'angle_increment': 0.1, 'range_max': 10.0,
+                    'ranges': [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
+                })
+            ],
+            name='scan_publisher'
+        ),
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            name='static_transform_publisher',
+            arguments=[
+                '--x', '0', '--y', '0', '--z', '0',
+                '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1',
+                '--frame-id', 'map', '--child-frame-id', 'scan'
+            ]
+        ),
+        Node(
+            package='pointcloud_to_laserscan',
+            executable='laserscan_to_pointcloud_node',
+            name='laserscan_to_pointcloud',
+            remappings=[('scan_in', [LaunchConfiguration(variable_name='scanner'), '/scan']),
+                        ('cloud', [LaunchConfiguration(variable_name='scanner'), '/cloud'])],
+            parameters=[{'target_frame': 'scan', 'transform_tolerance': 0.01}]
+        ),
+    ])
diff --git a/pointcloud_to_laserscan/launch/sample_pointcloud_to_laserscan_launch.py b/pointcloud_to_laserscan/launch/sample_pointcloud_to_laserscan_launch.py
new file mode 100644
index 0000000..b8ac473
--- /dev/null
+++ b/pointcloud_to_laserscan/launch/sample_pointcloud_to_laserscan_launch.py
@@ -0,0 +1,49 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            name='scanner', default_value='scanner',
+            description='Namespace for sample topics'
+        ),
+        Node(
+            package='pointcloud_to_laserscan', executable='dummy_pointcloud_publisher',
+            remappings=[('cloud', [LaunchConfiguration(variable_name='scanner'), '/cloud'])],
+            parameters=[{'cloud_frame_id': 'cloud', 'cloud_extent': 2.0, 'cloud_size': 500}],
+            name='cloud_publisher'
+        ),
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            name='static_transform_publisher',
+            arguments=[
+                '--x', '0', '--y', '0', '--z', '0',
+                '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1',
+                '--frame-id', 'map', '--child-frame-id', 'cloud'
+            ]
+        ),
+        Node(
+            package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node',
+            remappings=[('cloud_in', [LaunchConfiguration(variable_name='scanner'), '/cloud']),
+                        ('scan', [LaunchConfiguration(variable_name='scanner'), '/scan'])],
+            parameters=[{
+                'target_frame': 'cloud',
+                'transform_tolerance': 0.01,
+                'min_height': 0.0,
+                'max_height': 1.0,
+                'angle_min': -1.5708,  # -M_PI/2
+                'angle_max': 1.5708,  # M_PI/2
+                'angle_increment': 0.0087,  # M_PI/360.0
+                'scan_time': 0.3333,
+                'range_min': 0.45,
+                'range_max': 4.0,
+                'use_inf': True,
+                'inf_epsilon': 1.0
+            }],
+            name='pointcloud_to_laserscan'
+        )
+    ])
diff --git a/pointcloud_to_laserscan/package.xml b/pointcloud_to_laserscan/package.xml
new file mode 100644
index 0000000..3d9ec2f
--- /dev/null
+++ b/pointcloud_to_laserscan/package.xml
@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>pointcloud_to_laserscan</name>
+    <version>2.0.1</version>
+    <description>Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM).</description>
+
+    <maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
+    <maintainer email="michel@ekumenlabs.com">Michel Hidalgo</maintainer>
+    <author email="paul@bovbel.com">Paul Bovbel</author>
+    <author email="michel@ekumenlabs.com">Michel Hidalgo</author>
+    <author>Tully Foote</author>
+
+    <license>BSD</license>
+
+    <url type="website">http://ros.org/wiki/perception_pcl</url>
+    <url type="bugtracker">https://github.com/ros-perception/perception_pcl/issues</url>
+    <url type="repository">https://github.com/ros-perception/perception_pcl</url>
+
+    <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+    <depend>laser_geometry</depend>
+    <depend>message_filters</depend>
+    <depend>rclcpp</depend>
+    <depend>rclcpp_components</depend>
+    <depend>sensor_msgs</depend>
+    <depend>tf2</depend>
+    <depend>tf2_ros</depend>
+    <depend>tf2_sensor_msgs</depend>
+
+    <exec_depend>launch</exec_depend>
+    <exec_depend>launch_ros</exec_depend>
+
+    <test_depend>ament_lint_auto</test_depend>
+    <!-- <exec_depend>ament_cmake_copyright</exec_depend> -->
+    <test_depend>ament_cmake_cppcheck</test_depend>
+    <test_depend>ament_cmake_cpplint</test_depend>
+    <test_depend>ament_cmake_flake8</test_depend>
+    <test_depend>ament_cmake_lint_cmake</test_depend>
+    <test_depend>ament_cmake_pep257</test_depend>
+    <test_depend>ament_cmake_uncrustify</test_depend>
+    <test_depend>ament_cmake_xmllint</test_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>
diff --git a/pointcloud_to_laserscan/src/dummy_pointcloud_publisher.cpp b/pointcloud_to_laserscan/src/dummy_pointcloud_publisher.cpp
new file mode 100644
index 0000000..c4b7349
--- /dev/null
+++ b/pointcloud_to_laserscan/src/dummy_pointcloud_publisher.cpp
@@ -0,0 +1,85 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, Open Source Robotics Foundation, Inc.
+ *  All rights reserved.
+ *
+ *  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 Willow Garage, Inc. 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.
+ *
+ *
+ */
+
+#include <memory>
+#include <random>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  auto node = std::make_shared<rclcpp::Node>("dummy_pointcloud_publisher");
+  auto pub =
+    node->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::SensorDataQoS());
+
+  sensor_msgs::msg::PointCloud2 dummy_cloud;
+  sensor_msgs::PointCloud2Modifier modifier(dummy_cloud);
+  modifier.setPointCloud2Fields(
+    3,
+    "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+    "z", 1, sensor_msgs::msg::PointField::FLOAT32);
+  modifier.resize(node->declare_parameter("cloud_size", 100));
+  std::mt19937 gen(node->declare_parameter("cloud_seed", 0));
+  double extent = node->declare_parameter("cloud_extent", 10.0);
+  std::uniform_real_distribution<float> distribution(-extent / 2, extent / 2);
+  sensor_msgs::PointCloud2Iterator<float> it_x(dummy_cloud, "x");
+  sensor_msgs::PointCloud2Iterator<float> it_y(dummy_cloud, "y");
+  sensor_msgs::PointCloud2Iterator<float> it_z(dummy_cloud, "z");
+  for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) {
+    *it_x = distribution(gen);
+    *it_y = distribution(gen);
+    *it_z = distribution(gen);
+  }
+  dummy_cloud.header.frame_id = node->declare_parameter("cloud_frame_id", "");
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+  rclcpp::Rate rate(1.0);
+  while (rclcpp::ok()) {
+    dummy_cloud.header.stamp = node->get_clock()->now();
+    pub->publish(dummy_cloud);
+    executor.spin_some();
+    rate.sleep();
+  }
+
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/pointcloud_to_laserscan/src/laserscan_to_pointcloud_node.cpp b/pointcloud_to_laserscan/src/laserscan_to_pointcloud_node.cpp
new file mode 100644
index 0000000..2d2726f
--- /dev/null
+++ b/pointcloud_to_laserscan/src/laserscan_to_pointcloud_node.cpp
@@ -0,0 +1,150 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Eurotec, Netherlands
+ *  All rights reserved.
+ *
+ *  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 Willow Garage, Inc. 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.
+ *
+ *
+ */
+
+/*
+ * Author: Rein Appeldoorn
+ */
+
+#include "pointcloud_to_laserscan/laserscan_to_pointcloud_node.hpp"
+
+#include <chrono>
+#include <functional>
+#include <limits>
+#include <memory>
+#include <string>
+#include <thread>
+#include <utility>
+
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
+#include "tf2_ros/create_timer_ros.h"
+
+namespace pointcloud_to_laserscan
+{
+
+LaserScanToPointCloudNode::LaserScanToPointCloudNode(const rclcpp::NodeOptions & options)
+: rclcpp::Node("laserscan_to_pointcloud", options)
+{
+  target_frame_ = this->declare_parameter("target_frame", "");
+  tolerance_ = this->declare_parameter("transform_tolerance", 0.01);
+  // TODO(hidmic): adjust default input queue size based on actual concurrency levels
+  // achievable by the associated executor
+  input_queue_size_ = this->declare_parameter(
+    "queue_size", static_cast<int>(std::thread::hardware_concurrency()));
+
+  pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::SensorDataQoS());
+
+  using std::placeholders::_1;
+  // if pointcloud target frame specified, we need to filter by transform availability
+  if (!target_frame_.empty()) {
+    tf2_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+      this->get_node_base_interface(), this->get_node_timers_interface());
+    tf2_->setCreateTimerInterface(timer_interface);
+    tf2_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf2_);
+    message_filter_ = std::make_unique<MessageFilter>(
+      sub_, *tf2_, target_frame_, input_queue_size_,
+      this->get_node_logging_interface(),
+      this->get_node_clock_interface());
+    message_filter_->registerCallback(
+      std::bind(
+        &LaserScanToPointCloudNode::scanCallback, this, _1));
+  } else {  // otherwise setup direct subscription
+    sub_.registerCallback(std::bind(&LaserScanToPointCloudNode::scanCallback, this, _1));
+  }
+
+  subscription_listener_thread_ = std::thread(
+    std::bind(&LaserScanToPointCloudNode::subscriptionListenerThreadLoop, this));
+}
+
+LaserScanToPointCloudNode::~LaserScanToPointCloudNode()
+{
+  alive_.store(true);
+  subscription_listener_thread_.join();
+}
+
+void LaserScanToPointCloudNode::subscriptionListenerThreadLoop()
+{
+  rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
+
+  const std::chrono::milliseconds timeout(100);
+  while (rclcpp::ok(context) && alive_.load()) {
+    int subscription_count = pub_->get_subscription_count() +
+      pub_->get_intra_process_subscription_count();
+    if (subscription_count > 0) {
+      if (!sub_.getSubscriber()) {
+        RCLCPP_INFO(
+          this->get_logger(),
+          "Got a subscriber to pointcloud, starting laserscan subscriber");
+        rclcpp::SensorDataQoS qos;
+        qos.keep_last(input_queue_size_);
+        sub_.subscribe(this, "scan_in", qos.get_rmw_qos_profile());
+      }
+    } else if (sub_.getSubscriber()) {
+      RCLCPP_INFO(
+        this->get_logger(),
+        "No subscribers to pointcloud, shutting down laserscan subscriber");
+      sub_.unsubscribe();
+    }
+    rclcpp::Event::SharedPtr event = this->get_graph_event();
+    this->wait_for_graph_change(event, timeout);
+  }
+  sub_.unsubscribe();
+}
+
+void LaserScanToPointCloudNode::scanCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg)
+{
+  auto cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
+
+  projector_.projectLaser(*scan_msg, *cloud_msg);
+
+  // Transform cloud if necessary
+  if (!target_frame_.empty() && cloud_msg->header.frame_id != target_frame_) {
+    try {
+      *cloud_msg = tf2_->transform(*cloud_msg, target_frame_, tf2::durationFromSec(tolerance_));
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_ERROR_STREAM(this->get_logger(), "Transform failure: " << ex.what());
+      return;
+    }
+  }
+  pub_->publish(std::move(cloud_msg));
+}
+
+}  // namespace pointcloud_to_laserscan
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_to_laserscan::LaserScanToPointCloudNode)
diff --git a/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp
new file mode 100644
index 0000000..b95f1b5
--- /dev/null
+++ b/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp
@@ -0,0 +1,242 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  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 Willow Garage, Inc. 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.
+ *
+ *
+ */
+
+/*
+ * Author: Paul Bovbel
+ */
+
+#include "pointcloud_to_laserscan/pointcloud_to_laserscan_node.hpp"
+
+#include <chrono>
+#include <functional>
+#include <limits>
+#include <memory>
+#include <string>
+#include <thread>
+#include <utility>
+
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
+#include "tf2_ros/create_timer_ros.h"
+#include "rmw/qos_profiles.h"
+
+namespace pointcloud_to_laserscan
+{
+
+PointCloudToLaserScanNode::PointCloudToLaserScanNode(const rclcpp::NodeOptions & options)
+: rclcpp::Node("pointcloud_to_laserscan", options)
+{
+  target_frame_ = this->declare_parameter("target_frame", "");
+  tolerance_ = this->declare_parameter("transform_tolerance", 0.01);
+  // TODO(hidmic): adjust default input queue size based on actual concurrency levels
+  // achievable by the associated executor
+  input_queue_size_ = this->declare_parameter(
+    "queue_size", static_cast<int>(std::thread::hardware_concurrency()));
+  min_height_ = this->declare_parameter("min_height", std::numeric_limits<double>::min());
+  max_height_ = this->declare_parameter("max_height", std::numeric_limits<double>::max());
+  angle_min_ = this->declare_parameter("angle_min", -M_PI);
+  angle_max_ = this->declare_parameter("angle_max", M_PI);
+  angle_increment_ = this->declare_parameter("angle_increment", M_PI / 180.0);
+  scan_time_ = this->declare_parameter("scan_time", 1.0 / 30.0);
+  range_min_ = this->declare_parameter("range_min", 0.0);
+  range_max_ = this->declare_parameter("range_max", std::numeric_limits<double>::max());
+  inf_epsilon_ = this->declare_parameter("inf_epsilon", 1.0);
+  use_inf_ = this->declare_parameter("use_inf", true);
+
+  // Create a laser scan publisher, using BEST_EFFORT QoS to accommodate more subscribers.
+  rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
+  qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+  pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", qos_profile);
+
+  using std::placeholders::_1;
+  // if pointcloud target frame specified, we need to filter by transform availability
+  if (!target_frame_.empty()) {
+    tf2_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+      this->get_node_base_interface(), this->get_node_timers_interface());
+    tf2_->setCreateTimerInterface(timer_interface);
+    tf2_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf2_);
+    message_filter_ = std::make_unique<MessageFilter>(
+      sub_, *tf2_, target_frame_, input_queue_size_,
+      this->get_node_logging_interface(),
+      this->get_node_clock_interface());
+    message_filter_->registerCallback(
+      std::bind(&PointCloudToLaserScanNode::cloudCallback, this, _1));
+  } else {  // otherwise setup direct subscription
+    sub_.registerCallback(std::bind(&PointCloudToLaserScanNode::cloudCallback, this, _1));
+  }
+
+  subscription_listener_thread_ = std::thread(
+    std::bind(&PointCloudToLaserScanNode::subscriptionListenerThreadLoop, this));
+}
+
+PointCloudToLaserScanNode::~PointCloudToLaserScanNode()
+{
+  alive_.store(false);
+  subscription_listener_thread_.join();
+}
+
+void PointCloudToLaserScanNode::subscriptionListenerThreadLoop()
+{
+  rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
+
+  const std::chrono::milliseconds timeout(100);
+  while (rclcpp::ok(context) && alive_.load()) {
+    int subscription_count = pub_->get_subscription_count() +
+      pub_->get_intra_process_subscription_count();
+    if (subscription_count > 0) {
+      if (!sub_.getSubscriber()) {
+        RCLCPP_INFO(
+          this->get_logger(),
+          "Got a subscriber to laserscan, starting pointcloud subscriber");
+        rclcpp::SensorDataQoS qos;
+        qos.keep_last(input_queue_size_);
+        sub_.subscribe(this, "cloud_in", qos.get_rmw_qos_profile());
+      }
+    } else if (sub_.getSubscriber()) {
+      RCLCPP_INFO(
+        this->get_logger(),
+        "No subscribers to laserscan, shutting down pointcloud subscriber");
+      sub_.unsubscribe();
+    }
+    rclcpp::Event::SharedPtr event = this->get_graph_event();
+    this->wait_for_graph_change(event, timeout);
+  }
+  sub_.unsubscribe();
+}
+
+void PointCloudToLaserScanNode::cloudCallback(
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg)
+{
+  // build laserscan output
+  auto scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
+  scan_msg->header = cloud_msg->header;
+  if (!target_frame_.empty()) {
+    scan_msg->header.frame_id = target_frame_;
+  }
+
+  scan_msg->angle_min = angle_min_;
+  scan_msg->angle_max = angle_max_;
+  scan_msg->angle_increment = angle_increment_;
+  scan_msg->time_increment = 0.0;
+  scan_msg->scan_time = scan_time_;
+  scan_msg->range_min = range_min_;
+  scan_msg->range_max = range_max_;
+
+  // determine amount of rays to create
+  uint32_t ranges_size = std::ceil(
+    (scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
+
+  // determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
+  if (use_inf_) {
+    scan_msg->ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
+  } else {
+    scan_msg->ranges.assign(ranges_size, scan_msg->range_max + inf_epsilon_);
+  }
+
+  // Transform cloud if necessary
+  if (scan_msg->header.frame_id != cloud_msg->header.frame_id) {
+    try {
+      auto cloud = std::make_shared<sensor_msgs::msg::PointCloud2>();
+      tf2_->transform(*cloud_msg, *cloud, target_frame_, tf2::durationFromSec(tolerance_));
+      cloud_msg = cloud;
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_ERROR_STREAM(this->get_logger(), "Transform failure: " << ex.what());
+      return;
+    }
+  }
+
+  // Iterate through pointcloud
+  for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_msg, "x"),
+    iter_y(*cloud_msg, "y"), iter_z(*cloud_msg, "z");
+    iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
+  {
+    if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z)) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "rejected for nan in point(%f, %f, %f)\n",
+        *iter_x, *iter_y, *iter_z);
+      continue;
+    }
+
+    if (*iter_z > max_height_ || *iter_z < min_height_) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "rejected for height %f not in range (%f, %f)\n",
+        *iter_z, min_height_, max_height_);
+      continue;
+    }
+
+    double range = hypot(*iter_x, *iter_y);
+    if (range < range_min_) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "rejected for range %f below minimum value %f. Point: (%f, %f, %f)",
+        range, range_min_, *iter_x, *iter_y, *iter_z);
+      continue;
+    }
+    if (range > range_max_) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "rejected for range %f above maximum value %f. Point: (%f, %f, %f)",
+        range, range_max_, *iter_x, *iter_y, *iter_z);
+      continue;
+    }
+
+    double angle = atan2(*iter_y, *iter_x);
+    if (angle < scan_msg->angle_min || angle > scan_msg->angle_max) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "rejected for angle %f not in range (%f, %f)\n",
+        angle, scan_msg->angle_min, scan_msg->angle_max);
+      continue;
+    }
+
+    // overwrite range at laserscan ray if new range is smaller
+    int index = (angle - scan_msg->angle_min) / scan_msg->angle_increment;
+    if (range < scan_msg->ranges[index]) {
+      scan_msg->ranges[index] = range;
+    }
+  }
+  pub_->publish(std::move(scan_msg));
+}
+
+}  // namespace pointcloud_to_laserscan
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_to_laserscan::PointCloudToLaserScanNode)
+
diff --git a/unitree_lidar_ros2/CMakeLists.txt b/unitree_lidar_ros2/CMakeLists.txt
new file mode 100644
index 0000000..da14495
--- /dev/null
+++ b/unitree_lidar_ros2/CMakeLists.txt
@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 3.5)
+project(unitree_lidar_ros2)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(PCL REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+include_directories(
+  ${PCL_INCLUDE_DIRS} 
+  include
+  ../unitree_lidar_sdk/include
+)
+
+link_directories(
+  ${PCL_LIBRARY_DIRS}
+  ../unitree_lidar_sdk/lib/${CMAKE_SYSTEM_PROCESSOR}
+)
+
+add_definitions(${PCL_DEFINITIONS})
+
+add_executable(unitree_lidar_ros2_node src/unitree_lidar_ros2_node.cpp)
+
+target_link_libraries( unitree_lidar_ros2_node
+  ${Boost_SYSTEM_LIBRARY}
+  ${PCL_LIBRARIES}
+  unilidar_sdk2
+)
+
+ament_target_dependencies(
+  unitree_lidar_ros2_node
+  rclcpp std_msgs 
+  sensor_msgs
+  geometry_msgs 
+  tf2_ros
+  pcl_conversions
+)
+
+install(TARGETS
+unitree_lidar_ros2_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# install rviz file and launch file
+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
+install(DIRECTORY rviz/
+  DESTINATION share/${PROJECT_NAME}/rviz
+)
+
+# install config files if they exist
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config")
+  install(DIRECTORY config/
+    DESTINATION share/${PROJECT_NAME}/config
+  )
+endif()
+
+ament_package()
diff --git a/unitree_lidar_ros2/README.md b/unitree_lidar_ros2/README.md
new file mode 100644
index 0000000..f7c211d
--- /dev/null
+++ b/unitree_lidar_ros2/README.md
@@ -0,0 +1 @@
+This is a ROS2 package for Unitree Lidar L2.
\ No newline at end of file
diff --git a/unitree_lidar_ros2/include/unitree_lidar_ros2.h b/unitree_lidar_ros2/include/unitree_lidar_ros2.h
new file mode 100644
index 0000000..a59f143
--- /dev/null
+++ b/unitree_lidar_ros2/include/unitree_lidar_ros2.h
@@ -0,0 +1,230 @@
+/**********************************************************************
+ Copyright (c) 2020-2024, Unitree Robotics.Co.Ltd. All rights reserved.
+***********************************************************************/
+
+#pragma once
+
+#define BOOST_BIND_NO_PLACEHOLDERS
+
+#include <memory>
+#include "iostream"
+#include <string>
+#include <stdio.h>
+#include <iterator>
+#include <algorithm>
+#include <chrono>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "std_msgs/msg/header.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+// SDK
+#include "unitree_lidar_sdk_pcl.h"
+
+
+using std::placeholders::_1;
+
+class UnitreeLidarSDKNode : public rclcpp::Node
+{
+public:
+    explicit UnitreeLidarSDKNode(const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
+
+    ~UnitreeLidarSDKNode() {};
+
+    void timer_callback();
+
+protected:
+    // ROS
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cloud_;
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
+
+    // Unitree Lidar Reader
+    UnitreeLidarReader *lsdk_;
+
+    // Config params
+    int work_mode_;
+    int initialize_type_;
+    int local_port_;
+    std::string local_ip_;
+    int lidar_port_;
+    std::string lidar_ip_;
+    std::string serial_port_;
+    int baudrate_;
+    
+    int cloud_scan_num_;
+    bool use_system_timestamp_;
+    double range_min_;
+    double range_max_;
+
+    std::string cloud_frame_;
+    std::string cloud_topic_;
+
+    std::string imu_frame_;
+    std::string imu_topic_;
+};
+
+///////////////////////////////////////////////////////////////////
+
+UnitreeLidarSDKNode::UnitreeLidarSDKNode(const rclcpp::NodeOptions &options)
+    : Node("unitre_lidar_sdk_node", options)
+{
+    // load config parameters
+    declare_parameter<int>("initialize_type", 2);
+    declare_parameter<int>("work_mode", 0);
+    declare_parameter<bool>("use_system_timestamp", true);
+    declare_parameter<double>("range_min", 0);
+    declare_parameter<double>("range_max", 50);
+    declare_parameter<int>("cloud_scan_num", 18);
+
+    declare_parameter<std::string>("serial_port", "/dev/ttyACM0");
+    declare_parameter<int>("baudrate", 4000000);
+
+    declare_parameter<int>("lidar_port", 6101);
+    declare_parameter<std::string>("lidar_ip", "192.168.1.2");
+    declare_parameter<int>("local_port", 6201);
+    declare_parameter<std::string>("local_ip", "192.168.1.62");
+
+    declare_parameter<std::string>("cloud_frame", "unilidar_lidar");
+    declare_parameter<std::string>("cloud_topic", "unilidar/cloud");
+
+    declare_parameter<std::string>("imu_frame", "unilidar_imu");
+    declare_parameter<std::string>("imu_topic", "unilidar/imu");
+
+    work_mode_ = get_parameter("work_mode").as_int();
+    initialize_type_ = get_parameter("initialize_type").as_int();
+
+    serial_port_ = get_parameter("serial_port").as_string();
+    baudrate_ = get_parameter("baudrate").as_int();
+
+    lidar_port_ = get_parameter("lidar_port").as_int();
+    lidar_ip_ = get_parameter("lidar_ip").as_string();
+    local_port_ = get_parameter("local_port").as_int();
+    local_ip_ = get_parameter("local_ip").as_string();
+
+    cloud_scan_num_ = get_parameter("cloud_scan_num").as_int();
+    use_system_timestamp_ = get_parameter("use_system_timestamp").as_bool();
+    range_max_ = get_parameter("range_max").as_double();
+    range_min_ = get_parameter("range_min").as_double();
+
+    cloud_frame_ = get_parameter("cloud_frame").as_string();
+    cloud_topic_ = get_parameter("cloud_topic").as_string();
+    
+    imu_frame_ = get_parameter("imu_frame").as_string();
+    imu_topic_ = get_parameter("imu_topic").as_string();
+
+    // Initialize UnitreeLidarReader
+    lsdk_ = createUnitreeLidarReader();
+
+    std::cout << "initialize_type_ = " << initialize_type_ << std::endl;
+
+    if (initialize_type_ == 1)
+    {
+        lsdk_->initializeSerial(serial_port_, baudrate_,
+                                cloud_scan_num_, use_system_timestamp_, range_min_, range_max_);
+    }
+    else if (initialize_type_ == 2)
+    {
+        lsdk_->initializeUDP(lidar_port_, lidar_ip_, local_port_, local_ip_,
+                             cloud_scan_num_, use_system_timestamp_, range_min_, range_max_);
+    }
+    else
+    {
+        std::cout << "initialize_type is not right! exit now ...\n";
+        exit(0);
+    }
+
+    lsdk_->setLidarWorkMode(work_mode_);
+
+    // ROS2
+    broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
+    pub_cloud_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(cloud_topic_, 10);
+    pub_imu_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_topic_, 10);
+    timer_ = this->create_wall_timer(std::chrono::milliseconds(1), std::bind(&UnitreeLidarSDKNode::timer_callback, this));
+}
+
+void UnitreeLidarSDKNode::timer_callback()
+{
+    int result = lsdk_->runParse();
+    static pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
+
+    // RCLCPP_INFO(this->get_logger(), "result = %d", result);
+
+    if (result == LIDAR_IMU_DATA_PACKET_TYPE)
+    {
+        LidarImuData imu;
+        lsdk_->getImuData(imu);
+
+        if (lsdk_->getImuData(imu))
+        {
+            // publish imu message
+            rclcpp::Time timestamp(imu.info.stamp.sec, imu.info.stamp.nsec);
+
+            sensor_msgs::msg::Imu imuMsg;
+            imuMsg.header.frame_id = imu_frame_;
+            imuMsg.header.stamp = timestamp;
+
+            imuMsg.orientation.x = imu.quaternion[0];
+            imuMsg.orientation.y = imu.quaternion[1];
+            imuMsg.orientation.z = imu.quaternion[2];
+            imuMsg.orientation.w = imu.quaternion[3];
+
+            imuMsg.angular_velocity.x = imu.angular_velocity[0];
+            imuMsg.angular_velocity.y = imu.angular_velocity[1];
+            imuMsg.angular_velocity.z = imu.angular_velocity[2];
+
+            imuMsg.linear_acceleration.x = imu.linear_acceleration[0];
+            imuMsg.linear_acceleration.y = imu.linear_acceleration[1];
+            imuMsg.linear_acceleration.z = imu.linear_acceleration[2];
+
+            pub_imu_->publish(imuMsg);
+
+            // publish tf from imu to lidar
+            geometry_msgs::msg::TransformStamped transformStamped;
+            transformStamped.header.stamp = timestamp; // 使用IMU数据的时间戳保持同步
+            transformStamped.header.frame_id = imu_frame_; // 父坐标系
+            transformStamped.child_frame_id = cloud_frame_; // 子坐标系
+            transformStamped.transform.translation.x = 0.007698;
+            transformStamped.transform.translation.y = 0.014655;
+            transformStamped.transform.translation.z = -0.00667;
+            transformStamped.transform.rotation.x = 0;
+            transformStamped.transform.rotation.y = 0;
+            transformStamped.transform.rotation.z = 0;
+            transformStamped.transform.rotation.w = 1;
+            broadcaster_->sendTransform(transformStamped);
+        }
+    }
+    else if (result == LIDAR_POINT_DATA_PACKET_TYPE)
+    {
+        // RCLCPP_INFO(this->get_logger(), "POINT_CLOUD");
+        PointCloudUnitree cloud;
+        if (lsdk_->getPointCloud(cloud))
+        {
+            transformUnitreeCloudToPCL(cloud, cloudOut);
+
+            rclcpp::Time timestamp(
+                static_cast<int32_t>(cloud.stamp),
+                static_cast<uint32_t>((cloud.stamp - static_cast<int32_t>(cloud.stamp)) * 1e9));
+
+            sensor_msgs::msg::PointCloud2 cloud_msg;
+            pcl::toROSMsg(*cloudOut, cloud_msg);
+            cloud_msg.header.frame_id = cloud_frame_;
+            cloud_msg.header.stamp = timestamp;
+
+            pub_cloud_->publish(cloud_msg);
+        }
+    }
+}
+
diff --git a/unitree_lidar_ros2/launch/launch.py b/unitree_lidar_ros2/launch/launch.py
new file mode 100755
index 0000000..ac625aa
--- /dev/null
+++ b/unitree_lidar_ros2/launch/launch.py
@@ -0,0 +1,69 @@
+import os
+
+from launch.substitutions import LaunchConfiguration
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+from launch.conditions import IfCondition
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    
+    use_rviz = LaunchConfiguration('use_rviz')
+
+    declare_use_rviz = DeclareLaunchArgument(
+        'use_rviz',
+        default_value='false',
+        description='Whether to start RViz'
+    )
+
+    pkg_share = get_package_share_directory('unitree_lidar_ros2')
+    rviz_config_file = os.path.join(pkg_share, 
+                                    'rviz',
+                                    'view.rviz')
+
+    # Run unitree lidar
+    node1 = Node(
+        package='unitree_lidar_ros2',
+        executable='unitree_lidar_ros2_node',
+        name='unitree_lidar_ros2_node',
+        output='screen',
+        parameters= [
+                {'initialize_type': 1},
+                {'work_mode': 8},
+                {'use_system_timestamp': True},
+                {'range_min': 0.0},
+                {'range_max': 100.0},
+                {'cloud_scan_num': 18},
+
+                {'serial_port': '/dev/ttyACM1'},
+                {'baudrate': 4000000},
+
+                {'lidar_port': 6101},
+                {'lidar_ip': '192.168.1.62'},
+                {'local_port': 6201},
+                {'local_ip': '192.168.1.2'},
+                
+                {'cloud_frame': "unilidar_lidar"},
+                {'cloud_topic': "unilidar/cloud"},
+                {'imu_frame': "unilidar_imu"},
+                {'imu_topic': "unilidar/imu"},
+                ]
+    )
+
+    # Run Rviz
+    rviz_node = Node(
+       package='rviz2',
+       executable='rviz2',
+       name='rviz2',
+       arguments=['-d', rviz_config_file],
+       condition=IfCondition(use_rviz),
+       output='log'
+    )
+    return LaunchDescription(
+        [
+            declare_use_rviz,
+            node1, 
+            rviz_node,
+        ]
+    )
\ No newline at end of file
diff --git a/unitree_lidar_ros2/package.xml b/unitree_lidar_ros2/package.xml
new file mode 100644
index 0000000..74539aa
--- /dev/null
+++ b/unitree_lidar_ros2/package.xml
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>unitree_lidar_ros2</name>
+  <version>0.0.0</version>
+  <description>unitree lidar ros2</description>
+  <maintainer email="lukelbmeng@gmail.com">jo</maintainer>
+  <license>GPLv3</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  
+  <depend>pcl_conversions</depend>
+  <depend>pcl</depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/unitree_lidar_ros2/rviz/view.rviz b/unitree_lidar_ros2/rviz/view.rviz
new file mode 100644
index 0000000..fd24146
--- /dev/null
+++ b/unitree_lidar_ros2/rviz/view.rviz
@@ -0,0 +1,182 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /PointCloud21
+        - /TF1
+        - /TF1/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 607
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 3.4900152683258057
+        Min Value: 0
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 1
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 85
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /unilidar/cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        unilidar_imu:
+          Value: true
+        unilidar_imu_initial:
+          Value: true
+        unilidar_lidar:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        unilidar_imu_initial:
+          unilidar_imu:
+            unilidar_lidar:
+              {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: unilidar_lidar
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      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: 14.384671211242676
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5603978037834167
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 836
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002eafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ea000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002eafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002ea000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000049c000002ea00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1528
+  X: 72
+  Y: 27
diff --git a/unitree_lidar_ros2/src/unitree_lidar_ros2_node.cpp b/unitree_lidar_ros2/src/unitree_lidar_ros2_node.cpp
new file mode 100644
index 0000000..074efb6
--- /dev/null
+++ b/unitree_lidar_ros2/src/unitree_lidar_ros2_node.cpp
@@ -0,0 +1,14 @@
+/**********************************************************************
+ Copyright (c) 2020-2024, Unitree Robotics.Co.Ltd. All rights reserved.
+***********************************************************************/
+
+#include "unitree_lidar_ros2.h"
+
+int main(int argc, char *argv[])
+{
+  const rclcpp::NodeOptions options;
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<UnitreeLidarSDKNode>());
+  rclcpp::shutdown();
+  return 0;
+}
\ No newline at end of file