m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #5 from elephantrobotics/humble-slam

Browse Source 
Humble slam
This commit is contained in:
X-lanni
2026-01-13 19:15:10 +08:00
committed by GitHub
parent b50a0c4496 ddef31d11a
commit de06b7a1a3
239 changed files with 55881 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
FAST_LIO/.github/stale.yml
Vendored
+17
View File
@@ -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
+8
View File
@@ -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
+4
View File
@@ -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
+128
View File
@@ -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
+339
View File
@@ -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
+135
View File
@@ -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
+1
View File
@@ -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
+94
View File
@@ -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
+1
View File
@@ -0,0 +1 @@
1
FAST_LIO/README.md
+199
View File
@@ -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
+46
View File
@@ -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
+46
View File
@@ -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
+50
View File
@@ -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
+47
View File
@@ -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
+61
View File
@@ -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
+48
View File
@@ -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
+103
View File
@@ -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/esekfom.hpp
Executable
+2008
View File
File diff suppressed because it is too large Load Diff
FAST_LIO/include/IKFoM_toolkit/esekfom/util.hpp
Executable
+82
View File
@@ -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
Executable
+229
View File
@@ -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
Executable
+123
View File
@@ -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
Executable
+294
View File
@@ -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
Executable
+168
View File
@@ -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
Executable
+328
View File
@@ -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
Executable
+316
View File
@@ -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
Executable
+317
View File
@@ -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
Executable
+461
View File
@@ -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
Executable
+113
View File
@@ -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
+270
View File
@@ -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
+2
View File
@@ -0,0 +1,2 @@
# ikd-Tree
ikd-Tree is an incremental k-d tree for robotic applications.
FAST_LIO/include/ikd-Tree/ikd_Tree.cpp
+1728
View File
File diff suppressed because it is too large Load Diff
FAST_LIO/include/ikd-Tree/ikd_Tree.h
+344
View File
@@ -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/matplotlibcpp.h
+2499
View File
File diff suppressed because it is too large Load Diff
FAST_LIO/include/so3_math.h
+111
View File
@@ -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
+126
View File
@@ -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
+22
View File
@@ -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
+70
View File
@@ -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
+70
View File
@@ -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
+7
View File
@@ -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
+38
View File
@@ -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
+304
View File
@@ -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/.gitignore
View File
FAST_LIO/rviz_cfg/loam_livox.rviz
+358
View File
@@ -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
+379
View File
@@ -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/laserMapping.cpp
+1194
View File
File diff suppressed because it is too large Load Diff
FAST_LIO/src/preprocess.cpp
+1069
View File
File diff suppressed because it is too large Load Diff
FAST_LIO/src/preprocess.h
+196
View File
@@ -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_bringup/launch/agv_pro_bringup.launch.py
+74 -39
View File
@@ -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,
]
)
livox_ros_driver2/.gitignore
+4
View File
@@ -0,0 +1,4 @@
.vscode
build
package.xml
__pycache__
livox_ros_driver2/3rdparty/rapidjson/allocators.h
Vendored
+308
View File
@@ -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
Vendored
+81
View File
@@ -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/document.h
Vendored
+3309
View File
File diff suppressed because it is too large Load Diff
livox_ros_driver2/3rdparty/rapidjson/encodedstream.h
Vendored
+407
View File
@@ -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
Vendored
+816
View File
@@ -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
Vendored
+104
View File
@@ -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
Vendored
+186
View File
@@ -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
Vendored
+123
View File
@@ -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
Vendored
+128
View File
@@ -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
Vendored
+170
View File
@@ -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
Vendored
+295
View File
@@ -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
Vendored
+77
View File
@@ -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
Vendored
+305
View File
@@ -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
Vendored
+269
View File
@@ -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
Vendored
+100
View File
@@ -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
Vendored
+288
View File
@@ -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
Vendored
+243
View File
@@ -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
Vendored
+77
View File
@@ -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
Vendored
+753
View File
@@ -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
Vendored
+245
View File
@@ -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
Vendored
+74
View File
@@ -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
Vendored
+303
View File
@@ -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
Vendored
+50
View File
@@ -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_
livox_ros_driver2/3rdparty/rapidjson/istreamwrapper.h
Vendored
+161
View File
@@ -0,0 +1,161 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ISTREAMWRAPPER_H_
#define RAPIDJSON_ISTREAMWRAPPER_H_
#include <ios>
#include <iosfwd>
#include "stream.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
#elif defined(_MSC_VER)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4351) // new behavior: elements of array 'array' will be
// default initialized
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Wrapper of \c std::basic_istream into RapidJSON's Stream concept.
/*!
The classes can be wrapped including but not limited to:
- \c std::istringstream
- \c std::stringstream
- \c std::wistringstream
- \c std::wstringstream
- \c std::ifstream
- \c std::fstream
- \c std::wifstream
- \c std::wfstream
\tparam StreamType Class derived from \c std::basic_istream.
*/
template <typename StreamType>
class BasicIStreamWrapper {
public:
typedef typename StreamType::char_type Ch;
//! Constructor.
/*!
\param stream stream opened for read.
*/
BasicIStreamWrapper(StreamType &stream)
: stream_(stream),
buffer_(peekBuffer_),
bufferSize_(4),
bufferLast_(0),
current_(buffer_),
readCount_(0),
count_(0),
eof_(false) {
Read();
}
//! Constructor.
/*!
\param stream stream opened for read.
\param buffer user-supplied buffer.
\param bufferSize size of buffer in bytes. Must >=4 bytes.
*/
BasicIStreamWrapper(StreamType &stream, char *buffer, size_t bufferSize)
: stream_(stream),
buffer_(buffer),
bufferSize_(bufferSize),
bufferLast_(0),
current_(buffer_),
readCount_(0),
count_(0),
eof_(false) {
RAPIDJSON_ASSERT(bufferSize >= 4);
Read();
}
Ch Peek() const { return *current_; }
Ch Take() {
Ch c = *current_;
Read();
return c;
}
size_t Tell() const {
return count_ + static_cast<size_t>(current_ - buffer_);
}
// Not implemented
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
// For encoding detection only.
const Ch *Peek4() const {
return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
}
private:
BasicIStreamWrapper();
BasicIStreamWrapper(const BasicIStreamWrapper &);
BasicIStreamWrapper &operator=(const BasicIStreamWrapper &);
void Read() {
if (current_ < bufferLast_)
++current_;
else if (!eof_) {
count_ += readCount_;
readCount_ = bufferSize_;
bufferLast_ = buffer_ + readCount_ - 1;
current_ = buffer_;
if (!stream_.read(buffer_, static_cast<std::streamsize>(bufferSize_))) {
readCount_ = static_cast<size_t>(stream_.gcount());
*(bufferLast_ = buffer_ + readCount_) = '\0';
eof_ = true;
}
}
}
StreamType &stream_;
Ch peekBuffer_[4], *buffer_;
size_t bufferSize_;
Ch *bufferLast_;
Ch *current_;
size_t readCount_;
size_t count_; //!< Number of characters read
bool eof_;
};
typedef BasicIStreamWrapper<std::istream> IStreamWrapper;
typedef BasicIStreamWrapper<std::wistream> WIStreamWrapper;
#if defined(__clang__) || defined(_MSC_VER)
RAPIDJSON_DIAG_POP
#endif
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_ISTREAMWRAPPER_H_
livox_ros_driver2/3rdparty/rapidjson/license.txt
Vendored
+57
View File
@@ -0,0 +1,57 @@
Tencent is pleased to support the open source community by making RapidJSON available.
Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
If you have downloaded a copy of the RapidJSON binary from Tencent, please note that the RapidJSON binary is licensed under the MIT License.
If you have downloaded a copy of the RapidJSON source code from Tencent, please note that RapidJSON source code is licensed under the MIT License, except for the third-party components listed below which are subject to different license terms. Your integration of RapidJSON into your own projects may require compliance with the MIT License, as well as the other licenses applicable to the third-party components included within RapidJSON. To avoid the problematic JSON license in your own projects, it's sufficient to exclude the bin/jsonchecker/ directory, as it's the only code under the JSON license.
A copy of the MIT License is included in this file.
Other dependencies and licenses:
Open Source Software Licensed Under the BSD License:
--------------------------------------------------------------------
The msinttypes r29
Copyright (c) 2006-2013 Alexander Chemeris
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
* Neither the name of copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Open Source Software Licensed Under the JSON License:
--------------------------------------------------------------------
json.org
Copyright (c) 2002 JSON.org
All Rights Reserved.
JSON_checker
Copyright (c) 2002 JSON.org
All Rights Reserved.
Terms of the JSON License:
---------------------------------------------------
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
The Software shall be used for Good, not Evil.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
Terms of the MIT License:
--------------------------------------------------------------------
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
livox_ros_driver2/3rdparty/rapidjson/memorybuffer.h
Vendored
+78
View File
@@ -0,0 +1,78 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_MEMORYBUFFER_H_
#define RAPIDJSON_MEMORYBUFFER_H_
#include "internal/stack.h"
#include "stream.h"
RAPIDJSON_NAMESPACE_BEGIN
//! Represents an in-memory output byte stream.
/*!
This class is mainly for being wrapped by EncodedOutputStream or
AutoUTFOutputStream.
It is similar to FileWriteBuffer but the destination is an in-memory buffer
instead of a file.
Differences between MemoryBuffer and StringBuffer:
1. StringBuffer has Encoding but MemoryBuffer is only a byte buffer.
2. StringBuffer::GetString() returns a null-terminated string.
MemoryBuffer::GetBuffer() returns a buffer without terminator.
\tparam Allocator type for allocating memory buffer.
\note implements Stream concept
*/
template <typename Allocator = CrtAllocator>
struct GenericMemoryBuffer {
typedef char Ch; // byte
GenericMemoryBuffer(Allocator *allocator = 0,
size_t capacity = kDefaultCapacity)
: stack_(allocator, capacity) {}
void Put(Ch c) { *stack_.template Push<Ch>() = c; }
void Flush() {}
void Clear() { stack_.Clear(); }
void ShrinkToFit() { stack_.ShrinkToFit(); }
Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
void Pop(size_t count) { stack_.template Pop<Ch>(count); }
const Ch *GetBuffer() const { return stack_.template Bottom<Ch>(); }
size_t GetSize() const { return stack_.GetSize(); }
static const size_t kDefaultCapacity = 256;
mutable internal::Stack<Allocator> stack_;
};
typedef GenericMemoryBuffer<> MemoryBuffer;
//! Implement specialized version of PutN() with memset() for better
//! performance.
template <>
inline void PutN(MemoryBuffer &memoryBuffer, char c, size_t n) {
std::memset(memoryBuffer.stack_.Push<char>(n), c, n * sizeof(c));
}
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_MEMORYBUFFER_H_
livox_ros_driver2/3rdparty/rapidjson/memorystream.h
Vendored
+84
View File
@@ -0,0 +1,84 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_MEMORYSTREAM_H_
#define RAPIDJSON_MEMORYSTREAM_H_
#include "stream.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(unreachable - code)
RAPIDJSON_DIAG_OFF(missing - noreturn)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Represents an in-memory input byte stream.
/*!
This class is mainly for being wrapped by EncodedInputStream or
AutoUTFInputStream.
It is similar to FileReadBuffer but the source is an in-memory buffer
instead of a file.
Differences between MemoryStream and StringStream:
1. StringStream has encoding but MemoryStream is a byte stream.
2. MemoryStream needs size of the source buffer and the buffer don't need to
be null terminated. StringStream assume null-terminated string as source.
3. MemoryStream supports Peek4() for encoding detection. StringStream is
specified with an encoding so it should not have Peek4(). \note implements
Stream concept
*/
struct MemoryStream {
typedef char Ch; // byte
MemoryStream(const Ch *src, size_t size)
: src_(src), begin_(src), end_(src + size), size_(size) {}
Ch Peek() const { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_; }
Ch Take() { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_++; }
size_t Tell() const { return static_cast<size_t>(src_ - begin_); }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
// For encoding detection only.
const Ch *Peek4() const { return Tell() + 4 <= size_ ? src_ : 0; }
const Ch *src_; //!< Current read position.
const Ch *begin_; //!< Original head of the string.
const Ch *end_; //!< End of stream.
size_t size_; //!< Size of the stream.
};
RAPIDJSON_NAMESPACE_END
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_MEMORYBUFFER_H_
livox_ros_driver2/3rdparty/rapidjson/msinttypes/inttypes.h
Vendored
+316
View File
@@ -0,0 +1,316 @@
// ISO C9x compliant inttypes.h for Microsoft Visual Studio
// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
//
// Copyright (c) 2006-2013 Alexander Chemeris
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the product nor the names of its contributors may
// be used to endorse or promote products derived from this software
// without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////////
// The above software in this distribution may have been modified by
// THL A29 Limited ("Tencent Modifications").
// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
#ifndef _MSC_VER // [
#error "Use this header only with Microsoft Visual C++ compilers!"
#endif // _MSC_VER ]
#ifndef _MSC_INTTYPES_H_ // [
#define _MSC_INTTYPES_H_
#if _MSC_VER > 1000
#pragma once
#endif
#include "stdint.h"
// miloyip: VC supports inttypes.h since VC2013
#if _MSC_VER >= 1800
#include <inttypes.h>
#else
// 7.8 Format conversion of integer types
typedef struct {
intmax_t quot;
intmax_t rem;
} imaxdiv_t;
// 7.8.1 Macros for format specifiers
#if !defined(__cplusplus) || \
defined(__STDC_FORMAT_MACROS) // [ See footnote 185 at page 198
// The fprintf macros for signed integers are:
#define PRId8 "d"
#define PRIi8 "i"
#define PRIdLEAST8 "d"
#define PRIiLEAST8 "i"
#define PRIdFAST8 "d"
#define PRIiFAST8 "i"
#define PRId16 "hd"
#define PRIi16 "hi"
#define PRIdLEAST16 "hd"
#define PRIiLEAST16 "hi"
#define PRIdFAST16 "hd"
#define PRIiFAST16 "hi"
#define PRId32 "I32d"
#define PRIi32 "I32i"
#define PRIdLEAST32 "I32d"
#define PRIiLEAST32 "I32i"
#define PRIdFAST32 "I32d"
#define PRIiFAST32 "I32i"
#define PRId64 "I64d"
#define PRIi64 "I64i"
#define PRIdLEAST64 "I64d"
#define PRIiLEAST64 "I64i"
#define PRIdFAST64 "I64d"
#define PRIiFAST64 "I64i"
#define PRIdMAX "I64d"
#define PRIiMAX "I64i"
#define PRIdPTR "Id"
#define PRIiPTR "Ii"
// The fprintf macros for unsigned integers are:
#define PRIo8 "o"
#define PRIu8 "u"
#define PRIx8 "x"
#define PRIX8 "X"
#define PRIoLEAST8 "o"
#define PRIuLEAST8 "u"
#define PRIxLEAST8 "x"
#define PRIXLEAST8 "X"
#define PRIoFAST8 "o"
#define PRIuFAST8 "u"
#define PRIxFAST8 "x"
#define PRIXFAST8 "X"
#define PRIo16 "ho"
#define PRIu16 "hu"
#define PRIx16 "hx"
#define PRIX16 "hX"
#define PRIoLEAST16 "ho"
#define PRIuLEAST16 "hu"
#define PRIxLEAST16 "hx"
#define PRIXLEAST16 "hX"
#define PRIoFAST16 "ho"
#define PRIuFAST16 "hu"
#define PRIxFAST16 "hx"
#define PRIXFAST16 "hX"
#define PRIo32 "I32o"
#define PRIu32 "I32u"
#define PRIx32 "I32x"
#define PRIX32 "I32X"
#define PRIoLEAST32 "I32o"
#define PRIuLEAST32 "I32u"
#define PRIxLEAST32 "I32x"
#define PRIXLEAST32 "I32X"
#define PRIoFAST32 "I32o"
#define PRIuFAST32 "I32u"
#define PRIxFAST32 "I32x"
#define PRIXFAST32 "I32X"
#define PRIo64 "I64o"
#define PRIu64 "I64u"
#define PRIx64 "I64x"
#define PRIX64 "I64X"
#define PRIoLEAST64 "I64o"
#define PRIuLEAST64 "I64u"
#define PRIxLEAST64 "I64x"
#define PRIXLEAST64 "I64X"
#define PRIoFAST64 "I64o"
#define PRIuFAST64 "I64u"
#define PRIxFAST64 "I64x"
#define PRIXFAST64 "I64X"
#define PRIoMAX "I64o"
#define PRIuMAX "I64u"
#define PRIxMAX "I64x"
#define PRIXMAX "I64X"
#define PRIoPTR "Io"
#define PRIuPTR "Iu"
#define PRIxPTR "Ix"
#define PRIXPTR "IX"
// The fscanf macros for signed integers are:
#define SCNd8 "d"
#define SCNi8 "i"
#define SCNdLEAST8 "d"
#define SCNiLEAST8 "i"
#define SCNdFAST8 "d"
#define SCNiFAST8 "i"
#define SCNd16 "hd"
#define SCNi16 "hi"
#define SCNdLEAST16 "hd"
#define SCNiLEAST16 "hi"
#define SCNdFAST16 "hd"
#define SCNiFAST16 "hi"
#define SCNd32 "ld"
#define SCNi32 "li"
#define SCNdLEAST32 "ld"
#define SCNiLEAST32 "li"
#define SCNdFAST32 "ld"
#define SCNiFAST32 "li"
#define SCNd64 "I64d"
#define SCNi64 "I64i"
#define SCNdLEAST64 "I64d"
#define SCNiLEAST64 "I64i"
#define SCNdFAST64 "I64d"
#define SCNiFAST64 "I64i"
#define SCNdMAX "I64d"
#define SCNiMAX "I64i"
#ifdef _WIN64 // [
#define SCNdPTR "I64d"
#define SCNiPTR "I64i"
#else // _WIN64 ][
#define SCNdPTR "ld"
#define SCNiPTR "li"
#endif // _WIN64 ]
// The fscanf macros for unsigned integers are:
#define SCNo8 "o"
#define SCNu8 "u"
#define SCNx8 "x"
#define SCNX8 "X"
#define SCNoLEAST8 "o"
#define SCNuLEAST8 "u"
#define SCNxLEAST8 "x"
#define SCNXLEAST8 "X"
#define SCNoFAST8 "o"
#define SCNuFAST8 "u"
#define SCNxFAST8 "x"
#define SCNXFAST8 "X"
#define SCNo16 "ho"
#define SCNu16 "hu"
#define SCNx16 "hx"
#define SCNX16 "hX"
#define SCNoLEAST16 "ho"
#define SCNuLEAST16 "hu"
#define SCNxLEAST16 "hx"
#define SCNXLEAST16 "hX"
#define SCNoFAST16 "ho"
#define SCNuFAST16 "hu"
#define SCNxFAST16 "hx"
#define SCNXFAST16 "hX"
#define SCNo32 "lo"
#define SCNu32 "lu"
#define SCNx32 "lx"
#define SCNX32 "lX"
#define SCNoLEAST32 "lo"
#define SCNuLEAST32 "lu"
#define SCNxLEAST32 "lx"
#define SCNXLEAST32 "lX"
#define SCNoFAST32 "lo"
#define SCNuFAST32 "lu"
#define SCNxFAST32 "lx"
#define SCNXFAST32 "lX"
#define SCNo64 "I64o"
#define SCNu64 "I64u"
#define SCNx64 "I64x"
#define SCNX64 "I64X"
#define SCNoLEAST64 "I64o"
#define SCNuLEAST64 "I64u"
#define SCNxLEAST64 "I64x"
#define SCNXLEAST64 "I64X"
#define SCNoFAST64 "I64o"
#define SCNuFAST64 "I64u"
#define SCNxFAST64 "I64x"
#define SCNXFAST64 "I64X"
#define SCNoMAX "I64o"
#define SCNuMAX "I64u"
#define SCNxMAX "I64x"
#define SCNXMAX "I64X"
#ifdef _WIN64 // [
#define SCNoPTR "I64o"
#define SCNuPTR "I64u"
#define SCNxPTR "I64x"
#define SCNXPTR "I64X"
#else // _WIN64 ][
#define SCNoPTR "lo"
#define SCNuPTR "lu"
#define SCNxPTR "lx"
#define SCNXPTR "lX"
#endif // _WIN64 ]
#endif // __STDC_FORMAT_MACROS ]
// 7.8.2 Functions for greatest-width integer types
// 7.8.2.1 The imaxabs function
#define imaxabs _abs64
// 7.8.2.2 The imaxdiv function
// This is modified version of div() function from Microsoft's div.c found
// in %MSVC.NET%\crt\src\div.c
#ifdef STATIC_IMAXDIV // [
static
#else // STATIC_IMAXDIV ][
_inline
#endif // STATIC_IMAXDIV ]
imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) {
imaxdiv_t result;
result.quot = numer / denom;
result.rem = numer % denom;
if (numer < 0 && result.rem > 0) {
// did division wrong; must fix up
++result.quot;
result.rem -= denom;
}
return result;
}
// 7.8.2.3 The strtoimax and strtoumax functions
#define strtoimax _strtoi64
#define strtoumax _strtoui64
// 7.8.2.4 The wcstoimax and wcstoumax functions
#define wcstoimax _wcstoi64
#define wcstoumax _wcstoui64
#endif // _MSC_VER >= 1800
#endif // _MSC_INTTYPES_H_ ]
livox_ros_driver2/3rdparty/rapidjson/msinttypes/stdint.h
Vendored
+301
View File
@@ -0,0 +1,301 @@
// ISO C9x compliant stdint.h for Microsoft Visual Studio
// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
//
// Copyright (c) 2006-2013 Alexander Chemeris
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the product nor the names of its contributors may
// be used to endorse or promote products derived from this software
// without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////////
// The above software in this distribution may have been modified by
// THL A29 Limited ("Tencent Modifications").
// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
#ifndef _MSC_VER // [
#error "Use this header only with Microsoft Visual C++ compilers!"
#endif // _MSC_VER ]
#ifndef _MSC_STDINT_H_ // [
#define _MSC_STDINT_H_
#if _MSC_VER > 1000
#pragma once
#endif
// miloyip: Originally Visual Studio 2010 uses its own stdint.h. However it
// generates warning with INT64_C(), so change to use this file for vs2010.
#if _MSC_VER >= 1600 // [
#include <stdint.h>
#if !defined(__cplusplus) || \
defined(__STDC_CONSTANT_MACROS) // [ See footnote 224 at page 260
#undef INT8_C
#undef INT16_C
#undef INT32_C
#undef INT64_C
#undef UINT8_C
#undef UINT16_C
#undef UINT32_C
#undef UINT64_C
// 7.18.4.1 Macros for minimum-width integer constants
#define INT8_C(val) val##i8
#define INT16_C(val) val##i16
#define INT32_C(val) val##i32
#define INT64_C(val) val##i64
#define UINT8_C(val) val##ui8
#define UINT16_C(val) val##ui16
#define UINT32_C(val) val##ui32
#define UINT64_C(val) val##ui64
// 7.18.4.2 Macros for greatest-width integer constants
// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
// Check out Issue 9 for the details.
#ifndef INTMAX_C // [
#define INTMAX_C INT64_C
#endif // INTMAX_C ]
#ifndef UINTMAX_C // [
#define UINTMAX_C UINT64_C
#endif // UINTMAX_C ]
#endif // __STDC_CONSTANT_MACROS ]
#else // ] _MSC_VER >= 1700 [
#include <limits.h>
// For Visual Studio 6 in C++ mode and for many Visual Studio versions when
// compiling for ARM we have to wrap <wchar.h> include with 'extern "C++" {}'
// or compiler would give many errors like this:
// error C2733: second C linkage of overloaded function 'wmemchr' not allowed
#if defined(__cplusplus) && !defined(_M_ARM)
extern "C" {
#endif
#include <wchar.h>
#if defined(__cplusplus) && !defined(_M_ARM)
}
#endif
// Define _W64 macros to mark types changing their size, like intptr_t.
#ifndef _W64
#if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300
#define _W64 __w64
#else
#define _W64
#endif
#endif
// 7.18.1 Integer types
// 7.18.1.1 Exact-width integer types
// Visual Studio 6 and Embedded Visual C++ 4 doesn't
// realize that, e.g. char has the same size as __int8
// so we give up on __intX for them.
#if (_MSC_VER < 1300)
typedef signed char int8_t;
typedef signed short int16_t;
typedef signed int int32_t;
typedef unsigned char uint8_t;
typedef unsigned short uint16_t;
typedef unsigned int uint32_t;
#else
typedef signed __int8 int8_t;
typedef signed __int16 int16_t;
typedef signed __int32 int32_t;
typedef unsigned __int8 uint8_t;
typedef unsigned __int16 uint16_t;
typedef unsigned __int32 uint32_t;
#endif
typedef signed __int64 int64_t;
typedef unsigned __int64 uint64_t;
// 7.18.1.2 Minimum-width integer types
typedef int8_t int_least8_t;
typedef int16_t int_least16_t;
typedef int32_t int_least32_t;
typedef int64_t int_least64_t;
typedef uint8_t uint_least8_t;
typedef uint16_t uint_least16_t;
typedef uint32_t uint_least32_t;
typedef uint64_t uint_least64_t;
// 7.18.1.3 Fastest minimum-width integer types
typedef int8_t int_fast8_t;
typedef int16_t int_fast16_t;
typedef int32_t int_fast32_t;
typedef int64_t int_fast64_t;
typedef uint8_t uint_fast8_t;
typedef uint16_t uint_fast16_t;
typedef uint32_t uint_fast32_t;
typedef uint64_t uint_fast64_t;
// 7.18.1.4 Integer types capable of holding object pointers
#ifdef _WIN64 // [
typedef signed __int64 intptr_t;
typedef unsigned __int64 uintptr_t;
#else // _WIN64 ][
typedef _W64 signed int intptr_t;
typedef _W64 unsigned int uintptr_t;
#endif // _WIN64 ]
// 7.18.1.5 Greatest-width integer types
typedef int64_t intmax_t;
typedef uint64_t uintmax_t;
// 7.18.2 Limits of specified-width integer types
#if !defined(__cplusplus) || \
defined(__STDC_LIMIT_MACROS) // [ See footnote 220 at page 257 and
// footnote 221 at page 259
// 7.18.2.1 Limits of exact-width integer types
#define INT8_MIN ((int8_t)_I8_MIN)
#define INT8_MAX _I8_MAX
#define INT16_MIN ((int16_t)_I16_MIN)
#define INT16_MAX _I16_MAX
#define INT32_MIN ((int32_t)_I32_MIN)
#define INT32_MAX _I32_MAX
#define INT64_MIN ((int64_t)_I64_MIN)
#define INT64_MAX _I64_MAX
#define UINT8_MAX _UI8_MAX
#define UINT16_MAX _UI16_MAX
#define UINT32_MAX _UI32_MAX
#define UINT64_MAX _UI64_MAX
// 7.18.2.2 Limits of minimum-width integer types
#define INT_LEAST8_MIN INT8_MIN
#define INT_LEAST8_MAX INT8_MAX
#define INT_LEAST16_MIN INT16_MIN
#define INT_LEAST16_MAX INT16_MAX
#define INT_LEAST32_MIN INT32_MIN
#define INT_LEAST32_MAX INT32_MAX
#define INT_LEAST64_MIN INT64_MIN
#define INT_LEAST64_MAX INT64_MAX
#define UINT_LEAST8_MAX UINT8_MAX
#define UINT_LEAST16_MAX UINT16_MAX
#define UINT_LEAST32_MAX UINT32_MAX
#define UINT_LEAST64_MAX UINT64_MAX
// 7.18.2.3 Limits of fastest minimum-width integer types
#define INT_FAST8_MIN INT8_MIN
#define INT_FAST8_MAX INT8_MAX
#define INT_FAST16_MIN INT16_MIN
#define INT_FAST16_MAX INT16_MAX
#define INT_FAST32_MIN INT32_MIN
#define INT_FAST32_MAX INT32_MAX
#define INT_FAST64_MIN INT64_MIN
#define INT_FAST64_MAX INT64_MAX
#define UINT_FAST8_MAX UINT8_MAX
#define UINT_FAST16_MAX UINT16_MAX
#define UINT_FAST32_MAX UINT32_MAX
#define UINT_FAST64_MAX UINT64_MAX
// 7.18.2.4 Limits of integer types capable of holding object pointers
#ifdef _WIN64 // [
#define INTPTR_MIN INT64_MIN
#define INTPTR_MAX INT64_MAX
#define UINTPTR_MAX UINT64_MAX
#else // _WIN64 ][
#define INTPTR_MIN INT32_MIN
#define INTPTR_MAX INT32_MAX
#define UINTPTR_MAX UINT32_MAX
#endif // _WIN64 ]
// 7.18.2.5 Limits of greatest-width integer types
#define INTMAX_MIN INT64_MIN
#define INTMAX_MAX INT64_MAX
#define UINTMAX_MAX UINT64_MAX
// 7.18.3 Limits of other integer types
#ifdef _WIN64 // [
#define PTRDIFF_MIN _I64_MIN
#define PTRDIFF_MAX _I64_MAX
#else // _WIN64 ][
#define PTRDIFF_MIN _I32_MIN
#define PTRDIFF_MAX _I32_MAX
#endif // _WIN64 ]
#define SIG_ATOMIC_MIN INT_MIN
#define SIG_ATOMIC_MAX INT_MAX
#ifndef SIZE_MAX // [
#ifdef _WIN64 // [
#define SIZE_MAX _UI64_MAX
#else // _WIN64 ][
#define SIZE_MAX _UI32_MAX
#endif // _WIN64 ]
#endif // SIZE_MAX ]
// WCHAR_MIN and WCHAR_MAX are also defined in <wchar.h>
#ifndef WCHAR_MIN // [
#define WCHAR_MIN 0
#endif // WCHAR_MIN ]
#ifndef WCHAR_MAX // [
#define WCHAR_MAX _UI16_MAX
#endif // WCHAR_MAX ]
#define WINT_MIN 0
#define WINT_MAX _UI16_MAX
#endif // __STDC_LIMIT_MACROS ]
// 7.18.4 Limits of other integer types
#if !defined(__cplusplus) || \
defined(__STDC_CONSTANT_MACROS) // [ See footnote 224 at page 260
// 7.18.4.1 Macros for minimum-width integer constants
#define INT8_C(val) val##i8
#define INT16_C(val) val##i16
#define INT32_C(val) val##i32
#define INT64_C(val) val##i64
#define UINT8_C(val) val##ui8
#define UINT16_C(val) val##ui16
#define UINT32_C(val) val##ui32
#define UINT64_C(val) val##ui64
// 7.18.4.2 Macros for greatest-width integer constants
// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
// Check out Issue 9 for the details.
#ifndef INTMAX_C // [
#define INTMAX_C INT64_C
#endif // INTMAX_C ]
#ifndef UINTMAX_C // [
#define UINTMAX_C UINT64_C
#endif // UINTMAX_C ]
#endif // __STDC_CONSTANT_MACROS ]
#endif // _MSC_VER >= 1600 ]
#endif // _MSC_STDINT_H_ ]
livox_ros_driver2/3rdparty/rapidjson/ostreamwrapper.h
Vendored
+96
View File
@@ -0,0 +1,96 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_OSTREAMWRAPPER_H_
#define RAPIDJSON_OSTREAMWRAPPER_H_
#include <iosfwd>
#include "stream.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Wrapper of \c std::basic_ostream into RapidJSON's Stream concept.
/*!
The classes can be wrapped including but not limited to:
- \c std::ostringstream
- \c std::stringstream
- \c std::wpstringstream
- \c std::wstringstream
- \c std::ifstream
- \c std::fstream
- \c std::wofstream
- \c std::wfstream
\tparam StreamType Class derived from \c std::basic_ostream.
*/
template <typename StreamType>
class BasicOStreamWrapper {
public:
typedef typename StreamType::char_type Ch;
BasicOStreamWrapper(StreamType &stream) : stream_(stream) {}
void Put(Ch c) { stream_.put(c); }
void Flush() { stream_.flush(); }
// Not implemented
char Peek() const {
RAPIDJSON_ASSERT(false);
return 0;
}
char Take() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t Tell() const {
RAPIDJSON_ASSERT(false);
return 0;
}
char *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(char *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
BasicOStreamWrapper(const BasicOStreamWrapper &);
BasicOStreamWrapper &operator=(const BasicOStreamWrapper &);
StreamType &stream_;
};
typedef BasicOStreamWrapper<std::ostream> OStreamWrapper;
typedef BasicOStreamWrapper<std::wostream> WOStreamWrapper;
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_OSTREAMWRAPPER_H_
livox_ros_driver2/3rdparty/rapidjson/pointer.h
Vendored
+1712
View File
File diff suppressed because it is too large Load Diff
livox_ros_driver2/3rdparty/rapidjson/prettywriter.h
Vendored
+333
View File
@@ -0,0 +1,333 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_PRETTYWRITER_H_
#define RAPIDJSON_PRETTYWRITER_H_
#include "writer.h"
#ifdef __GNUC__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
#endif
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Combination of PrettyWriter format flags.
/*! \see PrettyWriter::SetFormatOptions
*/
enum PrettyFormatOptions {
kFormatDefault = 0, //!< Default pretty formatting.
kFormatSingleLineArray = 1 //!< Format arrays on a single line.
};
//! Writer with indentation and spacing.
/*!
\tparam OutputStream Type of output os.
\tparam SourceEncoding Encoding of source string.
\tparam TargetEncoding Encoding of output stream.
\tparam StackAllocator Type of allocator for allocating memory of stack.
*/
template <typename OutputStream, typename SourceEncoding = UTF8<>,
typename TargetEncoding = UTF8<>,
typename StackAllocator = CrtAllocator,
unsigned writeFlags = kWriteDefaultFlags>
class PrettyWriter : public Writer<OutputStream, SourceEncoding, TargetEncoding,
StackAllocator, writeFlags> {
public:
typedef Writer<OutputStream, SourceEncoding, TargetEncoding, StackAllocator,
writeFlags>
Base;
typedef typename Base::Ch Ch;
//! Constructor
/*! \param os Output stream.
\param allocator User supplied allocator. If it is null, it will create a
private one. \param levelDepth Initial capacity of stack.
*/
explicit PrettyWriter(OutputStream &os, StackAllocator *allocator = 0,
size_t levelDepth = Base::kDefaultLevelDepth)
: Base(os, allocator, levelDepth),
indentChar_(' '),
indentCharCount_(4),
formatOptions_(kFormatDefault) {}
explicit PrettyWriter(StackAllocator *allocator = 0,
size_t levelDepth = Base::kDefaultLevelDepth)
: Base(allocator, levelDepth), indentChar_(' '), indentCharCount_(4) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
PrettyWriter(PrettyWriter &&rhs)
: Base(std::forward<PrettyWriter>(rhs)),
indentChar_(rhs.indentChar_),
indentCharCount_(rhs.indentCharCount_),
formatOptions_(rhs.formatOptions_) {}
#endif
//! Set custom indentation.
/*! \param indentChar Character for indentation. Must be whitespace
character (' ', '\\t', '\\n', '\\r'). \param indentCharCount Number of
indent characters for each indentation level. \note The default indentation
is 4 spaces.
*/
PrettyWriter &SetIndent(Ch indentChar, unsigned indentCharCount) {
RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' ||
indentChar == '\n' || indentChar == '\r');
indentChar_ = indentChar;
indentCharCount_ = indentCharCount;
return *this;
}
//! Set pretty writer formatting options.
/*! \param options Formatting options.
*/
PrettyWriter &SetFormatOptions(PrettyFormatOptions options) {
formatOptions_ = options;
return *this;
}
/*! @name Implementation of Handler
\see Handler
*/
//@{
bool Null() {
PrettyPrefix(kNullType);
return Base::EndValue(Base::WriteNull());
}
bool Bool(bool b) {
PrettyPrefix(b ? kTrueType : kFalseType);
return Base::EndValue(Base::WriteBool(b));
}
bool Int(int i) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteInt(i));
}
bool Uint(unsigned u) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteUint(u));
}
bool Int64(int64_t i64) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteInt64(i64));
}
bool Uint64(uint64_t u64) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteUint64(u64));
}
bool Double(double d) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteDouble(d));
}
bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteString(str, length));
}
bool String(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
PrettyPrefix(kStringType);
return Base::EndValue(Base::WriteString(str, length));
}
#if RAPIDJSON_HAS_STDSTRING
bool String(const std::basic_string<Ch> &str) {
return String(str.data(), SizeType(str.size()));
}
#endif
bool StartObject() {
PrettyPrefix(kObjectType);
new (Base::level_stack_.template Push<typename Base::Level>())
typename Base::Level(false);
return Base::WriteStartObject();
}
bool Key(const Ch *str, SizeType length, bool copy = false) {
return String(str, length, copy);
}
#if RAPIDJSON_HAS_STDSTRING
bool Key(const std::basic_string<Ch> &str) {
return Key(str.data(), SizeType(str.size()));
}
#endif
bool EndObject(SizeType memberCount = 0) {
(void)memberCount;
RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
sizeof(typename Base::Level)); // not inside an Object
RAPIDJSON_ASSERT(!Base::level_stack_.template Top<typename Base::Level>()
->inArray); // currently inside an Array, not Object
RAPIDJSON_ASSERT(
0 ==
Base::level_stack_.template Top<typename Base::Level>()->valueCount %
2); // Object has a Key without a Value
bool empty =
Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
0;
if (!empty) {
Base::os_->Put('\n');
WriteIndent();
}
bool ret = Base::EndValue(Base::WriteEndObject());
(void)ret;
RAPIDJSON_ASSERT(ret == true);
if (Base::level_stack_.Empty()) // end of json text
Base::Flush();
return true;
}
bool StartArray() {
PrettyPrefix(kArrayType);
new (Base::level_stack_.template Push<typename Base::Level>())
typename Base::Level(true);
return Base::WriteStartArray();
}
bool EndArray(SizeType memberCount = 0) {
(void)memberCount;
RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
sizeof(typename Base::Level));
RAPIDJSON_ASSERT(
Base::level_stack_.template Top<typename Base::Level>()->inArray);
bool empty =
Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
0;
if (!empty && !(formatOptions_ & kFormatSingleLineArray)) {
Base::os_->Put('\n');
WriteIndent();
}
bool ret = Base::EndValue(Base::WriteEndArray());
(void)ret;
RAPIDJSON_ASSERT(ret == true);
if (Base::level_stack_.Empty()) // end of json text
Base::Flush();
return true;
}
//@}
/*! @name Convenience extensions */
//@{
//! Simpler but slower overload.
bool String(const Ch *str) { return String(str, internal::StrLen(str)); }
bool Key(const Ch *str) { return Key(str, internal::StrLen(str)); }
//@}
//! Write a raw JSON value.
/*!
For user to write a stringified JSON as a value.
\param json A well-formed JSON value. It should not contain null character
within [0, length - 1] range. \param length Length of the json. \param type
Type of the root of json. \note When using PrettyWriter::RawValue(), the
result json may not be indented correctly.
*/
bool RawValue(const Ch *json, size_t length, Type type) {
RAPIDJSON_ASSERT(json != 0);
PrettyPrefix(type);
return Base::EndValue(Base::WriteRawValue(json, length));
}
protected:
void PrettyPrefix(Type type) {
(void)type;
if (Base::level_stack_.GetSize() != 0) { // this value is not at root
typename Base::Level *level =
Base::level_stack_.template Top<typename Base::Level>();
if (level->inArray) {
if (level->valueCount > 0) {
Base::os_->Put(
','); // add comma if it is not the first element in array
if (formatOptions_ & kFormatSingleLineArray) Base::os_->Put(' ');
}
if (!(formatOptions_ & kFormatSingleLineArray)) {
Base::os_->Put('\n');
WriteIndent();
}
} else { // in object
if (level->valueCount > 0) {
if (level->valueCount % 2 == 0) {
Base::os_->Put(',');
Base::os_->Put('\n');
} else {
Base::os_->Put(':');
Base::os_->Put(' ');
}
} else
Base::os_->Put('\n');
if (level->valueCount % 2 == 0) WriteIndent();
}
if (!level->inArray && level->valueCount % 2 == 0)
RAPIDJSON_ASSERT(type == kStringType); // if it's in object, then even
// number should be a name
level->valueCount++;
} else {
RAPIDJSON_ASSERT(
!Base::hasRoot_); // Should only has one and only one root.
Base::hasRoot_ = true;
}
}
void WriteIndent() {
size_t count =
(Base::level_stack_.GetSize() / sizeof(typename Base::Level)) *
indentCharCount_;
PutN(*Base::os_, static_cast<typename OutputStream::Ch>(indentChar_),
count);
}
Ch indentChar_;
unsigned indentCharCount_;
PrettyFormatOptions formatOptions_;
private:
// Prohibit copy constructor & assignment operator.
PrettyWriter(const PrettyWriter &);
PrettyWriter &operator=(const PrettyWriter &);
};
RAPIDJSON_NAMESPACE_END
#if defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#ifdef __GNUC__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_RAPIDJSON_H_
livox_ros_driver2/3rdparty/rapidjson/rapidjson.h
Vendored
+719
View File
@@ -0,0 +1,719 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_RAPIDJSON_H_
#define RAPIDJSON_RAPIDJSON_H_
/*!\file rapidjson.h
\brief common definitions and configuration
\see RAPIDJSON_CONFIG
*/
/*! \defgroup RAPIDJSON_CONFIG RapidJSON configuration
\brief Configuration macros for library features
Some RapidJSON features are configurable to adapt the library to a wide
variety of platforms, environments and usage scenarios. Most of the
features can be configured in terms of overridden or predefined
preprocessor macros at compile-time.
Some additional customization is available in the \ref RAPIDJSON_ERRORS
APIs.
\note These macros should be given on the compiler command-line
(where applicable) to avoid inconsistent values when compiling
different translation units of a single application.
*/
#include <cstdlib> // malloc(), realloc(), free(), size_t
#include <cstring> // memset(), memcpy(), memmove(), memcmp()
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_VERSION_STRING
//
// ALWAYS synchronize the following 3 macros with corresponding variables in
// /CMakeLists.txt.
//
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
// token stringification
#define RAPIDJSON_STRINGIFY(x) RAPIDJSON_DO_STRINGIFY(x)
#define RAPIDJSON_DO_STRINGIFY(x) #x
// token concatenation
#define RAPIDJSON_JOIN(X, Y) RAPIDJSON_DO_JOIN(X, Y)
#define RAPIDJSON_DO_JOIN(X, Y) RAPIDJSON_DO_JOIN2(X, Y)
#define RAPIDJSON_DO_JOIN2(X, Y) X##Y
//!@endcond
/*! \def RAPIDJSON_MAJOR_VERSION
\ingroup RAPIDJSON_CONFIG
\brief Major version of RapidJSON in integer.
*/
/*! \def RAPIDJSON_MINOR_VERSION
\ingroup RAPIDJSON_CONFIG
\brief Minor version of RapidJSON in integer.
*/
/*! \def RAPIDJSON_PATCH_VERSION
\ingroup RAPIDJSON_CONFIG
\brief Patch version of RapidJSON in integer.
*/
/*! \def RAPIDJSON_VERSION_STRING
\ingroup RAPIDJSON_CONFIG
\brief Version of RapidJSON in "<major>.<minor>.<patch>" string format.
*/
#define RAPIDJSON_MAJOR_VERSION 1
#define RAPIDJSON_MINOR_VERSION 1
#define RAPIDJSON_PATCH_VERSION 0
#define RAPIDJSON_VERSION_STRING \
RAPIDJSON_STRINGIFY( \
RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION)
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NAMESPACE_(BEGIN|END)
/*! \def RAPIDJSON_NAMESPACE
\ingroup RAPIDJSON_CONFIG
\brief provide custom rapidjson namespace
In order to avoid symbol clashes and/or "One Definition Rule" errors
between multiple inclusions of (different versions of) RapidJSON in
a single binary, users can customize the name of the main RapidJSON
namespace.
In case of a single nesting level, defining \c RAPIDJSON_NAMESPACE
to a custom name (e.g. \c MyRapidJSON) is sufficient. If multiple
levels are needed, both \ref RAPIDJSON_NAMESPACE_BEGIN and \ref
RAPIDJSON_NAMESPACE_END need to be defined as well:
\code
// in some .cpp file
#define RAPIDJSON_NAMESPACE my::rapidjson
#define RAPIDJSON_NAMESPACE_BEGIN namespace my { namespace rapidjson {
#define RAPIDJSON_NAMESPACE_END } }
#include "rapidjson/..."
\endcode
\see rapidjson
*/
/*! \def RAPIDJSON_NAMESPACE_BEGIN
\ingroup RAPIDJSON_CONFIG
\brief provide custom rapidjson namespace (opening expression)
\see RAPIDJSON_NAMESPACE
*/
/*! \def RAPIDJSON_NAMESPACE_END
\ingroup RAPIDJSON_CONFIG
\brief provide custom rapidjson namespace (closing expression)
\see RAPIDJSON_NAMESPACE
*/
#ifndef RAPIDJSON_NAMESPACE
#define RAPIDJSON_NAMESPACE rapidjson
#endif
#ifndef RAPIDJSON_NAMESPACE_BEGIN
#define RAPIDJSON_NAMESPACE_BEGIN namespace RAPIDJSON_NAMESPACE {
#endif
#ifndef RAPIDJSON_NAMESPACE_END
#define RAPIDJSON_NAMESPACE_END }
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_HAS_STDSTRING
#ifndef RAPIDJSON_HAS_STDSTRING
#ifdef RAPIDJSON_DOXYGEN_RUNNING
#define RAPIDJSON_HAS_STDSTRING 1 // force generation of documentation
#else
#define RAPIDJSON_HAS_STDSTRING 0 // no std::string support by default
#endif
/*! \def RAPIDJSON_HAS_STDSTRING
\ingroup RAPIDJSON_CONFIG
\brief Enable RapidJSON support for \c std::string
By defining this preprocessor symbol to \c 1, several convenience functions
for using \ref rapidjson::GenericValue with \c std::string are enabled,
especially for construction and comparison.
\hideinitializer
*/
#endif // !defined(RAPIDJSON_HAS_STDSTRING)
#if RAPIDJSON_HAS_STDSTRING
#include <string>
#endif // RAPIDJSON_HAS_STDSTRING
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NO_INT64DEFINE
/*! \def RAPIDJSON_NO_INT64DEFINE
\ingroup RAPIDJSON_CONFIG
\brief Use external 64-bit integer types.
RapidJSON requires the 64-bit integer types \c int64_t and \c uint64_t
types to be available at global scope.
If users have their own definition, define RAPIDJSON_NO_INT64DEFINE to
prevent RapidJSON from defining its own types.
*/
#ifndef RAPIDJSON_NO_INT64DEFINE
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#if defined(_MSC_VER) && (_MSC_VER < 1800) // Visual Studio 2013
#include "msinttypes/inttypes.h"
#include "msinttypes/stdint.h"
#else
// Other compilers should have this.
#include <inttypes.h>
#include <stdint.h>
#endif
//!@endcond
#ifdef RAPIDJSON_DOXYGEN_RUNNING
#define RAPIDJSON_NO_INT64DEFINE
#endif
#endif // RAPIDJSON_NO_INT64TYPEDEF
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_FORCEINLINE
#ifndef RAPIDJSON_FORCEINLINE
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#if defined(_MSC_VER) && defined(NDEBUG)
#define RAPIDJSON_FORCEINLINE __forceinline
#elif defined(__GNUC__) && __GNUC__ >= 4 && defined(NDEBUG)
#define RAPIDJSON_FORCEINLINE __attribute__((always_inline))
#else
#define RAPIDJSON_FORCEINLINE
#endif
//!@endcond
#endif // RAPIDJSON_FORCEINLINE
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ENDIAN
#define RAPIDJSON_LITTLEENDIAN 0 //!< Little endian machine
#define RAPIDJSON_BIGENDIAN 1 //!< Big endian machine
//! Endianness of the machine.
/*!
\def RAPIDJSON_ENDIAN
\ingroup RAPIDJSON_CONFIG
GCC 4.6 provided macro for detecting endianness of the target machine. But
other compilers may not have this. User can define RAPIDJSON_ENDIAN to either
\ref RAPIDJSON_LITTLEENDIAN or \ref RAPIDJSON_BIGENDIAN.
Default detection implemented with reference to
\li
https://gcc.gnu.org/onlinedocs/gcc-4.6.0/cpp/Common-Predefined-Macros.html
\li http://www.boost.org/doc/libs/1_42_0/boost/detail/endian.hpp
*/
#ifndef RAPIDJSON_ENDIAN
// Detect with GCC 4.6's macro
#ifdef __BYTE_ORDER__
#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
#else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
#endif // __BYTE_ORDER__
// Detect with GLIBC's endian.h
#elif defined(__GLIBC__)
#include <endian.h>
#if (__BYTE_ORDER == __LITTLE_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif (__BYTE_ORDER == __BIG_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
#else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
#endif // __GLIBC__
// Detect with _LITTLE_ENDIAN and _BIG_ENDIAN macro
#elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
// Detect with architecture macros
#elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || \
defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || \
defined(__hppa) || defined(_MIPSEB) || defined(_POWER) || \
defined(__s390__)
#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
#elif defined(__i386__) || defined(__alpha__) || defined(__ia64) || \
defined(__ia64__) || defined(_M_IX86) || defined(_M_IA64) || \
defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) || \
defined(_M_AMD64) || defined(__x86_64) || defined(__x86_64__) || \
defined(_M_X64) || defined(__bfin__)
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64))
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif defined(RAPIDJSON_DOXYGEN_RUNNING)
#define RAPIDJSON_ENDIAN
#else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
#endif
#endif // RAPIDJSON_ENDIAN
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_64BIT
//! Whether using 64-bit architecture
#ifndef RAPIDJSON_64BIT
#if defined(__LP64__) || (defined(__x86_64__) && defined(__ILP32__)) || \
defined(_WIN64) || defined(__EMSCRIPTEN__)
#define RAPIDJSON_64BIT 1
#else
#define RAPIDJSON_64BIT 0
#endif
#endif // RAPIDJSON_64BIT
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ALIGN
//! Data alignment of the machine.
/*! \ingroup RAPIDJSON_CONFIG
\param x pointer to align
Some machines require strict data alignment. The default is 8 bytes.
User can customize by defining the RAPIDJSON_ALIGN function macro.
*/
#ifndef RAPIDJSON_ALIGN
#define RAPIDJSON_ALIGN(x) \
(((x) + static_cast<size_t>(7u)) & ~static_cast<size_t>(7u))
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_UINT64_C2
//! Construct a 64-bit literal by a pair of 32-bit integer.
/*!
64-bit literal with or without ULL suffix is prone to compiler warnings.
UINT64_C() is C macro which cause compilation problems.
Use this macro to define 64-bit constants by a pair of 32-bit integer.
*/
#ifndef RAPIDJSON_UINT64_C2
#define RAPIDJSON_UINT64_C2(high32, low32) \
((static_cast<uint64_t>(high32) << 32) | static_cast<uint64_t>(low32))
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_48BITPOINTER_OPTIMIZATION
//! Use only lower 48-bit address for some pointers.
/*!
\ingroup RAPIDJSON_CONFIG
This optimization uses the fact that current X86-64 architecture only
implement lower 48-bit virtual address. The higher 16-bit can be used for
storing other data. \c GenericValue uses this optimization to reduce its size
form 24 bytes to 16 bytes in 64-bit architecture.
*/
#ifndef RAPIDJSON_48BITPOINTER_OPTIMIZATION
#if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || \
defined(__x86_64) || defined(_M_X64) || defined(_M_AMD64)
#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 1
#else
#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 0
#endif
#endif // RAPIDJSON_48BITPOINTER_OPTIMIZATION
#if RAPIDJSON_48BITPOINTER_OPTIMIZATION == 1
#if RAPIDJSON_64BIT != 1
#error RAPIDJSON_48BITPOINTER_OPTIMIZATION can only be set to 1 when RAPIDJSON_64BIT=1
#endif
#define RAPIDJSON_SETPOINTER(type, p, x) \
(p = reinterpret_cast<type *>( \
(reinterpret_cast<uintptr_t>(p) & \
static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0xFFFF0000, 0x00000000))) | \
reinterpret_cast<uintptr_t>(reinterpret_cast<const void *>(x))))
#define RAPIDJSON_GETPOINTER(type, p) \
(reinterpret_cast<type *>( \
reinterpret_cast<uintptr_t>(p) & \
static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0x0000FFFF, 0xFFFFFFFF))))
#else
#define RAPIDJSON_SETPOINTER(type, p, x) (p = (x))
#define RAPIDJSON_GETPOINTER(type, p) (p)
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_SSE2/RAPIDJSON_SSE42/RAPIDJSON_NEON/RAPIDJSON_SIMD
/*! \def RAPIDJSON_SIMD
\ingroup RAPIDJSON_CONFIG
\brief Enable SSE2/SSE4.2/Neon optimization.
RapidJSON supports optimized implementations for some parsing operations
based on the SSE2, SSE4.2 or NEon SIMD extensions on modern Intel
or ARM compatible processors.
To enable these optimizations, three different symbols can be defined;
\code
// Enable SSE2 optimization.
#define RAPIDJSON_SSE2
// Enable SSE4.2 optimization.
#define RAPIDJSON_SSE42
\endcode
// Enable ARM Neon optimization.
#define RAPIDJSON_NEON
\endcode
\c RAPIDJSON_SSE42 takes precedence over SSE2, if both are defined.
If any of these symbols is defined, RapidJSON defines the macro
\c RAPIDJSON_SIMD to indicate the availability of the optimized code.
*/
#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) || \
defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING)
#define RAPIDJSON_SIMD
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NO_SIZETYPEDEFINE
#ifndef RAPIDJSON_NO_SIZETYPEDEFINE
/*! \def RAPIDJSON_NO_SIZETYPEDEFINE
\ingroup RAPIDJSON_CONFIG
\brief User-provided \c SizeType definition.
In order to avoid using 32-bit size types for indexing strings and arrays,
define this preprocessor symbol and provide the type rapidjson::SizeType
before including RapidJSON:
\code
#define RAPIDJSON_NO_SIZETYPEDEFINE
namespace rapidjson { typedef ::std::size_t SizeType; }
#include "rapidjson/..."
\endcode
\see rapidjson::SizeType
*/
#ifdef RAPIDJSON_DOXYGEN_RUNNING
#define RAPIDJSON_NO_SIZETYPEDEFINE
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Size type (for string lengths, array sizes, etc.)
/*! RapidJSON uses 32-bit array/string indices even on 64-bit platforms,
instead of using \c size_t. Users may override the SizeType by defining
\ref RAPIDJSON_NO_SIZETYPEDEFINE.
*/
typedef unsigned SizeType;
RAPIDJSON_NAMESPACE_END
#endif
// always import std::size_t to rapidjson namespace
RAPIDJSON_NAMESPACE_BEGIN
using std::size_t;
RAPIDJSON_NAMESPACE_END
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ASSERT
//! Assertion.
/*! \ingroup RAPIDJSON_CONFIG
By default, rapidjson uses C \c assert() for internal assertions.
User can override it by defining RAPIDJSON_ASSERT(x) macro.
\note Parsing errors are handled and can be customized by the
\ref RAPIDJSON_ERRORS APIs.
*/
#ifndef RAPIDJSON_ASSERT
#include <cassert>
#define RAPIDJSON_ASSERT(x) assert(x)
#endif // RAPIDJSON_ASSERT
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_STATIC_ASSERT
// Prefer C++11 static_assert, if available
#ifndef RAPIDJSON_STATIC_ASSERT
#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800)
#define RAPIDJSON_STATIC_ASSERT(x) static_assert(x, RAPIDJSON_STRINGIFY(x))
#endif // C++11
#endif // RAPIDJSON_STATIC_ASSERT
// Adopt C++03 implementation from boost
#ifndef RAPIDJSON_STATIC_ASSERT
#ifndef __clang__
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#endif
RAPIDJSON_NAMESPACE_BEGIN
template <bool x>
struct STATIC_ASSERTION_FAILURE;
template <>
struct STATIC_ASSERTION_FAILURE<true> {
enum { value = 1 };
};
template <size_t x>
struct StaticAssertTest {};
RAPIDJSON_NAMESPACE_END
#if defined(__GNUC__) || defined(__clang__)
#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE __attribute__((unused))
#else
#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#endif
#ifndef __clang__
//!@endcond
#endif
/*! \def RAPIDJSON_STATIC_ASSERT
\brief (Internal) macro to check for conditions at compile-time
\param x compile-time condition
\hideinitializer
*/
#define RAPIDJSON_STATIC_ASSERT(x) \
typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest<sizeof( \
::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x)>)> \
RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__) \
RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#endif // RAPIDJSON_STATIC_ASSERT
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_LIKELY, RAPIDJSON_UNLIKELY
//! Compiler branching hint for expression with high probability to be true.
/*!
\ingroup RAPIDJSON_CONFIG
\param x Boolean expression likely to be true.
*/
#ifndef RAPIDJSON_LIKELY
#if defined(__GNUC__) || defined(__clang__)
#define RAPIDJSON_LIKELY(x) __builtin_expect(!!(x), 1)
#else
#define RAPIDJSON_LIKELY(x) (x)
#endif
#endif
//! Compiler branching hint for expression with low probability to be true.
/*!
\ingroup RAPIDJSON_CONFIG
\param x Boolean expression unlikely to be true.
*/
#ifndef RAPIDJSON_UNLIKELY
#if defined(__GNUC__) || defined(__clang__)
#define RAPIDJSON_UNLIKELY(x) __builtin_expect(!!(x), 0)
#else
#define RAPIDJSON_UNLIKELY(x) (x)
#endif
#endif
///////////////////////////////////////////////////////////////////////////////
// Helpers
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#define RAPIDJSON_MULTILINEMACRO_BEGIN do {
#define RAPIDJSON_MULTILINEMACRO_END \
} \
while ((void)0, 0)
// adopted from Boost
#define RAPIDJSON_VERSION_CODE(x, y, z) (((x)*100000) + ((y)*100) + (z))
#if defined(__has_builtin)
#define RAPIDJSON_HAS_BUILTIN(x) __has_builtin(x)
#else
#define RAPIDJSON_HAS_BUILTIN(x) 0
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_DIAG_PUSH/POP, RAPIDJSON_DIAG_OFF
#if defined(__GNUC__)
#define RAPIDJSON_GNUC \
RAPIDJSON_VERSION_CODE(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__)
#endif
#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 2, 0))
#define RAPIDJSON_PRAGMA(x) _Pragma(RAPIDJSON_STRINGIFY(x))
#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(GCC diagnostic x)
#define RAPIDJSON_DIAG_OFF(x) \
RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W, x)))
// push/pop support in Clang and GCC>=4.6
#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0))
#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
#else // GCC >= 4.2, < 4.6
#define RAPIDJSON_DIAG_PUSH /* ignored */
#define RAPIDJSON_DIAG_POP /* ignored */
#endif
#elif defined(_MSC_VER)
// pragma (MSVC specific)
#define RAPIDJSON_PRAGMA(x) __pragma(x)
#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(warning(x))
#define RAPIDJSON_DIAG_OFF(x) RAPIDJSON_DIAG_PRAGMA(disable : x)
#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
#else
#define RAPIDJSON_DIAG_OFF(x) /* ignored */
#define RAPIDJSON_DIAG_PUSH /* ignored */
#define RAPIDJSON_DIAG_POP /* ignored */
#endif // RAPIDJSON_DIAG_*
///////////////////////////////////////////////////////////////////////////////
// C++11 features
#ifndef RAPIDJSON_HAS_CXX11_RVALUE_REFS
#if defined(__clang__)
#if __has_feature(cxx_rvalue_references) && \
(defined(_MSC_VER) || defined(_LIBCPP_VERSION) || \
defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306)
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
#else
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
#endif
#elif (defined(RAPIDJSON_GNUC) && \
(RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 3, 0)) && \
defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1600) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
#else
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
#endif
#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS
#ifndef RAPIDJSON_HAS_CXX11_NOEXCEPT
#if defined(__clang__)
#define RAPIDJSON_HAS_CXX11_NOEXCEPT __has_feature(cxx_noexcept)
#elif (defined(RAPIDJSON_GNUC) && \
(RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1900) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 1
#else
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 0
#endif
#endif
#if RAPIDJSON_HAS_CXX11_NOEXCEPT
#define RAPIDJSON_NOEXCEPT noexcept
#else
#define RAPIDJSON_NOEXCEPT /* noexcept */
#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT
// no automatic detection, yet
#ifndef RAPIDJSON_HAS_CXX11_TYPETRAITS
#if (defined(_MSC_VER) && _MSC_VER >= 1700)
#define RAPIDJSON_HAS_CXX11_TYPETRAITS 1
#else
#define RAPIDJSON_HAS_CXX11_TYPETRAITS 0
#endif
#endif
#ifndef RAPIDJSON_HAS_CXX11_RANGE_FOR
#if defined(__clang__)
#define RAPIDJSON_HAS_CXX11_RANGE_FOR __has_feature(cxx_range_for)
#elif (defined(RAPIDJSON_GNUC) && \
(RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1700) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 1
#else
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 0
#endif
#endif // RAPIDJSON_HAS_CXX11_RANGE_FOR
///////////////////////////////////////////////////////////////////////////////
// C++17 features
#if defined(__has_cpp_attribute)
#if __has_cpp_attribute(fallthrough)
#define RAPIDJSON_DELIBERATE_FALLTHROUGH [[fallthrough]]
#else
#define RAPIDJSON_DELIBERATE_FALLTHROUGH
#endif
#else
#define RAPIDJSON_DELIBERATE_FALLTHROUGH
#endif
//!@endcond
//! Assertion (in non-throwing contexts).
/*! \ingroup RAPIDJSON_CONFIG
Some functions provide a \c noexcept guarantee, if the compiler supports it.
In these cases, the \ref RAPIDJSON_ASSERT macro cannot be overridden to
throw an exception. This macro adds a separate customization point for
such cases.
Defaults to C \c assert() (as \ref RAPIDJSON_ASSERT), if \c noexcept is
supported, and to \ref RAPIDJSON_ASSERT otherwise.
*/
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NOEXCEPT_ASSERT
#ifndef RAPIDJSON_NOEXCEPT_ASSERT
#ifdef RAPIDJSON_ASSERT_THROWS
#if RAPIDJSON_HAS_CXX11_NOEXCEPT
#define RAPIDJSON_NOEXCEPT_ASSERT(x)
#else
#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT
#else
#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
#endif // RAPIDJSON_ASSERT_THROWS
#endif // RAPIDJSON_NOEXCEPT_ASSERT
///////////////////////////////////////////////////////////////////////////////
// new/delete
#ifndef RAPIDJSON_NEW
///! customization point for global \c new
#define RAPIDJSON_NEW(TypeName) new TypeName
#endif
#ifndef RAPIDJSON_DELETE
///! customization point for global \c delete
#define RAPIDJSON_DELETE(x) delete x
#endif
///////////////////////////////////////////////////////////////////////////////
// Type
/*! \namespace rapidjson
\brief main RapidJSON namespace
\see RAPIDJSON_NAMESPACE
*/
RAPIDJSON_NAMESPACE_BEGIN
//! Type of JSON value
enum Type {
kNullType = 0, //!< null
kFalseType = 1, //!< false
kTrueType = 2, //!< true
kObjectType = 3, //!< object
kArrayType = 4, //!< array
kStringType = 5, //!< string
kNumberType = 6 //!< number
};
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_RAPIDJSON_H_
livox_ros_driver2/3rdparty/rapidjson/reader.h
Vendored
+2458
View File
File diff suppressed because it is too large Load Diff
livox_ros_driver2/3rdparty/rapidjson/schema.h
Vendored
+2743
View File
File diff suppressed because it is too large Load Diff
livox_ros_driver2/3rdparty/rapidjson/stream.h
Vendored
+242
View File
@@ -0,0 +1,242 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#include "rapidjson.h"
#ifndef RAPIDJSON_STREAM_H_
#define RAPIDJSON_STREAM_H_
#include "encodings.h"
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// Stream
/*! \class rapidjson::Stream
\brief Concept for reading and writing characters.
For read-only stream, no need to implement PutBegin(), Put(), Flush() and
PutEnd().
For write-only stream, only need to implement Put() and Flush().
\code
concept Stream {
typename Ch; //!< Character type of the stream.
//! Read the current character from stream without moving the read cursor.
Ch Peek() const;
//! Read the current character from stream and moving the read cursor to
next character. Ch Take();
//! Get the current read cursor.
//! \return Number of characters read from start.
size_t Tell();
//! Begin writing operation at the current read pointer.
//! \return The begin writer pointer.
Ch* PutBegin();
//! Write a character.
void Put(Ch c);
//! Flush the buffer.
void Flush();
//! End the writing operation.
//! \param begin The begin write pointer returned by PutBegin().
//! \return Number of characters written.
size_t PutEnd(Ch* begin);
}
\endcode
*/
//! Provides additional information for stream.
/*!
By using traits pattern, this type provides a default configuration for
stream. For custom stream, this type can be specialized for other
configuration. See TEST(Reader, CustomStringStream) in readertest.cpp for
example.
*/
template <typename Stream>
struct StreamTraits {
//! Whether to make local copy of stream for optimization during parsing.
/*!
By default, for safety, streams do not use local copy optimization.
Stream that can be copied fast should specialize this, like
StreamTraits<StringStream>.
*/
enum { copyOptimization = 0 };
};
//! Reserve n characters for writing to a stream.
template <typename Stream>
inline void PutReserve(Stream &stream, size_t count) {
(void)stream;
(void)count;
}
//! Write character to a stream, presuming buffer is reserved.
template <typename Stream>
inline void PutUnsafe(Stream &stream, typename Stream::Ch c) {
stream.Put(c);
}
//! Put N copies of a character to a stream.
template <typename Stream, typename Ch>
inline void PutN(Stream &stream, Ch c, size_t n) {
PutReserve(stream, n);
for (size_t i = 0; i < n; i++) PutUnsafe(stream, c);
}
///////////////////////////////////////////////////////////////////////////////
// GenericStreamWrapper
//! A Stream Wrapper
/*! \tThis string stream is a wrapper for any stream by just forwarding any
\treceived message to the origin stream.
\note implements Stream concept
*/
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif
template <typename InputStream, typename Encoding = UTF8<>>
class GenericStreamWrapper {
public:
typedef typename Encoding::Ch Ch;
GenericStreamWrapper(InputStream &is) : is_(is) {}
Ch Peek() const { return is_.Peek(); }
Ch Take() { return is_.Take(); }
size_t Tell() { return is_.Tell(); }
Ch *PutBegin() { return is_.PutBegin(); }
void Put(Ch ch) { is_.Put(ch); }
void Flush() { is_.Flush(); }
size_t PutEnd(Ch *ch) { return is_.PutEnd(ch); }
// wrapper for MemoryStream
const Ch *Peek4() const { return is_.Peek4(); }
// wrapper for AutoUTFInputStream
UTFType GetType() const { return is_.GetType(); }
bool HasBOM() const { return is_.HasBOM(); }
protected:
InputStream &is_;
};
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_POP
#endif
///////////////////////////////////////////////////////////////////////////////
// StringStream
//! Read-only string stream.
/*! \note implements Stream concept
*/
template <typename Encoding>
struct GenericStringStream {
typedef typename Encoding::Ch Ch;
GenericStringStream(const Ch *src) : src_(src), head_(src) {}
Ch Peek() const { return *src_; }
Ch Take() { return *src_++; }
size_t Tell() const { return static_cast<size_t>(src_ - head_); }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
const Ch *src_; //!< Current read position.
const Ch *head_; //!< Original head of the string.
};
template <typename Encoding>
struct StreamTraits<GenericStringStream<Encoding>> {
enum { copyOptimization = 1 };
};
//! String stream with UTF8 encoding.
typedef GenericStringStream<UTF8<>> StringStream;
///////////////////////////////////////////////////////////////////////////////
// InsituStringStream
//! A read-write string stream.
/*! This string stream is particularly designed for in-situ parsing.
\note implements Stream concept
*/
template <typename Encoding>
struct GenericInsituStringStream {
typedef typename Encoding::Ch Ch;
GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {}
// Read
Ch Peek() { return *src_; }
Ch Take() { return *src_++; }
size_t Tell() { return static_cast<size_t>(src_ - head_); }
// Write
void Put(Ch c) {
RAPIDJSON_ASSERT(dst_ != 0);
*dst_++ = c;
}
Ch *PutBegin() { return dst_ = src_; }
size_t PutEnd(Ch *begin) { return static_cast<size_t>(dst_ - begin); }
void Flush() {}
Ch *Push(size_t count) {
Ch *begin = dst_;
dst_ += count;
return begin;
}
void Pop(size_t count) { dst_ -= count; }
Ch *src_;
Ch *dst_;
Ch *head_;
};
template <typename Encoding>
struct StreamTraits<GenericInsituStringStream<Encoding>> {
enum { copyOptimization = 1 };
};
//! Insitu string stream with UTF8 encoding.
typedef GenericInsituStringStream<UTF8<>> InsituStringStream;
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_STREAM_H_
livox_ros_driver2/3rdparty/rapidjson/stringbuffer.h
Vendored
+130
View File
@@ -0,0 +1,130 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_STRINGBUFFER_H_
#define RAPIDJSON_STRINGBUFFER_H_
#include "internal/stack.h"
#include "stream.h"
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
#include <utility> // std::move
#endif
#include "internal/stack.h"
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Represents an in-memory output stream.
/*!
\tparam Encoding Encoding of the stream.
\tparam Allocator type for allocating memory buffer.
\note implements Stream concept
*/
template <typename Encoding, typename Allocator = CrtAllocator>
class GenericStringBuffer {
public:
typedef typename Encoding::Ch Ch;
GenericStringBuffer(Allocator *allocator = 0,
size_t capacity = kDefaultCapacity)
: stack_(allocator, capacity) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
GenericStringBuffer(GenericStringBuffer &&rhs)
: stack_(std::move(rhs.stack_)) {}
GenericStringBuffer &operator=(GenericStringBuffer &&rhs) {
if (&rhs != this) stack_ = std::move(rhs.stack_);
return *this;
}
#endif
void Put(Ch c) { *stack_.template Push<Ch>() = c; }
void PutUnsafe(Ch c) { *stack_.template PushUnsafe<Ch>() = c; }
void Flush() {}
void Clear() { stack_.Clear(); }
void ShrinkToFit() {
// Push and pop a null terminator. This is safe.
*stack_.template Push<Ch>() = '\0';
stack_.ShrinkToFit();
stack_.template Pop<Ch>(1);
}
void Reserve(size_t count) { stack_.template Reserve<Ch>(count); }
Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
Ch *PushUnsafe(size_t count) { return stack_.template PushUnsafe<Ch>(count); }
void Pop(size_t count) { stack_.template Pop<Ch>(count); }
const Ch *GetString() const {
// Push and pop a null terminator. This is safe.
*stack_.template Push<Ch>() = '\0';
stack_.template Pop<Ch>(1);
return stack_.template Bottom<Ch>();
}
//! Get the size of string in bytes in the string buffer.
size_t GetSize() const { return stack_.GetSize(); }
//! Get the length of string in Ch in the string buffer.
size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); }
static const size_t kDefaultCapacity = 256;
mutable internal::Stack<Allocator> stack_;
private:
// Prohibit copy constructor & assignment operator.
GenericStringBuffer(const GenericStringBuffer &);
GenericStringBuffer &operator=(const GenericStringBuffer &);
};
//! String buffer with UTF8 encoding
typedef GenericStringBuffer<UTF8<>> StringBuffer;
template <typename Encoding, typename Allocator>
inline void PutReserve(GenericStringBuffer<Encoding, Allocator> &stream,
size_t count) {
stream.Reserve(count);
}
template <typename Encoding, typename Allocator>
inline void PutUnsafe(GenericStringBuffer<Encoding, Allocator> &stream,
typename Encoding::Ch c) {
stream.PutUnsafe(c);
}
//! Implement specialized version of PutN() with memset() for better
//! performance.
template <>
inline void PutN(GenericStringBuffer<UTF8<>> &stream, char c, size_t n) {
std::memset(stream.stack_.Push<char>(n), c, n * sizeof(c));
}
RAPIDJSON_NAMESPACE_END
#if defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_STRINGBUFFER_H_
livox_ros_driver2/3rdparty/rapidjson/writer.h
Vendored
+811
View File
@@ -0,0 +1,811 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_WRITER_H_
#define RAPIDJSON_WRITER_H_
#include <new> // placement new
#include "internal/clzll.h"
#include "internal/dtoa.h"
#include "internal/itoa.h"
#include "internal/meta.h"
#include "internal/stack.h"
#include "internal/strfunc.h"
#include "stream.h"
#include "stringbuffer.h"
#if defined(RAPIDJSON_SIMD) && defined(_MSC_VER)
#include <intrin.h>
#pragma intrinsic(_BitScanForward)
#endif
#ifdef RAPIDJSON_SSE42
#include <nmmintrin.h>
#elif defined(RAPIDJSON_SSE2)
#include <emmintrin.h>
#elif defined(RAPIDJSON_NEON)
#include <arm_neon.h>
#endif
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
RAPIDJSON_DIAG_OFF(unreachable - code)
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#elif defined(_MSC_VER)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4127) // conditional expression is constant
#endif
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// WriteFlag
/*! \def RAPIDJSON_WRITE_DEFAULT_FLAGS
\ingroup RAPIDJSON_CONFIG
\brief User-defined kWriteDefaultFlags definition.
User can define this as any \c WriteFlag combinations.
*/
#ifndef RAPIDJSON_WRITE_DEFAULT_FLAGS
#define RAPIDJSON_WRITE_DEFAULT_FLAGS kWriteNoFlags
#endif
//! Combination of writeFlags
enum WriteFlag {
kWriteNoFlags = 0, //!< No flags are set.
kWriteValidateEncodingFlag = 1, //!< Validate encoding of JSON strings.
kWriteNanAndInfFlag = 2, //!< Allow writing of Infinity, -Infinity and NaN.
kWriteDefaultFlags =
RAPIDJSON_WRITE_DEFAULT_FLAGS //!< Default write flags. Can be customized
//!< by defining
//!< RAPIDJSON_WRITE_DEFAULT_FLAGS
};
//! JSON writer
/*! Writer implements the concept Handler.
It generates JSON text by events to an output os.
User may programmatically calls the functions of a writer to generate JSON
text.
On the other side, a writer can also be passed to objects that generates
events,
for example Reader::Parse() and Document::Accept().
\tparam OutputStream Type of output stream.
\tparam SourceEncoding Encoding of source string.
\tparam TargetEncoding Encoding of output stream.
\tparam StackAllocator Type of allocator for allocating memory of stack.
\note implements Handler concept
*/
template <typename OutputStream, typename SourceEncoding = UTF8<>,
typename TargetEncoding = UTF8<>,
typename StackAllocator = CrtAllocator,
unsigned writeFlags = kWriteDefaultFlags>
class Writer {
public:
typedef typename SourceEncoding::Ch Ch;
static const int kDefaultMaxDecimalPlaces = 324;
//! Constructor
/*! \param os Output stream.
\param stackAllocator User supplied allocator. If it is null, it will
create a private one. \param levelDepth Initial capacity of stack.
*/
explicit Writer(OutputStream &os, StackAllocator *stackAllocator = 0,
size_t levelDepth = kDefaultLevelDepth)
: os_(&os),
level_stack_(stackAllocator, levelDepth * sizeof(Level)),
maxDecimalPlaces_(kDefaultMaxDecimalPlaces),
hasRoot_(false) {}
explicit Writer(StackAllocator *allocator = 0,
size_t levelDepth = kDefaultLevelDepth)
: os_(0),
level_stack_(allocator, levelDepth * sizeof(Level)),
maxDecimalPlaces_(kDefaultMaxDecimalPlaces),
hasRoot_(false) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
Writer(Writer &&rhs)
: os_(rhs.os_),
level_stack_(std::move(rhs.level_stack_)),
maxDecimalPlaces_(rhs.maxDecimalPlaces_),
hasRoot_(rhs.hasRoot_) {
rhs.os_ = 0;
}
#endif
//! Reset the writer with a new stream.
/*!
This function reset the writer with a new stream and default settings,
in order to make a Writer object reusable for output multiple JSONs.
\param os New output stream.
\code
Writer<OutputStream> writer(os1);
writer.StartObject();
// ...
writer.EndObject();
writer.Reset(os2);
writer.StartObject();
// ...
writer.EndObject();
\endcode
*/
void Reset(OutputStream &os) {
os_ = &os;
hasRoot_ = false;
level_stack_.Clear();
}
//! Checks whether the output is a complete JSON.
/*!
A complete JSON has a complete root object or array.
*/
bool IsComplete() const { return hasRoot_ && level_stack_.Empty(); }
int GetMaxDecimalPlaces() const { return maxDecimalPlaces_; }
//! Sets the maximum number of decimal places for double output.
/*!
This setting truncates the output with specified number of decimal places.
For example,
\code
writer.SetMaxDecimalPlaces(3);
writer.StartArray();
writer.Double(0.12345); // "0.123"
writer.Double(0.0001); // "0.0"
writer.Double(1.234567890123456e30); // "1.234567890123456e30" (do not
truncate significand for positive exponent) writer.Double(1.23e-4); //
"0.0" (do truncate significand for negative exponent)
writer.EndArray();
\endcode
The default setting does not truncate any decimal places. You can restore
to this setting by calling \code
writer.SetMaxDecimalPlaces(Writer::kDefaultMaxDecimalPlaces);
\endcode
*/
void SetMaxDecimalPlaces(int maxDecimalPlaces) {
maxDecimalPlaces_ = maxDecimalPlaces;
}
/*!@name Implementation of Handler
\see Handler
*/
//@{
bool Null() {
Prefix(kNullType);
return EndValue(WriteNull());
}
bool Bool(bool b) {
Prefix(b ? kTrueType : kFalseType);
return EndValue(WriteBool(b));
}
bool Int(int i) {
Prefix(kNumberType);
return EndValue(WriteInt(i));
}
bool Uint(unsigned u) {
Prefix(kNumberType);
return EndValue(WriteUint(u));
}
bool Int64(int64_t i64) {
Prefix(kNumberType);
return EndValue(WriteInt64(i64));
}
bool Uint64(uint64_t u64) {
Prefix(kNumberType);
return EndValue(WriteUint64(u64));
}
//! Writes the given \c double value to the stream
/*!
\param d The value to be written.
\return Whether it is succeed.
*/
bool Double(double d) {
Prefix(kNumberType);
return EndValue(WriteDouble(d));
}
bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
Prefix(kNumberType);
return EndValue(WriteString(str, length));
}
bool String(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
Prefix(kStringType);
return EndValue(WriteString(str, length));
}
#if RAPIDJSON_HAS_STDSTRING
bool String(const std::basic_string<Ch> &str) {
return String(str.data(), SizeType(str.size()));
}
#endif
bool StartObject() {
Prefix(kObjectType);
new (level_stack_.template Push<Level>()) Level(false);
return WriteStartObject();
}
bool Key(const Ch *str, SizeType length, bool copy = false) {
return String(str, length, copy);
}
#if RAPIDJSON_HAS_STDSTRING
bool Key(const std::basic_string<Ch> &str) {
return Key(str.data(), SizeType(str.size()));
}
#endif
bool EndObject(SizeType memberCount = 0) {
(void)memberCount;
RAPIDJSON_ASSERT(level_stack_.GetSize() >=
sizeof(Level)); // not inside an Object
RAPIDJSON_ASSERT(!level_stack_.template Top<Level>()
->inArray); // currently inside an Array, not Object
RAPIDJSON_ASSERT(0 ==
level_stack_.template Top<Level>()->valueCount %
2); // Object has a Key without a Value
level_stack_.template Pop<Level>(1);
return EndValue(WriteEndObject());
}
bool StartArray() {
Prefix(kArrayType);
new (level_stack_.template Push<Level>()) Level(true);
return WriteStartArray();
}
bool EndArray(SizeType elementCount = 0) {
(void)elementCount;
RAPIDJSON_ASSERT(level_stack_.GetSize() >= sizeof(Level));
RAPIDJSON_ASSERT(level_stack_.template Top<Level>()->inArray);
level_stack_.template Pop<Level>(1);
return EndValue(WriteEndArray());
}
//@}
/*! @name Convenience extensions */
//@{
//! Simpler but slower overload.
bool String(const Ch *const &str) {
return String(str, internal::StrLen(str));
}
bool Key(const Ch *const &str) { return Key(str, internal::StrLen(str)); }
//@}
//! Write a raw JSON value.
/*!
For user to write a stringified JSON as a value.
\param json A well-formed JSON value. It should not contain null character
within [0, length - 1] range. \param length Length of the json. \param type
Type of the root of json.
*/
bool RawValue(const Ch *json, size_t length, Type type) {
RAPIDJSON_ASSERT(json != 0);
Prefix(type);
return EndValue(WriteRawValue(json, length));
}
//! Flush the output stream.
/*!
Allows the user to flush the output stream immediately.
*/
void Flush() { os_->Flush(); }
protected:
//! Information for each nested level
struct Level {
Level(bool inArray_) : valueCount(0), inArray(inArray_) {}
size_t valueCount; //!< number of values in this level
bool inArray; //!< true if in array, otherwise in object
};
static const size_t kDefaultLevelDepth = 32;
bool WriteNull() {
PutReserve(*os_, 4);
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'u');
PutUnsafe(*os_, 'l');
PutUnsafe(*os_, 'l');
return true;
}
bool WriteBool(bool b) {
if (b) {
PutReserve(*os_, 4);
PutUnsafe(*os_, 't');
PutUnsafe(*os_, 'r');
PutUnsafe(*os_, 'u');
PutUnsafe(*os_, 'e');
} else {
PutReserve(*os_, 5);
PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'a');
PutUnsafe(*os_, 'l');
PutUnsafe(*os_, 's');
PutUnsafe(*os_, 'e');
}
return true;
}
bool WriteInt(int i) {
char buffer[11];
const char *end = internal::i32toa(i, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (const char *p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteUint(unsigned u) {
char buffer[10];
const char *end = internal::u32toa(u, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (const char *p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteInt64(int64_t i64) {
char buffer[21];
const char *end = internal::i64toa(i64, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (const char *p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteUint64(uint64_t u64) {
char buffer[20];
char *end = internal::u64toa(u64, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (char *p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteDouble(double d) {
if (internal::Double(d).IsNanOrInf()) {
if (!(writeFlags & kWriteNanAndInfFlag)) return false;
if (internal::Double(d).IsNan()) {
PutReserve(*os_, 3);
PutUnsafe(*os_, 'N');
PutUnsafe(*os_, 'a');
PutUnsafe(*os_, 'N');
return true;
}
if (internal::Double(d).Sign()) {
PutReserve(*os_, 9);
PutUnsafe(*os_, '-');
} else
PutReserve(*os_, 8);
PutUnsafe(*os_, 'I');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 't');
PutUnsafe(*os_, 'y');
return true;
}
char buffer[25];
char *end = internal::dtoa(d, buffer, maxDecimalPlaces_);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (char *p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteString(const Ch *str, SizeType length) {
static const typename OutputStream::Ch hexDigits[16] = {
'0', '1', '2', '3', '4', '5', '6', '7',
'8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
static const char escape[256] = {
#define Z16 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
// 0 1 2 3 4 5 6 7 8 9 A B C D E
// F
'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'b', 't',
'n', 'u', 'f', 'r', 'u', 'u', // 00
'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u',
'u', 'u', 'u', 'u', 'u', 'u', // 10
0, 0, '"', 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, // 20
Z16, Z16, // 30~4F
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, '\\', 0, 0, 0, // 50
Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16 // 60~FF
#undef Z16
};
if (TargetEncoding::supportUnicode)
PutReserve(*os_, 2 + length * 6); // "\uxxxx..."
else
PutReserve(*os_, 2 + length * 12); // "\uxxxx\uyyyy..."
PutUnsafe(*os_, '\"');
GenericStringStream<SourceEncoding> is(str);
while (ScanWriteUnescapedString(is, length)) {
const Ch c = is.Peek();
if (!TargetEncoding::supportUnicode && static_cast<unsigned>(c) >= 0x80) {
// Unicode escaping
unsigned codepoint;
if (RAPIDJSON_UNLIKELY(!SourceEncoding::Decode(is, &codepoint)))
return false;
PutUnsafe(*os_, '\\');
PutUnsafe(*os_, 'u');
if (codepoint <= 0xD7FF ||
(codepoint >= 0xE000 && codepoint <= 0xFFFF)) {
PutUnsafe(*os_, hexDigits[(codepoint >> 12) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint >> 8) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint >> 4) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint)&15]);
} else {
RAPIDJSON_ASSERT(codepoint >= 0x010000 && codepoint <= 0x10FFFF);
// Surrogate pair
unsigned s = codepoint - 0x010000;
unsigned lead = (s >> 10) + 0xD800;
unsigned trail = (s & 0x3FF) + 0xDC00;
PutUnsafe(*os_, hexDigits[(lead >> 12) & 15]);
PutUnsafe(*os_, hexDigits[(lead >> 8) & 15]);
PutUnsafe(*os_, hexDigits[(lead >> 4) & 15]);
PutUnsafe(*os_, hexDigits[(lead)&15]);
PutUnsafe(*os_, '\\');
PutUnsafe(*os_, 'u');
PutUnsafe(*os_, hexDigits[(trail >> 12) & 15]);
PutUnsafe(*os_, hexDigits[(trail >> 8) & 15]);
PutUnsafe(*os_, hexDigits[(trail >> 4) & 15]);
PutUnsafe(*os_, hexDigits[(trail)&15]);
}
} else if ((sizeof(Ch) == 1 || static_cast<unsigned>(c) < 256) &&
RAPIDJSON_UNLIKELY(escape[static_cast<unsigned char>(c)])) {
is.Take();
PutUnsafe(*os_, '\\');
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(
escape[static_cast<unsigned char>(c)]));
if (escape[static_cast<unsigned char>(c)] == 'u') {
PutUnsafe(*os_, '0');
PutUnsafe(*os_, '0');
PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) >> 4]);
PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) & 0xF]);
}
} else if (RAPIDJSON_UNLIKELY(!(
writeFlags & kWriteValidateEncodingFlag
? Transcoder<SourceEncoding, TargetEncoding>::Validate(
is, *os_)
: Transcoder<SourceEncoding,
TargetEncoding>::TranscodeUnsafe(is,
*os_))))
return false;
}
PutUnsafe(*os_, '\"');
return true;
}
bool ScanWriteUnescapedString(GenericStringStream<SourceEncoding> &is,
size_t length) {
return RAPIDJSON_LIKELY(is.Tell() < length);
}
bool WriteStartObject() {
os_->Put('{');
return true;
}
bool WriteEndObject() {
os_->Put('}');
return true;
}
bool WriteStartArray() {
os_->Put('[');
return true;
}
bool WriteEndArray() {
os_->Put(']');
return true;
}
bool WriteRawValue(const Ch *json, size_t length) {
PutReserve(*os_, length);
GenericStringStream<SourceEncoding> is(json);
while (RAPIDJSON_LIKELY(is.Tell() < length)) {
RAPIDJSON_ASSERT(is.Peek() != '\0');
if (RAPIDJSON_UNLIKELY(!(
writeFlags & kWriteValidateEncodingFlag
? Transcoder<SourceEncoding, TargetEncoding>::Validate(is,
*os_)
: Transcoder<SourceEncoding, TargetEncoding>::TranscodeUnsafe(
is, *os_))))
return false;
}
return true;
}
void Prefix(Type type) {
(void)type;
if (RAPIDJSON_LIKELY(level_stack_.GetSize() !=
0)) { // this value is not at root
Level *level = level_stack_.template Top<Level>();
if (level->valueCount > 0) {
if (level->inArray)
os_->Put(','); // add comma if it is not the first element in array
else // in object
os_->Put((level->valueCount % 2 == 0) ? ',' : ':');
}
if (!level->inArray && level->valueCount % 2 == 0)
RAPIDJSON_ASSERT(type == kStringType); // if it's in object, then even
// number should be a name
level->valueCount++;
} else {
RAPIDJSON_ASSERT(!hasRoot_); // Should only has one and only one root.
hasRoot_ = true;
}
}
// Flush the value if it is the top level one.
bool EndValue(bool ret) {
if (RAPIDJSON_UNLIKELY(level_stack_.Empty())) // end of json text
Flush();
return ret;
}
OutputStream *os_;
internal::Stack<StackAllocator> level_stack_;
int maxDecimalPlaces_;
bool hasRoot_;
private:
// Prohibit copy constructor & assignment operator.
Writer(const Writer &);
Writer &operator=(const Writer &);
};
// Full specialization for StringStream to prevent memory copying
template <>
inline bool Writer<StringBuffer>::WriteInt(int i) {
char *buffer = os_->Push(11);
const char *end = internal::i32toa(i, buffer);
os_->Pop(static_cast<size_t>(11 - (end - buffer)));
return true;
}
template <>
inline bool Writer<StringBuffer>::WriteUint(unsigned u) {
char *buffer = os_->Push(10);
const char *end = internal::u32toa(u, buffer);
os_->Pop(static_cast<size_t>(10 - (end - buffer)));
return true;
}
template <>
inline bool Writer<StringBuffer>::WriteInt64(int64_t i64) {
char *buffer = os_->Push(21);
const char *end = internal::i64toa(i64, buffer);
os_->Pop(static_cast<size_t>(21 - (end - buffer)));
return true;
}
template <>
inline bool Writer<StringBuffer>::WriteUint64(uint64_t u) {
char *buffer = os_->Push(20);
const char *end = internal::u64toa(u, buffer);
os_->Pop(static_cast<size_t>(20 - (end - buffer)));
return true;
}
template <>
inline bool Writer<StringBuffer>::WriteDouble(double d) {
if (internal::Double(d).IsNanOrInf()) {
// Note: This code path can only be reached if
// (RAPIDJSON_WRITE_DEFAULT_FLAGS & kWriteNanAndInfFlag).
if (!(kWriteDefaultFlags & kWriteNanAndInfFlag)) return false;
if (internal::Double(d).IsNan()) {
PutReserve(*os_, 3);
PutUnsafe(*os_, 'N');
PutUnsafe(*os_, 'a');
PutUnsafe(*os_, 'N');
return true;
}
if (internal::Double(d).Sign()) {
PutReserve(*os_, 9);
PutUnsafe(*os_, '-');
} else
PutReserve(*os_, 8);
PutUnsafe(*os_, 'I');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 't');
PutUnsafe(*os_, 'y');
return true;
}
char *buffer = os_->Push(25);
char *end = internal::dtoa(d, buffer, maxDecimalPlaces_);
os_->Pop(static_cast<size_t>(25 - (end - buffer)));
return true;
}
#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42)
template <>
inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
size_t length) {
if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
const char *p = is.src_;
const char *end = is.head_ + length;
const char *nextAligned = reinterpret_cast<const char *>(
(reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
const char *endAligned = reinterpret_cast<const char *>(
reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
if (nextAligned > end) return true;
while (p != nextAligned)
if (*p < 0x20 || *p == '\"' || *p == '\\') {
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
} else
os_->PutUnsafe(*p++);
// The rest of string using SIMD
static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"',
'\"', '\"', '\"', '\"', '\"', '\"',
'\"', '\"', '\"', '\"'};
static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\',
'\\', '\\', '\\', '\\', '\\', '\\',
'\\', '\\', '\\', '\\'};
static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
0x1F, 0x1F, 0x1F, 0x1F};
const __m128i dq =
_mm_loadu_si128(reinterpret_cast<const __m128i *>(&dquote[0]));
const __m128i bs =
_mm_loadu_si128(reinterpret_cast<const __m128i *>(&bslash[0]));
const __m128i sp =
_mm_loadu_si128(reinterpret_cast<const __m128i *>(&space[0]));
for (; p != endAligned; p += 16) {
const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
const __m128i t1 = _mm_cmpeq_epi8(s, dq);
const __m128i t2 = _mm_cmpeq_epi8(s, bs);
const __m128i t3 = _mm_cmpeq_epi8(_mm_max_epu8(s, sp),
sp); // s < 0x20 <=> max(s, 0x1F) == 0x1F
const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3);
unsigned short r = static_cast<unsigned short>(_mm_movemask_epi8(x));
if (RAPIDJSON_UNLIKELY(r != 0)) { // some of characters is escaped
SizeType len;
#ifdef _MSC_VER // Find the index of first escaped
unsigned long offset;
_BitScanForward(&offset, r);
len = offset;
#else
len = static_cast<SizeType>(__builtin_ffs(r) - 1);
#endif
char *q = reinterpret_cast<char *>(os_->PushUnsafe(len));
for (size_t i = 0; i < len; i++) q[i] = p[i];
p += len;
break;
}
_mm_storeu_si128(reinterpret_cast<__m128i *>(os_->PushUnsafe(16)), s);
}
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
}
#elif defined(RAPIDJSON_NEON)
template <>
inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
size_t length) {
if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
const char *p = is.src_;
const char *end = is.head_ + length;
const char *nextAligned = reinterpret_cast<const char *>(
(reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
const char *endAligned = reinterpret_cast<const char *>(
reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
if (nextAligned > end) return true;
while (p != nextAligned)
if (*p < 0x20 || *p == '\"' || *p == '\\') {
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
} else
os_->PutUnsafe(*p++);
// The rest of string using SIMD
const uint8x16_t s0 = vmovq_n_u8('"');
const uint8x16_t s1 = vmovq_n_u8('\\');
const uint8x16_t s2 = vmovq_n_u8('\b');
const uint8x16_t s3 = vmovq_n_u8(32);
for (; p != endAligned; p += 16) {
const uint8x16_t s = vld1q_u8(reinterpret_cast<const uint8_t *>(p));
uint8x16_t x = vceqq_u8(s, s0);
x = vorrq_u8(x, vceqq_u8(s, s1));
x = vorrq_u8(x, vceqq_u8(s, s2));
x = vorrq_u8(x, vcltq_u8(s, s3));
x = vrev64q_u8(x); // Rev in 64
uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0); // extract
uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1); // extract
SizeType len = 0;
bool escaped = false;
if (low == 0) {
if (high != 0) {
uint32_t lz = RAPIDJSON_CLZLL(high);
len = 8 + (lz >> 3);
escaped = true;
}
} else {
uint32_t lz = RAPIDJSON_CLZLL(low);
len = lz >> 3;
escaped = true;
}
if (RAPIDJSON_UNLIKELY(escaped)) { // some of characters is escaped
char *q = reinterpret_cast<char *>(os_->PushUnsafe(len));
for (size_t i = 0; i < len; i++) q[i] = p[i];
p += len;
break;
}
vst1q_u8(reinterpret_cast<uint8_t *>(os_->PushUnsafe(16)), s);
}
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
}
#endif // RAPIDJSON_NEON
RAPIDJSON_NAMESPACE_END
#if defined(_MSC_VER) || defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_RAPIDJSON_H_
livox_ros_driver2/CHANGELOG.md
+57
View File
@@ -0,0 +1,57 @@
# Changelog
All notable changes to this project will be documented in this file.
## [1.2.4]
### Fixed
- Optimize framing performance
## [1.2.3]
### Fixed
- Optimize framing logic and reduce CPU usage
- Fixed some known issues
## [1.2.1]
### Fixed
- Fix offset time error regarding CustomMsg format message publishment.
## [1.2.0]
### Added
- Revise the frame segmentation logic.
- (Notice!!!) Add Timestamp to each point in Livox pointcloud2 (PointXYZRTLT) format. The PointXYZRTL format has been updated to PointXYZRTLT format. Compatibility needs to be considered.
### Fixed
- Improve support for gPTP and GPS synchronizations.
---
## [1.1.3]
### Fixed
- Improve performance when running in ROS2 Humble.
---
## [1.1.2]
### Changed
- Change publish frequency range to [0.5Hz, 10 Hz].
### Fixed
- Fix a high CPU-usage problem.
---
## [1.1.1]
### Added
- Offer valid line-number info in the point cloud data of MID-360 Lidar.
- Enable IMU by default.
### Changed
- Update the README slightly.
---
## [1.0.0]
### Added
- Support Mid-360 Lidar.
- Support for Ubuntu 22.04 ROS2 humble.
- Support multi-topic fuction, the suffix of the topic name corresponds to the ip address of each Lidar.
### Changed
- Remove the embedded SDK.
- Constraint: Livox ROS Driver 2 for ROS2 does not support message passing with PCL native data types.
### Fixed
- Fix IMU packet loss.
- Fix some conflicts with livox ros driver.
- Fixed HAP Lidar publishing PointCloud2 and CustomMsg format point clouds with no line number.
livox_ros_driver2/CMakeLists.txt
+336
View File
@@ -0,0 +1,336 @@
# judge which cmake codes to use
if(ROS_EDITION STREQUAL "ROS1")
# Copyright(c) 2019 livoxtech limited.
cmake_minimum_required(VERSION 3.0)
#---------------------------------------------------------------------------------------
# Start livox_ros_driver2 project
#---------------------------------------------------------------------------------------
include(cmake/version.cmake)
project(livox_ros_driver2 VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
message(STATUS "livox_ros_driver2 version: ${LIVOX_ROS_DRIVER2_VERSION}")
#---------------------------------------------------------------------------------------
# Add ROS Version MACRO
#---------------------------------------------------------------------------------------
add_definitions(-DBUILDING_ROS1)
#---------------------------------------------------------------------------------------
# find package and the dependecy
#---------------------------------------------------------------------------------------
find_package(Boost 1.54 REQUIRED COMPONENTS
system
thread
chrono
)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
sensor_msgs
std_msgs
message_generation
rosbag
pcl_ros
)
## Find pcl lib
find_package(PCL REQUIRED)
## Generate messages in the 'msg' folder
add_message_files(FILES
CustomPoint.msg
CustomMsg.msg
# Message2.msg
)
## Generate added messages and services with any dependencies listed here
generate_messages(DEPENDENCIES
std_msgs
)
find_package(PkgConfig)
pkg_check_modules(APR apr-1)
if (APR_FOUND)
message(${APR_INCLUDE_DIRS})
message(${APR_LIBRARIES})
endif (APR_FOUND)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects als o need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also n eed
catkin_package(CATKIN_DEPENDS
roscpp rospy std_msgs message_runtime
pcl_ros
)
#---------------------------------------------------------------------------------------
# Set default build to release
#---------------------------------------------------------------------------------------
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose Release or Debug" FORCE)
endif()
#---------------------------------------------------------------------------------------
# Compiler config
#---------------------------------------------------------------------------------------
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
## make sure the livox_lidar_sdk_static library is installed
find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_static.a /usr/local/lib)
## PCL library
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
#---------------------------------------------------------------------------------------
# generate excutable and add libraries
#---------------------------------------------------------------------------------------
add_executable(${PROJECT_NAME}_node
""
)
#---------------------------------------------------------------------------------------
# precompile macro and compile option
#---------------------------------------------------------------------------------------
target_compile_options(${PROJECT_NAME}_node
PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall>
)
#---------------------------------------------------------------------------------------
# add projects that depend on
#---------------------------------------------------------------------------------------
add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp)
#---------------------------------------------------------------------------------------
# source file
#---------------------------------------------------------------------------------------
target_sources(${PROJECT_NAME}_node
PRIVATE
src/driver_node.cpp
src/lds.cpp
src/lds_lidar.cpp
src/lddc.cpp
src/livox_ros_driver2.cpp
src/comm/comm.cpp
src/comm/ldq.cpp
src/comm/semaphore.cpp
src/comm/lidar_imu_data_queue.cpp
src/comm/cache_index.cpp
src/comm/pub_handler.cpp
src/parse_cfg_file/parse_cfg_file.cpp
src/parse_cfg_file/parse_livox_lidar_cfg.cpp
src/call_back/lidar_common_callback.cpp
src/call_back/livox_lidar_callback.cpp
)
#---------------------------------------------------------------------------------------
# include file
#---------------------------------------------------------------------------------------
target_include_directories(${PROJECT_NAME}_node
PUBLIC
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${APR_INCLUDE_DIRS}
3rdparty
src
)
#---------------------------------------------------------------------------------------
# link libraries
#---------------------------------------------------------------------------------------
target_link_libraries(${PROJECT_NAME}_node
${LIVOX_LIDAR_SDK_LIBRARY}
${Boost_LIBRARY}
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${APR_LIBRARIES}
)
#---------------------------------------------------------------------------------------
# Install
#---------------------------------------------------------------------------------------
install(TARGETS ${PROJECT_NAME}_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY launch_ROS1/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch_ROS1
)
#---------------------------------------------------------------------------------------
# end of CMakeList.txt
#---------------------------------------------------------------------------------------
else(ROS_EDITION STREQUAL "ROS2")
# Copyright(c) 2020 livoxtech limited.
cmake_minimum_required(VERSION 3.14)
project(livox_ros_driver2)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules")
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
endif()
# Printf version info
include(cmake/version.cmake)
project(${PROJECT_NAME} VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}")
#---------------------------------------------------------------------------------------
# Add ROS Version MACRO
#---------------------------------------------------------------------------------------
add_definitions(-DBUILDING_ROS2)
# find dependencies
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()
find_package(PCL REQUIRED)
find_package(std_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(rosidl_default_generators REQUIRED)
# check apr
find_package(PkgConfig)
pkg_check_modules(APR apr-1)
if (APR_FOUND)
message(${APR_INCLUDE_DIRS})
message(${APR_LIBRARIES})
endif (APR_FOUND)
# generate custom msg headers
set(LIVOX_INTERFACES livox_interfaces2)
rosidl_generate_interfaces(${LIVOX_INTERFACES}
"msg/CustomPoint.msg"
"msg/CustomMsg.msg"
DEPENDENCIES builtin_interfaces std_msgs
LIBRARY_NAME ${PROJECT_NAME}
)
## make sure the livox_lidar_sdk_shared library is installed
find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED)
##
find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR
NAMES "livox_lidar_api.h" "livox_lidar_def.h"
REQUIRED)
## PCL library
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
# livox ros2 driver target
ament_auto_add_library(${PROJECT_NAME} SHARED
src/livox_ros_driver2.cpp
src/lddc.cpp
src/driver_node.cpp
src/lds.cpp
src/lds_lidar.cpp
src/comm/comm.cpp
src/comm/ldq.cpp
src/comm/semaphore.cpp
src/comm/lidar_imu_data_queue.cpp
src/comm/cache_index.cpp
src/comm/pub_handler.cpp
src/parse_cfg_file/parse_cfg_file.cpp
src/parse_cfg_file/parse_livox_lidar_cfg.cpp
src/call_back/lidar_common_callback.cpp
src/call_back/livox_lidar_callback.cpp
)
target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS})
# get include directories of custom msg headers
if(HUMBLE_ROS STREQUAL "humble")
rosidl_get_typesupport_target(cpp_typesupport_target
${LIVOX_INTERFACES} "rosidl_typesupport_cpp")
target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}")
else()
set(LIVOX_INTERFACE_TARGET "${LIVOX_INTERFACES}__rosidl_typesupport_cpp")
add_dependencies(${PROJECT_NAME} ${LIVOX_INTERFACES})
get_target_property(LIVOX_INTERFACES_INCLUDE_DIRECTORIES ${LIVOX_INTERFACE_TARGET} INTERFACE_INCLUDE_DIRECTORIES)
endif()
# include file direcotry
target_include_directories(${PROJECT_NAME} PUBLIC
${PCL_INCLUDE_DIRS}
${APR_INCLUDE_DIRS}
${LIVOX_LIDAR_SDK_INCLUDE_DIR}
${LIVOX_INTERFACES_INCLUDE_DIRECTORIES} # for custom msgs
3rdparty
src
)
# link libraries
target_link_libraries(${PROJECT_NAME}
${LIVOX_LIDAR_SDK_LIBRARY}
${LIVOX_INTERFACE_TARGET} # for custom msgs
${PPT_LIBRARY}
${Boost_LIBRARY}
${PCL_LIBRARIES}
${APR_LIBRARIES}
)
rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "livox_ros::DriverNode"
EXECUTABLE ${PROJECT_NAME}_node
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_auto_package(INSTALL_TO_SHARE
config
launch_ROS2
)
endif()
livox_ros_driver2/LICENSE.txt
+412
View File
@@ -0,0 +1,412 @@
The following portions of the LIVOX’s Livox ROS Driver2 (“Software” referred to in the terms below) are made available to you under the terms of the MIT License provided below and is also available at https://opensource.org/licenses/MIT.
livox_ros_driver2
├── build.sh
├── cmake
│   └── version.cmake
├── CMakeLists.txt
├── config
│   ├── display_point_cloud_ROS1.rviz
│   ├── display_point_cloud_ROS2.rviz
│   ├── HAP_config.json
│   ├── MID360_config.json
│   └── mixed_HAP_MID360_config.json
├── launch_ROS1
│   ├── msg_HAP.launch
│   ├── msg_MID360.launch
│   ├── msg_mixed.launch
│   ├── rviz_HAP.launch
│   ├── rviz_MID360.launch
│   └── rviz_mixed.launch
├── launch_ROS2
│   ├── msg_HAP_launch.py
│   ├── msg_MID360_launch.py
│   ├── rviz_HAP_launch.py
│   ├── rviz_MID360_launch.py
│   └── rviz_mixed.py
├── msg
│   ├── CustomMsg.msg
│   └── CustomPoint.msg
├── package_ROS1.xml
├── package_ROS2.xml
├── package.xml
├── README.md
└── src
├── call_back
│   ├── lidar_common_callback.cpp
│   ├── lidar_common_callback.h
│   ├── livox_lidar_callback.cpp
│   └── livox_lidar_callback.h
├── comm
│   ├── cache_index.cpp
│   ├── cache_index.h
│   ├── comm.cpp
│   ├── comm.h
│   ├── ldq.cpp
│   ├── ldq.h
│   ├── lidar_imu_data_queue.cpp
│   ├── lidar_imu_data_queue.h
│   ├── pub_handler.cpp
│   ├── pub_handler.h
│   ├── semaphore.cpp
│   └── semaphore.h
├── driver_node.cpp
├── driver_node.h
├── include
│   ├── livox_ros_driver2.h
│   ├── ros1_headers.h
│   ├── ros2_headers.h
│   └── ros_headers.h
├── lddc.cpp
├── lddc.h
├── lds.cpp
├── lds.h
├── lds_lidar.cpp
├── lds_lidar.h
├── livox_ros_driver2.cpp
└── parse_cfg_file
├── parse_cfg_file.cpp
├── parse_cfg_file.h
├── parse_livox_lidar_cfg.cpp
└── parse_livox_lidar_cfg.h
---------------------------------
The MIT License (MIT)
Copyright (c) 2022 Livox. All rights reserved.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
===============================================================
LIVOX’s Livox ROS Driver2 uses unmodified source code of RapidJSON (https://github.com/Tencent/rapidjson), which is also licensed under MIT license. A copy of the MIT license is provided below and is also available at https://opensource.org/licenses/MIT.
livox_ros_driver2
├── 3rdparty
│   └── rapidjson
│   ├── allocators.h
│   ├── cursorstreamwrapper.h
│   ├── document.h
│   ├── encodedstream.h
│   ├── encodings.h
│   ├── error
│   │   ├── en.h
│   │   └── error.h
│   ├── filereadstream.h
│   ├── filewritestream.h
│   ├── fwd.h
│   ├── internal
│   │   ├── biginteger.h
│   │   ├── clzll.h
│   │   ├── diyfp.h
│   │   ├── dtoa.h
│   │   ├── ieee754.h
│   │   ├── itoa.h
│   │   ├── meta.h
│   │   ├── pow10.h
│   │   ├── regex.h
│   │   ├── stack.h
│   │   ├── strfunc.h
│   │   ├── strtod.h
│   │   └── swap.h
│   ├── istreamwrapper.h
│   ├── memorybuffer.h
│   ├── memorystream.h
│   ├── msinttypes
│   │   ├── inttypes.h
│   │   └── stdint.h
│   ├── ostreamwrapper.h
│   ├── pointer.h
│   ├── prettywriter.h
│   ├── rapidjson.h
│   ├── reader.h
│   ├── schema.h
│   ├── stream.h
│   ├── stringbuffer.h
│   └── writer.h
-------------------------------------------------------------
Tencent is pleased to support the open source community by making RapidJSON
available.
Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
rights reserved.
Licensed under the MIT License (the "License"); you may not use this file
except in compliance with the License. You may obtain a copy of the License
at
http://opensource.org/licenses/MIT
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
License for the specific language governing permissions and limitations under
the License.
===============================================================
LIVOX’s Livox ROS Driver2 uses unmodified libraries and interfaces of ROS (https://www.ros.org/), which is also licensed under 3-Clause-BSD license. A copy of the 3-Clause-BSD license is provided below and is also available at https://opensource.org/licenses/BSD-3-Clause.
-------------------------------------------------------------
The 3-Clause BSD License
Copyright (c) 2001 - 2009, The Board of Trustees of the University of Illinois.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
* Redistributions of source code must retain the above
copyright notice, this list of conditions and the
following disclaimer.
* Redistributions in binary form must reproduce the
above copyright notice, this list of conditions
and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the University of Illinois
nor the names of its contributors may be used to
endorse or promote products derived from this
software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
===============================================================
LIVOX’s Livox ROS Driver2 uses unmodified libraries and interfaces of ROS2-rclcpp (https://github.com/ros2), which is also licensed under Apache License 2.0. A copy of the Apache License 2.0 is provided below and is also available at https://www.apache.org/licenses/LICENSE-2.0.
-------------------------------------------------------------
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
livox_ros_driver2/README.md
+545
View File
@@ -0,0 +1,545 @@
# Livox ROS Driver 2
Livox ROS Driver 2 is the 2nd-generation driver package used to connect LiDAR products produced by Livox, applicable for ROS (noetic recommended) and ROS2 (foxy or humble recommended).
**Note :**
As a debugging tool, Livox ROS Driver is not recommended for mass production but limited to test scenarios. You should optimize the code based on the original source to meet your various needs.
## 1. Preparation
### 1.1 OS requirements
* Ubuntu 18.04 for ROS Melodic;
* Ubuntu 20.04 for ROS Noetic and ROS2 Foxy;
* Ubuntu 22.04 for ROS2 Humble;
**Tips:**
Colcon is a build tool used in ROS2.
How to install colcon: [Colcon installation instructions](https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Colcon-Tutorial.html)
### 1.2 Install ROS & ROS2
For ROS Melodic installation, please refer to:
[ROS Melodic installation instructions](https://wiki.ros.org/melodic/Installation)
For ROS Noetic installation, please refer to:
[ROS Noetic installation instructions](https://wiki.ros.org/noetic/Installation)
For ROS2 Foxy installation, please refer to:
[ROS Foxy installation instructions](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html)
For ROS2 Humble installation, please refer to:
[ROS Humble installation instructions](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html)
Desktop-Full installation is recommend.
## 2. Build & Run Livox ROS Driver 2
### 2.1 Clone Livox ROS Driver 2 source code:
```shell
git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2
```
**Note :**
Be sure to clone the source code in a '[work_space]/src/' folder (as shown above), otherwise compilation errors will occur due to the compilation tool restriction.
### 2.2 Build & install the Livox-SDK2
**Note :**
Please follow the guidance of installation in the [Livox-SDK2/README.md](https://github.com/Livox-SDK/Livox-SDK2/blob/master/README.md)
### 2.3 Build the Livox ROS Driver 2:
#### For ROS (take Noetic as an example):
```shell
source /opt/ros/noetic/setup.sh
./build.sh ROS1
```
#### For ROS2 Foxy:
```shell
source /opt/ros/foxy/setup.sh
./build.sh ROS2
```
#### For ROS2 Humble:
```shell
source /opt/ros/humble/setup.sh
./build.sh humble
```
### 2.4 Run Livox ROS Driver 2:
#### For ROS:
```shell
source ../../devel/setup.sh
roslaunch livox_ros_driver2 [launch file]
```
in which,
* **livox_ros_driver2** : is the ROS package name of Livox ROS Driver 2;
* **[launch file]** : is the ROS launch file you want to use; the 'launch_ROS1' folder contains several launch samples for your reference;
An rviz launch example for HAP LiDAR would be:
```shell
roslaunch livox_ros_driver2 rviz_HAP.launch
```
#### For ROS2:
```shell
source ../../install/setup.sh
ros2 launch livox_ros_driver2 [launch file]
```
in which,
* **[launch file]** : is the ROS2 launch file you want to use; the 'launch_ROS2' folder contains several launch samples for your reference.
A rviz launch example for HAP LiDAR would be:
```shell
ros2 launch livox_ros_driver2 rviz_HAP_launch.py
```
## 3. Launch file and livox_ros_driver2 internal parameter configuration instructions
### 3.1 Launch file configuration instructions
Launch files of ROS are in the "ws_livox/src/livox_ros_driver2/launch_ROS1" directory and launch files of ROS2 are in the "ws_livox/src/livox_ros_driver2/launch_ROS2" directory. Different launch files have different configuration parameter values and are used in different scenarios:
| launch file name | Description |
| ------------------------- | ------------------------------------------------------------ |
| rviz_HAP.launch | Connect to HAP LiDAR device<br>Publish pointcloud2 format data<br>Autoload rviz |
| msg_HAP.launch | Connect to HAP LiDAR device<br>Publish livox customized pointcloud data|
| rviz_MID360.launch | Connect to MID360 LiDAR device<br>Publish pointcloud2 format data <br>Autoload rviz|
| msg_MID360.launch | Connect to MID360 LiDAR device<br>Publish livox customized pointcloud data |
| rviz_mixed.launch | Connect to HAP and MID360 LiDAR device<br>Publish pointcloud2 format data <br>Autoload rviz|
| msg_mixed.launch | Connect to HAP and MID360 LiDAR device<br>Publish livox customized pointcloud data |
### 3.2 Livox ros driver 2 internal main parameter configuration instructions
All internal parameters of Livox_ros_driver2 are in the launch file. Below are detailed descriptions of the three commonly used parameters :
| Parameter | Detailed description | Default |
| ------------ | ------------------------------------------------------------ | ------- |
| publish_freq | Set the frequency of point cloud publish <br>Floating-point data type, recommended values 5.0, 10.0, 20.0, 50.0, etc. The maximum publish frequency is 100.0 Hz.| 10.0 |
| multi_topic | If the LiDAR device has an independent topic to publish pointcloud data<br>0 -- All LiDAR devices use the same topic to publish pointcloud data<br>1 -- Each LiDAR device has its own topic to publish point cloud data | 0 |
| xfer_format | Set pointcloud format<br>0 -- Livox pointcloud2(PointXYZRTLT) pointcloud format<br>1 -- Livox customized pointcloud format<br>2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS) | 0 |
**Note :**
Other parameters not mentioned in this table are not suggested to be changed unless fully understood.
&ensp;&ensp;&ensp;&ensp;***Livox_ros_driver2 pointcloud data detailed description :***
1. Livox pointcloud2 (PointXYZRTLT) point cloud format, as follows :
```c
float32 x # X axis, unit:m
float32 y # Y axis, unit:m
float32 z # Z axis, unit:m
float32 intensity # the value is reflectivity, 0.0~255.0
uint8 tag # livox tag
uint8 line # laser number in lidar
float64 timestamp # Timestamp of point
```
**Note :**
The number of points in the frame may be different, but each point provides a timestamp.
2. Livox customized data package format, as follows :
```c
std_msgs/Header header # ROS standard message header
uint64 timebase # The time of first point
uint32 point_num # Total number of pointclouds
uint8 lidar_id # Lidar device id number
uint8[3] rsvd # Reserved use
CustomPoint[] points # Pointcloud data
```
&ensp;&ensp;&ensp;&ensp;Customized Point Cloud (CustomPoint) format in the above customized data package :
```c
uint32 offset_time # offset time relative to the base time
float32 x # X axis, unit:m
float32 y # Y axis, unit:m
float32 z # Z axis, unit:m
uint8 reflectivity # reflectivity, 0~255
uint8 tag # livox tag
uint8 line # laser number in lidar
```
3. The standard pointcloud2 (pcl :: PointXYZI) format in the PCL library (only ROS can publish):
&ensp;&ensp;&ensp;&ensp;Please refer to the pcl :: PointXYZI data structure in the point_types.hpp file of the PCL library.
## 4. LiDAR config
LiDAR Configurations (such as ip, port, data type... etc.) can be set via a json-style config file. Config files for single HAP, Mid360 and mixed-LiDARs are in the "config" folder. The parameter naming *'user_config_path'* in launch files indicates such json file path.
1. Follow is a configuration example for HAP LiDAR (located in config/HAP_config.json):
```json
{
"lidar_summary_info" : {
"lidar_type": 8 # protocol type index, please don't revise this value
},
"HAP": {
"device_type" : "HAP",
"lidar_ipaddr": "",
"lidar_net_info" : {
"cmd_data_port": 56000, # command port
"push_msg_port": 0,
"point_data_port": 57000,
"imu_data_port": 58000,
"log_data_port": 59000
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5", # host ip (it can be revised)
"cmd_data_port": 56000,
"push_msg_ip": "",
"push_msg_port": 0,
"point_data_ip": "192.168.1.5", # host ip
"point_data_port": 57000,
"imu_data_ip" : "192.168.1.5", # host ip
"imu_data_port": 58000,
"log_data_ip" : "",
"log_data_port": 59000
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.100", # ip of the LiDAR you want to config
"pcl_data_type" : 1,
"pattern_mode" : 0,
"blind_spot_set" : 50,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
```
The parameter attributes in the above json file are described in the following table :
**LiDAR configuration parameter**
| Parameter | Type | Description | Default |
| :------------------------- | ------- | ------------------------------------------------------------ | --------------- |
| ip | String | Ip of the LiDAR you want to config | 192.168.1.100 |
| pcl_data_type | Int | Choose the resolution of the point cloud data to send<br>1 -- Cartesian coordinate data (32 bits)<br>2 -- Cartesian coordinate data (16 bits) <br>3 --Spherical coordinate data| 1 |
| pattern_mode | Int | Space scan pattern<br>0 -- non-repeating scanning pattern mode<br>1 -- repeating scanning pattern mode <br>2 -- repeating scanning pattern mode (low scanning rate) | 0 |
| blind_spot_set (Only for HAP LiDAR) | Int | Set blind spot<br>Range from 50 cm to 200 cm | 50 |
| extrinsic_parameter | | Set extrinsic parameter<br> The data types of "roll" "picth" "yaw" are float <br> The data types of "x" "y" "z" are int<br> |
For more infomation about the HAP config, please refer to:
[HAP Config File Description](https://github.com/Livox-SDK/Livox-SDK2/wiki/hap-config-file-description)
2. When connecting multiple LiDARs, add objects corresponding to different LiDARs to the "lidar_configs" array. Examples of mixed-LiDARs config file contents are as follows :
```json
{
"lidar_summary_info" : {
"lidar_type": 8 # protocol type index, please don't revise this value
},
"HAP": {
"lidar_net_info" : { # HAP ports, please don't revise these values
"cmd_data_port": 56000, # HAP command port
"push_msg_port": 0,
"point_data_port": 57000,
"imu_data_port": 58000,
"log_data_port": 59000
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5", # host ip
"cmd_data_port": 56000,
"push_msg_ip": "",
"push_msg_port": 0,
"point_data_ip": "192.168.1.5", # host ip
"point_data_port": 57000,
"imu_data_ip" : "192.168.1.5", # host ip
"imu_data_port": 58000,
"log_data_ip" : "",
"log_data_port": 59000
}
},
"MID360": {
"lidar_net_info" : { # Mid360 ports, please don't revise these values
"cmd_data_port": 56100, # Mid360 command port
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5", # host ip
"cmd_data_port": 56101,
"push_msg_ip": "192.168.1.5", # host ip
"push_msg_port": 56201,
"point_data_ip": "192.168.1.5", # host ip
"point_data_port": 56301,
"imu_data_ip" : "192.168.1.5", # host ip
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.100", # ip of the HAP you want to config
"pcl_data_type" : 1,
"pattern_mode" : 0,
"blind_spot_set" : 50,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
},
{
"ip" : "192.168.1.12", # ip of the Mid360 you want to config
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
```
3. when multiple nics on the host connect to multiple LiDARs, you need to add objects corresponding to different LiDARs to the lidar_configs array. Run different luanch files separately, and the following is an example of mixing lidar configuration file contents:
**MID360_config1:**
```json
{
"lidar_summary_info" : {
"lidar_type": 8 # protocol type index，please don't revise this value
},
"MID360": {
"lidar_net_info": {
"cmd_data_port": 56100, # command port
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info": [
{
"lidar_ip": ["192.168.1.100"], # Lidar ip
"host_ip": "192.168.1.5", # host ip
"cmd_data_port": 56101,
"push_msg_port": 56201,
"point_data_port": 56301,
"imu_data_port": 56401,
"log_data_port": 56501
}
]
},
"lidar_configs": [
{
"ip": "192.168.1.100", # ip of the LiDAR you want to config
"pcl_data_type": 1,
"pattern_mode": 0,
"extrinsic_parameter": {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
```
**MID360_config2:**
```json
{
"lidar_summary_info" : {
"lidar_type": 8 # protocol type index，please don't revise this value
},
"MID360": {
"lidar_net_info": {
"cmd_data_port": 56100, # command port
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info": [
{
"lidar_ip": ["192.168.2.100"], # Lidar ip
"host_ip": "192.168.2.5", # host ip
"cmd_data_port": 56101,
"push_msg_port": 56201,
"point_data_port": 56301,
"imu_data_port": 56401,
"log_data_port": 56501
}
]
},
"lidar_configs": [
{
"ip": "192.168.2.100", # ip of the LiDAR you want to config
"pcl_data_type": 1,
"pattern_mode": 0,
"extrinsic_parameter": {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
```
**Launch1:**
```
<launch>
<!--user configure parameters for ros start-->
<arg name="lvx_file_path" default="livox_test.lvx"/>
<arg name="bd_list" default="100000000000000"/>
<arg name="xfer_format" default="0"/>
<arg name="multi_topic" default="1"/>
<arg name="data_src" default="0"/>
<arg name="publish_freq" default="10.0"/>
<arg name="output_type" default="0"/>
<arg name="rviz_enable" default="true"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="true"/>
<!--user configure parameters for ros end-->
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver2)/config/MID360_config1.json"/> # Mid360 MID360_config1 name
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher1" pkg="livox_ros_driver2"
type="livox_ros_driver2_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rviz_enable)">
<node name="livox_rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver2)/config/display_point_cloud_ROS1.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
</launch>
```
**Launch2:**
```
<launch>
<!--user configure parameters for ros start-->
<arg name="lvx_file_path" default="livox_test.lvx"/>
<arg name="bd_list" default="100000000000000"/>
<arg name="xfer_format" default="0"/>
<arg name="multi_topic" default="1"/>
<arg name="data_src" default="0"/>
<arg name="publish_freq" default="10.0"/>
<arg name="output_type" default="0"/>
<arg name="rviz_enable" default="true"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<arg name="lidar_bag" default="true"/>
<arg name="imu_bag" default="true"/>
<!--user configure parameters for ros end-->
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
<param name="data_src" value="$(arg data_src)"/>
<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver2)/config/MID360_config2.json"/> # Mid360 MID360_config2 name
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>
<node name="livox_lidar_publisher2" pkg="livox_ros_driver2"
type="livox_ros_driver2_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>
<group if="$(arg rviz_enable)">
<node name="livox_rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver2)/config/display_point_cloud_ROS1.rviz"/>
</group>
<group if="$(arg rosbag_enable)">
<node pkg="rosbag" type="record" name="record" output="screen"
args="-a"/>
</group>
</launch>
```
## 5. Supported LiDAR list
* HAP
* Mid360
* (more types are comming soon...)
## 6. FAQ
### 6.1 launch with "livox_lidar_rviz_HAP.launch" but no point cloud display on the grid?
Please check the "Global Options - Fixed Frame" field in the RViz "Display" pannel. Set the field value to "livox_frame" and check the "PointCloud2" option in the pannel.
### 6.2 launch with command "ros2 launch livox_lidar_rviz_HAP_launch.py" but cannot open shared object file "liblivox_sdk_shared.so" ?
Please add '/usr/local/lib' to the env LD_LIBRARY_PATH.
* If you want to add to current terminal:
```shell
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib
```
* If you want to add to current user:
```shell
vim ~/.bashrc
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib
source ~/.bashrc
```
livox_ros_driver2/build.sh
Executable
+70
View File
@@ -0,0 +1,70 @@
#!/bin/bash
readonly VERSION_ROS1="ROS1"
readonly VERSION_ROS2="ROS2"
readonly VERSION_HUMBLE="humble"
pushd `pwd` > /dev/null
cd `dirname $0`
echo "Working Path: "`pwd`
ROS_VERSION=""
ROS_HUMBLE=""
# Set working ROS version
if [ "$1" = "ROS2" ]; then
ROS_VERSION=${VERSION_ROS2}
elif [ "$1" = "humble" ]; then
ROS_VERSION=${VERSION_ROS2}
ROS_HUMBLE=${VERSION_HUMBLE}
elif [ "$1" = "ROS1" ]; then
ROS_VERSION=${VERSION_ROS1}
else
echo "Invalid Argument"
exit
fi
echo "ROS version is: "$ROS_VERSION
# clear `build/` folder.
# TODO: Do not clear these folders, if the last build is based on the same ROS version.
rm -rf ../../build/
rm -rf ../../devel/
rm -rf ../../install/
# clear src/CMakeLists.txt if it exists.
if [ -f ../CMakeLists.txt ]; then
rm -f ../CMakeLists.txt
fi
# exit
# substitute the files/folders: CMakeList.txt, package.xml(s)
if [ ${ROS_VERSION} = ${VERSION_ROS1} ]; then
if [ -f package.xml ]; then
rm package.xml
fi
cp -f package_ROS1.xml package.xml
elif [ ${ROS_VERSION} = ${VERSION_ROS2} ]; then
if [ -f package.xml ]; then
rm package.xml
fi
cp -f package_ROS2.xml package.xml
cp -rf launch_ROS2/ launch/
fi
# build
pushd `pwd` > /dev/null
if [ $ROS_VERSION = ${VERSION_ROS1} ]; then
cd ../../
catkin_make -DROS_EDITION=${VERSION_ROS1}
elif [ $ROS_VERSION = ${VERSION_ROS2} ]; then
cd ../../
colcon build --cmake-args -DROS_EDITION=${VERSION_ROS2} -DHUMBLE_ROS=${ROS_HUMBLE}
fi
popd > /dev/null
# remove the substituted folders/files
if [ $ROS_VERSION = ${VERSION_ROS2} ]; then
rm -rf launch/
fi
popd > /dev/null
livox_ros_driver2/cmake/version.cmake
+16
View File
@@ -0,0 +1,16 @@
#---------------------------------------------------------------------------------------
# Get livox_ros_driver2 version from include/livox_ros_driver2.h
#---------------------------------------------------------------------------------------
file(READ "${CMAKE_CURRENT_LIST_DIR}/../src/include/livox_ros_driver2.h" LIVOX_ROS_DRIVER2_VERSION_FILE)
string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MAJOR ([0-9]+)" _ "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
set(ver_major ${CMAKE_MATCH_1})
string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MINOR ([0-9]+)" _ "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
set(ver_minor ${CMAKE_MATCH_1})
string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_PATCH ([0-9]+)" _ "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
set(ver_patch ${CMAKE_MATCH_1})
if (NOT DEFINED ver_major OR NOT DEFINED ver_minor OR NOT DEFINED ver_patch)
message(FATAL_ERROR "Could not extract valid version from include/livox_ros_driver2.h")
endif()
set (LIVOX_ROS_DRIVER2_VERSION "${ver_major}.${ver_minor}.${ver_patch}")
livox_ros_driver2/config/HAP_config.json
+42
View File
@@ -0,0 +1,42 @@
{
"lidar_summary_info" : {
"lidar_type": 8
},
"HAP": {
"lidar_net_info" : {
"cmd_data_port": 56000,
"push_msg_port": 0,
"point_data_port": 57000,
"imu_data_port": 58000,
"log_data_port": 59000
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5",
"cmd_data_port": 56000,
"push_msg_ip": "",
"push_msg_port": 0,
"point_data_ip": "192.168.1.5",
"point_data_port": 57000,
"imu_data_ip" : "192.168.1.5",
"imu_data_port": 58000,
"log_data_ip" : "",
"log_data_port": 59000
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.100",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
livox_ros_driver2/config/MID360_config.json
+42
View File
@@ -0,0 +1,42 @@
{
"lidar_summary_info" : {
"lidar_type": 8
},
"MID360": {
"lidar_net_info" : {
"cmd_data_port": 56100,
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.50",
"cmd_data_port": 56101,
"push_msg_ip": "192.168.1.50",
"push_msg_port": 56201,
"point_data_ip": "192.168.1.50",
"point_data_port": 56301,
"imu_data_ip" : "192.168.1.50",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.3",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
livox_ros_driver2/config/display_point_cloud_ROS1.rviz
+172
View File
@@ -0,0 +1,172 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /PointCloud21
Splitter Ratio: 0.500694990158081
Tree Height: 728
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.4569999873638153
Min Value: -0.367000013589859
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 255
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 2
Size (m): 0.004999999888241291
Style: Points
Topic: /livox/lidar
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: livox_frame
Frame Rate: 40
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 25.80008888244629
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.2672550082206726
Y: 0.061853598803281784
Z: 0.15087400376796722
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5597995519638062
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.065610408782959
Saved:
- Class: rviz/Orbit
Distance: 10
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Orbit
Near Clip Distance: 0.009999999776482582
Pitch: 1.1103999614715576
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.5703970193862915
Window Geometry:
Displays:
collapsed: false
Height: 1025
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001c400000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005730000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1853
X: 67
Y: 27
livox_ros_driver2/config/display_point_cloud_ROS2.rviz
+137
View File
@@ -0,0 +1,137 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /PointCloud21
Splitter Ratio: 0.5
Tree Height: 796
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0.20000000298023224
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 150
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /livox/lidar
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: livox_frame
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic: /move_base_simple/goal
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 14.215983390808105
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.9503982067108154
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.6603963375091553
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1025
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000015f000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c3000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1853
X: 67
Y: 27
livox_ros_driver2/config/mixed_HAP_MID360_config.json
+76
View File
@@ -0,0 +1,76 @@
{
"lidar_summary_info" : {
"lidar_type": 8
},
"HAP": {
"lidar_net_info" : {
"cmd_data_port": 56000,
"push_msg_port": 0,
"point_data_port": 57000,
"imu_data_port": 58000,
"log_data_port": 59000
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5",
"cmd_data_port": 56000,
"push_msg_ip": "",
"push_msg_port": 0,
"point_data_ip": "192.168.1.5",
"point_data_port": 57000,
"imu_data_ip" : "192.168.1.5",
"imu_data_port": 58000,
"log_data_ip" : "",
"log_data_port": 59000
}
},
"MID360": {
"lidar_net_info" : {
"cmd_data_port": 56100,
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5",
"cmd_data_port": 56101,
"push_msg_ip": "192.168.1.5",
"push_msg_port": 56201,
"point_data_ip": "192.168.1.5",
"point_data_port": 56301,
"imu_data_ip" : "192.168.1.5",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.100",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
},
{
"ip" : "192.168.1.12",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
livox_ros_driver2/launch_ROS2/msg_HAP_launch.py
+55
View File
@@ -0,0 +1,55 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
import launch
################### user configure parameters for ros2 start ###################
xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic
data_src = 0 # 0-lidar, others-Invalid data src
publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
output_type = 0
frame_id = 'livox_frame'
lvx_file_path = '/home/livox/livox_test.lvx'
cmdline_bd_code = 'livox0000000001'
cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
cur_config_path = cur_path + '../config'
rviz_config_path = os.path.join(cur_config_path, 'livox_lidar.rviz')
user_config_path = os.path.join(cur_config_path, 'HAP_config.json')
################### user configure parameters for ros2 end #####################
livox_ros2_params = [
{"xfer_format": xfer_format},
{"multi_topic": multi_topic},
{"data_src": data_src},
{"publish_freq": publish_freq},
{"output_data_type": output_type},
{"frame_id": frame_id},
{"lvx_file_path": lvx_file_path},
{"user_config_path": user_config_path},
{"cmdline_input_bd_code": cmdline_bd_code}
]
def generate_launch_description():
livox_driver = Node(
package='livox_ros_driver2',
executable='livox_ros_driver2_node',
name='livox_lidar_publisher',
output='screen',
parameters=livox_ros2_params
)
return LaunchDescription([
livox_driver,
# launch.actions.RegisterEventHandler(
# event_handler=launch.event_handlers.OnProcessExit(
# target_action=livox_rviz,
# on_exit=[
# launch.actions.EmitEvent(event=launch.events.Shutdown()),
# ]
# )
# )
])
livox_ros_driver2/launch_ROS2/msg_MID360_launch.py
+54
View File
@@ -0,0 +1,54 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
import launch
################### user configure parameters for ros2 start ###################
xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic
data_src = 0 # 0-lidar, others-Invalid data src
publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
output_type = 0
frame_id = 'livox_frame'
lvx_file_path = '/home/livox/livox_test.lvx'
cmdline_bd_code = 'livox0000000001'
cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
cur_config_path = cur_path + '../config'
user_config_path = os.path.join(cur_config_path, 'MID360_config.json')
################### user configure parameters for ros2 end #####################
livox_ros2_params = [
{"xfer_format": xfer_format},
{"multi_topic": multi_topic},
{"data_src": data_src},
{"publish_freq": publish_freq},
{"output_data_type": output_type},
{"frame_id": frame_id},
{"lvx_file_path": lvx_file_path},
{"user_config_path": user_config_path},
{"cmdline_input_bd_code": cmdline_bd_code}
]
def generate_launch_description():
livox_driver = Node(
package='livox_ros_driver2',
executable='livox_ros_driver2_node',
name='livox_lidar_publisher',
output='screen',
parameters=livox_ros2_params
)
return LaunchDescription([
livox_driver,
# launch.actions.RegisterEventHandler(
# event_handler=launch.event_handlers.OnProcessExit(
# target_action=livox_rviz,
# on_exit=[
# launch.actions.EmitEvent(event=launch.events.Shutdown()),
# ]
# )
# )
])

Some files were not shown because too many files have changed in this diff Show More