Merge pull request #5 from elephantrobotics/humble-slam

Humble slam

FAST_LIO/.github/stale.yml

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

FAST_LIO/.gitignore

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

FAST_LIO/.gitmodules

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

FAST_LIO/CMakeLists.txt

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

FAST_LIO/LICENSE

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

FAST_LIO/Log/fast_lio_time_log_analysis.m

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

FAST_LIO/Log/guide.md

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

FAST_LIO/Log/plot.py

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

FAST_LIO/PCD/1

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

FAST_LIO/README.md

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

FAST_LIO/config/avia.yaml

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

FAST_LIO/config/horizon.yaml

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

FAST_LIO/config/mid360.yaml

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

FAST_LIO/config/ouster64.yaml

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

FAST_LIO/config/unilidar_l2.yaml

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

FAST_LIO/config/velodyne.yaml

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

FAST_LIO/include/Exp_mat.h

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

FAST_LIO/include/IKFoM_toolkit/esekfom/util.hpp

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

FAST_LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp

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

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

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

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

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

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

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

FAST_LIO/include/IKFoM_toolkit/mtk/startIdx.hpp

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

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

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

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

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

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

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

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

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

FAST_LIO/include/common_lib.h

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

FAST_LIO/include/ikd-Tree/README.md

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

FAST_LIO/include/ikd-Tree/ikd_Tree.h

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

FAST_LIO/include/so3_math.h

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

FAST_LIO/include/use-ikfom.hpp

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

FAST_LIO/launch/gdb_debug_example.launch

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

FAST_LIO/launch/mapping.launch.py

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

FAST_LIO/launch/unilidar_l2.launch.py

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

FAST_LIO/msg/Pose6D.msg

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

FAST_LIO/package.xml

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

FAST_LIO/rviz/fastlio.rviz

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

FAST_LIO/rviz_cfg/loam_livox.rviz

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

FAST_LIO/src/IMU_Processing.hpp

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

FAST_LIO/src/preprocess.h

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

agv_pro_autocharge/README.md

@@ -0,0 +1,134 @@
+# AGV AutoCharge ROS2 Package
+
+这是一个为ROS2 Humble设计的AGV自动充电系统软件包。
+
+## 功能特性
+
+- 监听充电桩位置更新
+- 持续发布可视化标记
+- 键盘交互启动导航功能
+- 导航成功后启动串口控制
+- 支持充电状态检测和控制
+
+## 安装依赖
+
+确保您的系统已安装以下依赖：
+
+```bash
+# ROS2 Humble基础包
+sudo apt install ros-humble-rclpy
+sudo apt install ros-humble-geometry-msgs
+sudo apt install ros-humble-std-msgs
+sudo apt install ros-humble-nav-msgs
+sudo apt install ros-humble-visualization-msgs
+sudo apt install ros-humble-nav2-simple-commander
+
+# Python依赖
+pip3 install pyserial
+```
+
+## 编译安装
+
+```bash
+# 进入ROS2工作空间
+cd /path/to/your/ros2_ws/src
+
+# 复制软件包到工作空间
+cp -r agv_autocharge_ros2 .
+
+# 编译软件包
+cd ..
+colcon build --packages-select agv_autocharge_ros2
+
+# 加载环境变量
+source install/setup.bash
+```
+
+## 使用方法
+
+### 启动节点
+
+```bash
+# 启动自动充电控制器节点
+ros2 run agv_autocharge_ros2 combined_auto_recharger
+```
+
+### 节点功能
+
+- 监听 `/charger_position_update` 话题，接收充电桩位置更新
+- 发布 `/goal_marker` 话题，在RViz中显示充电桩位置标记
+- 发布 `/cmd_vel` 话题，控制机器人运动
+- 按键 `q` 启动导航到充电桩
+- 导航成功后自动启动串口控制
+
+### 配置文件
+
+充电桩位置配置文件位于：
+```
+config/charger_position.json
+```
+
+文件格式：
+```json
+{
+  "p_x": 1.329015495451769,
+  "p_y": 0.31961151635354823,
+  "orien_z": 0.4981823289472456,
+  "orien_w": 0.8670722963655905
+}
+```
+
+### 话题接口
+
+#### 订阅话题
+- `/charger_position_update` (geometry_msgs/PoseStamped): 充电桩位置更新
+
+#### 发布话题
+- `/goal_marker` (visualization_msgs/MarkerArray): 充电桩位置可视化标记
+- `/cmd_vel` (geometry_msgs/Twist): 机器人运动控制
+- `/chassis_security` (std_msgs/Int8): 底盘安全控制
+
+### 串口配置
+
+默认串口配置：
+- 端口: `/dev/ttyCH341USB0`
+- 波特率: 9600
+- 超时: 1秒
+
+可以根据需要修改代码中的串口参数。
+
+## 操作说明
+
+1. 启动节点后，系统会自动加载充电桩位置配置
+2. 系统会定期发布充电桩标记到RViz进行可视化
+3. 按下键盘上的 `q` 键启动导航到充电桩
+4. 导航成功后，系统会自动启动串口控制功能
+5. 串口控制会根据接收到的数据控制机器人运动
+6. 按 `Ctrl+C` 退出程序
+
+## 故障排除
+
+### 常见问题
+
+1. **串口无法打开**
+   - 检查串口设备是否连接
+   - 确认串口权限设置
+   - 验证串口设备名称
+
+2. **导航失败**
+   - 确认Nav2导航系统正常运行
+   - 检查充电桩位置配置是否正确
+   - 验证地图和定位系统状态
+
+3. **RViz中看不到标记**
+   - 确认RViz已订阅 `/goal_marker` 话题
+   - 检查MarkerArray显示设置
+   - 验证坐标系设置是否为'map'
+
+## 许可证
+
+MIT License
+
+## 维护者
+
+请联系维护者获取技术支持。

agv_pro_autocharge/agv_pro_autocharge/__init__.py

@@ -0,0 +1,10 @@
+"""
+AGV AutoCharge ROS2 Package
+
+This package provides automatic charging functionality for AGV robots using ROS2 Humble.
+It includes position management, visualization, navigation, and serial control features.
+"""
+
+__version__ = '1.0.0'
+__author__ = 'Your Name'
+__email__ = 'your-email@example.com'

agv_pro_autocharge/agv_pro_autocharge/combined_auto_recharger.py

@@ -0,0 +1,612 @@
+#!/usr/bin/env python3 
+# coding=utf-8
+
+"""
+合并的自动充电控制器 - 结合位置管理、可视化和导航功能
+- 监听充电桩位置更新
+- 持续发布可视化标记
+- 按键'q'启动导航功能
+- 导航成功后启动串口控制
+"""
+
+# 引用ros库
+import rclpy
+from rclpy.node import Node
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+from rclpy.duration import Duration
+
+# 用到的变量定义
+from std_msgs.msg import Bool 
+from std_msgs.msg import Int8 
+from std_msgs.msg import UInt8
+from std_msgs.msg import Float32
+
+# 用于记录充电桩位置、发布导航点
+from geometry_msgs.msg import PoseStamped, Twist
+
+# rviz可视化相关
+from visualization_msgs.msg import Marker
+from visualization_msgs.msg import MarkerArray
+
+# 里程计话题相关
+from nav_msgs.msg import Odometry
+
+# 键盘控制相关
+import sys
+import select
+import termios
+import tty
+
+# 延迟相关
+import time
+import threading
+
+# 读写充电桩位置文件
+import json
+import math
+import yaml
+import os
+
+# 导入串口解析模块
+from .serial_can_parser import SerialCANParser
+
+# 存放充电桩位置的文件位置 - 参考原始auto_recharger.py的路径设置方式
+def find_config_files():
+    """查找配置文件路径"""
+    # 首先尝试几个可能的位置
+    possible_paths = [
+        # 开发环境路径
+        '/home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config',
+        # 你的工作空间路径
+        '/home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config',
+        # 当前包的相对路径
+        os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'config'),
+        # 安装路径
+        '/home/elephant/agv_pro_ros2/install/agv_pro_autocharge/share/agv_pro_autocharge/config'
+    ]
+    
+    for config_dir in possible_paths:
+        yaml_path = os.path.join(config_dir, 'nav_goal_params.yaml')
+        json_path = os.path.join(config_dir, 'charger_position.json')
+        
+        print(f"Checking config directory: {config_dir}")
+        if os.path.exists(yaml_path) and os.path.exists(json_path):
+            print(f"Found config files in: {config_dir}")
+            return yaml_path, json_path
+    
+    # 如果都找不到，直接报错
+    print("ERROR: Could not find config files in any of the following locations:")
+    for path in possible_paths:
+        print(f"  - {path}")
+    print("Please ensure the config files exist in one of these directories.")
+    
+    # 返回第一个路径作为默认值，但文件可能不存在
+    return os.path.join(possible_paths[0], 'nav_goal_params.yaml'), os.path.join(possible_paths[0], 'charger_position.json')
+
+# 获取配置文件路径
+yaml_file, json_file = find_config_files()
+
+# print_and_fixRetract相关，用于打印带颜色的信息
+RESET = '\033[0m'
+RED   = '\033[1;31m'
+GREEN = '\033[1;32m'
+YELLOW= '\033[1;33m'
+BLUE  = '\033[1;34m'
+PURPLE= '\033[1;35m'
+CYAN  = '\033[1;36m'
+
+# 圆周率
+PI = 3.1415926535897
+
+if os.name == 'nt':
+    import msvcrt
+else:
+    import termios
+    import tty
+	
+settings = None
+if os.name != 'nt' and sys.stdin.isatty():
+    settings = list(termios.tcgetattr(sys.stdin))
+
+def get_key(settings):
+    if os.name == 'nt':
+        return msvcrt.getch().decode('utf-8')
+    else:
+        if sys.stdin.isatty():
+            tty.setraw(sys.stdin.fileno())
+        rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
+        if rlist:
+            key = sys.stdin.read(1)
+        else:
+            key = ''
+        if sys.stdin.isatty() and settings:
+            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+        return key
+
+def print_and_fixRetract(str):
+    global settings
+    '''键盘控制会导致回调函数内使用print()出现自动缩进的问题，此函数可以解决该现象'''
+    if sys.stdin.isatty() and settings:
+        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+    print(str)
+
+class CombinedAutoRecharger(Node):
+    def __init__(self):
+        
+        # 创建节点
+        super().__init__("combined_auto_recharger")
+
+        print_and_fixRetract('Combined Auto Recharger Node Started!')
+
+        # 导航状态标记
+        self.navigation_active = False
+        
+        # 串口控制相关
+        self.parser = None
+        self.serial_control_active = False
+        self.navigation_requested = False  # 添加导航请求标志
+        
+        # 创建导航器
+        self.navigator = BasicNavigator()
+
+        # 加载充电桩位置信息
+        self.load_charger_position()
+        # 加载导航参数
+        self.load_nav_goal_params()
+
+        # 创建发布者
+        self.robot_security_off_pub = self.create_publisher(Int8, '/chassis_security', 10) 
+        self.Charger_marker_pub = self.create_publisher(MarkerArray, '/goal_marker', 10)
+        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
+
+        # 创建订阅者 - 只订阅充电桩位置更新
+        self.Charger_Position_Update_sub = self.create_subscription(
+            PoseStamped, "/charger_position_update", 
+            self.Position_Update_callback, 10)
+
+        # 创建定时器，定期发布充电桩标记（每2秒发布一次）
+        self.marker_timer = self.create_timer(2.0, self.timer_callback)
+        
+        # 创建导航检查定时器（每0.5秒检查一次导航请求）
+        self.navigation_timer = self.create_timer(0.5, self.check_navigation_request)
+
+        # 发布初始充电桩位置标记
+        self.update_charger_visualization()
+
+        print_and_fixRetract('Combined auto recharger node initialized successfully!')
+        print_and_fixRetract(f'{GREEN}Press "q" to start navigation, Ctrl+C to exit{RESET}')
+
+    def load_nav_goal_params(self):
+        """加载导航目标参数（前方距离和角度）"""
+        print_and_fixRetract(f"Attempting to load nav goal params from: {yaml_file}")
+        print_and_fixRetract(f"File exists? {os.path.exists(yaml_file)}")
+        
+        try:
+            with open(yaml_file, 'r', encoding='utf-8') as f:
+                params = yaml.safe_load(f)
+            
+            print_and_fixRetract(f"Raw params from file: {params}")
+            
+            self.forward_distance = float(params.get('forward_distance', 1.0))
+            self.yaw_offset_deg = float(params.get('yaw_offset_deg', 0.0))
+            
+            print_and_fixRetract(f"Successfully loaded nav goal params: forward_distance={self.forward_distance}, yaw_offset_deg={self.yaw_offset_deg}")
+            
+        except FileNotFoundError:
+            print_and_fixRetract(f"{RED}Nav goal params file not found: {yaml_file}{RESET}")
+            print_and_fixRetract(f"{RED}Please create the configuration file with the required parameters{RESET}")
+            # 使用默认值
+            self.forward_distance = 1.0
+            self.yaw_offset_deg = 0.0
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Failed to load nav goal params: {e}{RESET}")
+            self.forward_distance = 1.0
+            self.yaw_offset_deg = 0.0
+
+    def timer_callback(self):
+        '''定时器回调函数，定期发布充电桩标记'''
+        # 始终发布当前JSON文件中的位置信息
+        if hasattr(self, 'json_data') and self.json_data:
+            self.Pub_Charger_marker(
+                self.json_data['p_x'],
+                self.json_data['p_y'], 
+                self.json_data['orien_z'],
+                self.json_data['orien_w']
+            )
+
+    def check_navigation_request(self):
+        '''检查是否有导航请求'''
+        if self.navigation_requested and not self.navigation_active:
+            self.navigation_requested = False
+            self.navigation_active = True
+            print_and_fixRetract(f"{BLUE}Processing navigation request...{RESET}")
+            
+            # 在ROS2线程中执行导航
+            result = self.execute_navigation_internal()
+            
+            if result:
+                print_and_fixRetract(f"{GREEN}Navigation successful! Starting serial control...{RESET}")
+                # 在ROS2线程中启动串口控制
+                self.start_serial_control_async()
+            else:
+                print_and_fixRetract(f"{RED}Navigation failed{RESET}")
+                self.navigation_active = False
+
+    def request_navigation(self):
+        '''请求开始导航'''
+        if not self.navigation_active:
+            self.navigation_requested = True
+            print_and_fixRetract(f"{BLUE}Navigation request queued...{RESET}")
+        else:
+            print_and_fixRetract(f"{YELLOW}Navigation already in progress{RESET}")
+
+    def load_charger_position(self):
+        '''加载充电桩位置信息'''
+        try:
+            with open(json_file, 'r', encoding='utf-8') as fp:
+                self.json_data = json.load(fp)
+            print_and_fixRetract(f"Loaded charger position: x={self.json_data['p_x']:.3f}, y={self.json_data['p_y']:.3f}")
+        except FileNotFoundError:
+            print_and_fixRetract(f"{RED}Charger position file {json_file} not found{RESET}")
+            print_and_fixRetract(f"{RED}Please create the configuration file with default charger position{RESET}")
+            # 使用默认位置
+            self.json_data = {
+                'p_x': 0.0,
+                'p_y': 0.0, 
+                'orien_z': 0.0,
+                'orien_w': 1.0
+            }
+        except Exception as e:
+            print_and_fixRetract(f"Error loading charger position: {e}")
+            self.json_data = {
+                'p_x': 0.0,
+                'p_y': 0.0,
+                'orien_z': 0.0, 
+                'orien_w': 1.0
+            }
+
+    def save_charger_position(self):
+        '''保存充电桩位置信息到JSON文件'''
+        try:
+            with open(json_file, 'w', encoding='utf-8') as fp:
+                json.dump(self.json_data, fp, ensure_ascii=False, indent=2)
+            print_and_fixRetract(f"{GREEN}Charger position saved to {json_file}{RESET}")
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Error saving charger position: {e}{RESET}")
+
+    def Pub_Charger_Position(self):
+        '''更新充电桩位置信息并保存到JSON文件'''
+        # 发布充电桩位置的可视化
+        self.Pub_Charger_marker(
+            self.json_data['p_x'], 
+            self.json_data['p_y'], 
+            self.json_data['orien_z'], 
+            self.json_data['orien_w'])
+        
+        # 保存当前充电桩位置到JSON文件
+        position_data = {
+            'p_x': self.json_data['p_x'],
+            'p_y': self.json_data['p_y'],
+            'orien_z': self.json_data['orien_z'],
+            'orien_w': self.json_data['orien_w']
+        }
+        
+        self.json_data = position_data
+        self.save_charger_position()
+        print_and_fixRetract(f"Position: x={self.json_data['p_x']:.3f}, y={self.json_data['p_y']:.3f}")
+
+    def Pub_Charger_marker(self, p_x, p_y, o_z, o_w):
+        '''发布目标点可视化话题'''
+        
+        markerArray = MarkerArray()
+        
+        # 获取当前时间戳
+        current_time = self.get_clock().now().to_msg()
+
+        marker_shape = Marker()  # 创建marker对象
+        marker_shape.id = 0  # 必须赋值id
+        marker_shape.header.frame_id = 'map'  # 以哪一个TF坐标为原点
+        marker_shape.header.stamp = current_time  # 添加时间戳
+        marker_shape.type = Marker.ARROW  # TEXT_VIEW_FACING #一直面向屏幕的字符格式
+        marker_shape.action = Marker.ADD  # 添加marker
+        marker_shape.scale.x = 0.5  # marker大小
+        marker_shape.scale.y = 0.05  # marker大小
+        marker_shape.scale.z = 0.05  # marker大小，对于字符只有z起作用
+        marker_shape.pose.position.x = p_x  # 字符位置
+        marker_shape.pose.position.y = p_y  # 字符位置
+        marker_shape.pose.position.z = 0.1  # msg.position.z #字符位置
+        marker_shape.pose.orientation.z = o_z  # 字符位置
+        marker_shape.pose.orientation.w = o_w  # 字符位置
+        marker_shape.color.r = 1.0  # 字符颜色R(红色)通道
+        marker_shape.color.g = 0.0  # 字符颜色G(绿色)通道
+        marker_shape.color.b = 0.0  # 字符颜色B(蓝色)通道
+        marker_shape.color.a = 1.0  # 字符透明度
+        markerArray.markers.append(marker_shape)  # 添加元素进数组
+        
+        marker_string = Marker()  # 创建marker对象
+        marker_string.id = 1  # 必须赋值id
+        marker_string.header.frame_id = 'map'  # 以哪一个TF坐标为原点
+        marker_string.header.stamp = current_time  # 添加时间戳
+        marker_string.type = Marker.TEXT_VIEW_FACING  # 一直面向屏幕的字符格式
+        marker_string.action = Marker.ADD  # 添加marker
+        marker_string.scale.x = 0.5  # marker大小
+        marker_string.scale.y = 0.5  # marker大小
+        marker_string.scale.z = 0.5  # marker大小，对于字符只有z起作用
+        marker_string.color.a = 1.0  # 字符透明度
+        marker_string.color.r = 1.0  # 字符颜色R(红色)通道
+        marker_string.color.g = 0.0  # 字符颜色G(绿色)通道
+        marker_string.color.b = 0.0  # 字符颜色B(蓝色)通道
+        marker_string.pose.position.x = p_x  # 字符位置
+        marker_string.pose.position.y = p_y  # 字符位置
+        marker_string.pose.position.z = 0.1  # msg.position.z #字符位置
+        marker_string.pose.orientation.z = o_z  # 字符位置
+        marker_string.pose.orientation.w = o_w  # 字符位置
+        marker_string.text = 'Charger'  # 字符内容
+        markerArray.markers.append(marker_string)  # 添加元素进数组
+        self.Charger_marker_pub.publish(markerArray)  # 发布markerArray，rviz订阅并进行可视化
+
+    def Position_Update_callback(self, topic):
+        '''更新json文件中的充电桩位置'''
+        position_dic = {'p_x': 0, 'p_y': 0, 'orien_z': 0, 'orien_w': 0}
+        position_dic['p_x'] = topic.pose.position.x
+        position_dic['p_y'] = topic.pose.position.y
+        position_dic['orien_z'] = topic.pose.orientation.z
+        position_dic['orien_w'] = topic.pose.orientation.w
+
+        # 保存最新的充电桩位置到json文件
+        self.json_data = position_dic
+        self.save_charger_position()
+        print_and_fixRetract("New charging pile position saved.")
+        
+        # 位置更新后立即发布一次新的标记，然后继续定时发布
+        self.update_charger_visualization()
+        print_and_fixRetract(f"{GREEN}Charger position updated and will be published continuously{RESET}")
+
+    def update_charger_visualization(self):
+        '''更新充电桩可视化标记'''
+        if hasattr(self, 'json_data'):
+            self.Pub_Charger_marker(
+                self.json_data['p_x'],
+                self.json_data['p_y'], 
+                self.json_data['orien_z'],
+                self.json_data['orien_w']
+            )
+
+    def execute_navigation(self):
+        """外部调用的导航接口"""
+        self.request_navigation()
+        return True  # 返回True表示请求已提交
+
+    def execute_navigation_internal(self):
+        """内部执行导航任务"""
+        print_and_fixRetract(f"{BLUE}Starting navigation...{RESET}")
+        # 从JSON文件读取充电桩位置
+        try:
+            with open(json_file, 'r', encoding='utf-8') as f:
+                charger_data = json.load(f)
+            px = charger_data['p_x']
+            py = charger_data['p_y']
+            # 充电桩姿态四元数转欧拉角
+            orien_z = charger_data['orien_z']
+            orien_w = charger_data['orien_w']
+            yaw = 2 * math.atan2(orien_z, orien_w)  # 只考虑z/w分量
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Failed to read charger position file: {e}{RESET}")
+            self.navigation_active = False
+            return False
+
+        # 计算目标点位置
+        x_offset = self.forward_distance * math.cos(yaw)
+        y_offset = self.forward_distance * math.sin(yaw)
+        goal_x = px + x_offset
+        goal_y = py + y_offset
+
+        # 计算目标点姿态（z轴顺时针yaw_offset_deg）
+        goal_yaw = yaw - math.radians(self.yaw_offset_deg)
+        goal_qz = math.sin(goal_yaw / 2)
+        goal_qw = math.cos(goal_yaw / 2)
+
+        print_and_fixRetract(f"Nav goal: x={goal_x:.3f}, y={goal_y:.3f}, yaw={math.degrees(goal_yaw):.1f}°")
+        goal_pose = self.create_pose(goal_x, goal_y, goal_qz, goal_qw)
+
+        # 执行导航
+        print_and_fixRetract(f"{BLUE}Executing navigation...{RESET}")
+        result1 = self.nav_through_pose([goal_pose], verbose=False)
+        print_and_fixRetract(f"Navigation result: {result1}")
+        self.navigation_active = False
+        return result1
+
+    def create_pose(self, x, y, z, w):
+        """创建单个目标点的位姿信息"""
+        pose = PoseStamped()
+        pose.header.frame_id = 'map'
+        pose.header.stamp = self.get_clock().now().to_msg()
+        pose.pose.position.x = x
+        pose.pose.position.y = y
+        pose.pose.orientation.z = z
+        pose.pose.orientation.w = w
+        return pose
+
+    def nav_through_pose(self, goal_poses, verbose: bool = False) -> bool:
+        """执行多点导航任务"""
+        # 开始执行多点导航任务
+        self.navigator.goThroughPoses(goal_poses)
+
+        # 等待导航任务完成，监控导航状态
+        while not self.navigator.isTaskComplete():
+            feedback = self.navigator.getFeedback()
+            if feedback and verbose:
+                remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9
+                print_and_fixRetract(f"预计到达时间: {remaining:.0f} 秒")
+
+        # 根据导航结果返回相应状态
+        result = self.navigator.getResult()
+        if result == TaskResult.SUCCEEDED:
+            print_and_fixRetract(f'{GREEN}Navigation successful!{RESET}')
+            return True
+        elif result == TaskResult.CANCELED:
+            print_and_fixRetract(f'{YELLOW}Navigation canceled!{RESET}')
+        elif result == TaskResult.FAILED:
+            print_and_fixRetract(f'{RED}Navigation failed!{RESET}')
+        else:
+            print_and_fixRetract(f'{RED}Invalid navigation result!{RESET}')
+        return False
+
+    def start_serial_control_async(self):
+        """异步启动串口控制功能"""
+        def serial_control_thread():
+            self.start_serial_control()
+        
+        # 在新线程中启动串口控制，避免阻塞ROS2主线程
+        serial_thread = threading.Thread(target=serial_control_thread, daemon=True)
+        serial_thread.start()
+
+    def start_serial_control(self):
+        """启动串口控制功能"""
+        print_and_fixRetract(f"{BLUE}Starting serial control...{RESET}")
+        try:
+            self.parser = SerialCANParser('/dev/agvpro_ec130', 9600, 1)
+            self.parser.open_serial()  # 打开usb串口
+            
+            # 发送AT 命令从透传模式进入AT指令模式
+            self.parser.send_at_commands(["AT+CG", "AT+AT"])
+            
+            self.serial_control_active = True
+            print_and_fixRetract(f'{GREEN}Serial control started successfully{RESET}')
+            
+            while self.serial_control_active:
+                # 开始读取数据
+                x_speed, z_speed, which_mode, infrared_bits = self.parser.read_serial_data()
+                
+                if infrared_bits[7] == 0:  # 无障碍物
+                    if which_mode == 0x01:  # 正常模式
+                        # 直接发布ROS2 Twist消息
+                        twist_msg = Twist()
+                        twist_msg.linear.x = float(x_speed)
+                        twist_msg.linear.y = 0.0
+                        twist_msg.angular.z = float(z_speed)
+                        self.cmd_vel_pub.publish(twist_msg)
+                        print_and_fixRetract(f'Normal mode - Speed: x={x_speed}, z={z_speed}')
+                        
+                    elif which_mode == 0xBB:  # 测压区
+                        # 停止运动
+                        stop_msg = Twist()
+                        self.cmd_vel_pub.publish(stop_msg)
+                        print_and_fixRetract(f'{YELLOW}Pressure zone - Stop movement{RESET}')
+                        
+                    elif which_mode == 0xAA:  # 充电区
+                        # 停止运动
+                        stop_msg = Twist()
+                        self.cmd_vel_pub.publish(stop_msg)
+                        print_and_fixRetract(f'{GREEN}Charging zone - Stop movement{RESET}')
+                        break  # 充电完成后退出
+                        
+                    elif which_mode == 0xCF:  # 急停模式
+                        emergency_stop_msg = Twist()  # 所有速度都为0
+                        self.cmd_vel_pub.publish(emergency_stop_msg)
+                        print_and_fixRetract(f'{RED}Emergency stop mode - Immediate stop{RESET}')
+                        break
+                        
+                else:  # 检测到障碍物
+                    obstacle_stop_msg = Twist()  # 所有速度都为0
+                    self.cmd_vel_pub.publish(obstacle_stop_msg)
+                    print_and_fixRetract(f'{RED}Obstacle detected - Stop movement{RESET}')
+                    break
+                    
+        except KeyboardInterrupt:
+            print_and_fixRetract("Serial control interrupted by user")
+            # 发布停止消息
+            emergency_stop = Twist()
+            self.cmd_vel_pub.publish(emergency_stop)
+        except Exception as e:
+            print_and_fixRetract(f"{RED}Serial control error: {e}{RESET}")
+            # 发布停止消息
+            emergency_stop = Twist()
+            self.cmd_vel_pub.publish(emergency_stop)
+        finally:
+            if self.parser:
+                self.parser.close_serial()
+            print_and_fixRetract("Serial control stopped")
+
+    def stop_serial_control(self):
+        """停止串口控制功能"""
+        self.serial_control_active = False
+
+    def get_charger_info(self):
+        '''获取充电桩位置信息'''
+        if hasattr(self, 'json_data'):
+            return {
+                'position': {
+                    'x': self.json_data['p_x'],
+                    'y': self.json_data['p_y']
+                },
+                'orientation': {
+                    'z': self.json_data['orien_z'],
+                    'w': self.json_data['orien_w']
+                }
+            }
+        return None
+
+
+def main(args=None):
+    '''主函数'''
+    rclpy.init(args=args)
+    
+    combined_recharger = None
+    try:
+        combined_recharger = CombinedAutoRecharger()
+        
+        print_and_fixRetract("Combined auto recharger node is running...")
+        print_and_fixRetract("Node functions:")
+        print_and_fixRetract("- Listening for charger position updates on /charger_position_update")
+        print_and_fixRetract("- Publishing visualization markers on /goal_marker every 2 seconds")
+        print_and_fixRetract("- Press 'q' to start navigation to charger position")
+        print_and_fixRetract("- Navigation success will trigger serial control")
+        
+        # 启动ROS2事件循环线程
+        def ros2_spin():
+            try:
+                rclpy.spin(combined_recharger)
+            except Exception as spin_error:
+                print_and_fixRetract(f"ROS2 spin error: {spin_error}")
+
+        ros2_thread = threading.Thread(target=ros2_spin, daemon=True)
+        ros2_thread.start()
+        
+        # 键盘监听循环
+        print_and_fixRetract(f"{GREEN}Press 'q' to start navigation, Ctrl+C to exit{RESET}")
+        print_and_fixRetract("Waiting for keyboard input...")
+        
+        while True:
+            try:
+                key = get_key(settings)
+                if key:
+                    print_and_fixRetract(f"Key pressed: {repr(key)}")  # 调试信息
+                    if key.lower() == 'q':
+                        print_and_fixRetract(f"{BLUE}Navigation command received!{RESET}")
+                        
+                        # 请求导航任务（异步执行）
+                        combined_recharger.execute_navigation()
+                    
+                    elif key == '\x03':  # Ctrl+C
+                        break
+                
+                time.sleep(0.1)  # 避免过度占用CPU
+            except KeyboardInterrupt:
+                break
+            except Exception as e:
+                print_and_fixRetract(f"Keyboard input error: {e}")
+                time.sleep(0.5)
+            
+    except KeyboardInterrupt:
+        print_and_fixRetract("\nShutting down combined auto recharger node...")
+    finally:
+        if combined_recharger:
+            combined_recharger.stop_serial_control()
+            combined_recharger.destroy_node()
+        rclpy.shutdown()
+        print_and_fixRetract("Program exited safely")
+
+if __name__ == '__main__':
+    main()
+

agv_pro_autocharge/agv_pro_autocharge/serial_can_parser.py

@@ -0,0 +1,173 @@
+#!/usr/bin/env python3                                                                                                           
+#coding=UTF-8
+import serial
+import time
+
+class SerialCANParser:
+    def __init__(self, serial_port='/dev/ttyCH341USB0', baudrate=9600, timeout=1):
+        self.serial_port = serial_port  # 串口名称
+        self.baudrate = baudrate  # 波特率
+        self.timeout = timeout  # 超时设置
+        self.ser = None  # 串口对象
+        self.buffer = bytearray()  # 存储当前读取的字节
+        self.max_retries = 3  # 最大重试次数
+        # 存储实时数据
+        self.x_speed = 0.0
+        self.z_speed = 0.0
+        self.infrared_bits = []
+        
+    def open_serial(self):
+        """打开串口"""
+        try:
+            self.ser = serial.Serial(self.serial_port, self.baudrate, timeout=self.timeout)
+            print(f"串口 {self.serial_port} 已打开，波特率：{self.baudrate}")
+        except Exception as e:
+            print(f"打开串口失败: {e}")
+            
+    def close_serial(self):
+        """关闭串口"""
+        if self.ser and self.ser.is_open:
+            self.ser.close()
+            print("串口已关闭。")
+        else:
+            print("串口未打开或已关闭。")
+            
+    def can_id_check(self, date):
+        high_byte, low_byte = date[0:2]
+        # 高字节左移 3 位
+        can_id = (high_byte << 3)
+        # 低字节右移 5 位
+        can_id |= (low_byte >> 5)
+        return can_id
+        
+    def parse_can_data(self, data):
+        """解析8字节CAN数据帧"""
+        if len(data) != 8:
+            print("数据帧长度不正确")
+            return None
+            
+        # 解析 X、Y 和 Z 速度
+        x_speed_raw = ((data[0] << 8) | data[1])  # X速度的原始数据
+        z_speed_raw = ((data[4] << 8) | data[5])  # Z速度的原始数据
+        
+        # 将原始数据转换为浮动数值，并考虑正负
+        if x_speed_raw & 0x8000:  # 如果最高位为1，表示负数
+            x_speed_raw = -((65536 - x_speed_raw) & 0xFFFF)  # 补码转换为负数
+        if z_speed_raw & 0x8000:  # 如果最高位为1，表示负数
+            z_speed_raw = -((65536 - z_speed_raw) & 0xFFFF)  # 补码转换为负数
+            
+        # 转换单位为 m/s 和 rad/s
+        self.x_speed = x_speed_raw / 1000.0  # X速度单位为 m/s
+        self.y_speed = 0  # Y速度为0
+        self.z_speed = z_speed_raw / 1000.0  # Z速度单位为 rad/s
+        self.which_mode = data[2]
+        self.infrared = data[6]  # 红外数据
+        self.raw_current = data[7]  # 电流数据
+        
+        if self.raw_current > 32767:  # 无符号数大于 32767 表示负值（因为最大值是 65535）
+            # 转换为负数
+            self.actual_current = -(65536 - self.raw_current) * 30.0
+        else:
+            # 正数直接转换
+            self.actual_current = self.raw_current * 30.0
+            
+        # 处理红外数据
+        self.infrared_bits = [(self.infrared >> (7 - i)) & 0x01 for i in range(8)]
+        
+        # 打印或处理数据
+        print(f"X Speed: {self.x_speed:.3f}, Y Speed: {self.y_speed}, Z Speed: {self.z_speed:.3f}, "
+              f"Actual Current: {self.actual_current:.3f} mA, Infrared: {self.infrared}")
+              
+        # 打印或处理红外位信息
+        print(f"L_A: {self.infrared_bits[2]}, L_B: {self.infrared_bits[3]}, R_B: {self.infrared_bits[4]}, "
+              f"R_A: {self.infrared_bits[5]}, infrared_flag : {self.infrared_bits[6]}, "
+              f"Charging flag: {self.infrared_bits[7]}")
+              
+    def read_serial_data(self):
+        """读取串口数据并解析"""
+        while True:  # 修改为简单的无限循环，由上层控制退出
+            if self.ser.in_waiting > 0:
+                byte = self.ser.read(1)   # 读取一个字节
+                if len(self.buffer) < 2:
+                    self.buffer.extend(byte)  
+                    if len(self.buffer) == 2:
+                        # 如果帧头为 0x41 0x54，表示为AT帧头，则开始接收数据
+                        if self.buffer[0] != 0x41 or self.buffer[1] != 0x54:
+                            # 如果不是有效的帧头，则清空缓冲区并跳到下次循环
+                            self.buffer.clear()
+                            continue  # 继续等待下一个字节                 
+                else:
+                    self.buffer.extend(byte)
+                    # print("缓冲区内容:", ' '.join(f'{b:02x}' for b in self.buffer)) # debug
+                    # 如果缓冲区字节长度大于等于 17 字节（数据帧长度）
+                    if len(self.buffer) >= 17:
+                        # print("Received Frame (Hex):", ' '.join(f'{byte:02x}' for byte in self.buffer)) # debug
+                        # 解析帧头、CAN帧ID、格式、类型和数据
+                        # at_frame_header = self.buffer[0:2]  # AT帧头
+                        can_frame_id = self.can_id_check(self.buffer[2:4])  # CAN标准帧ID
+                        # can_frame_format = self.buffer[4]  # CAN帧格式（0,标准帧;1,扩展帧）
+                        # can_frame_type = self.buffer[5]  # CAN帧类型（0,数据帧;1,远程帧）
+                        data_length = self.buffer[6]  # 数据长度
+                        data = self.buffer[7:15]  # 数据帧
+                        # print(f"帧ID: 0x{can_frame_id:X}")    # debug
+                        if can_frame_id == 0x182 and data_length == 0x08: # 根据can帧id进行判断
+                            # 如果帧ID为0x182,校验通过,进行数据赋值
+                            self.parse_can_data(data)
+                            # 清空缓冲区，准备下一帧数据
+                            self.buffer.clear()
+                            return self.x_speed, self.z_speed, self.which_mode, self.infrared_bits   # 返回解析后的数据
+                        else:
+                            # 清空缓冲区，准备下一帧数据
+                            self.buffer.clear()
+                            
+    def read_serial_response(self):
+        """读取串口响应数据，直到接收到 '\r\n' 或超时"""
+        response = bytearray()  # 使用 bytearray 来存储原始字节流
+        while True:
+            if self.ser.in_waiting > 0:
+                byte = self.ser.read(1)
+                response += byte
+            # 检查是否已接收到完整响应
+            if b'\r\n' in response:
+                break
+            # 超时机制，防止死循环
+            if len(response) > 100:
+                break
+        return bytes(response)  # 返回原始字节流（bytes）
+        
+    def send_at_commands(self, commands):
+        """发送 AT 命令并等待响应"""
+        for command in commands:
+            retries = 0
+            while retries < self.max_retries:
+                self.ser.write(command.encode() + b'\r\n')
+                print(f"发送命令: {command}")
+                # 等待响应并读取数据
+                response = self.read_serial_response()
+                # 检查响应是否包含 "OK"
+                if b"OK" in response:
+                    print(f"收到响应: {response}")
+                    break  # 如果收到 OK，退出重试循环
+                else:
+                    retries += 1
+                    print(f"未收到预期的响应，收到: {response}")
+            if retries == self.max_retries:
+                print(f"重试 {self.max_retries} 次后仍未收到有效响应，请检查设备。")
+                break
+                
+    def start(self):
+        """开始读取和处理数据"""
+        self.open_serial()
+        try:
+            # 发送AT 命令从透传模式进入AT指令模式
+            self.send_at_commands(["AT+CG", "AT+AT"])
+            # 开始读取数据
+            self.read_serial_data()
+        except KeyboardInterrupt:
+            print("手动中止程序。")
+        finally:
+            self.close_serial()
+
+if __name__ == '__main__':
+    parser = SerialCANParser('/dev/ttyCH341USB0', 9600, 1)
+    parser.start()

agv_pro_autocharge/config/charger_position.json

@@ -0,0 +1,6 @@
+{
+  "p_x": -0.04373347759246826,
+  "p_y": -4.024151802062988,
+  "orien_z": 0.09378381732290733,
+  "orien_w": 0.9955925851513477
+}

agv_pro_autocharge/config/nav_goal_params.yaml

@@ -0,0 +1,3 @@
+# 导航参数配置
+forward_distance: 1  # 距离充电桩前方1米
+yaw_offset_deg: 10.0   # 顺时针旋转10度

agv_pro_autocharge/package.xml

@@ -0,0 +1,30 @@
+<?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>agv_pro_autocharge</name>
+  <version>1.0.8</version>
+  <description>AGV automatic charging system for ROS2 Humble</description>
+  <maintainer email="your-email@example.com">Your Name</maintainer>
+  <license>MIT</license>
+
+  <!-- Build dependencies -->
+  <buildtool_depend>ament_python</buildtool_depend>
+  
+  <!-- Runtime dependencies -->
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>nav2_simple_commander</depend>
+  
+  <!-- Test dependencies -->
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

agv_pro_autocharge/resource/agv_pro_autocharge

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

agv_pro_autocharge/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/agv_pro_autocharge
+[install]
+install_scripts=$base/lib/agv_pro_autocharge

agv_pro_autocharge/setup.py

@@ -0,0 +1,30 @@
+from setuptools import setup, find_packages
+import os
+from glob import glob
+
+package_name = 'agv_pro_autocharge'
+
+setup(
+    name=package_name,
+    version='1.0.0',
+    packages=find_packages(),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        # Include config files
+        (os.path.join('share', package_name, 'config'), glob('config/*')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Your Name',
+    maintainer_email='your-email@example.com',
+    description='AGV automatic charging system for ROS2 Humble',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'combined_auto_recharger = agv_pro_autocharge.combined_auto_recharger:main',
+        ],
+    },
+)

agv_pro_base/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(agv_pro_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -42,6 +43,7 @@ ament_target_dependencies(agv_pro_node
   geometry_msgs
   serial_driver
   nav_msgs
+  agv_pro_msgs
 )
 
 install(TARGETS agv_pro_node

agv_pro_base/include/agv_pro_base/agv_pro_driver.h

@@ -2,7 +2,8 @@
 #define AGV_PRO_DRIVER_H
 
 #include <algorithm> 
-#include "serial_driver/serial_driver.hpp"
+#include <iostream>
+#include <boost/asio.hpp>
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -12,8 +13,18 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <agv_pro_msgs/srv/set_digital_output.hpp>
+#include <agv_pro_msgs/srv/get_digital_input.hpp>
 
-#define RECEIVE_DATA_SIZE 14         //The length of the data sent by the esp32
+#define SEND_DATA_SIZE 14                               // Total bytes in a command frame to ESP32(version>=V1.0.8)
+#define RECEIVE_FRAME_SIZE 31                           // Total bytes in a frame from ESP32(version>=V1.0.8)
+#define RECEIVE_PAYLOAD_SIZE (RECEIVE_FRAME_SIZE - 3)   // Payload length (excluding header)
+
+#define POWER_ON 0x10
+#define GET_POWER_STATE 0x12
+#define SET_AUTO_REPORT_STATE 0x23
+#define SET_OUTPUT_IO 0x40
+#define GET_INPUT_IO 0x41
 
 extern std::array<double, 36> odom_pose_covariance;
 extern std::array<double, 36> odom_twist_covariance;
@@ -21,20 +32,172 @@ extern std::array<double, 36> odom_twist_covariance;
 class AGV_PRO : public rclcpp::Node
 {
 public:
+  /**
+   * @brief Constructor
+   */
   AGV_PRO(std::string node_name);
+
+  /**
+   * @brief Destructor
+   */
   ~AGV_PRO();
+
 private:
+  /**
+   * @brief Main control loop for the AGV.
+   */
   void Control();
-  void print_hex(const std::string& label, const std::vector<uint8_t>& data, std::optional<size_t> override_size = std::nullopt);
+
+  /**
+   * @brief Print a vector of bytes in hexadecimal format to the ROS logger.
+   *
+   * @param[in] label A label to prepend to the printed data.
+   * @param[in] data The byte vector to print.
+   * @param[in] override_size Optional size to display instead of the full data length.
+   */
+  void print_hex(const std::string& label,
+    const std::vector<uint8_t>& data,
+    std::optional<size_t> override_size = std::nullopt);
+
+  /**
+   * @brief Send a serial frame to the AGV and optionally print it in hex.
+   *
+   * @param[in] frame The byte vector representing the serial frame to send.
+   * @param[in] debug If true, prints the transmitted frame using print_hex().
+   */
   void send_serial_frame(const std::vector<uint8_t>& frame, bool debug);
-  void is_power_on();
-  void set_auto_report();
+
+  /**
+   * @brief Query and print the current power status of the AGV.
+   * @return true if AGV is successfully power on; false otherwise.
+   */
+  bool is_power_on();
+
+  /**
+   * @brief Enable or disable AGV auto-reporting
+   * @param[in] enable 0 = disable, 1 = enable
+   */
+  void set_auto_report(bool enable);
+
+  /**
+   * @brief Clear the serial port input and output buffers
+   * @param[in] fd File descriptor of the serial port
+   */
+  void clearSerialBuffer(int fd);
+
+  /**
+   * @brief Disable the DTR (Data Terminal Ready) and RTS (Request To Send) lines of the serial port
+   * @param[in] fd File descriptor of the serial port
+   */
+  void disableDTR_RTS(int fd);
+
+  /**
+   * @brief Read sensor and motor data from the AGV via serial port.
+   * @return true if data is successfully read and verified; false otherwise.
+   */
   bool readData();
+
+  /**
+   * @brief Odometry publisher
+   * @param[in] dt Time difference (in seconds) since the last odometry update.
+   */
   void publisherOdom(double dt);
+
+  /**
+   * @brief Voltage publisher
+   */
   void publisherVoltage();
+
+  /**
+   * @brief ImuSensor publisher
+   */
+  void publisherImuSensor();
+
+  /**
+   * @brief Callback for velocity command updates
+   * @param[in] msg The Twist message containing desired linear and angular velocities
+   */
   void cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+  
+  /**
+   * @brief Build a standard AGV serial frame with header, payload, and CRC.
+   *
+   * The frame has a fixed size of RECEIVE_DATA_SIZE, starts with 0xFE 0xFE 0x0B,
+   * includes a command ID and up to 8 payload bytes, and ends with a 16-bit CRC.
+   *
+   * @param[in] cmd_id The command ID for the serial frame.
+   * @param[in] payload The payload bytes to include (up to 8 bytes).
+   * @return A vector containing the complete serial frame ready to transmit.
+   */
   std::vector<uint8_t> build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload);
-  std::vector<uint8_t> read_serial_response(const std::vector<uint8_t>& expected_header, size_t payload_size, double timeout_sec);
+
+  /**
+   * @brief Read a serial response from the AGV device, waiting for a specific header.
+   *
+   * This function reads bytes from the serial port until the expected header
+   * sequence is detected or the timeout expires. After detecting the header,
+   * it reads the remaining payload bytes along with a 2-byte CRC.
+   *
+   * @param[in] expected_header The byte sequence to identify the start of a valid frame.
+   * @param[in] payload_size The expected number of payload bytes following the header.
+   * @param[in] timeout_sec Maximum time (in seconds) to wait for the header.
+   * @return A vector containing the complete frame (header + payload + CRC).
+   *         Returns an empty vector if a timeout occurs or the full payload is not received.
+   */
+  std::vector<uint8_t> read_serial_response(
+    const std::vector<uint8_t>& expected_header,
+    size_t payload_size,
+    double timeout_sec);
+
+  /**
+   * @brief Compute the CRC-16-IBM checksum for a byte array.
+   *
+   * This function calculates the CRC using the standard IBM polynomial 0xA001.
+   *
+   * @param[in] data Pointer to the byte array.
+   * @param[in] length Number of bytes to include in the CRC calculation.
+   * @return The computed 16-bit CRC value.
+   */
+  uint16_t crc16_ibm(const uint8_t* data, size_t length);
+
+  /**
+   * @brief Handle the SetDigitalOutput service request.
+   *
+   * This service sets the state (HIGH/LOW) of a specific digital output pin on the AGV device.
+   * The request contains the pin number and desired state, which are sent to the hardware
+   * via the serial interface. The response reports whether the operation succeeded.
+   *
+   * @param[in] request The service request, containing:
+   *   - pin: The digital output pin number.
+   *   - state: Desired output state (true = HIGH, false = LOW).
+   * @param[out] response The service response, containing:
+   *   - success: True if the operation succeeded.
+   *   - message: Optional status or error description.
+   */
+  void handleSetDigitalOutput(
+    const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> response);
+
+  /**
+   * @brief Handle the GetDigitalInput service request.
+   *
+   * This service reads the state (HIGH/LOW) of a specific digital input pin on the AGV device.
+   * The request specifies the pin number, and the node queries the hardware via the serial
+   * interface to retrieve its current state.
+   *
+   * @param[in] request The service request, containing:
+   *   - pin: The digital input pin number to read.
+   * @param[out] response The service response, containing:
+   *   - state: Current pin state (true = HIGH, false = LOW).
+   *   - success: True if the read operation succeeded.
+   *   - message: Optional status or error description.
+   */
+  void handleGetDigitalInput(
+    const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response);
+
+  boost::asio::io_service io_;
+  std::unique_ptr<boost::asio::serial_port> serial_port_;
 
   std::string frame_id_of_odometry_;
   std::string child_frame_id_of_odometry_;
@@ -54,6 +217,18 @@ private:
   double linearY = 0.0;
   double angularZ = 0.0;
 
+  double ax= 0.0;
+  double ay= 0.0;
+  double az= 0.0;
+
+  double wx= 0.0;
+  double wy= 0.0;
+  double wz= 0.0;
+
+  double roll = 0.0;
+  double pitch = 0.0;
+  double yaw = 0.0;
+
   int is_poweron_status = 0;
   int poweron_status = 0;
 
@@ -63,17 +238,33 @@ private:
   
   float battery_voltage = 0.0f;
 
+  std::array<double, 36> odom_pose_covariance = {
+    {1e-9, 0, 0, 0, 0, 0,
+    0, 1e-3, 1e-9, 0, 0, 0,
+    0, 0, 1e6, 0, 0, 0,
+    0, 0, 0, 1e6, 0, 0,
+    0, 0, 0, 0, 1e6, 0,
+    0, 0, 0, 0, 0, 1e-9} };
+
+  std::array<double, 36> odom_twist_covariance = {
+    {1e-9, 0, 0, 0, 0, 0,
+    0, 1e-3, 1e-9, 0, 0, 0,
+    0, 0, 1e6, 0, 0, 0,
+    0, 0, 0, 1e6, 0, 0,
+    0, 0, 0, 0, 1e6, 0,
+    0, 0, 0, 0, 0, 1e-9} };
+
   rclcpp::Time currentTime, lastTime;
   rclcpp::TimerBase::SharedPtr control_timer_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
   rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu;
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_voltage;
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub;
+  rclcpp::Service<agv_pro_msgs::srv::SetDigitalOutput>::SharedPtr set_output_service;
+  rclcpp::Service<agv_pro_msgs::srv::GetDigitalInput>::SharedPtr get_input_service;
 
+  sensor_msgs::msg::Imu imu_data;
   std::unique_ptr<tf2_ros::TransformBroadcaster> odomBroadcaster;
-  std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
-  std::shared_ptr<drivers::common::IoContext> io_context_;
-
 };
 
 #endif

agv_pro_base/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>agv_pro_base</name>
-  <version>1.0.3</version>
+  <version>1.0.8</version>
   <description>Control Nodes for AGV Pro</description>
   <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
   <license>BSD-3-Clause license</license>
@@ -16,6 +16,7 @@
   <depend>asio_cmake_module</depend>
   <depend>io_context</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>agv_pro_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

agv_pro_base/src/agv_pro_ros.cpp

@@ -1,22 +1,6 @@
 #include "agv_pro_base/agv_pro_driver.h"
 
-std::array<double, 36> odom_pose_covariance = {
-  {1e-9, 0, 0, 0, 0, 0,
-  0, 1e-3, 1e-9, 0, 0, 0,
-  0, 0, 1e6, 0, 0, 0,
-  0, 0, 0, 1e6, 0, 0,
-  0, 0, 0, 0, 1e6, 0,
-  0, 0, 0, 0, 0, 1e-9} };
-
-std::array<double, 36> odom_twist_covariance = {
-  {1e-9, 0, 0, 0, 0, 0,
-  0, 1e-3, 1e-9, 0, 0, 0,
-  0, 0, 1e6, 0, 0, 0,
-  0, 0, 0, 1e6, 0, 0,
-  0, 0, 0, 0, 1e6, 0,
-  0, 0, 0, 0, 0, 1e-9} };
-
-uint16_t crc16_ibm(const uint8_t* data, size_t length) {
+uint16_t AGV_PRO::crc16_ibm(const uint8_t* data, size_t length) {
   uint16_t crc = 0xFFFF;
   for (size_t i = 0; i < length; ++i) {
     crc ^= static_cast<uint16_t>(data[i]);
@@ -32,7 +16,7 @@ uint16_t crc16_ibm(const uint8_t* data, size_t length) {
 
 std::vector<uint8_t> AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload)
 {
-  std::vector<uint8_t> frame(RECEIVE_DATA_SIZE, 0x00);
+  std::vector<uint8_t> frame(SEND_DATA_SIZE, 0x00);
   frame[0] = 0xFE;
   frame[1] = 0xFE;
   frame[2] = 0x0B;
@@ -62,8 +46,7 @@ void AGV_PRO::print_hex(const std::string& label, const std::vector<uint8_t>& da
 void AGV_PRO::send_serial_frame(const std::vector<uint8_t>& frame, bool debug)
 {
   try {
-    auto port = serial_driver_->port();
-    size_t bytes_transmit_size = port->send(frame);
+    size_t bytes_transmit_size = boost::asio::write(*serial_port_, boost::asio::buffer(frame));
     if (debug) {
       print_hex("Sent", frame, bytes_transmit_size);
     }
@@ -72,9 +55,11 @@ void AGV_PRO::send_serial_frame(const std::vector<uint8_t>& frame, bool debug)
   }
 }
 
-std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& expected_header, size_t payload_size, double timeout_sec)
+std::vector<uint8_t> AGV_PRO::read_serial_response(
+  const std::vector<uint8_t>& expected_header,
+  size_t payload_size,
+  double timeout_sec)
 {
-  auto port = serial_driver_->port();
   std::vector<uint8_t> sliding_buf;
   uint8_t byte = 0;
 
@@ -82,21 +67,24 @@ std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& e
   rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec);
 
   while ((this->now() - start_time) < timeout) {
-    std::vector<uint8_t> temp_buf(1);
-    if (port->receive(temp_buf) == 1) {
-      byte = temp_buf[0];
+    boost::asio::mutable_buffers_1 buf(&byte, 1);
+    boost::system::error_code ec;
+    size_t n = serial_port_->read_some(buf, ec);
+    if (ec) {
+        RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+        return {};
+    }
+    if (n == 1) {
       sliding_buf.push_back(byte);
-
       if (sliding_buf.size() > expected_header.size()) {
-        sliding_buf.erase(sliding_buf.begin());
+          sliding_buf.erase(sliding_buf.begin());
       }
-
       if (sliding_buf == expected_header) {
-        break;
+          break;
       }
     }
   }
-
+  
   if (sliding_buf != expected_header) {
     RCLCPP_WARN(this->get_logger(), "Timeout waiting for header");
     return {};
@@ -104,7 +92,20 @@ std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& e
 
   size_t remain_len = payload_size + 2;
   std::vector<uint8_t> remain_buf(remain_len);
-  if (port->receive(remain_buf) != remain_len) {
+  size_t total_read = 0;
+
+  while (total_read < remain_len && (this->now() - start_time) < timeout) {
+    boost::asio::mutable_buffers_1 buf(&remain_buf[total_read], remain_len - total_read);
+    boost::system::error_code ec;
+    size_t n = serial_port_->read_some(buf, ec);
+    if (ec) {
+        RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+        return {};
+    }
+    total_read += n;
+  }
+
+  if (total_read != remain_len) {
     RCLCPP_WARN(this->get_logger(), "Timeout or incomplete data payload");
     return {};
   }
@@ -115,29 +116,29 @@ std::vector<uint8_t> AGV_PRO::read_serial_response(const std::vector<uint8_t>& e
   return full_buf;
 }
 
-void AGV_PRO::is_power_on(){
-  auto power_query_frame = build_serial_frame(0x12, {});
+bool AGV_PRO::is_power_on(){
+  auto power_query_frame = build_serial_frame(GET_POWER_STATE, {});
   send_serial_frame(power_query_frame,true);
 
   const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, 0x12};
-  auto power_query_response = read_serial_response(expected_header, 8, 1.0);
+  auto power_query_response = read_serial_response(expected_header, 8, 12.0);
 
   print_hex("recv_buf", power_query_response);
   
-  if (power_query_response.size() != 14) return;
+  if (power_query_response.size() != 14) return false;
 
   uint16_t received_crc = (power_query_response[12] << 8) | power_query_response[13];
   uint16_t computed_crc = crc16_ibm(power_query_response.data(), 12);
   if (received_crc != computed_crc) {
     RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
-    return;
+    return false;
   }
 
   int is_poweron_status = static_cast<int8_t>(power_query_response[4]);
   RCLCPP_INFO(this->get_logger(), "is_poweron_status: %d", is_poweron_status);
 
   if (is_poweron_status == 0){
-    auto status_query_frame = build_serial_frame(0x10, {});
+    auto status_query_frame = build_serial_frame(POWER_ON, {});
     send_serial_frame(status_query_frame,true);
 
     rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command
@@ -146,13 +147,13 @@ void AGV_PRO::is_power_on(){
     auto status_query_response = read_serial_response(expected_header, 8, 5.0);// Read the serial response with the specified expected header, payload size, and timeout of 5 seconds
     print_hex("recv_buf", status_query_response);
   
-    if (status_query_response.size() != 14) return;
+    if (status_query_response.size() != 14) return false;
 
     uint16_t received_crc = (status_query_response[12] << 8) | status_query_response[13];
     uint16_t computed_crc = crc16_ibm(status_query_response.data(), 12);
     if (received_crc != computed_crc) {
       RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
-      return;
+      return false;
     }
 
     int poweron_status = static_cast<int8_t>(status_query_response[4]);
@@ -162,37 +163,61 @@ void AGV_PRO::is_power_on(){
       case 1:
         status_msg = "Motor is operating normally.";
         RCLCPP_INFO(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return true;
       case 2:
         status_msg = "Emergency stop button is not released.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       case 3:
         status_msg = "Battery voltage is below 19.5V.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       case 4:
         status_msg = "CAN initialization error.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       case 5:
         status_msg = "Motor initialization error.";
         RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
-        break;
+        return false;
       default:
         RCLCPP_WARN(this->get_logger(), "power_status: %d, Unknown power status code", poweron_status);
-        break;
+        return false;
     }
   }
-  else
+  else{
     RCLCPP_INFO(this->get_logger(), "Motor is operating normally.");
+    return true;
+  }
 }
 
-void AGV_PRO::set_auto_report(){
-  auto frame = build_serial_frame(0x23, {0x01});
+void AGV_PRO::set_auto_report(bool enable){
+  auto frame = build_serial_frame(0x23, {static_cast<uint8_t>(enable)});
   send_serial_frame(frame,true);
 }
 
+void AGV_PRO::clearSerialBuffer(int fd) {
+  if (tcflush(fd, TCIOFLUSH) < 0) {
+    RCLCPP_WARN(this->get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno));
+  } else {
+    RCLCPP_INFO(this->get_logger(), "Serial buffer flushed.");
+  }
+}
+
+void AGV_PRO::disableDTR_RTS(int fd) {
+  int status;
+  if (::ioctl(fd, TIOCMGET, &status) == 0) {
+    status &= ~(TIOCM_DTR | TIOCM_RTS);
+    if (::ioctl(fd, TIOCMSET, &status) != 0) {
+      RCLCPP_WARN(this->get_logger(), "Failed to clear DTR and RTS: %s", std::strerror(errno));
+    } else {
+      RCLCPP_INFO(this->get_logger(), "DTR and RTS lines disabled successfully.");
+    }
+  } else {
+    RCLCPP_WARN(this->get_logger(), "Failed to read modem status: %s", std::strerror(errno));
+  }
+}
+
 void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 {
   linearX = std::clamp(msg->linear.x, -1.5, 1.5);
@@ -220,11 +245,9 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 
   std::vector<uint8_t> data_vec(buf, buf + sizeof(buf));
 
-  auto port = serial_driver_->port();
-
   try
   {
-    port->send(data_vec);
+    boost::asio::write(*serial_port_,boost::asio::buffer(data_vec));
     // print_hex("Sent", data_vec);//debug
   }
   catch(const std::exception &ex)
@@ -233,36 +256,114 @@ void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
   }
 }
 
+void AGV_PRO::handleSetDigitalOutput(
+  const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> response)
+{
+  uint8_t output_number = request->pin;
+  uint8_t output_state = request->state;
+
+  if (output_number < 1 || output_number > 6){
+    RCLCPP_ERROR(this->get_logger(), "Invalid output pin number: %u", output_number);
+    response->success = false;
+    response->message = "Invalid output pin number";
+    return;
+  }
+
+  auto frame = build_serial_frame(SET_OUTPUT_IO, {output_number, output_state});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_OUTPUT_IO};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[4];
+  if (status == 0x01) {
+    RCLCPP_INFO(this->get_logger(), "SetDigitalOutput succeeded");
+    response->success = true;
+    response->message = "Success";
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "SetDigitalOutput failed with status: 0x%02X", status);
+    response->success = false;
+    response->message = "Failed with status code";
+  }
+}
+
+void AGV_PRO::handleGetDigitalInput(
+  const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response)
+{
+  uint8_t input_number = request->pin;
+
+  if (input_number < 1 || input_number > 6){
+    RCLCPP_ERROR(this->get_logger(), "Invalid input pin number: %u", input_number);
+    response->success = false;
+    response->message = "Invalid input pin number";
+    return;
+  }
+
+  auto frame = build_serial_frame(GET_INPUT_IO, {input_number});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, GET_INPUT_IO};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[5];
+  if (status == 0xff) {
+    RCLCPP_ERROR(this->get_logger(), "GetDigitalInput failed with status: 0x%02X", status);
+    response->success = false;
+  } else {
+    RCLCPP_INFO(this->get_logger(), "GetDigitalInput succeeded, state: %u", status);
+    response->state = static_cast<int32_t>(status);
+    response->success = true;
+    response->message = "Success";
+  }
+}
+
 bool AGV_PRO::readData()
 {
-  std::vector<uint8_t> buf_header(1);
   std::vector<uint8_t> buf_length(1);
-  std::vector<uint8_t> data_buf(RECEIVE_DATA_SIZE-3);
+  std::vector<uint8_t> data_buf(RECEIVE_PAYLOAD_SIZE);
 
-  auto port = serial_driver_->port();
+  uint8_t byte = 0;
+  boost::system::error_code ec;
   
   while (true)
   {
-    size_t ret = port->receive(buf_header);
-    if (ret != 1 || buf_header[0] != 0xfe) {
+    size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
+    if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+    }
+    if (ret != 1 || byte != 0xfe) {
       continue;
     }
 
-    ret = port->receive(buf_header);
-    if (ret == 1 && buf_header[0] == 0xfe) {
+    ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
+    if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+    }
+    if (ret == 1 && byte == 0xfe) {
       break; 
     }
   }
 
-  size_t ret = port->receive(buf_length);
+  size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(buf_length), ec);
+  if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+  }
 
-  if (buf_length[0] != 0x0b) {
-    RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
+  if (buf_length[0] != RECEIVE_PAYLOAD_SIZE) {
+    //RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
     return false;
   }
 
-  ret = port->receive(data_buf);
-  if (ret != data_buf.size())
+  ret = boost::asio::read(*serial_port_, boost::asio::buffer(data_buf), ec);
+  if (ec || ret != data_buf.size())
   {
     RCLCPP_ERROR(this->get_logger(), "Failed to receive full payload");
     return false;
@@ -271,18 +372,18 @@ bool AGV_PRO::readData()
   std::vector<uint8_t> recv_buf;
   recv_buf.push_back(0xFE);
   recv_buf.push_back(0xFE);
-  recv_buf.push_back(0x0B);
+  recv_buf.push_back(0x1C);
   recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end());
   
   // print_hex("recv_buf", recv_buf); //debug
 
   if (recv_buf[3] != 0x25) {
-    // RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]);
+    //RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); //debug
     return false;
   }
 
-  uint16_t received_crc = recv_buf[13] | (recv_buf[12] << 8);
-  uint16_t computed_crc = crc16_ibm(recv_buf.data(), 12);
+  uint16_t received_crc = recv_buf[RECEIVE_FRAME_SIZE-1] | (recv_buf[RECEIVE_FRAME_SIZE-2] << 8);
+  uint16_t computed_crc = crc16_ibm(recv_buf.data(), RECEIVE_FRAME_SIZE-2);
 
   if (received_crc != computed_crc) {
     RCLCPP_WARN(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc);
@@ -298,6 +399,30 @@ bool AGV_PRO::readData()
   battery_voltage = static_cast<float>(recv_buf[9]) / 10.0f;
   enable_status = recv_buf[10];
 
+  imu_data.linear_acceleration.x = static_cast<double>(static_cast<int16_t>((recv_buf[11] << 8) | recv_buf[12])) * 0.01;
+  imu_data.linear_acceleration.y = static_cast<double>(static_cast<int16_t>((recv_buf[13] << 8) | recv_buf[14])) * 0.01;
+  imu_data.linear_acceleration.z = static_cast<double>(static_cast<int16_t>((recv_buf[15] << 8) | recv_buf[16])) * 0.01;
+
+  imu_data.angular_velocity.x = static_cast<double>(static_cast<int16_t>((recv_buf[17] << 8) | recv_buf[18])) * 0.01;
+  imu_data.angular_velocity.y = static_cast<double>(static_cast<int16_t>((recv_buf[19] << 8) | recv_buf[20])) * 0.01;
+  imu_data.angular_velocity.z = static_cast<double>(static_cast<int16_t>((recv_buf[21] << 8) | recv_buf[22])) * 0.01;
+
+  roll  = static_cast<double>(static_cast<int16_t>((recv_buf[23] << 8) | recv_buf[24])) * 0.01;
+  pitch = static_cast<double>(static_cast<int16_t>((recv_buf[25] << 8) | recv_buf[26])) * 0.01;
+  yaw   = static_cast<double>(static_cast<int16_t>((recv_buf[27] << 8) | recv_buf[28])) * 0.01;
+
+  // RCLCPP_INFO(this->get_logger(),
+  // "IMU Data - Accel[x: %.2f, y: %.2f, z: %.2f], "
+  // "Gyro[x: %.2f, y: %.2f, z: %.2f], "
+  // "RPY[roll: %.2f, pitch: %.2f, yaw: %.2f]",
+  // imu_data.linear_acceleration.x,
+  // imu_data.linear_acceleration.y,
+  // imu_data.linear_acceleration.z,
+  // imu_data.angular_velocity.x,
+  // imu_data.angular_velocity.y,
+  // imu_data.angular_velocity.z,
+  // roll, pitch, yaw);
+
   return true;
 }
 
@@ -308,6 +433,40 @@ void AGV_PRO::publisherVoltage()
   pub_voltage->publish(voltage_msg);
 }
 
+void AGV_PRO::publisherImuSensor()
+{
+  sensor_msgs::msg::Imu ImuSensor;
+
+  ImuSensor.header.stamp = this->get_clock()->now();
+  ImuSensor.header.frame_id = "imu_link";
+
+  tf2::Quaternion qua;
+  qua.setRPY(0, 0, yaw * M_PI / 180.0);
+
+  ImuSensor.orientation.x = qua[0];
+  ImuSensor.orientation.y = qua[1];
+  ImuSensor.orientation.z = qua[2];
+  ImuSensor.orientation.w = qua[3];
+
+  ImuSensor.angular_velocity.x = imu_data.angular_velocity.x;
+  ImuSensor.angular_velocity.y = imu_data.angular_velocity.y;
+  ImuSensor.angular_velocity.z = imu_data.angular_velocity.z;
+
+  ImuSensor.linear_acceleration.x = imu_data.linear_acceleration.x;
+  ImuSensor.linear_acceleration.y = imu_data.linear_acceleration.y;
+  ImuSensor.linear_acceleration.z = imu_data.linear_acceleration.z;
+
+  ImuSensor.orientation_covariance[0] = 1e6;
+  ImuSensor.orientation_covariance[4] = 1e6;
+  ImuSensor.orientation_covariance[8] = 1e-6;
+
+  ImuSensor.angular_velocity_covariance[0] = 1e6;
+  ImuSensor.angular_velocity_covariance[4] = 1e6;
+  ImuSensor.angular_velocity_covariance[8] = 1e-6;
+
+  pub_imu->publish(ImuSensor);
+}
+
 void AGV_PRO::publisherOdom(double dt)
 {
   currentTime = this->get_clock()->now();
@@ -345,12 +504,12 @@ void AGV_PRO::publisherOdom(double dt)
   odom.pose.pose.position.y = y;
   odom.pose.pose.position.z = 0.0;
   odom.pose.pose.orientation = odom_quat;
-  odom.pose.covariance = odom_pose_covariance;
+  odom.pose.covariance = this->odom_pose_covariance;
 
   odom.twist.twist.linear.x = vx;
   odom.twist.twist.linear.y = vy;
   odom.twist.twist.angular.z = vtheta;
-  odom.twist.covariance = odom_twist_covariance;
+  odom.twist.covariance = this->odom_twist_covariance;
 
   pub_odom->publish(odom);
 }
@@ -369,6 +528,7 @@ void AGV_PRO::Control()
     publisherOdom(dt);
     // RCLCPP_INFO(this->get_logger(), "dt:%f", dt);
     publisherVoltage();
+    publisherImuSensor();
   }
 }
 
@@ -399,66 +559,66 @@ AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
   cmd_sub = this->create_subscription<geometry_msgs::msg::Twist>(
     "/cmd_vel", 10, std::bind(&AGV_PRO::cmdCallback, this, std::placeholders::_1));
 
-  lastTime = this->get_clock()->now();
-      
-  drivers::serial_driver::SerialPortConfig config(
-    1000000,
-    drivers::serial_driver::FlowControl::NONE,
-    drivers::serial_driver::Parity::NONE,
-    drivers::serial_driver::StopBits::ONE
+  set_output_service = this->create_service<agv_pro_msgs::srv::SetDigitalOutput>(
+    "set_digital_output",
+    std::bind(&AGV_PRO::handleSetDigitalOutput, this, std::placeholders::_1, std::placeholders::_2)
   );
 
-  try{
-    io_context_ = std::make_shared<drivers::common::IoContext>(1);
-    serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
-    serial_driver_->init_port(device_name_, config);
-    serial_driver_->port()->open();
-    
-    RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
-    RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
-    RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
+  get_input_service = this->create_service<agv_pro_msgs::srv::GetDigitalInput>(
+    "get_digital_input",
+    std::bind(&AGV_PRO::handleGetDigitalInput, this, std::placeholders::_1, std::placeholders::_2)
+  );
 
-    AGV_PRO::is_power_on();
-    AGV_PRO::set_auto_report();
+  lastTime = this->get_clock()->now();
+      
+  try{
+    serial_port_ = std::make_unique<boost::asio::serial_port>(io_);
+
+    serial_port_->open(device_name_);
+    serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000));
+    serial_port_->set_option(boost::asio::serial_port_base::character_size(8));
+    serial_port_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
+    serial_port_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
+    serial_port_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
+
+    int fd = serial_port_->native_handle();
+    this->clearSerialBuffer(fd);
+    this->disableDTR_RTS(fd);
+
+    rclcpp::sleep_for(std::chrono::milliseconds(3000));//esp32 Restart time
+
+    RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
+    RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str());
+
+    boost::asio::serial_port_base::baud_rate baud_option;
+    serial_port_->get_option(baud_option);
+    unsigned int current_baud = baud_option.value();
+    RCLCPP_INFO(this->get_logger(), "Baud_rate: %u", current_baud);
   }
   catch (const std::exception &ex){
     RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
     return;
   }
-  
-  control_timer_ = this->create_wall_timer(
-  std::chrono::milliseconds(20),
-  std::bind(&AGV_PRO::Control, this)
-  );
-  RCLCPP_INFO(this->get_logger(), "Control timer started");
 
+  if (this->is_power_on()) {
+    this->set_auto_report(1);
+
+    control_timer_ = this->create_wall_timer(
+    std::chrono::milliseconds(20),
+    std::bind(&AGV_PRO::Control, this)
+    );
+    RCLCPP_INFO(this->get_logger(), "Control timer started");
+  }
+  else {
+    RCLCPP_WARN(this->get_logger(), "Control timer not started.");
+  }
 }
 
 AGV_PRO::~AGV_PRO()
-{ 
-  std::array<uint8_t, 14> buf = {
-    0xFE, 0xFE, 0x0b, 0x22, 
-    0x01, 0x00, 0x00, 0x00,
-    0x00, 0x00, 0x00, 0x00
-  };
-
-  uint16_t crc = crc16_ibm(buf.data(), 12);
-  buf[12] = (crc >> 8) & 0xff;
-  buf[13] = crc & 0xff;
-
-  std::vector<uint8_t> data_vec(buf.begin(), buf.end());
-
-  auto port = serial_driver_->port();
-
-  try
-  {
-    port->send(data_vec);
-  }
-  catch(const std::exception &ex)
-  {
-    RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
-  }
-
-  serial_driver_->port()->close();
-  RCLCPP_INFO(this->get_logger(),"Shutting down");
+{
+  if (serial_port_ && serial_port_->is_open()) {
+    this->set_auto_report(0);
+    serial_port_->cancel();
+    serial_port_->close();
+  } 
 }

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,
+        ]
+    )

agv_pro_bringup/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>agv_pro_bringup</name>
-  <version>1.0.0</version>
+  <version>1.0.8</version>
   <description>ROS 2 launch scripts for starting the AGV Pro</description>
   <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
   <license>BSD-3-Clause license</license>

agv_pro_description/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>agv_pro_description</name>
-  <version>1.0.1</version>
+  <version>1.0.8</version>
   <description>
     <p>URDF Description package for AGV pro</p>
   </description>

agv_pro_description/urdf/agv_pro.urdf

@@ -212,4 +212,21 @@
     <parent link="${namespace}base_link" />
     <child link="${namespace}laser_link" />
   </joint>
+
+  <link name="${namespace}camera_link"/>
+
+  <joint name="${namespace}camera_joint" type="fixed">
+    <origin xyz="0.23191 0 0.14928" rpy="0 0 0" />
+    <parent link="${namespace}base_link" />
+    <child link="${namespace}camera_link" />
+  </joint>
+
+  <link name="${namespace}imu_link"/>
+
+  <joint name="${namespace}imu_joint" type="fixed">
+    <origin xyz="-0.17181 -0.0270532 0.14928" rpy="0 0 1.5707" />
+    <parent link="${namespace}base_link" />
+    <child link="${namespace}imu_link" />
+  </joint>
+
 </robot>

agv_pro_msgs/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(agv_pro_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/AGVProStatus.msg"
+  "srv/SetDigitalOutput.srv"
+  "srv/GetDigitalInput.srv"
+  DEPENDENCIES std_msgs
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

agv_pro_msgs/msg/AGVProStatus.msg

@@ -0,0 +1,6 @@
+std_msgs/Header header
+  
+uint8 motor_status
+uint8 motor_error
+float64 battery_voltage
+uint8 enable_status

agv_pro_msgs/package.xml

@@ -0,0 +1,23 @@
+<?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>agv_pro_msgs</name>
+  <version>1.0.8</version>
+  <description>TODO: Package description</description>
+  <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

agv_pro_msgs/srv/GetDigitalInput.srv

@@ -0,0 +1,5 @@
+int32 pin
+---
+bool success
+int32 state
+string message

agv_pro_msgs/srv/SetDigitalOutput.srv

@@ -0,0 +1,5 @@
+int32 pin
+int32 state
+---
+bool success
+string message

agv_pro_navigation2/CMakeLists.txt

@@ -24,7 +24,7 @@ if(BUILD_TESTING)
 endif()
 
 install(
-  DIRECTORY launch map param rviz
+  DIRECTORY launch map param rviz scripts
   DESTINATION share/${PROJECT_NAME}
 )
 

agv_pro_navigation2/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>agv_pro_navigation2</name>
-  <version>1.0.0</version>
+  <version>1.0.8</version>
   <description>ROS2 launch scripts for navigation2</description>
   <maintainer email="weijun.xie@elephantrobotics.com">lanni</maintainer>
   <license>BSD-3-Clause license</license>

agv_pro_navigation2/param/agvpro.yaml

@@ -109,11 +109,11 @@ bt_navigator_rclcpp_node:
 controller_server:
   ros__parameters:
     use_sim_time: False
-    controller_frequency: 5.0
+    controller_frequency: 20.0
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
-    failure_tolerance: 3.0
+    failure_tolerance: 0.3
     progress_checker_plugin: "progress_checker"
     goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
     controller_plugins: ["FollowPath"]
@@ -121,8 +121,8 @@ controller_server:
     # Progress checker parameters
     progress_checker:
       plugin: "nav2_controller::SimpleProgressChecker"
-      required_movement_radius: 0.5
-      movement_time_allowance: 10.0
+      required_movement_radius: 0.15
+      movement_time_allowance: 8.0
     # Goal checker parameters
     #precise_goal_checker:
     #  plugin: "nav2_controller::SimpleGoalChecker"
@@ -150,13 +150,13 @@ controller_server:
       # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
       acc_lim_x: 2.5
       acc_lim_y: 0.0
-      acc_lim_theta: 0.25
+      acc_lim_theta: 3.2
       decel_lim_x: -2.5
       decel_lim_y: 0.0
-      decel_lim_theta: -0.25
+      decel_lim_theta: -3.2
       vx_samples: 20
       vy_samples: 5
-      vtheta_samples: 20
+      vtheta_samples: 40
       sim_time: 1.7
       linear_granularity: 0.05
       angular_granularity: 0.025
@@ -167,9 +167,9 @@ controller_server:
       stateful: True
       critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
       BaseObstacle.scale: 0.02
-      PathAlign.scale: 32.0
+      PathAlign.scale: 23.0
       PathAlign.forward_point_distance: 0.1
-      GoalAlign.scale: 24.0
+      GoalAlign.scale: 18.0
       GoalAlign.forward_point_distance: 0.1
       PathDist.scale: 32.0
       GoalDist.scale: 24.0
@@ -197,8 +197,8 @@ local_costmap:
       plugins: ["voxel_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 5.0
-        inflation_radius: 0.25
+        cost_scaling_factor: 4.0
+        inflation_radius: 0.30
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -232,12 +232,12 @@ local_costmap:
 global_costmap:
   global_costmap:
     ros__parameters:
-      update_frequency: 0.3
-      publish_frequency: 0.3
+      update_frequency: 1.0
+      publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_footprint
       use_sim_time: False
-      robot_radius: 0.22
+      footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]"
       resolution: 0.05
       track_unknown_space: true
       plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
@@ -260,8 +260,8 @@ global_costmap:
         map_subscribe_transient_local: True
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 5.0
-        inflation_radius: 0.25
+        cost_scaling_factor: 4.0
+        inflation_radius: 0.30
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
@@ -273,7 +273,9 @@ global_costmap:
 map_server:
   ros__parameters:
     use_sim_time: False
-    yaml_filename: "turtlebot3_world.yaml"
+    # Overridden in launch by the "map" launch configuration or provided default value.
+    # To use in yaml, remove the default "map" value in the navigation2_active.launch.py file & provide full path to map below.
+    yaml_filename: ""
 
 map_saver:
   ros__parameters:
@@ -298,6 +300,16 @@ planner_server_rclcpp_node:
   ros__parameters:
     use_sim_time: False
 
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
 recoveries_server:
   ros__parameters:
     costmap_topic: local_costmap/costmap_raw
@@ -332,3 +344,18 @@ waypoint_follower:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
       enabled: True
       waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: True
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.26, 0.0, 0.5]
+    min_velocity: [-0.26, 0.0, -0.5]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0

agv_pro_navigation2/scripts/example_nav_through_poses.py

@@ -0,0 +1,101 @@
+#! /usr/bin/env python3
+
+from geometry_msgs.msg import PoseStamped
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+import rclpy
+from rclpy.duration import Duration
+
+
+"""
+Basic navigation demo to go to poses.
+"""
+
+def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float):
+    """
+    Set the initial pose of the robot for AMCL localization.
+
+    Args:
+        navigator (BasicNavigator): The navigator instance controlling the robot.
+        x (float): Initial X position in the map frame.
+        y (float): Initial Y position in the map frame.
+        oz (float): Orientation Z component (quaternion).
+        ow (float): Orientation W component (quaternion).
+    """
+    initial_pose = PoseStamped()
+    initial_pose.header.frame_id = 'map'
+    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
+    initial_pose.pose.position.x = x
+    initial_pose.pose.position.y = y
+    initial_pose.pose.orientation.z = oz
+    initial_pose.pose.orientation.w = ow
+    navigator.setInitialPose(initial_pose)
+
+def create_pose(navigator: BasicNavigator, x, y, z, w):
+    pose = PoseStamped()
+    pose.header.frame_id = 'map'
+    pose.header.stamp = navigator.get_clock().now().to_msg()
+    pose.pose.position.x = x
+    pose.pose.position.y = y
+    pose.pose.orientation.z = z
+    pose.pose.orientation.w = w
+    return pose
+
+def nav_through_pose(navigator: BasicNavigator, goal_poses, verbose: bool = False) -> bool:
+    
+    nav_start = navigator.get_clock().now()
+    navigator.goThroughPoses(goal_poses)
+
+    while not navigator.isTaskComplete():
+        feedback = navigator.getFeedback()
+        if feedback and verbose:
+            remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9
+            print(f"Estimated time of arrival: {remaining:.0f} seconds")
+
+    # Do something depending on the return code
+    result = navigator.getResult()
+    if result == TaskResult.SUCCEEDED:
+        print('Goal succeeded!')
+        return True
+    elif result == TaskResult.CANCELED:
+        print('Goal was canceled!')
+    elif result == TaskResult.FAILED:
+        print('Goal failed!')
+    else:
+        print('Goal has an invalid return status!')
+    return False
+
+if __name__ == '__main__':
+    rclpy.init()
+
+    navigator = BasicNavigator()
+
+    # Set robot initial pose
+    # set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)\
+    
+    # Wait for navigation to fully activate, since autostarting nav2
+    # navigator.waitUntilNav2Active()
+
+    way1_goals = [
+        [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
+        [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
+        [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
+        [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
+    ]
+    way2_goals = [
+        [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
+        [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
+        [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
+        [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
+    ]
+
+    goal_poses_1 = [create_pose(navigator, *g) for g in way1_goals]
+    goal_poses_2 = [create_pose(navigator, *g) for g in way2_goals]
+    
+    result1 = nav_through_pose(navigator, goal_poses_1, verbose=True)
+    print(f"First segment navigation result: {result1}")
+    
+    if result1 ==True:
+        result2 = nav_through_pose(navigator, goal_poses_2, verbose=True)
+        print(f"Second segment navigation result: {result2}")
+
+    rclpy.shutdown()

agv_pro_navigation2/scripts/example_nav_to_pose.py

@@ -0,0 +1,97 @@
+#! /usr/bin/env python3
+
+from geometry_msgs.msg import PoseStamped
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+import rclpy
+from rclpy.duration import Duration
+
+"""
+Basic navigation demo to go to pose.
+"""
+
+def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float):
+    """
+    Set the initial pose of the robot for AMCL localization.
+
+    Args:
+        navigator (BasicNavigator): The navigator instance controlling the robot.
+        x (float): Initial X position in the map frame.
+        y (float): Initial Y position in the map frame.
+        oz (float): Orientation Z component (quaternion).
+        ow (float): Orientation W component (quaternion).
+    """
+    initial_pose = PoseStamped()
+    initial_pose.header.frame_id = 'map'
+    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
+    initial_pose.pose.position.x = x
+    initial_pose.pose.position.y = y
+    initial_pose.pose.orientation.z = oz
+    initial_pose.pose.orientation.w = ow
+    navigator.setInitialPose(initial_pose)
+
+
+def navigate_to_goal(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float, verbose: bool = False) -> bool:
+    """
+    Navigate the robot to a target goal pose.
+
+    Args:
+        navigator (BasicNavigator): The navigator instance controlling the robot.
+        x (float): Goal X position in the map frame.
+        y (float): Goal Y position in the map frame.
+        oz (float): Orientation Z component (quaternion).
+        ow (float): Orientation W component (quaternion).
+        verbose (bool, optional): If True, prints navigation feedback such as estimated arrival time. Default is False.
+
+    Returns:
+        bool: True if navigation succeeded, False otherwise.
+    """
+    goal_pose = PoseStamped()
+    goal_pose.header.frame_id = 'map'
+    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
+    goal_pose.pose.position.x = x
+    goal_pose.pose.position.y = y
+    goal_pose.pose.orientation.z = oz
+    goal_pose.pose.orientation.w = ow
+
+    navigator.goToPose(goal_pose)
+
+    while not navigator.isTaskComplete():
+        feedback = navigator.getFeedback()
+        if feedback and verbose:
+            remaining = Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9
+            print(f"Estimated time of arrival: {remaining:.0f} seconds")
+
+    result = navigator.getResult()
+    if result == TaskResult.SUCCEEDED:
+        print('Goal succeeded!')
+        return True
+    elif result == TaskResult.CANCELED:
+        print('Goal was canceled!')
+    elif result == TaskResult.FAILED:
+        print('Goal failed!')
+    else:
+        print('Goal has an invalid return status!')
+    return False
+
+if __name__ == '__main__':
+    rclpy.init()
+    navigator = BasicNavigator()
+
+    # Set robot initial pose
+    # set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)
+
+    # Wait for navigation to fully activate, since autostarting nav2
+    # navigator.waitUntilNav2Active()
+
+    goal_A = [1.6766083240509033,0.37930558800697327,-0.03491306994337919, 0.9993903529387947]
+    goal_B = [-0.5062443017959595,1.559376835823059,0.6869307039904945,0.7267229237578264]
+
+    x_goal, y_goal, orientation_z, orientation_w = goal_A
+    success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w)
+    print("Navigation result:", success)
+
+    x_goal, y_goal, orientation_z, orientation_w = goal_B
+    success = navigate_to_goal(navigator, x_goal, y_goal, orientation_z, orientation_w)
+    print("Navigation result:", success)
+
+    rclpy.shutdown()

agv_pro_navigation2/scripts/example_waypoint_follower.py

@@ -0,0 +1,108 @@
+#! /usr/bin/env python3
+
+from geometry_msgs.msg import PoseStamped
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+import rclpy
+from rclpy.duration import Duration
+
+"""
+Basic navigation demo to go to poses.
+"""
+
+def set_initial_pose(navigator: BasicNavigator, x: float, y: float, oz: float, ow: float):
+    """
+    Set the initial pose of the robot for AMCL localization.
+
+    Args:
+        navigator (BasicNavigator): The navigator instance controlling the robot.
+        x (float): Initial X position in the map frame.
+        y (float): Initial Y position in the map frame.
+        oz (float): Orientation Z component (quaternion).
+        ow (float): Orientation W component (quaternion).
+    """
+    initial_pose = PoseStamped()
+    initial_pose.header.frame_id = 'map'
+    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
+    initial_pose.pose.position.x = x
+    initial_pose.pose.position.y = y
+    initial_pose.pose.orientation.z = oz
+    initial_pose.pose.orientation.w = ow
+    navigator.setInitialPose(initial_pose)
+
+def create_pose(navigator: BasicNavigator, x, y, z, w):
+    pose = PoseStamped()
+    pose.header.frame_id = 'map'
+    pose.header.stamp = navigator.get_clock().now().to_msg()
+    pose.pose.position.x = x
+    pose.pose.position.y = y
+    pose.pose.orientation.z = z
+    pose.pose.orientation.w = w
+    return pose
+
+def nav_waypoint_follower(navigator: BasicNavigator, goal_poses, verbose: bool = False) -> bool:
+    
+    nav_start = navigator.get_clock().now()
+    navigator.followWaypoints(goal_poses)
+
+    i = 0
+    while not navigator.isTaskComplete():
+        # Do something with the feedback
+        i = i + 1
+        feedback = navigator.getFeedback()
+        if (feedback and i % 5) and verbose == 0:
+            print('Executing current waypoint: ' +
+                  str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses)))
+            now = navigator.get_clock().now()
+
+            # Some navigation timeout to demo cancellation
+            if now - nav_start > Duration(seconds=600.0):
+                navigator.cancelTask()
+
+    # Do something depending on the return code
+    result = navigator.getResult()
+    if result == TaskResult.SUCCEEDED:
+        print('Goal succeeded!')
+        return True
+    elif result == TaskResult.CANCELED:
+        print('Goal was canceled!')
+    elif result == TaskResult.FAILED:
+        print('Goal failed!')
+    else:
+        print('Goal has an invalid return status!')
+    return False
+
+if __name__ == '__main__':
+    rclpy.init()
+
+    navigator = BasicNavigator()
+
+    # Set robot initial pose
+    # set_initial_pose(navigator, x=-1.9248794317245483, y=-0.5366987586021423, oz=-1.8463129131030735e-06, ow=0.9999999999982956)\
+    
+    # Wait for navigation to fully activate, since autostarting nav2
+    # navigator.waitUntilNav2Active()
+
+    way1_goals = [
+        [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
+        [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
+        [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
+        [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
+    ]
+    way2_goals = [
+        [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
+        [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
+        [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
+        [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877]
+    ]
+
+    goal_poses_1 = [create_pose(navigator, *g) for g in way1_goals]
+    goal_poses_2 = [create_pose(navigator, *g) for g in way2_goals]
+  
+    result1 = nav_waypoint_follower(navigator, goal_poses_1, verbose=False)
+    print(f"First segment navigation result: {result1}")
+    
+    if result1 ==True:
+        result2 = nav_waypoint_follower(navigator, goal_poses_2, verbose=False)
+        print(f"Second segment navigation result: {result2}")
+
+    rclpy.shutdown()

livox_ros_driver2/.gitignore

@@ -0,0 +1,4 @@
+.vscode
+build
+package.xml
+__pycache__

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_

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_