azmo/fori_ws
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit: workspace configuration and src

Browse Source
This commit is contained in:
robin
2026-08-07 14:22:54 +09:00
commit 0b8f64d39e
260 changed files with 53361 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+10
View File
@@ -0,0 +1,10 @@
# ROS 2 build artifacts
build/
install/
log/
# IDE / System files
.vscode/
.idea/
*.swp
.DS_Store
src/FAST-LIVO2/.gitignore
+2
View File
@@ -0,0 +1,2 @@
Log/*
build/*
src/FAST-LIVO2/CMakeLists.txt
Executable
+216
View File
@@ -0,0 +1,216 @@
cmake_minimum_required(VERSION 3.10)
project(fast_livo)
set(CMAKE_BUILD_TYPE "Release")
message(STATUS "Build Type: ${CMAKE_BUILD_TYPE}")
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
# Set common compile options
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions")
# Specific settings for Debug build
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0 -g")
# Detect CPU architecture
message(STATUS "Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}")
# Specific settings for Release build
if(CMAKE_SYSTEM_PROCESSOR MATCHES "^(arm|aarch64|ARM|AARCH64)")
if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
# 64-bit ARM optimizations (e.g., RK3588 and Jetson Orin NX)
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -mcpu=native -mtune=native -ffast-math")
message(STATUS "Using 64-bit ARM optimizations: -O3 -mcpu=native -mtune=native -ffast-math")
else()
# 32-bit ARM optimizations with NEON support
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -mcpu=native -mtune=native -mfpu=neon -ffast-math")
message(STATUS "Using 32-bit ARM optimizations: -O3 -mcpu=native -mtune=native -mfpu=neon -ffast-math")
endif()
add_definitions(-DARM_ARCH)
else()
# x86-64 (Intel/AMD) optimizations
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native -mtune=native -funroll-loops") #-flto
message(STATUS "Using general x86 optimizations: -O3 -march=native -mtune=native -funroll-loops")
add_definitions(-DX86_ARCH)
endif()
# Define project root directory
add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
# Detect CPU core count for potential multithreading optimization
include(ProcessorCount)
ProcessorCount(N)
message(STATUS "Processor count: ${N}")
# Set the number of cores for multithreading
if(N GREATER 4)
math(EXPR PROC_NUM "4")
add_definitions(-DMP_EN -DMP_PROC_NUM=${PROC_NUM})
message(STATUS "Multithreading enabled. Cores: ${PROC_NUM}")
elseif(N GREATER 1)
math(EXPR PROC_NUM "${N}")
add_definitions(-DMP_EN -DMP_PROC_NUM=${PROC_NUM})
message(STATUS "Multithreading enabled. Cores: ${PROC_NUM}")
else()
add_definitions(-DMP_PROC_NUM=1)
message(STATUS "Single core detected. Multithreading disabled.")
endif()
# Check for OpenMP support
find_package(OpenMP QUIET)
if(OpenMP_CXX_FOUND)
message(STATUS "OpenMP found")
add_compile_options(${OpenMP_CXX_FLAGS})
else()
message(STATUS "OpenMP not found, proceeding without it")
endif()
# Check for mimalloc support
find_package(mimalloc QUIET)
if(mimalloc_FOUND)
message(STATUS "mimalloc found")
else()
message(STATUS "mimalloc not found, proceeding without it")
endif()
# Find ament and required dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(pcl_ros REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(livox_ros_driver2 REQUIRED)
find_package(vikit_common REQUIRED)
find_package(vikit_ros REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(image_transport REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Sophus REQUIRED)
# Support modern cmake Sophus target (ros-humble-sophus) which doesn't set Sophus_INCLUDE_DIRS
if(TARGET Sophus::Sophus AND NOT Sophus_INCLUDE_DIRS)
get_target_property(Sophus_INCLUDE_DIRS Sophus::Sophus INTERFACE_INCLUDE_DIRECTORIES)
endif()
find_package(Boost REQUIRED COMPONENTS thread)
find_package(fmt REQUIRED)
# Include directories for dependencies
include_directories(
${EIGEN3_INCLUDE_DIR}
${PCL_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${Sophus_INCLUDE_DIRS}
${vikit_common_INCLUDE_DIRS}
${vikit_ros_INCLUDE_DIRS}
include
)
set(dependencies
rclcpp
rclpy
geometry_msgs
nav_msgs
sensor_msgs
visualization_msgs
cv_bridge
vikit_common
vikit_ros
image_transport
pcl_ros
pcl_conversions
tf2_ros
livox_ros_driver2
)
set(COMMON_DEPENDENCIES OpenMP::OpenMP_CXX fmt::fmt)
# link_directories(${COMMON_DEPENDENCIES}
# ${vikit_common_LIBRARIES}/libvikit_common.so
# ${vikit_ros_LIBRARIES}/libvikit_ros.so
# )
# Add libraries
add_library(vio src/vio.cpp src/frame.cpp src/visual_point.cpp)
add_library(lio src/voxel_map.cpp)
add_library(pre src/preprocess.cpp)
add_library(imu_proc src/IMU_Processing.cpp)
add_library(laser_mapping src/LIVMapper.cpp)
add_library(utils src/utils.cpp)
ament_target_dependencies(vio ${dependencies} )
ament_target_dependencies(lio ${dependencies})
ament_target_dependencies(pre ${dependencies})
ament_target_dependencies(imu_proc ${dependencies})
ament_target_dependencies(laser_mapping ${dependencies})
# linking libraries or executables to public dependencies
target_link_libraries(laser_mapping
${CMAKE_SOURCE_DIR}/../../install/vikit_common/lib/libvikit_common.so
${CMAKE_SOURCE_DIR}/../../install/vikit_ros/lib/libvikit_ros.so
${COMMON_DEPENDENCIES}
)
target_link_libraries(vio ${COMMON_DEPENDENCIES})
target_link_libraries(lio utils ${COMMON_DEPENDENCIES})
target_link_libraries(pre ${COMMON_DEPENDENCIES})
target_link_libraries(imu_proc ${COMMON_DEPENDENCIES})
# Add the main executable
add_executable(fastlivo_mapping src/main.cpp)
ament_target_dependencies(fastlivo_mapping ${dependencies})
# Link libraries to the executable
target_link_libraries(fastlivo_mapping
laser_mapping
vio
lio
pre
imu_proc
${PCL_LIBRARIES}
${OpenCV_LIBRARIES}
${Sophus_LIBRARIES}
${Boost_LIBRARIES}
)
# Link mimalloc if found
if(mimalloc_FOUND)
target_link_libraries(fastlivo_mapping mimalloc)
endif()
# Install the executable
install(TARGETS
fastlivo_mapping
DESTINATION lib/${PROJECT_NAME}
)
install(
DIRECTORY config launch rviz_cfg urdf
DESTINATION share/${PROJECT_NAME}
)
# Export dependencies
ament_export_dependencies(
rclcpp
rclpy
geometry_msgs
nav_msgs
sensor_msgs
pcl_ros
pcl_conversions
tf2_ros
livox_ros_driver2
Eigen3
PCL
OpenCV
Sophus
)
ament_package()
src/FAST-LIVO2/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.
src/FAST-LIVO2/README.md
+195
View File
@@ -0,0 +1,195 @@
# FAST-LIVO2 ROS2 HUMBLE
## FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry
Thanks to hku mars lab chunran zheng for the open source excellent work
### 📢 News
- 🔓 **2025-01-23**: Code released!
- 🎉 **2024-10-01**: Accepted by **T-RO '24**!
- 🚀 **2024-07-02**: Conditionally accepted.
### 📬 Contact
For further inquiries or assistance, please contact [zhengcr@connect.hku.hk](mailto:zhengcr@connect.hku.hk).
## 1. Introduction
FAST-LIVO2 is an efficient and accurate LiDAR-inertial-visual fusion localization and mapping system, demonstrating significant potential for real-time 3D reconstruction and onboard robotic localization in severely degraded environments.
**Developer**: [Chunran Zheng 郑纯然](https://github.com/xuankuzcr)
<div align="center">
<img src="pics/Framework.png" width = 100% >
</div>
### 1.1 Related video
Our accompanying video is now available on [**Bilibili**](https://www.bilibili.com/video/BV1Ezxge7EEi) and [**YouTube**](https://youtu.be/6dF2DzgbtlY).
### 1.2 Related paper
[FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry](https://arxiv.org/pdf/2408.14035)
[FAST-LIVO2 on Resource-Constrained Platforms](https://arxiv.org/pdf/2501.13876)
[FAST-LIVO: Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry](https://arxiv.org/pdf/2203.00893)
[FAST-Calib: LiDAR-Camera Extrinsic Calibration in One Second](https://www.arxiv.org/pdf/2507.17210)
### 1.3 Our hard-synchronized equipment
We open-source our handheld device, including CAD files, synchronization scheme, STM32 source code, wiring instructions, and sensor ROS driver. Access these resources at this repository: [**LIV_handhold**](https://github.com/xuankuzcr/LIV_handhold).
### 1.4 Our associate dataset: FAST-LIVO2-Dataset
Our associate dataset [**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z) used for evaluation is also available online.
### 1.5 Our LiDAR-camera calibration method
The [**FAST-Calib**](https://github.com/hku-mars/FAST-Calib) toolkit is recommended. Its output extrinsic parameters can be directly filled into the YAML file.
### MARS-LVIG dataset
[**MARS-LVIG dataset**](https://mars.hku.hk/dataset.html)：A multi-sensor aerial robots SLAM dataset for LiDAR-visual-inertial-GNSS fusion
## 2. Prerequisited
### 2.1 Ubuntu and ROS
Ubuntu 22.04. [ROS Installation](http://wiki.ros.org/ROS/Installation).
### 2.2 PCL && Eigen && OpenCV
PCL>=1.8, Follow [PCL Installation](https://pointclouds.org/).
Eigen>=3.3.4, Follow [Eigen Installation](https://eigen.tuxfamily.org/index.php?title=Main_Page).
OpenCV>=4.2, Follow [Opencv Installation](http://opencv.org/).
### 2.3 Sophus
#### Binary installation
```bash
sudo apt install ros-$ROS_DISTRO-sophus
```
#### Building from source
Sophus Installation for the non-templated/double-only version.
```bash
git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff
mkdir build && cd build && cmake ..
make
sudo make install
```
if build fails due to `so2.cpp:32:26: error: lvalue required as left operand of assignment`, modify the code as follows:
**so2.cpp**
```diff
namespace Sophus
{
SO2::SO2()
{
- unit_complex_.real() = 1.;
- unit_complex_.imag() = 0.;
+ unit_complex_.real(1.);
+ unit_complex_.imag(0.);
}
```
### 2.4 Vikit
Vikit contains camera models, some math and interpolation functions that we need. Vikit is a catkin project, therefore, download it into your catkin workspace source folder.
For well-known reasons, ROS2 does not have a direct global parameter server and a simple method to obtain the corresponding parameters. For details, please refer to https://discourse.ros.org/t/ros2-global-parameter-server-status/10114/11. I use a special way to get camera parameters in Vikit. While the method I've provided so far is quite simple and not perfect, it meets my needs. More contributions to improve `rpg_vikit` are hoped.
```bash
# Different from the one used in fast-livo1
cd fast_ws/src
git clone https://github.com/Robotic-Developer-Road/rpg_vikit.git
```
Thanks to the following repositories for the code reference:
- [uzh-rpg/rpg_vikit](https://github.com/uzh-rpg/rpg_vikit)
- [xuankuzcr/rpg_vikit](https://github.com/xuankuzcr/rpg_vikit)
- [uavfly/vikit](https://github.com/uavfly/vikit)
### 2.5 **livox_ros_driver2**
Follow [livox_ros_driver2 Installation](https://github.com/Livox-SDK/livox_ros_driver2).
why not use `livox_ros_driver`? Because it is not compatible with ROS2 directly. actually i am not think there s any difference between [livox ros driver](https://github.com/Livox-SDK/livox_ros_driver.git) and [livox ros driver2](https://github.com/Livox-SDK/livox_ros_driver2.git) 's `CustomMsg`, the latter 's ros2 version is sufficient.
## 3. Build
Clone the repository and colcon build:
```
cd ~/fast_ws/src
git clone https://github.com/Robotic-Developer-Road/FAST-LIVO2.git
cd ../
colcon build --symlink-install --continue-on-error
source ~/fast_ws/install/setup.bash
```
## 4. Run our examples
Download our collected rosbag files via OneDrive ([**FAST-LIVO2-Dataset**](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/zhengcr_connect_hku_hk/ErdFNQtjMxZOorYKDTtK4ugBkogXfq1OfDm90GECouuIQA?e=KngY9Z)).
### convert rosbag
convert ROS1 rosbag to ROS2 rosbag
```bash
pip install rosbags
rosbags-convert --src Retail_Street.bag --dst Retail_Street
```
- [gitlab rosbags](https://gitlab.com/ternaris/rosbags)
- [pypi rosbags](https://pypi.org/project/rosbags/)
### change the msg type on rosbag
Such as dataset `Retail_Street.db3`, because we use `livox_ros2_driver2`'s `CustomMsg`, we need to change the msg type in the rosbag file.
1. use `rosbags-convert` to convert rosbag from ROS1 to ROS2.
2. change the msg type of msg type in **metadata.yaml** as follows:
**metadata.yaml**
```diff
rosbag2_bagfile_information:
compression_format: ''
compression_mode: ''
custom_data: {}
duration:
nanoseconds: 135470252209
files:
- duration:
nanoseconds: 135470252209
message_count: 30157
path: Retail_Street.db3
..............
topic_metadata:
name: /livox/lidar
offered_qos_profiles: ''
serialization_format: cdr
- type: livox_ros_driver/msg/CustomMsg
+ type: livox_ros_driver2/msg/CustomMsg
type_description_hash: RIHS01_94041b4794f52c1d81def2989107fc898a62dacb7a39d5dbe80d4b55e538bf6d
...............
.....
```
### Run the demo
Do not forget to `source` your ROS2 workspace before running the following command.
```bash
ros2 launch fast_livo mapping_aviz.launch.py use_rviz:=True
ros2 bag play -p Retail_Street # space bar controls play/pause
```
## 5. License
The source code of this package is released under the [**GPLv2**](http://www.gnu.org/licenses/) license. For commercial use, please contact me at <zhengcr@connect.hku.hk> and Prof. Fu Zhang at <fuzhang@hku.hk> to discuss an alternative license.
src/FAST-LIVO2/Supplementary/LIVO2_supplementary.pdf
BIN
View File
Binary file not shown.
src/FAST-LIVO2/config/HILTI22.yaml
+100
View File
@@ -0,0 +1,100 @@
common:
img_topic: "/alphasense/cam0/image_raw"
lid_topic: "/hesai/pandar"
imu_topic: "/alphasense/imu"
img_en: 1
lidar_en: 1
ros_driver_bug_fix: false
extrin_calib:
# Hilti-2022
extrinsic_T: [-0.001, -0.00855, 0.055]
extrinsic_R: [0, -1, 0, -1, 0, 0, 0, 0, -1]
# Hilti-2023
# extrinsic_T: [-0.006730146149038548, -0.006897049862999071, 0.049898628062256645]
# extrinsic_R: [0.006609639848469365, -0.9999773650294649, 0.0012578115132016717,
# -0.9999762249571927, -0.006612093869054189, -0.0019569708811106104,
# 0.001965243352927244, -0.0012448467359610184, -0.9999972940839232]
# Hilti
Rcl: [ -0.999926, -0.00670802, 0.0101073,
-0.0100912, -0.00242564, -0.999946,
0.00673218,-0.999975, 0.00235777 ]
Pcl: [ -0.0549762, 0.0675401, -0.0520599 ]
time_offset:
imu_time_offset: 0.0
img_time_offset: 0.0
exposure_time_init: 0.0
preprocess:
hilti_en: true
point_filter_num: 1
filter_size_surf: 0.1 # 0.2
lidar_type: 5 # HesaiXT32
scan_line: 32
blind: 0.6 # 0.1 0.3
vio:
max_iterations: 5
outlier_threshold: 500
img_point_cov: 1000
patch_size: 8
patch_pyrimid_level: 4
normal_en: true
raycast_en: false
inverse_composition_en: false
exposure_estimate_en: true
inv_expo_cov: 0.1
imu:
imu_en: true
imu_int_frame: 30
acc_cov: 0.5 # 0.1
gyr_cov: 0.01
b_acc_cov: 0.0001 # 0.1
b_gyr_cov: 0.0001 # 0.1
lio:
max_iterations: 5
dept_err: 0.02
beam_err: 0.05
min_eigen_value: 0.0001 # 0.0025
voxel_size: 0.4
max_layer: 2
max_points_num: 100
layer_init_num: [5, 5, 5, 5, 5]
local_map:
map_sliding_en: false
half_map_size: 100
sliding_thresh: 8
uav:
imu_rate_odom: false
gravity_align_en: false
publish:
dense_map_en: true
pub_effect_point_en: false
pub_plane_en: false
pub_scan_num: 1
blind_rgb_points: 0.0
evo:
seq_name: "exp09_cupola"
pose_output_en: true
pcd_save:
pcd_save_en: false
type: 0 # 0: World Frame, 1: Body Frame;
colmap_output_en: false # need to set interval = -1
filter_size_pcd: 0.15
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.
image_save:
img_save_en: false
interval: 1
src/FAST-LIVO2/config/MARS_LVIG.yaml
+118
View File
@@ -0,0 +1,118 @@
/**:
ros__parameters:
common:
img_topic: "/left_camera/image"
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
img_en: 1
lidar_en: 1
ros_driver_bug_fix: false
extrin_calib:
extrinsic_T: [0.04165, 0.02326, -0.0284]
extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
# MARS_LVIG HKisland HKairport
# Rcl: [0.00438814,-0.999807,-0.0191582,
# -0.00978695,0.0191145,-0.999769,
# 0.999942,0.00457463,-0.00970118]
# Pcl: [0.016069, 0.0871753, -0.0718021]
# MARS_LVIG AMtown AMvalley
Rcl: [ -0.0022464, -0.9997299, -0.0231319,
-0.0084211, 0.0231501, -0.9996966,
0.9999620, -0.0020509, -0.0084708]
Pcl: [-0.0025563, 0.0567484, -0.0512149]
time_offset:
imu_time_offset: 0.0
img_time_offset: -0.1
exposure_time_init: 0.0132
# ╔═══════════════════════════════════════════════════════════════════════════════════════╗
# ║ Configuration Settings ║
# ╠═══════════════════════════════════════════════════════════════════════════════════════╣
# ║ Series │ ID │ img_time_offset │ exposure_time_init │ -s (start hover) ║
# ╠═══════════════════════════════════════════════════════════════════════════════════════╣
# ║ HKairport │ HKairport01 │ 0.1 │ 0.0 │ 75 ║
# ║ │ HKairport02 │ -0.1 │ 0.0 │ 60 ║
# ║ │ HKairport03 │ -0.1 │ 0.0 │ 62 ║
# ╠═══════════════════════════════════════════════════════════════════════════════════════╣
# ║ HKisland │ HKisland01 │ 0.0 │ 0.0 │ 118 ║
# ║ │ HKisland02 │ 0.1 │ 0.0 │ 80 ║
# ║ │ HKisland03 │ -0.1 │ 0.0 │ 72 ║
# ╠═══════════════════════════════════════════════════════════════════════════════════════╣
# ║ AMtown │ AMtown01 │ -0.1 │ 0.0285 │ 75 ║
# ║ │ AMtown02 │ -0.1 │ 0.0285 │ 50 ║
# ║ │ AMtown03 │ -0.1 │ 0.0285 │ 106 ║
# ╠═══════════════════════════════════════════════════════════════════════════════════════╣
# ║ AMvalley │ AMvalley01 │ -0.1 │ 0.0132 │ 70 ║
# ║ │ AMvalley02 │ -0.1 │ 0.0132 │ 65 ║
# ║ │ AMvalley03 │ -0.1 │ 0.0132 │ 68 ║
# ╚═══════════════════════════════════════════════════════════════════════════════════════╝
preprocess:
point_filter_num: 1
filter_size_surf: 0.1
lidar_type: 1 # Livox Avia LiDAR
scan_line: 6
blind: 0.8
vio:
max_iterations: 5
outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite
img_point_cov: 1000 # 100 1000
patch_size: 8
patch_pyrimid_level: 4
normal_en: true
raycast_en: false
inverse_composition_en: false
exposure_estimate_en: true
inv_expo_cov: 0.1
imu:
imu_en: true
imu_int_frame: 30
acc_cov: 2.0 # 0.5
gyr_cov: 0.1 # 0.3
b_acc_cov: 0.0001 # 0.1
b_gyr_cov: 0.0001 # 0.1
lio:
max_iterations: 5
dept_err: 0.02
beam_err: 0.05
min_eigen_value: 0.005
voxel_size: 2.0 # 1.0
max_layer: 2
max_points_num: 50
layer_init_num: [5, 5, 5, 5, 5]
local_map:
map_sliding_en: false
half_map_size: 100
sliding_thresh: 8.0
uav:
imu_rate_odom: false
gravity_align_en: false
publish:
dense_map_en: true
pub_effect_point_en: false
pub_plane_en: false
pub_scan_num: 1
blind_rgb_points: 0.0
evo:
seq_name: "HKisland03"
pose_output_en: false
pcd_save:
pcd_save_en: false
type: 0 # 0: World Frame, 1: Body Frame;
colmap_output_en: false # need to set interval = -1
filter_size_pcd: 0.15
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.
image_save:
img_save_en: false
interval: 1
src/FAST-LIVO2/config/NTU_VIRAL.yaml
+94
View File
@@ -0,0 +1,94 @@
/**:
ros__parameters:
common:
img_topic: "/left/image_raw"
lid_topic: "/os1_cloud_node1/points"
imu_topic: "/imu/imu"
img_en: 1
lidar_en: 1
ros_driver_bug_fix: false
extrin_calib:
extrinsic_T: [-0.050, 0.000, 0.055]
extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
# NTU_VIRAL
Rcl: [0.0218308, 0.99976, -0.00201407,
-0.0131205, 0.00230088, 0.999911,
0.999676, -0.0218025, 0.0131676]
Pcl: [0.122993, 0.0398643, -0.0577101]
time_offset:
lidar_time_offset: -0.1
imu_time_offset: 0.0
img_time_offset: 0.0
exposure_time_init: 0.0
preprocess:
point_filter_num: 3
filter_size_surf: 0.1
lidar_type: 3 # Ouster
scan_line: 16
blind: 1.0
vio:
max_iterations: 5
outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite
img_point_cov: 100 # 100 1000
patch_size: 8
patch_pyrimid_level: 3
normal_en: true
raycast_en: false
inverse_composition_en: false
exposure_estimate_en: true
inv_expo_cov: 0.1
imu:
imu_en: true
imu_int_frame: 30
acc_cov: 0.5 # 0.2
gyr_cov: 0.3 # 0.5
b_acc_cov: 0.0001 # 0.1
b_gyr_cov: 0.0001 # 0.1
lio:
max_iterations: 5
dept_err: 0.02
beam_err: 0.01
min_eigen_value: 0.0025 # 0.0025
voxel_size: 0.5
max_layer: 2
max_points_num: 50
layer_init_num: [5, 5, 5, 5, 5]
local_map:
map_sliding_en: false
half_map_size: 100
sliding_thresh: 8.0
uav:
imu_rate_odom: false
gravity_align_en: false
publish:
dense_map_en: true
pub_effect_point_en: false
pub_plane_en: false
pub_scan_num: 1
blind_rgb_points: 0.0
evo:
seq_name: "eee_01"
pose_output_en: true
pcd_save:
pcd_save_en: false
type: 0 # 0: World Frame, 1: Body Frame;
colmap_output_en: false # need to set interval = -1
filter_size_pcd: 0.15
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.
image_save:
img_save_en: false
interval: 1
src/FAST-LIVO2/config/avia.yaml
Executable
+94
View File
@@ -0,0 +1,94 @@
/**:
ros__parameters:
common:
img_topic: "/left_camera/image"
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
img_en: 1
lidar_en: 1
ros_driver_bug_fix: false
extrin_calib:
extrinsic_T: [0.04165, 0.02326, -0.0284]
extrinsic_R: [1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0]
Rcl: [0.00610193,-0.999863,-0.0154172,
-0.00615449,0.0153796,-0.999863,
0.999962,0.00619598,-0.0060598]
Pcl: [0.0194384, 0.104689,-0.0251952]
time_offset:
imu_time_offset: 0.0
img_time_offset: 0.1
exposure_time_init: 0.0
preprocess:
point_filter_num: 1
filter_size_surf: 0.1
lidar_type: 1 # Livox Avia LiDAR
scan_line: 6
blind: 0.8
vio:
max_iterations: 5
outlier_threshold: 1000 # 78 100 156 #100 200 500 700 infinite
img_point_cov: 100 # 100 1000
patch_size: 8
patch_pyrimid_level: 4
normal_en: true
raycast_en: false
inverse_composition_en: false
exposure_estimate_en: true
inv_expo_cov: 0.1
imu:
imu_en: true
imu_int_frame: 30
acc_cov: 0.5 # 0.2
gyr_cov: 0.3 # 0.5
b_acc_cov: 0.0001 # 0.1
b_gyr_cov: 0.0001 # 0.1
lio:
max_iterations: 5
dept_err: 0.02
beam_err: 0.05
min_eigen_value: 0.0025 # 0.005
voxel_size: 0.5
max_layer: 2
max_points_num: 50
layer_init_num: [5, 5, 5, 5, 5]
local_map:
map_sliding_en: false
half_map_size: 100
sliding_thresh: 8.0
uav:
imu_rate_odom: false
gravity_align_en: false
publish:
dense_map_en: true
pub_effect_point_en: false
pub_plane_en: false
pub_scan_num: 1
blind_rgb_points: 0.0
evo:
seq_name: "CBD_Building_01"
pose_output_en: false
pcd_save:
pcd_save_en: false
type: 0 # 0: World Frame, 1: Body Frame;
colmap_output_en: false # need to set interval = -1
filter_size_pcd: 0.15
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.
image_save:
img_save_en: false
interval: 1
src/FAST-LIVO2/config/camera_MARS_LVIG.yaml
+28
View File
@@ -0,0 +1,28 @@
/**:
ros__parameters:
cam_model: Pinhole
# HKisland HKairport
cam_width: 2448
cam_height: 2048
scale: 0.25
cam_fx: 1444.431662789634
cam_fy: 1444.343536688358
cam_cx: 1177.801079401826
cam_cy: 1043.601026568268
cam_d0: -0.05729528706141188
cam_d1: 0.1210407244166642
cam_d2: 0.001274128378760289
cam_d3: 0.0004389741530109464
# AMtown AMvalley
# cam_width: 2448
# cam_height: 2048
# scale: 0.25
# cam_fx: 1453.88
# cam_fy: 1452.85
# cam_cx: 1182.53
# cam_cy: 1045.82
# cam_d0: -0.052
# cam_d1: 0.1168
# cam_d2: 0.0015
# cam_d3: 0.00013
src/FAST-LIVO2/config/camera_NTU_VIRAL.yaml
+14
View File
@@ -0,0 +1,14 @@
/**:
ros__parameters:
cam_model: Pinhole
cam_width: 752
cam_height: 480
scale: 1.0
cam_fx: 4.250258563372763e+02
cam_fy: 4.267976260903337e+02
cam_cx: 3.860151866550880e+02
cam_cy: 2.419130336743440e+02
cam_d0: -0.288105327549552
cam_d1: 0.074578284234601
cam_d2: 7.784489598138802e-04
cam_d3: -2.277853975035461e-04
src/FAST-LIVO2/config/camera_cam1.yaml
+43
View File
@@ -0,0 +1,43 @@
/**:
ros__parameters:
cam_model: Pinhole
# cam1: 1440x1080 풀 해상도 입력, FAST-LIVO2 내부에서 scale=0.5 적용 → 720x540 처리
cam_width: 1440
cam_height: 1080
scale: 1.0
# --- 6mm lens (cam1, 2026-06-16 재캘리브레이션, 현재 활성) ---
cam_fx: 1789.271411
cam_fy: 1794.848936
cam_cx: 744.170668
cam_cy: 545.346079
cam_d0: -0.093183
cam_d1: 0.189899
cam_d2: 0.002084
cam_d3: 0.004098
# --- 6mm lens (cam1, 2026-06-09 재캘리브레이션, 폐기) ---
# cam_fx: 2317.833528
# cam_fy: 2322.780813
# cam_cx: 657.065936
# cam_cy: 587.150296
# cam_d0: -0.111649
# cam_d1: 0.359324
# cam_d2: -0.001199
# cam_d3: -0.005997
# --- 8mm lens v4 (cam1, 2026-05-26) ---
# cam_fx: 2347.038631
# cam_fy: 2353.175990
# cam_cx: 791.950958
# cam_cy: 566.608413
# cam_d0: -0.128253
# cam_d1: 0.437160
# cam_d2: 0.002047
# cam_d3: 0.007568
# --- 6mm lens (1440x1080) ---
# cam_fx: 1731.285808
# cam_fy: 1739.429604
# cam_cx: 713.349417
# cam_cy: 547.551014
# cam_d0: -0.091047
# cam_d1: 0.101541
# cam_d2: -0.001141
# cam_d3: -0.001854
src/FAST-LIVO2/config/camera_cam3.yaml
+24
View File
@@ -0,0 +1,24 @@
/**:
ros__parameters:
cam_model: Pinhole
cam_width: 1440
cam_height: 1080
scale: 1.0
# --- 4mm lens (cam3, 2026-06-18 재캘리브레이션, 1440x1080) ---
cam_fx: 1203.078148
cam_fy: 1206.096396
cam_cx: 699.186863
cam_cy: 565.715472
cam_d0: -0.102740
cam_d1: 0.093985
cam_d2: -0.000759
cam_d3: -0.001804
# --- 4mm lens (cam3, 2026-06-08 구버전, 폐기) ---
# cam_fx: 1189.152341
# cam_fy: 1192.753914
# cam_cx: 736.714571
# cam_cy: 558.680089
# cam_d0: -0.101030
# cam_d1: 0.082065
# cam_d2: -0.000642
# cam_d3: 0.002411
src/FAST-LIVO2/config/camera_fisheye_HILTI22.yaml
+12
View File
@@ -0,0 +1,12 @@
cam_model: EquidistantCamera
cam_width: 720
cam_height: 540
scale: 1.0
cam_fx: 351.31400364193297
cam_fy: 351.4911744656785
cam_cx: 367.8522793375995
cam_cy: 253.8402144980996
k1: -0.03696737352869157
k2: -0.008917880497032812
k3: 0.008912969593422046
k4: -0.0037685977496087313
src/FAST-LIVO2/config/camera_mid360s.yaml
+37
View File
@@ -0,0 +1,37 @@
/**:
ros__parameters:
cam_model: Pinhole
# 카메라: MVS BinningHorizontal=2, BinningVertical=2 → 720x540 출력
# scale=1.0 (하드웨어 binning이 이미 해상도 축소)
cam_width: 720
cam_height: 540
scale: 1.0
# --- 8mm lens v4, 720x540 binning 2x2 (1440x1080 × 0.5) ---
cam_fx: 1173.519316
cam_fy: 1176.587995
cam_cx: 395.975479
cam_cy: 283.304207
cam_d0: -0.128253
cam_d1: 0.437160
cam_d2: 0.002047
cam_d3: 0.007568
# --- 8mm lens v4, 1440x1080 + software scale=0.5 (binning 미지원 시 대안) ---
# cam_width: 1440
# cam_height: 1080
# scale: 0.5
# cam_fx: 2347.038631
# cam_fy: 2353.175990
# cam_cx: 791.950958
# cam_cy: 566.608413
# --- 6mm lens, 1440x1080 + scale=0.5 ---
# cam_width: 1440
# cam_height: 1080
# scale: 0.5
# cam_fx: 1731.285808
# cam_fy: 1739.429604
# cam_cx: 713.349417
# cam_cy: 547.551014
# cam_d0: -0.091047
# cam_d1: 0.101541
# cam_d2: -0.001141
# cam_d3: -0.001854
src/FAST-LIVO2/config/camera_pinhole.yaml
+42
View File
@@ -0,0 +1,42 @@
/**:
ros__parameters:
cam_model: Pinhole
cam_width: 1440
cam_height: 1080
scale: 0.5
# --- 6mm lens (cam1/cam2) ---
# cam_fx: 1731.285808
# cam_fy: 1739.429604
# cam_cx: 713.349417
# cam_cy: 547.551014
# cam_d0: -0.091047
# cam_d1: 0.101541
# cam_d2: -0.001141
# cam_d3: -0.001854
# --- 6mm lens (cam1, 2026-06-09 재캘리브레이션) → camera_cam1.yaml 사용 권장 ---
# cam_fx: 2317.833528
# cam_fy: 2322.780813
# cam_cx: 657.065936
# cam_cy: 587.150296
# cam_d0: -0.111649
# cam_d1: 0.359324
# cam_d2: -0.001199
# cam_d3: -0.005997
# --- 8mm lens v4 (cam1/cam2, 2026-05-26) ---
cam_fx: 2347.038631
cam_fy: 2353.175990
cam_cx: 791.950958
cam_cy: 566.608413
cam_d0: -0.128253
cam_d1: 0.437160
cam_d2: 0.002047
cam_d3: 0.007568
# --- 4mm lens (cam3, 2026-06-08 재캘리브레이션) → camera_cam3.yaml 사용 권장 ---
# cam_fx: 1189.152341
# cam_fy: 1192.753914
# cam_cx: 736.714571
# cam_cy: 558.680089
# cam_d0: -0.101030
# cam_d1: 0.082065
# cam_d2: -0.000642
# cam_d3: 0.002411
src/FAST-LIVO2/config/extrin_cam1.yaml
+32
View File
@@ -0,0 +1,32 @@
/**:
ros__parameters:
extrin_calib:
# Camera 1 (6mm lens) extrinsics
# --- 2026-05-15 구버전 ---
# Rcl: [ 0.02069918, -0.99972195, 0.01129488,
# 0.27370973, -0.00519927, -0.96179829,
# 0.96158958, 0.02299996, 0.27352600]
# Pcl: [-0.02917199, -0.07698411, -0.11157462]
# --- 2026-06-09 구버전 (폐기) ---
# az=0.9° el=15.9°
# Rcl: [0.01255090, -0.99982700, 0.01374180,
# 0.27400100, -0.00977792, -0.96168000,
# 0.96164800, 0.01583530, 0.27383100]
# Pcl: [0.04144749, -0.10107580, -0.01683002]
# --- 2026-06-09 재캘리브레이션 v2 (폐기) ---
# T_lidar_camera: [tx=0.10476, ty=-0.00247, tz=-0.08220]
# az=1.5° el=15.9°
# Rcl: [0.0219646, -0.999651, 0.0146488,
# 0.274753, -0.00805253, -0.961481,
# 0.961264, 0.0251433, 0.274481]
# Pcl: [-0.00356413, -0.10783703, -0.07807754]
# --- 2026-06-16 재캘리브레이션 (현재 활성, 내부 캘 2026-06-16 기반) ---
# T_lidar_camera: [tx=0.07719, ty=-0.00783, tz=-0.05823, qx=-0.40351, qy=0.40878, qz=-0.58252, qw=0.57510]
# az=-0.7° el=19.9°
Rcl: [-0.01286891, -0.99991719, -0.00007353,
0.34011452, -0.00430811, -0.94037415,
0.94029596, -0.01212660, 0.34014180]
Pcl: [-0.00683908, -0.08104266, -0.05287530]
src/FAST-LIVO2/config/extrin_cam2.yaml
+11
View File
@@ -0,0 +1,11 @@
/**:
ros__parameters:
extrin_calib:
# Camera 2 (hik_camera_ros2_driver) extrinsics
# direct_visual_lidar_calibration result (2026-05-27), 8mm lens v6
# T_lidar_camera: [tx=0.05454, ty=0.00438, tz=-0.09330, qx=-0.41061, qy=0.43393, qz=-0.57567, qw=0.55830]
# az=-2.5° el=16.6°
Rcl: [-0.03939004, -0.99915456, -0.01177214,
0.28645369, -0.00000461, -0.95809409,
0.95728403, -0.04111154, 0.28621170]
Pcl: [0.00542794, -0.10500922, -0.02532945]
src/FAST-LIVO2/config/extrin_cam3.yaml
+11
View File
@@ -0,0 +1,11 @@
/**:
ros__parameters:
extrin_calib:
# Camera 3 (4mm lens) extrinsics
# --- 2026-06-18 캘리브레이션 (현재 활성, 내부 캘 2026-06-18 기반) ---
# T_lidar_camera: [tx=0.07680, ty=-0.00628, tz=-0.08776, qx=-0.40882, qy=0.39927, qz=-0.57777, qw=0.58277]
# az=0.9° el=-20.3°
Rcl: [ 0.01351752, -0.99988379, 0.00704826,
0.34695354, -0.00192070, -0.93788035,
0.93778490, 0.01512324, 0.34688726]
Pcl: [-0.00669994, -0.10896580, -0.04148943]
src/FAST-LIVO2/config/mid360s.yaml
+246
View File
@@ -0,0 +1,246 @@
/**:
ros__parameters:
common:
img_topic: "/camera/image"
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
img_en: 1
lidar_en: 1
ros_driver_bug_fix: false
extrin_calib:
# IMU-to-LiDAR extrinsics (MID-360 internal IMU)
extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
extrinsic_R: [1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0]
# Camera-to-LiDAR extrinsics (physical estimate 2026-05-14)
# Source: T_lidar_camera roll=-73deg pitch=0deg yaw=-90deg
# tx=-0.0198m ty=-0.0695m tz=0.0172m (from calibration)
# Rcl = R_camera_lidar = R_lidar_camera.T
# Pcl = -Rcl @ t_lidar_camera
# Rcl: [ 0.00000000, -1.00000000, 0.00000000, # this value is base
# 0.29237170, 0.00000000, -0.95630476,
# 0.95630476, 0.00000000, 0.29237170]
# Pcl: [-0.06950000, 0.02223740, 0.01390604]
# Rcl: [-0.25354040, -0.96693219, 0.02755752, this value is fail
# -0.39707519, 0.07805545, -0.91446084,
# 0.88207061, -0.24279517, -0.40373500]
# Pcl: [0.76509620, 1.81348647, 0.02357659]
# Rcl: [0.00000000, -1.00000000, 0.00000000, # try 2
# 0.29237170, 0.00000000, -0.95630476,
# 0.95630476, 0.00000000, 0.29237170]
# Pcl: [-0.11960285, 0.17087142, 0.08245752]
# Rcl: [ 0.06956, -0.99721, -0.02705, # try 3
# 0.23900, 0.04298, -0.97007,
# 0.96852, 0.06101, 0.24133]
# Pcl: [-0.06746, 0.02440, 0.01927]
# Rcl: [0.06956000, -0.99721000, -0.02705000, # try 4
# 0.23900000, 0.04298000, -0.97007000,
# 0.96852000, 0.06101000, 0.24133000]
# Pcl: [0.06043056, 0.01212226, 0.13300489]
# Rcl: [0.06956000, -0.99721000, -0.02705000, # try 5
# 0.23900000, 0.04298000, -0.97007000,
# 0.96852000, 0.06101000, 0.24133000]
# Pcl: [0.06029619, 0.01647827, 0.07757671]
# direct_visual_lidar_calibration result 1st run (2026-05-15)
# T_lidar_camera: [0.14293, -0.02917, -0.03467, qx=-0.4344, qy=0.4181, qz=-0.5614, qw=0.5668]
# Rcl: [ 0.01995325, -0.99970731, 0.01368083, # try 6
# 0.27319897, -0.00771101, -0.96192664,
# 0.96175059, 0.02293115, 0.27296514]
# Pcl: [-0.03154008, -0.07261870, -0.12733124]
# direct_visual_lidar_calibration result 2nd run (2026-05-15)
# T_lidar_camera: [0.12896, -0.02700, -0.04320, qx=-0.4337, qy=0.4185, qz=-0.5608, qw=0.5677]
# Rcl: [ 0.02069918, -0.99972195, 0.01129488,
# 0.27370973, -0.00519927, -0.96179829, # best
# 0.96158958, 0.02299996, 0.27352600]
# Pcl: [-0.02917199, -0.07698411, -0.11157462]
# Rcl: [0.00705089, -0.99997358, -0.00176582,
# 0.27363987, 0.00362791, -0.96182538,
# 0.96180638, 0.00629853, 0.27365822]
# Pcl: [-0.00462873, -0.01235428, -0.59325839]
# Rcl: [0.00520558, -0.99997233, 0.00531431,
# 0.28104599, -0.00363718, -0.95968741,
# 0.95968018, 0.00648929, 0.28101928]
# Pcl: [0.00145690, -0.13454580, -0.56836240]
# --- Camera 1 (6mm lens) extrinsics ---
# direct_visual_lidar_calibration result (2026-05-15)
# Rcl: [0.02069918, -0.99972195, 0.01129488,
# 0.27370973, -0.00519927, -0.96179829,
# 0.96158958, 0.02299996, 0.27352600]
# Pcl: [-0.02917199, -0.07698411, -0.11157462]
# direct_visual_lidar_calibration result (2026-06-09), 6mm 내부캘 재적용, az=0.9° el=15.9°
# Rcl: [0.01255090, -0.99982700, 0.01374180,
# 0.27400100, -0.00977792, -0.96168000,
# 0.96164800, 0.01583530, 0.27383100]
# Pcl: [0.04144749, -0.10107580, -0.01683002]
# direct_visual_lidar_calibration result (2026-06-16), 6mm 내부캘 재적용, az=-0.7° el=19.9° ← 현재 extrin_cam1.yaml
# Rcl: [-0.01286891, -0.99991719, -0.00007353,
# 0.34011452, -0.00430811, -0.94037415,
# 0.94029596, -0.01212660, 0.34014180]
# Pcl: [-0.00683908, -0.08104266, -0.05287530]
# --- Camera 2 (hik_camera_ros2_driver, camera_info_cam2.yaml) extrinsics ---
# direct_visual_lidar_calibration result (2026-05-22), 6mm lens
# T_lidar_camera: [tx=0.05225, ty=-0.00876, tz=-0.07362, qx=-0.42082, qy=0.42731, qz=-0.56696, qw=0.56469]
# Rcl: [-0.00806821, -0.99995281, -0.00541114,
# 0.28067349, 0.00292921, -0.95979884,
# 0.95976940, -0.00926262, 0.28063661]
# Pcl: [-0.00873246, -0.08530013, -0.02956579]
# --- 8mm lens v2 extrinsics ---
# direct_visual_lidar_calibration result (2026-05-24), intrinsic v2 기반
# T_lidar_camera: [tx=0.01145, ty=-0.00062, tz=-0.05991, qx=-0.41931, qy=0.42038, qz=-0.56709, qw=0.57085]
# Rcl: [0.00337986, -0.99998470, -0.00437918,
# 0.29491832, 0.00518120, -0.95550842,
# 0.95551649, 0.00193799, 0.29493132]
# Pcl: [-0.00091689, -0.06061769, 0.00672712]
# --- 8mm lens v4 extrinsics ---
# direct_visual_lidar_calibration result (2026-05-26), intrinsic v4 + 파노라마 방위각 반전 크롭 적용
# T_lidar_camera: [tx=0.03270, ty=-0.09704, tz=-0.10467, qx=-0.42407, qy=0.41384, qz=-0.57397, qw=0.56520]
# Rcl: [-0.00142083, 0.29782367, 0.95461984, # WRONG — az=-89°, 잘못된 크롭에서 얻은 결과
# -0.99981856, -0.01855673, 0.00430125,
# 0.01899564, -0.95444052, 0.29779599]
# Pcl: [0.03270414, -0.09703870, -0.10467137]
# --- 8mm lens v2 extrinsics ---
# direct_visual_lidar_calibration result (2026-05-24), intrinsic v2 기반, az≈0° 방향 정상
# T_lidar_camera: [tx=0.01145, ty=-0.00062, tz=-0.05991, qx=-0.41931, qy=0.42038, qz=-0.56709, qw=0.57085]
# Rcl: [0.00337986, -0.99998470, -0.00437918,
# 0.29491832, 0.00518120, -0.95550842,
# 0.95551649, 0.00193799, 0.29493132]
# Pcl: [-0.00091689, -0.06061769, 0.00672712]
# --- 8mm lens v5 extrinsics ---
# direct_visual_lidar_calibration result (2026-05-27), intrinsic v4 + 반사판 + LiDAR 마스킹, 4 bags
# T_lidar_camera: [tx=0.00312, ty=0.00582, tz=-0.07062, qx=-0.41467, qy=0.43797, qz=-0.57307, qw=0.55481]
# az=-2.5° el=15.8°
# Rcl: [-0.04046417, -0.99912356, -0.01071296,
# 0.27266698, -0.00072685, -0.96210820,
# 0.96125718, -0.04185198, 0.27245741]
# Pcl: [0.00518588, -0.06878734, 0.01648196]
# --- 8mm lens v6 extrinsics ---
# direct_visual_lidar_calibration result (2026-05-27), intrinsic v4 + 반사판 + LiDAR 마스킹, 5 bags
# T_lidar_camera: [tx=0.05454, ty=0.00438, tz=-0.09330, qx=-0.41061, qy=0.43393, qz=-0.57567, qw=0.55830]
# az=-2.5° el=16.6°
# Rcl: [-0.03939004, -0.99915456, -0.01177214,
# 0.28645369, -0.00000461, -0.95809409,
# 0.95728403, -0.04111154, 0.28621170]
# Pcl: [0.00542794, -0.10500922, -0.02532945]
# --- 4mm lens cam3 extrinsics v1 (intrinsic 구값 사용, 폐기) ---
# T_lidar_camera: [tx=0.07440, ty=-0.00376, tz=-0.07364, qx=-0.43050, qy=0.41776, qz=-0.55952, qw=0.57191]
# az=1.5° el=16.3°
# Rcl: [0.02482051, -0.99968430, 0.00390447,
# 0.28030319, 0.00321036, -0.95990615,
# 0.95959057, 0.02491980, 0.28029438]
# Pcl: [-0.00532104, -0.09153097, -0.05065450]
# --- 4mm lens cam3 extrinsics v2 (현재 활성, intrinsic 재캘 적용) ---
# direct_visual_lidar_calibration result (2026-06-08), intrinsic fx=1189.152341
# T_lidar_camera: [tx=0.10182, ty=-0.00949, tz=-0.06163]
# az=0.0° el=16.0° (물리적으로 타당)
Rcl: [0.00141494, -0.99999701, -0.00199385,
0.27599729, 0.00230693, -0.96115564,
0.96115737, 0.00080968, 0.27599973]
Pcl: [-0.00975341, -0.08731812, -0.08085047]
time_offset:
imu_time_offset: 0.0
# use_trigger_timestamp: true → 카메라 헤더 stamp = LiDAR 트리거 시각 (노출 시작)
# img_time_offset = exposure_time / 2 = 노출 중간값으로 보정
# cam1: exposure_time=5000µs → 5ms/2 = 0.0025s
# bag 검증: JUN09_1 기준 camera stamp == lidar stamp (0.000ms 오차, 200/200 exact match)
# ─────────────────────────────────────────────────────
# 이전값 (구버전, 사용 안 함):
# img_time_offset: 0.18 ← free-run 모드, 카메라 클럭 ~180ms 뒤처진 상황
# img_time_offset: 0.054 ← 불명확한 중간값
# img_time_offset: 0.018 ← 잘못된 계산값
# img_time_offset: 0.101 ← free-run 모드 수신 지연 보정값
img_time_offset: 0.18 # = exposure_time(5ms) / 2, trigger 동기화 모드
exposure_time_init: 0.0
preprocess:
point_filter_num: 1
filter_size_surf: 0.1 # tuned: 0.03 (more points, higher CPU)
lidar_type: 1 # Livox MID-360 (uses same CustomMsg as AVIA)
scan_line: 6
blind: 0.5 # tuned: 0.2~0.3 (include closer points)
vio:
max_iterations: 10 # tuned: 10 (better accuracy, higher CPU)
outlier_threshold: 1000
img_point_cov: 100
patch_size: 8
patch_pyrimid_level: 3
normal_en: true
raycast_en: false # tuned: true (better accuracy, higher CPU)
inverse_composition_en: false
exposure_estimate_en: true
inv_expo_cov: 0.1
imu:
imu_en: true
imu_int_frame: 30
acc_cov: 0.5
gyr_cov: 0.3
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
lio:
max_iterations: 10
dept_err: 0.02
beam_err: 0.05
min_eigen_value: 0.001 # tuned: 0.001 (accept weaker planes, more points)
voxel_size: 0.5 # tuned: 0.2~0.3 (finer map, higher CPU)
max_layer: 3 # tuned: 3
max_points_num: 100 # tuned: 100
layer_init_num: [5, 5, 5, 5, 5]
local_map:
map_sliding_en: true
half_map_size: 50
sliding_thresh: 8.0
uav:
imu_rate_odom: false # tuned: true
gravity_align_en: true
publish:
dense_map_en: true
pub_effect_point_en: false
pub_plane_en: false
pub_scan_num: 1
blind_rgb_points: 0.0
evo:
seq_name: "mid360s_mapping"
pose_output_en: false
pcd_save:
pcd_save_en: true
type: 0 # 0: World Frame, 1: Body Frame
colmap_output_en: false
filter_size_pcd: 0.1
interval: 100 # tuned: 100 (periodic auto-save every 100 scans)
image_save:
img_save_en: false
interval: 1
src/FAST-LIVO2/include/IMU_Processing.h
+91
View File
@@ -0,0 +1,91 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#ifndef IMU_PROCESSING_H
#define IMU_PROCESSING_H
#include <Eigen/Eigen>
#include <fstream>
#include "common_lib.h"
#include <condition_variable>
#include <nav_msgs/msg/odometry.hpp>
#include <utils/so3_math.h>
#include <fstream>
extern const bool time_list(PointType &x, PointType &y);
/// *************IMU Process and undistortion
class ImuProcess
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ImuProcess();
~ImuProcess();
void Reset();
void Reset(double start_timestamp, const sensor_msgs::msg::Imu::ConstSharedPtr &lastimu);
void 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_scale(const V3D &scaler);
void set_acc_cov_scale(const V3D &scaler);
void set_gyr_bias_cov(const V3D &b_g);
void set_acc_bias_cov(const V3D &b_a);
void set_inv_expo_cov(const double &inv_expo);
void set_imu_init_frame_num(const int &num);
void disable_imu();
void disable_gravity_est();
void disable_bias_est();
void disable_exposure_est();
void Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_);
void UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out);
ofstream fout_imu;
double IMU_mean_acc_norm;
V3D unbiased_gyr;
V3D cov_acc;
V3D cov_gyr;
V3D cov_bias_gyr;
V3D cov_bias_acc;
double cov_inv_expo;
double first_lidar_time;
bool imu_time_init = false;
bool imu_need_init = true;
M3D Eye3d;
V3D Zero3d;
int lidar_type;
private:
void IMU_init(const MeasureGroup &meas, StatesGroup &state, int &N);
void Forward_without_imu(LidarMeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out);
PointCloudXYZI pcl_wait_proc;
sensor_msgs::msg::Imu::ConstSharedPtr last_imu;
PointCloudXYZI::Ptr cur_pcl_un_;
vector<Pose6D> IMUpose;
M3D Lid_rot_to_IMU;
V3D Lid_offset_to_IMU;
V3D mean_acc;
V3D mean_gyr;
V3D angvel_last;
V3D acc_s_last;
double last_prop_end_time;
double time_last_scan;
int init_iter_num = 1, MAX_INI_COUNT = 20;
bool b_first_frame = true;
bool imu_en = true;
bool gravity_est_en = true;
bool ba_bg_est_en = true;
bool exposure_estimate_en = true;
};
typedef std::shared_ptr<ImuProcess> ImuProcessPtr;
#endif
src/FAST-LIVO2/include/LIVMapper.h
+194
View File
@@ -0,0 +1,194 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#ifndef LIV_MAPPER_H
#define LIV_MAPPER_H
#include "IMU_Processing.h"
#include "vio.h"
#include "preprocess.h"
#if __has_include(<cv_bridge/cv_bridge.hpp>)
#include <cv_bridge/cv_bridge.hpp>
#else
#include <cv_bridge/cv_bridge.h>
#endif
#include <image_transport/image_transport.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/path.hpp>
#include <vikit/camera_loader.h>
class LIVMapper
{
public:
LIVMapper(rclcpp::Node::SharedPtr &node, std::string node_name);
~LIVMapper();
void initializeSubscribersAndPublishers(rclcpp::Node::SharedPtr &nh, image_transport::ImageTransport &it_);
void initializeComponents(rclcpp::Node::SharedPtr &node);
void initializeFiles();
void run(rclcpp::Node::SharedPtr &node);
void gravityAlignment();
void handleFirstFrame();
void stateEstimationAndMapping();
void handleVIO();
void handleLIO();
void savePCD();
void processImu();
bool sync_packages(LidarMeasureGroup &meas);
void prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, V3D angvel_avr);
void imu_prop_callback();
void transformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, PointCloudXYZI::Ptr &trans_cloud);
void pointBodyToWorld(const PointType &pi, PointType &po);
void RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po);
void RGBpointBodyToWorld(PointType const *const pi, PointType *const po);
void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg_in);
void imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in);
void img_cbk(const sensor_msgs::msg::Image::ConstSharedPtr &msg_in);
void publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager);
void publish_frame_world(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubLaserCloudFullRes, VIOManagerPtr vio_manager);
void publish_visual_sub_map(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubSubVisualMap);
void publish_effect_world(const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr &pubLaserCloudEffect, const std::vector<PointToPlane> &ptpl_list);
void publish_odometry(const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr &pmavros_pose_publisherubOdomAftMapped);
void publish_mavros(const rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr &mavros_pose_publisher);
void publish_path(const rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr &pubPath);
void readParameters(rclcpp::Node::SharedPtr &node);
template <typename T> void set_posestamp(T &out);
template <typename T> void pointBodyToWorld(const Eigen::Matrix<T, 3, 1> &pi, Eigen::Matrix<T, 3, 1> &po);
template <typename T> Eigen::Matrix<T, 3, 1> pointBodyToWorld(const Eigen::Matrix<T, 3, 1> &pi);
cv::Mat getImageFromMsg(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg);
std::mutex mtx_buffer, mtx_buffer_imu_prop;
std::condition_variable sig_buffer;
SLAM_MODE slam_mode_;
std::unordered_map<VOXEL_LOCATION, VoxelOctoTree *> voxel_map;
string root_dir;
string lid_topic, imu_topic, seq_name, img_topic;
V3D extT;
M3D extR;
int feats_down_size = 0, max_iterations = 0;
double res_mean_last = 0.05;
double gyr_cov = 0, acc_cov = 0, inv_expo_cov = 0;
double blind_rgb_points = 0.0;
double last_timestamp_lidar = -1.0, last_timestamp_imu = -1.0, last_timestamp_img = -1.0;
double filter_size_surf_min = 0;
double filter_size_pcd = 0;
double _first_lidar_time = 0.0;
double match_time = 0, solve_time = 0, solve_const_H_time = 0;
bool lidar_map_inited = false, pcd_save_en = false, img_save_en = false, pub_effect_point_en = false, pose_output_en = false, ros_driver_fix_en = false, hilti_en = false;
int img_save_interval = 1, pcd_save_interval = -1, pcd_save_type = 0;
int pub_scan_num = 1;
StatesGroup imu_propagate, latest_ekf_state;
bool new_imu = false, state_update_flg = false, imu_prop_enable = true, ekf_finish_once = false;
deque<sensor_msgs::msg::Imu> prop_imu_buffer;
sensor_msgs::msg::Imu newest_imu;
double latest_ekf_time;
nav_msgs::msg::Odometry imu_prop_odom;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubImuPropOdom;
double imu_time_offset = 0.0;
double lidar_time_offset = 0.0;
bool gravity_align_en = false, gravity_align_finished = false;
bool sync_jump_flag = false;
bool lidar_pushed = false, imu_en, gravity_est_en, flg_reset = false, ba_bg_est_en = true;
bool dense_map_en = false;
int img_en = 1, imu_int_frame = 3;
bool normal_en = true;
bool exposure_estimate_en = false;
double exposure_time_init = 0.0;
bool inverse_composition_en = false;
bool raycast_en = false;
int lidar_en = 1;
bool is_first_frame = false;
int grid_size, patch_size, grid_n_width, grid_n_height, patch_pyrimid_level;
int outlier_threshold;
double plot_time;
int frame_cnt;
double img_time_offset = 0.0;
deque<PointCloudXYZI::Ptr> lid_raw_data_buffer;
deque<double> lid_header_time_buffer;
deque<sensor_msgs::msg::Imu::ConstSharedPtr> imu_buffer;
deque<cv::Mat> img_buffer;
deque<double> img_time_buffer;
vector<pointWithVar> _pv_list;
vector<double> extrinT;
vector<double> extrinR;
vector<double> cameraextrinT;
vector<double> cameraextrinR;
int IMG_POINT_COV;
PointCloudXYZI::Ptr visual_sub_map;
PointCloudXYZI::Ptr feats_undistort;
PointCloudXYZI::Ptr feats_down_body;
PointCloudXYZI::Ptr feats_down_world;
PointCloudXYZI::Ptr pcl_w_wait_pub;
PointCloudXYZI::Ptr pcl_wait_pub;
PointCloudXYZRGB::Ptr pcl_wait_save;
PointCloudXYZI::Ptr pcl_wait_save_intensity;
ofstream fout_pre, fout_out, fout_visual_pos, fout_lidar_pos, fout_points;
pcl::VoxelGrid<PointType> downSizeFilterSurf;
V3D euler_cur;
LidarMeasureGroup LidarMeasures;
StatesGroup _state;
StatesGroup state_propagat;
nav_msgs::msg::Path path;
nav_msgs::msg::Odometry odomAftMapped;
geometry_msgs::msg::Quaternion geoQuat;
geometry_msgs::msg::PoseStamped msg_body_pose;
PreprocessPtr p_pre;
ImuProcessPtr p_imu;
VoxelMapManagerPtr voxelmap_manager;
VIOManagerPtr vio_manager;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr plane_pub;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr voxel_pub;
std::shared_ptr<rclcpp::SubscriptionBase> sub_pcl;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_img;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pubNormal;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubSubVisualMap;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudEffect;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudMap;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDyn;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDynRmed;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudDynDbg;
image_transport::Publisher pubImage;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr mavros_pose_publisher;
rclcpp::TimerBase::SharedPtr imu_prop_timer;
rclcpp::Node::SharedPtr node;
int frame_num = 0;
double aver_time_consu = 0;
double aver_time_icp = 0;
double aver_time_map_inre = 0;
bool colmap_output_en = false;
};
#endif
src/FAST-LIVO2/include/common_lib.h
Executable
+247
View File
@@ -0,0 +1,247 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#ifndef COMMON_LIB_H
#define COMMON_LIB_H
#include <utils/so3_math.h>
#include <utils/types.h>
#include <utils/color.h>
#include <utils/utils.h>
#include <opencv2/opencv.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sophus/se3.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Transform.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
using namespace std;
// using namespace Eigen; // avoid cmake error: reference to ‘Matrix’ is ambiguous
using namespace Sophus;
#define print_line std::cout << __FILE__ << ", " << __LINE__ << std::endl;
#define G_m_s2 (9.81) // Gravaty const in GuangDong/China
#define DIM_STATE (19) // Dimension of states (Let Dim(SO(3)) = 3)
#define INIT_COV (0.01)
#define SIZE_LARGE (500)
#define SIZE_SMALL (100)
#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 DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/" + name))
enum LID_TYPE
{
AVIA = 1,
VELO16 = 2,
OUST64 = 3,
L515 = 4,
XT32 = 5,
PANDAR128 = 6,
ROBOSENSE = 7
};
enum SLAM_MODE
{
ONLY_LO = 0,
ONLY_LIO = 1,
LIVO = 2
};
enum EKF_STATE
{
WAIT = 0,
VIO = 1,
LIO = 2,
LO = 3
};
struct MeasureGroup
{
double vio_time;
double lio_time;
deque<sensor_msgs::msg::Imu::ConstSharedPtr> imu;
cv::Mat img;
MeasureGroup()
{
vio_time = 0.0;
lio_time = 0.0;
};
};
struct LidarMeasureGroup
{
double lidar_frame_beg_time;
double lidar_frame_end_time;
double last_lio_update_time;
PointCloudXYZI::Ptr lidar;
PointCloudXYZI::Ptr pcl_proc_cur;
PointCloudXYZI::Ptr pcl_proc_next;
deque<struct MeasureGroup> measures;
EKF_STATE lio_vio_flg;
int lidar_scan_index_now;
LidarMeasureGroup()
{
lidar_frame_beg_time = -0.0;
lidar_frame_end_time = 0.0;
last_lio_update_time = -1.0;
lio_vio_flg = WAIT;
this->lidar.reset(new PointCloudXYZI());
this->pcl_proc_cur.reset(new PointCloudXYZI());
this->pcl_proc_next.reset(new PointCloudXYZI());
this->measures.clear();
lidar_scan_index_now = 0;
last_lio_update_time = -1.0;
};
};
typedef struct pointWithVar
{
Eigen::Vector3d point_b; // point in the lidar body frame
Eigen::Vector3d point_i; // point in the imu body frame
Eigen::Vector3d point_w; // point in the world frame
Eigen::Matrix3d var_nostate; // the var removed the state covarience
Eigen::Matrix3d body_var;
Eigen::Matrix3d var;
Eigen::Matrix3d point_crossmat;
Eigen::Vector3d normal;
pointWithVar()
{
var_nostate = Eigen::Matrix3d::Zero();
var = Eigen::Matrix3d::Zero();
body_var = Eigen::Matrix3d::Zero();
point_crossmat = Eigen::Matrix3d::Zero();
point_b = Eigen::Vector3d::Zero();
point_i = Eigen::Vector3d::Zero();
point_w = Eigen::Vector3d::Zero();
normal = Eigen::Vector3d::Zero();
};
} pointWithVar;
struct StatesGroup
{
StatesGroup()
{
this->rot_end = M3D::Identity();
this->pos_end = V3D::Zero();
this->vel_end = V3D::Zero();
this->bias_g = V3D::Zero();
this->bias_a = V3D::Zero();
this->gravity = V3D::Zero();
this->inv_expo_time = 1.0;
this->cov = MD(DIM_STATE, DIM_STATE)::Identity() * INIT_COV;
this->cov(6, 6) = 0.00001;
this->cov.block<9, 9>(10, 10) = 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->inv_expo_time = b.inv_expo_time;
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->inv_expo_time = b.inv_expo_time;
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.inv_expo_time = this->inv_expo_time + state_add(6, 0);
a.vel_end = this->vel_end + state_add.block<3, 1>(7, 0);
a.bias_g = this->bias_g + state_add.block<3, 1>(10, 0);
a.bias_a = this->bias_a + state_add.block<3, 1>(13, 0);
a.gravity = this->gravity + state_add.block<3, 1>(16, 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->inv_expo_time += state_add(6, 0);
this->vel_end += state_add.block<3, 1>(7, 0);
this->bias_g += state_add.block<3, 1>(10, 0);
this->bias_a += state_add.block<3, 1>(13, 0);
this->gravity += state_add.block<3, 1>(16, 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(6, 0) = this->inv_expo_time - b.inv_expo_time;
a.block<3, 1>(7, 0) = this->vel_end - b.vel_end;
a.block<3, 1>(10, 0) = this->bias_g - b.bias_g;
a.block<3, 1>(13, 0) = this->bias_a - b.bias_a;
a.block<3, 1>(16, 0) = this->gravity - b.gravity;
return a;
};
void resetpose()
{
this->rot_end = M3D::Identity();
this->pos_end = V3D::Zero();
this->vel_end = V3D::Zero();
}
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)
double inv_expo_time; // the estimated inverse exposure time (no scale)
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>
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);
}
// Map<M3D>(rot_kp.rot, 3,3) = R;
return move(rot_kp);
}
#endif
src/FAST-LIVO2/include/feature.h
+56
View File
@@ -0,0 +1,56 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#ifndef LIVO_FEATURE_H_
#define LIVO_FEATURE_H_
#include "visual_point.h"
// A salient image region that is tracked across frames.
struct Feature
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum FeatureType
{
CORNER,
EDGELET
};
int id_;
FeatureType type_; //!< Type can be corner or edgelet.
cv::Mat img_; //!< Image associated with the patch feature
Vector2d px_; //!< Coordinates in pixels on pyramid level 0.
Vector3d f_; //!< Unit-bearing vector of the patch feature.
int level_; //!< Image pyramid level where patch feature was extracted.
VisualPoint *point_; //!< Pointer to 3D point which corresponds to the patch feature.
Vector2d grad_; //!< Dominant gradient direction for edglets, normalized.
SE3<double> T_f_w_; //!< Pose of the frame where the patch feature was extracted.
float *patch_; //!< Pointer to the image patch data.
float score_; //!< Score of the patch feature.
float mean_; //!< Mean intensity of the image patch feature, used for normalization.
double inv_expo_time_; //!< Inverse exposure time of the image where the patch feature was extracted.
Feature(VisualPoint *_point, float *_patch, const Vector2d &_px, const Vector3d &_f, const SE3<double> &_T_f_w, int _level)
: type_(CORNER), px_(_px), f_(_f), T_f_w_(_T_f_w), mean_(0), score_(0), level_(_level), patch_(_patch), point_(_point)
{
}
inline Vector3d pos() const { return T_f_w_.inverse().translation(); }
~Feature()
{
// ROS_WARN("The feature %d has been destructed.", id_);
delete[] patch_;
}
};
#endif // LIVO_FEATURE_H_
src/FAST-LIVO2/include/frame.h
+84
View File
@@ -0,0 +1,84 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#ifndef LIVO_FRAME_H_
#define LIVO_FRAME_H_
#include <boost/noncopyable.hpp>
#include <vikit/abstract_camera.h>
class VisualPoint;
struct Feature;
typedef list<Feature *> Features;
typedef vector<cv::Mat> ImgPyr;
/// A frame saves the image, the associated features and the estimated pose.
class Frame : boost::noncopyable
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id.
int id_; //!< Unique id of the frame.
vk::AbstractCamera *cam_; //!< Camera model.
SE3<double> T_f_w_; //!< Transform (f)rame from (w)orld.
SE3<double> T_f_w_prior_; //!< Transform (f)rame from (w)orld provided by the IMU prior.
cv::Mat img_; //!< Image of the frame.
Features fts_; //!< List of features in the image.
Frame(vk::AbstractCamera *cam, const cv::Mat &img);
~Frame();
/// Initialize new frame and create image pyramid.
void initFrame(const cv::Mat &img);
/// Return number of point observations.
inline size_t nObs() const { return fts_.size(); }
/// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
inline Vector2d w2c(const Vector3d &xyz_w) const { return cam_->world2cam(T_f_w_ * xyz_w); }
/// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c) using the IMU prior pose.
inline Vector2d w2c_prior(const Vector3d &xyz_w) const { return cam_->world2cam(T_f_w_prior_ * xyz_w); }
/// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
inline Vector3d c2f(const Vector2d &px) const { return cam_->cam2world(px[0], px[1]); }
/// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); }
/// Transforms point coordinates in world-frame (w) to camera-frams (f).
inline Vector3d w2f(const Vector3d &xyz_w) const { return T_f_w_ * xyz_w; }
/// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
inline Vector3d f2w(const Vector3d &f) const { return T_f_w_.inverse() * f; }
/// Projects Point from unit sphere (f) in camera pixels (c).
inline Vector2d f2c(const Vector3d &f) const { return cam_->world2cam(f); }
/// Return the pose of the frame in the (w)orld coordinate frame.
inline Vector3d pos() const { return T_f_w_.inverse().translation(); }
};
typedef std::unique_ptr<Frame> FramePtr;
/// Some helper functions for the frame object.
namespace frame_utils
{
/// Creates an image pyramid of half-sampled images.
void createImgPyramid(const cv::Mat &img_level_0, int n_levels, ImgPyr &pyr);
} // namespace frame_utils
#endif // LIVO_FRAME_H_
src/FAST-LIVO2/include/livox_ros_driver/CustomMsg.h
+292
View File
@@ -0,0 +1,292 @@
// Generated by gencpp from file livox_ros_driver/CustomMsg.msg
// DO NOT EDIT!
#ifndef LIVOX_ROS_DRIVER_MESSAGE_CUSTOMMSG_H
#define LIVOX_ROS_DRIVER_MESSAGE_CUSTOMMSG_H
#include <string>
#include <vector>
#include <memory>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <std_msgs/Header.h>
#include <livox_ros_driver/CustomPoint.h>
namespace livox_ros_driver
{
template <class ContainerAllocator>
struct CustomMsg_
{
typedef CustomMsg_<ContainerAllocator> Type;
CustomMsg_()
: header()
, timebase(0)
, point_num(0)
, lidar_id(0)
, rsvd()
, points() {
rsvd.assign(0);
}
CustomMsg_(const ContainerAllocator& _alloc)
: header(_alloc)
, timebase(0)
, point_num(0)
, lidar_id(0)
, rsvd()
, points(_alloc) {
(void)_alloc;
rsvd.assign(0);
}
typedef ::std_msgs::Header_<ContainerAllocator> _header_type;
_header_type header;
typedef uint64_t _timebase_type;
_timebase_type timebase;
typedef uint32_t _point_num_type;
_point_num_type point_num;
typedef uint8_t _lidar_id_type;
_lidar_id_type lidar_id;
typedef boost::array<uint8_t, 3> _rsvd_type;
_rsvd_type rsvd;
typedef std::vector< ::livox_ros_driver::CustomPoint_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >> _points_type;
_points_type points;
typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg_<ContainerAllocator> const> ConstPtr;
}; // struct CustomMsg_
typedef ::livox_ros_driver::CustomMsg_<std::allocator<void> > CustomMsg;
typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg > CustomMsgPtr;
typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg const> CustomMsgConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::livox_ros_driver::CustomMsg_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::livox_ros_driver::CustomMsg_<ContainerAllocator1> & lhs, const ::livox_ros_driver::CustomMsg_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.timebase == rhs.timebase &&
lhs.point_num == rhs.point_num &&
lhs.lidar_id == rhs.lidar_id &&
lhs.rsvd == rhs.rsvd &&
lhs.points == rhs.points;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::livox_ros_driver::CustomMsg_<ContainerAllocator1> & lhs, const ::livox_ros_driver::CustomMsg_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace livox_ros_driver
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsMessage< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::livox_ros_driver::CustomMsg_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::livox_ros_driver::CustomMsg_<ContainerAllocator> const>
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::livox_ros_driver::CustomMsg_<ContainerAllocator> const>
: TrueType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
{
static const char* value()
{
return "e4d6829bdfe657cb6c21a746c86b21a6";
}
static const char* value(const ::livox_ros_driver::CustomMsg_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xe4d6829bdfe657cbULL;
static const uint64_t static_value2 = 0x6c21a746c86b21a6ULL;
};
template<class ContainerAllocator>
struct DataType< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
{
static const char* value()
{
return "livox_ros_driver/CustomMsg";
}
static const char* value(const ::livox_ros_driver::CustomMsg_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
{
static const char* value()
{
return "# Livox publish pointcloud msg format.\n"
"\n"
"Header header # ROS standard message header\n"
"uint64 timebase # The time of first point\n"
"uint32 point_num # Total number of pointclouds\n"
"uint8 lidar_id # Lidar device id number\n"
"uint8[3] rsvd # Reserved use\n"
"CustomPoint[] points # Pointcloud data\n"
"\n"
"\n"
"================================================================================\n"
"MSG: std_msgs/Header\n"
"# Standard metadata for higher-level stamped data types.\n"
"# This is generally used to communicate timestamped data \n"
"# in a particular coordinate frame.\n"
"# \n"
"# sequence ID: consecutively increasing ID \n"
"uint32 seq\n"
"#Two-integer timestamp that is expressed as:\n"
"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n"
"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n"
"# time-handling sugar is provided by the client library\n"
"time stamp\n"
"#Frame this data is associated with\n"
"string frame_id\n"
"\n"
"================================================================================\n"
"MSG: livox_ros_driver/CustomPoint\n"
"# Livox costom pointcloud format.\n"
"\n"
"uint32 offset_time # offset time relative to the base time\n"
"float32 x # X axis, unit:m\n"
"float32 y # Y axis, unit:m\n"
"float32 z # Z axis, unit:m\n"
"uint8 reflectivity # reflectivity, 0~255\n"
"uint8 tag # livox tag\n"
"uint8 line # laser number in lidar\n"
"\n"
;
}
static const char* value(const ::livox_ros_driver::CustomMsg_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.header);
stream.next(m.timebase);
stream.next(m.point_num);
stream.next(m.lidar_id);
stream.next(m.rsvd);
stream.next(m.points);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct CustomMsg_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::livox_ros_driver::CustomMsg_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::livox_ros_driver::CustomMsg_<ContainerAllocator>& v)
{
s << indent << "header: ";
s << std::endl;
Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + " ", v.header);
s << indent << "timebase: ";
Printer<uint64_t>::stream(s, indent + " ", v.timebase);
s << indent << "point_num: ";
Printer<uint32_t>::stream(s, indent + " ", v.point_num);
s << indent << "lidar_id: ";
Printer<uint8_t>::stream(s, indent + " ", v.lidar_id);
s << indent << "rsvd[]" << std::endl;
for (size_t i = 0; i < v.rsvd.size(); ++i)
{
s << indent << " rsvd[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.rsvd[i]);
}
s << indent << "points[]" << std::endl;
for (size_t i = 0; i < v.points.size(); ++i)
{
s << indent << " points[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >::stream(s, indent + " ", v.points[i]);
}
}
};
} // namespace message_operations
} // namespace ros
#endif // LIVOX_ROS_DRIVER_MESSAGE_CUSTOMMSG_H
src/FAST-LIVO2/include/livox_ros_driver/CustomPoint.h
+258
View File
@@ -0,0 +1,258 @@
// Generated by gencpp from file livox_ros_driver/CustomPoint.msg
// DO NOT EDIT!
#ifndef LIVOX_ROS_DRIVER_MESSAGE_CUSTOMPOINT_H
#define LIVOX_ROS_DRIVER_MESSAGE_CUSTOMPOINT_H
#include <string>
#include <vector>
#include <memory>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
namespace livox_ros_driver
{
template <class ContainerAllocator>
struct CustomPoint_
{
typedef CustomPoint_<ContainerAllocator> Type;
CustomPoint_()
: offset_time(0)
, x(0.0)
, y(0.0)
, z(0.0)
, reflectivity(0)
, tag(0)
, line(0) {
}
CustomPoint_(const ContainerAllocator& _alloc)
: offset_time(0)
, x(0.0)
, y(0.0)
, z(0.0)
, reflectivity(0)
, tag(0)
, line(0) {
(void)_alloc;
}
typedef uint32_t _offset_time_type;
_offset_time_type offset_time;
typedef float _x_type;
_x_type x;
typedef float _y_type;
_y_type y;
typedef float _z_type;
_z_type z;
typedef uint8_t _reflectivity_type;
_reflectivity_type reflectivity;
typedef uint8_t _tag_type;
_tag_type tag;
typedef uint8_t _line_type;
_line_type line;
typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint_<ContainerAllocator> const> ConstPtr;
}; // struct CustomPoint_
typedef ::livox_ros_driver::CustomPoint_<std::allocator<void> > CustomPoint;
typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint > CustomPointPtr;
typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint const> CustomPointConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::livox_ros_driver::CustomPoint_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::livox_ros_driver::CustomPoint_<ContainerAllocator1> & lhs, const ::livox_ros_driver::CustomPoint_<ContainerAllocator2> & rhs)
{
return lhs.offset_time == rhs.offset_time &&
lhs.x == rhs.x &&
lhs.y == rhs.y &&
lhs.z == rhs.z &&
lhs.reflectivity == rhs.reflectivity &&
lhs.tag == rhs.tag &&
lhs.line == rhs.line;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::livox_ros_driver::CustomPoint_<ContainerAllocator1> & lhs, const ::livox_ros_driver::CustomPoint_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace livox_ros_driver
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsMessage< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::livox_ros_driver::CustomPoint_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::livox_ros_driver::CustomPoint_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::livox_ros_driver::CustomPoint_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
{
static const char* value()
{
return "109a3cc548bb1f96626be89a5008bd6d";
}
static const char* value(const ::livox_ros_driver::CustomPoint_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x109a3cc548bb1f96ULL;
static const uint64_t static_value2 = 0x626be89a5008bd6dULL;
};
template<class ContainerAllocator>
struct DataType< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
{
static const char* value()
{
return "livox_ros_driver/CustomPoint";
}
static const char* value(const ::livox_ros_driver::CustomPoint_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
{
static const char* value()
{
return "# Livox costom pointcloud format.\n"
"\n"
"uint32 offset_time # offset time relative to the base time\n"
"float32 x # X axis, unit:m\n"
"float32 y # Y axis, unit:m\n"
"float32 z # Z axis, unit:m\n"
"uint8 reflectivity # reflectivity, 0~255\n"
"uint8 tag # livox tag\n"
"uint8 line # laser number in lidar\n"
"\n"
;
}
static const char* value(const ::livox_ros_driver::CustomPoint_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.offset_time);
stream.next(m.x);
stream.next(m.y);
stream.next(m.z);
stream.next(m.reflectivity);
stream.next(m.tag);
stream.next(m.line);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct CustomPoint_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::livox_ros_driver::CustomPoint_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::livox_ros_driver::CustomPoint_<ContainerAllocator>& v)
{
s << indent << "offset_time: ";
Printer<uint32_t>::stream(s, indent + " ", v.offset_time);
s << indent << "x: ";
Printer<float>::stream(s, indent + " ", v.x);
s << indent << "y: ";
Printer<float>::stream(s, indent + " ", v.y);
s << indent << "z: ";
Printer<float>::stream(s, indent + " ", v.z);
s << indent << "reflectivity: ";
Printer<uint8_t>::stream(s, indent + " ", v.reflectivity);
s << indent << "tag: ";
Printer<uint8_t>::stream(s, indent + " ", v.tag);
s << indent << "line: ";
Printer<uint8_t>::stream(s, indent + " ", v.line);
}
};
} // namespace message_operations
} // namespace ros
#endif // LIVOX_ROS_DRIVER_MESSAGE_CUSTOMPOINT_H
src/FAST-LIVO2/include/preprocess.h
Executable
+200
View File
@@ -0,0 +1,200 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#ifndef PREPROCESS_H_
#define PREPROCESS_H_
#include "common_lib.h"
#include <livox_ros_driver2/msg/custom_msg.hpp>
#include <pcl_conversions/pcl_conversions.h>
using namespace std;
#define IS_VALID(a) ((abs(a) > 1e8) ? true : false)
enum LiDARFeature
{
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];
LiDARFeature ftype;
orgtype()
{
range = 0;
edj[Prev] = Nr_nor;
edj[Next] = Nr_nor;
ftype = Nor;
intersect = 2;
}
};
/*** Velodyne ***/
namespace velodyne_ros
{
struct EIGEN_ALIGN16 Point
{
PCL_ADD_POINT4D;
float intensity;
float time;
std::uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace velodyne_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(std::uint16_t, ring, ring))
/****************/
/*** Ouster ***/
namespace ouster_ros
{
struct EIGEN_ALIGN16 Point
{
PCL_ADD_POINT4D;
float intensity;
std::uint32_t t;
std::uint16_t reflectivity;
uint8_t ring;
std::uint16_t ambient;
std::uint32_t range;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace ouster_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
(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))
/****************/
/*** Hesai_XT32 ***/
namespace xt32_ros
{
struct EIGEN_ALIGN16 Point
{
PCL_ADD_POINT4D;
float intensity;
double timestamp;
std::uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace xt32_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(xt32_ros::Point,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring))
/*****************/
/*** Hesai_Pandar128 ***/
namespace Pandar128_ros
{
struct EIGEN_ALIGN16 Point
{
PCL_ADD_POINT4D;
uint8_t intensity;
double timestamp;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace Pandar128_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(Pandar128_ros::Point,
(float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring))
/*****************/
/*** Robosense_Airy ***/
namespace robosense_ros
{
struct EIGEN_ALIGN16 Point
{
PCL_ADD_POINT4D;
float intensity;
double timestamp;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace robosense_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(robosense_ros::Point,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp)(std::uint16_t, ring, ring))
/*****************/
class Preprocess
{
public:
// EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Preprocess();
~Preprocess();
void process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void set(bool feat_en, int lid_type, double bld, int pfilt_num);
// sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud;
PointCloudXYZI pl_full, pl_corn, pl_surf;
PointCloudXYZI pl_buff[128]; // maximum 128 line lidar
vector<orgtype> typess[128]; // maximum 128 line lidar
int lidar_type, point_filter_num, N_SCANS;
double blind, blind_sqr;
bool feature_enabled, given_offset_time;
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_full;
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_surf;
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> pub_corn;
private:
void avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg);
void oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void xt32_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void Pandar128_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void robosense_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
void l515_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &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;
};
typedef std::shared_ptr<Preprocess> PreprocessPtr;
#endif // PREPROCESS_H_
src/FAST-LIVO2/include/utils/color.h
+24
View File
@@ -0,0 +1,24 @@
#ifndef COLOR_H
#define COLOR_H
#define RESET "\033[0m"
#define BLACK "\033[30m" /* Black */
#define RED "\033[31m" /* Red */
#define GREEN "\033[32m" /* Green */
#define YELLOW "\033[33m" /* Yellow */
#define BLUE "\033[34m" /* Blue */
#define MAGENTA "\033[35m" /* Magenta */
#define CYAN "\033[36m" /* Cyan */
#define WHITE "\033[37m" /* White */
#define REDPURPLE "\033[95m" /* Red Purple */
#define BOLDBLACK "\033[1m\033[30m" /* Bold Black */
#define BOLDRED "\033[1m\033[31m" /* Bold Red */
#define BOLDGREEN "\033[1m\033[32m" /* Bold Green */
#define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */
#define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */
#define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */
#define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */
#define BOLDWHITE "\033[1m\033[37m" /* Bold White */
#define BOLDREDPURPLE "\033[1m\033[95m" /* Bold Red Purple */
#endif // COLOR_H
src/FAST-LIVO2/include/utils/so3_math.h
Executable
+89
View File
@@ -0,0 +1,89 @@
#ifndef SO3_MATH_H
#define SO3_MATH_H
#include <Eigen/Core>
#include <math.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 = (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
src/FAST-LIVO2/include/utils/types.h
+39
View File
@@ -0,0 +1,39 @@
#ifndef TYPES_H
#define TYPES_H
#include <Eigen/Eigen>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointXYZRGB PointTypeRGB;
typedef pcl::PointXYZRGBA PointTypeRGBA;
typedef pcl::PointCloud<PointType> PointCloudXYZI;
typedef std::vector<PointType, Eigen::aligned_allocator<PointType>> PointVector;
typedef pcl::PointCloud<PointTypeRGB> PointCloudXYZRGB;
typedef pcl::PointCloud<PointTypeRGBA> PointCloudXYZRGBA;
typedef Eigen::Vector2f V2F;
typedef Eigen::Vector2d V2D;
typedef Eigen::Vector3d V3D;
typedef Eigen::Matrix3d M3D;
typedef Eigen::Vector3f V3F;
typedef Eigen::Matrix3f M3F;
#define MD(a, b) Eigen::Matrix<double, (a), (b)>
#define VD(a) Eigen::Matrix<double, (a), 1>
#define MF(a, b) Eigen::Matrix<float, (a), (b)>
#define VF(a) Eigen::Matrix<float, (a), 1>
struct Pose6D
{
/*** the preintegrated Lidar states at the time of IMU measurements in a frame ***/
double offset_time; // the offset time of IMU measurement w.r.t the first lidar point
double acc[3]; // the preintegrated total acceleration (global frame) at the Lidar origin
double gyr[3]; // the unbiased angular velocity (body frame) at the Lidar origin
double vel[3]; // the preintegrated velocity (global frame) at the Lidar origin
double pos[3]; // the preintegrated position (global frame) at the Lidar origin
double rot[9]; // the preintegrated rotation (global frame) at the Lidar origin
};
#endif
src/FAST-LIVO2/include/utils/utils.h
+76
View File
@@ -0,0 +1,76 @@
#ifndef UTILS_H
#define UTILS_H
#include <vector>
#include <cstdint> // for int64_t
#include <limits> // for std::numeric_limits
#include <stdexcept> // for std::out_of_range
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/LinearMath/Quaternion.h>
std::vector<int> convertToIntVectorSafe(const std::vector<int64_t>& int64_vector);
inline double stamp2Sec(const builtin_interfaces::msg::Time& stamp)
{
return rclcpp::Time(stamp).seconds();
}
inline rclcpp::Time sec2Stamp(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);
}
namespace tf
{
inline geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw)
{
tf2::Quaternion q;
q.setRPY(0, 0, yaw);
return tf2::toMsg(q);
}
inline geometry_msgs::msg::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw)
{
tf2::Quaternion q;
q.setRPY(roll, pitch, yaw);
return tf2::toMsg(q);
}
inline tf2::Quaternion createQuaternionFromYaw(double yaw)
{
tf2::Quaternion q;
q.setRPY(0, 0, yaw);
return q;
}
inline tf2::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
{
tf2::Quaternion q;
q.setRPY(roll, pitch, yaw);
return q;
}
}
inline geometry_msgs::msg::TransformStamped createTransformStamped(
const tf2::Transform &transform,
const builtin_interfaces::msg::Time &stamp,
const std::string &frame_id,
const std::string &child_frame_id)
{
geometry_msgs::msg::TransformStamped transform_stamped;
transform_stamped.header.stamp = stamp;
transform_stamped.header.frame_id = frame_id;
transform_stamped.child_frame_id = child_frame_id;
transform_stamped.transform = tf2::toMsg(transform);
return transform_stamped;
}
#endif // UTILS_H
src/FAST-LIVO2/include/vio.h
Executable
+187
View File
@@ -0,0 +1,187 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#ifndef VIO_H_
#define VIO_H_
#include "voxel_map.h"
#include "feature.h"
#include <opencv2/imgproc/imgproc_c.h>
#include <pcl/filters/voxel_grid.h>
#include <set>
#include <vikit/math_utils.h>
#include <vikit/robust_cost.h>
#include <vikit/vision.h>
#include <vikit/pinhole_camera.h>
struct SubSparseMap
{
vector<float> propa_errors;
vector<float> errors;
vector<vector<float>> warp_patch;
vector<int> search_levels;
vector<VisualPoint *> voxel_points;
vector<double> inv_expo_list;
vector<pointWithVar> add_from_voxel_map;
SubSparseMap()
{
propa_errors.reserve(SIZE_LARGE);
errors.reserve(SIZE_LARGE);
warp_patch.reserve(SIZE_LARGE);
search_levels.reserve(SIZE_LARGE);
voxel_points.reserve(SIZE_LARGE);
inv_expo_list.reserve(SIZE_LARGE);
add_from_voxel_map.reserve(SIZE_SMALL);
};
void reset()
{
propa_errors.clear();
errors.clear();
warp_patch.clear();
search_levels.clear();
voxel_points.clear();
inv_expo_list.clear();
add_from_voxel_map.clear();
}
};
class Warp
{
public:
Matrix2d A_cur_ref;
int search_level;
Warp(int level, Matrix2d warp_matrix) : search_level(level), A_cur_ref(warp_matrix) {}
~Warp() {}
};
class VOXEL_POINTS
{
public:
std::vector<VisualPoint *> voxel_points;
int count;
VOXEL_POINTS(int num) : count(num) {}
~VOXEL_POINTS()
{
for (VisualPoint* vp : voxel_points)
{
if (vp != nullptr) { delete vp; vp = nullptr; }
}
}
};
class VIOManager
{
public:
int grid_size;
vk::AbstractCamera *cam;
vk::PinholeCamera *pinhole_cam;
StatesGroup *state;
StatesGroup *state_propagat;
M3D Rli, Rci, Rcl, Rcw, Jdphi_dR, Jdp_dt, Jdp_dR;
V3D Pli, Pci, Pcl, Pcw;
vector<int> grid_num;
vector<int> map_index;
vector<int> border_flag;
vector<int> update_flag;
vector<float> map_dist;
vector<float> scan_value;
vector<float> patch_buffer;
bool normal_en, inverse_composition_en, exposure_estimate_en, raycast_en, has_ref_patch_cache;
bool ncc_en = false, colmap_output_en = false;
int width, height, grid_n_width, grid_n_height, length;
double image_resize_factor;
double fx, fy, cx, cy;
int patch_pyrimid_level, patch_size, patch_size_total, patch_size_half, border, warp_len;
int max_iterations, total_points;
double img_point_cov, outlier_threshold, ncc_thre;
SubSparseMap *visual_submap;
std::vector<std::vector<V3D>> rays_with_sample_points;
double compute_jacobian_time, update_ekf_time;
double ave_total = 0;
// double ave_build_residual_time = 0;
// double ave_ekf_time = 0;
int frame_count = 0;
bool plot_flag;
Eigen::Matrix<double, DIM_STATE, DIM_STATE> G, H_T_H;
Eigen::MatrixXd K, H_sub_inv;
ofstream fout_camera, fout_colmap;
unordered_map<VOXEL_LOCATION, VOXEL_POINTS *> feat_map;
unordered_map<VOXEL_LOCATION, int> sub_feat_map;
unordered_map<int, Warp *> warp_map;
vector<VisualPoint *> retrieve_voxel_points;
vector<pointWithVar> append_voxel_points;
FramePtr new_frame_;
cv::Mat img_cp, img_rgb, img_test;
enum CellType
{
TYPE_MAP = 1,
TYPE_POINTCLOUD,
TYPE_UNKNOWN
};
VIOManager();
~VIOManager();
void updateStateInverse(cv::Mat img, int level);
void updateState(cv::Mat img, int level);
void processFrame(cv::Mat &img, vector<pointWithVar> &pg, const unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &feat_map, double img_time);
void retrieveFromVisualSparseMap(cv::Mat img, vector<pointWithVar> &pg, const unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &plane_map);
void generateVisualMapPoints(cv::Mat img, vector<pointWithVar> &pg);
void setImuToLidarExtrinsic(const V3D &transl, const M3D &rot);
void setLidarToCameraExtrinsic(vector<double> &R, vector<double> &P);
void initializeVIO();
void getImagePatch(cv::Mat img, V2D pc, float *patch_tmp, int level);
void computeProjectionJacobian(V3D p, MD(2, 3) & J);
void computeJacobianAndUpdateEKF(cv::Mat img);
void resetGrid();
void updateVisualMapPoints(cv::Mat img);
void getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, const SE3<double> &T_cur_ref,
const int level_ref,
const int pyramid_level, const int halfpatch_size, Matrix2d &A_cur_ref);
void getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref,
const V3D &xyz_ref, const V3D &normal_ref, const SE3<double> &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref);
void warpAffine(const Matrix2d &A_cur_ref, const cv::Mat &img_ref, const Vector2d &px_ref, const int level_ref, const int search_level,
const int pyramid_level, const int halfpatch_size, float *patch);
void insertPointIntoVoxelMap(VisualPoint *pt_new);
void plotTrackedPoints();
void updateFrameState(StatesGroup state);
void projectPatchFromRefToCur(const unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &plane_map);
void updateReferencePatch(const unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &plane_map);
void precomputeReferencePatches(int level);
void dumpDataForColmap();
double calculateNCC(float *ref_patch, float *cur_patch, int patch_size);
int getBestSearchLevel(const Matrix2d &A_cur_ref, const int max_level);
V3F getInterpolatedPixel(cv::Mat img, V2D pc);
// void resetRvizDisplay();
// deque<VisualPoint *> map_cur_frame;
// deque<VisualPoint *> sub_map_ray;
// deque<VisualPoint *> sub_map_ray_fov;
// deque<VisualPoint *> visual_sub_map_cur;
// deque<VisualPoint *> visual_converged_point;
// std::vector<std::vector<V3D>> sample_points;
// PointCloudXYZI::Ptr pg_down;
// pcl::VoxelGrid<PointType> downSizeFilter;
};
typedef std::shared_ptr<VIOManager> VIOManagerPtr;
#endif // VIO_H_
src/FAST-LIVO2/include/visual_point.h
+48
View File
@@ -0,0 +1,48 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#ifndef LIVO_POINT_H_
#define LIVO_POINT_H_
#include <boost/noncopyable.hpp>
#include "common_lib.h"
#include "frame.h"
class Feature;
/// A visual map point on the surface of the scene.
class VisualPoint : boost::noncopyable
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Vector3d pos_; //!< 3d pos of the point in the world coordinate frame.
Vector3d normal_; //!< Surface normal at point.
Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation.
Vector3d previous_normal_; //!< Last updated normal vector.
list<Feature *> obs_; //!< Reference patches which observe the point.
Eigen::Matrix3d covariance_; //!< Covariance of the point.
bool is_converged_; //!< True if the point is converged.
bool is_normal_initialized_; //!< True if the normal is initialized.
bool has_ref_patch_; //!< True if the point has a reference patch.
Feature *ref_patch; //!< Reference patch of the point.
VisualPoint(const Vector3d &pos);
~VisualPoint();
void findMinScoreFeature(const Vector3d &framepos, Feature *&ftr) const;
void deleteNonRefPatchFeatures();
void deleteFeatureRef(Feature *ftr);
void addFrameRef(Feature *ftr);
bool getCloseViewObs(const Vector3d &pos, Feature *&obs, const Vector2d &cur_px) const;
};
#endif // LIVO_POINT_H_
src/FAST-LIVO2/include/voxel_map.h
+259
View File
@@ -0,0 +1,259 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#ifndef VOXEL_MAP_H_
#define VOXEL_MAP_H_
#include "common_lib.h"
#include <Eigen/Dense>
#include <fstream>
#include <math.h>
#include <mutex>
#include <omp.h>
#include <pcl/common/io.h>
#include <rclcpp/rclcpp.hpp>
#include <thread>
#include <unistd.h>
#include <unordered_map>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#define VOXELMAP_HASH_P 116101
#define VOXELMAP_MAX_N 10000000000
static int voxel_plane_id = 0;
typedef struct VoxelMapConfig
{
double max_voxel_size_;
int max_layer_;
int max_iterations_;
std::vector<int64_t> layer_init_num_;
int max_points_num_;
double planner_threshold_;
double beam_err_;
double dept_err_;
double sigma_num_;
bool is_pub_plane_map_;
// config of local map sliding
double sliding_thresh;
bool map_sliding_en;
int half_map_size;
} VoxelMapConfig;
typedef struct PointToPlane
{
Eigen::Vector3d point_b_;
Eigen::Vector3d point_w_;
Eigen::Vector3d normal_;
Eigen::Vector3d center_;
Eigen::Matrix<double, 6, 6> plane_var_;
M3D body_cov_;
int layer_;
double d_;
double eigen_value_;
bool is_valid_;
float dis_to_plane_;
} PointToPlane;
typedef struct VoxelPlane
{
Eigen::Vector3d center_;
Eigen::Vector3d normal_;
Eigen::Vector3d y_normal_;
Eigen::Vector3d x_normal_;
Eigen::Matrix3d covariance_;
Eigen::Matrix<double, 6, 6> plane_var_;
float radius_ = 0;
float min_eigen_value_ = 1;
float mid_eigen_value_ = 1;
float max_eigen_value_ = 1;
float d_ = 0;
int points_size_ = 0;
bool is_plane_ = false;
bool is_init_ = false;
int id_ = 0;
bool is_update_ = false;
VoxelPlane()
{
plane_var_ = Eigen::Matrix<double, 6, 6>::Zero();
covariance_ = Eigen::Matrix3d::Zero();
center_ = Eigen::Vector3d::Zero();
normal_ = Eigen::Vector3d::Zero();
}
} VoxelPlane;
class VOXEL_LOCATION
{
public:
int64_t x, y, z;
VOXEL_LOCATION(int64_t vx = 0, int64_t vy = 0, int64_t vz = 0) : x(vx), y(vy), z(vz) {}
bool operator==(const VOXEL_LOCATION &other) const { return (x == other.x && y == other.y && z == other.z); }
};
// Hash value
namespace std
{
template <> struct hash<VOXEL_LOCATION>
{
int64_t operator()(const VOXEL_LOCATION &s) const
{
using std::hash;
using std::size_t;
return ((((s.z) * VOXELMAP_HASH_P) % VOXELMAP_MAX_N + (s.y)) * VOXELMAP_HASH_P) % VOXELMAP_MAX_N + (s.x);
}
};
} // namespace std
struct DS_POINT
{
float xyz[3];
float intensity;
int count = 0;
};
void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_inc, Eigen::Matrix3d &cov);
class VoxelOctoTree
{
public:
VoxelOctoTree() = default;
std::vector<pointWithVar> temp_points_;
VoxelPlane *plane_ptr_;
int layer_;
int octo_state_; // 0 is end of tree, 1 is not
VoxelOctoTree *leaves_[8];
double voxel_center_[3]; // x, y, z
std::vector<int> layer_init_num_;
float quater_length_;
float planer_threshold_;
int points_size_threshold_;
int update_size_threshold_;
int max_points_num_;
int max_layer_;
int new_points_;
bool init_octo_;
bool update_enable_;
VoxelOctoTree(int max_layer, int layer, int points_size_threshold, int max_points_num, float planer_threshold)
: max_layer_(max_layer), layer_(layer), points_size_threshold_(points_size_threshold), max_points_num_(max_points_num),
planer_threshold_(planer_threshold)
{
temp_points_.clear();
octo_state_ = 0;
new_points_ = 0;
update_size_threshold_ = 5;
init_octo_ = false;
update_enable_ = true;
for (int i = 0; i < 8; i++)
{
leaves_[i] = nullptr;
}
plane_ptr_ = new VoxelPlane;
}
~VoxelOctoTree()
{
for (int i = 0; i < 8; i++)
{
delete leaves_[i];
}
delete plane_ptr_;
}
void init_plane(const std::vector<pointWithVar> &points, VoxelPlane *plane);
void init_octo_tree();
void cut_octo_tree();
void UpdateOctoTree(const pointWithVar &pv);
VoxelOctoTree *find_correspond(Eigen::Vector3d pw);
VoxelOctoTree *Insert(const pointWithVar &pv);
};
void loadVoxelConfig(rclcpp::Node::SharedPtr &node, VoxelMapConfig &voxel_config);
class VoxelMapManager
{
public:
VoxelMapManager() = default;
VoxelMapConfig config_setting_;
int current_frame_id_ = 0;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr voxel_map_pub_;
std::unordered_map<VOXEL_LOCATION, VoxelOctoTree *> voxel_map_;
PointCloudXYZI::Ptr feats_undistort_;
PointCloudXYZI::Ptr feats_down_body_;
PointCloudXYZI::Ptr feats_down_world_;
M3D extR_;
V3D extT_;
float build_residual_time, ekf_time;
float ave_build_residual_time = 0.0;
float ave_ekf_time = 0.0;
int scan_count = 0;
StatesGroup state_;
V3D position_last_;
V3D last_slide_position = {0,0,0};
geometry_msgs::msg::Quaternion geoQuat_;
int feats_down_size_;
int effct_feat_num_;
std::vector<M3D> cross_mat_list_;
std::vector<M3D> body_cov_list_;
std::vector<pointWithVar> pv_list_;
std::vector<PointToPlane> ptpl_list_;
VoxelMapManager(VoxelMapConfig &config_setting, std::unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &voxel_map)
: config_setting_(config_setting), voxel_map_(voxel_map)
{
current_frame_id_ = 0;
feats_undistort_.reset(new PointCloudXYZI());
feats_down_body_.reset(new PointCloudXYZI());
feats_down_world_.reset(new PointCloudXYZI());
};
void StateEstimation(StatesGroup &state_propagat);
void TransformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud,
pcl::PointCloud<pcl::PointXYZI>::Ptr &trans_cloud);
void BuildVoxelMap();
V3F RGBFromVoxel(const V3D &input_point);
void UpdateVoxelMap(const std::vector<pointWithVar> &input_points);
void BuildResidualListOMP(std::vector<pointWithVar> &pv_list, std::vector<PointToPlane> &ptpl_list);
void build_single_residual(pointWithVar &pv, const VoxelOctoTree *current_octo, const int current_layer, bool &is_sucess, double &prob,
PointToPlane &single_ptpl);
void pubVoxelMap();
void mapSliding();
void clearMemOutOfMap(const int& x_max,const int& x_min,const int& y_max,const int& y_min,const int& z_max,const int& z_min );
private:
void GetUpdatePlane(const VoxelOctoTree *current_octo, const int pub_max_voxel_layer, std::vector<VoxelPlane> &plane_list);
void pubSinglePlane(visualization_msgs::msg::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane, const float alpha,
const Eigen::Vector3d rgb);
void CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec, geometry_msgs::msg::Quaternion &q);
void mapJet(double v, double vmin, double vmax, uint8_t &r, uint8_t &g, uint8_t &b);
};
typedef std::shared_ptr<VoxelMapManager> VoxelMapManagerPtr;
#endif // VOXEL_MAP_H_
src/FAST-LIVO2/launch/mapping_avia_marslvig.launch.py
Executable
+111
View File
@@ -0,0 +1,111 @@
#!/usr/bin/python3
# -- coding: utf-8 --**
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
def generate_launch_description():
# Find path
config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config")
rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "M300.rviz")
#Load parameters
avia_config_cmd = os.path.join(config_file_dir, "MARS_LVIG.yaml")
camera_config_cmd = os.path.join(config_file_dir, "camera_MARS_LVIG.yaml")
# Param use_rviz
use_rviz_arg = DeclareLaunchArgument(
"use_rviz",
default_value="False",
description="Whether to launch Rviz2",
)
avia_config_arg = DeclareLaunchArgument(
'avia_params_file',
default_value=avia_config_cmd,
description='Full path to the ROS2 parameters file to use for fast_livo2 nodes',
)
camera_config_arg = DeclareLaunchArgument(
'camera_params_file',
default_value=camera_config_cmd,
description='Full path to the ROS2 parameters file to use for vikit_ros nodes',
)
# https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40
use_respawn_arg = DeclareLaunchArgument(
'use_respawn',
default_value='True',
description='Whether to respawn if a node crashes. Applied when composition is disabled.')
avia_params_file = LaunchConfiguration('avia_params_file')
camera_params_file = LaunchConfiguration('camera_params_file')
use_respawn = LaunchConfiguration('use_respawn')
return LaunchDescription([
use_rviz_arg,
avia_config_arg,
camera_config_arg,
use_respawn_arg,
# use parameter_blackboard as global parameters server and load camera params
Node(
package='demo_nodes_cpp',
executable='parameter_blackboard',
name='parameter_blackboard',
# namespace='laserMapping',
parameters=[
camera_params_file,
],
output='screen'
),
# republish compressed image to raw image
# https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2
# ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image
Node(
package="image_transport",
executable="republish",
name="republish",
arguments=[ # Array of strings/parametric arguments that will end up in process's argv
'compressed',
'raw',
],
remappings=[
("in", "/left_camera/image"),
("out", "/left_camera/image")
],
output="screen",
respawn=use_respawn,
),
Node(
package="fast_livo",
executable="fastlivo_mapping",
name="laserMapping",
parameters=[
avia_params_file,
],
# https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html
prefix=[
# ("gdb -ex run --args"),
# ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes")
],
output="screen"
),
Node(
condition=IfCondition(LaunchConfiguration("use_rviz")),
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
output="screen"
),
])
src/FAST-LIVO2/launch/mapping_aviz.launch.py
+117
View File
@@ -0,0 +1,117 @@
#!/usr/bin/python3
# -- coding: utf-8 --**
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
def generate_launch_description():
# Find path
config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config")
rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "fast_livo2.rviz")
#Load parameters
avia_config_cmd = os.path.join(config_file_dir, "avia.yaml")
camera_config_cmd = os.path.join(config_file_dir, "camera_pinhole.yaml")
# Param use_rviz
use_rviz_arg = DeclareLaunchArgument(
"use_rviz",
default_value="False",
description="Whether to launch Rviz2",
)
avia_config_arg = DeclareLaunchArgument(
'avia_params_file',
default_value=avia_config_cmd,
description='Full path to the ROS2 parameters file to use for fast_livo2 nodes',
)
camera_config_arg = DeclareLaunchArgument(
'camera_params_file',
default_value=camera_config_cmd,
description='Full path to the ROS2 parameters file to use for vikit_ros nodes',
)
# https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40
use_respawn_arg = DeclareLaunchArgument(
'use_respawn',
default_value='True',
description='Whether to respawn if a node crashes. Applied when composition is disabled.')
avia_params_file = LaunchConfiguration('avia_params_file')
camera_params_file = LaunchConfiguration('camera_params_file')
use_respawn = LaunchConfiguration('use_respawn')
return LaunchDescription([
use_rviz_arg,
avia_config_arg,
camera_config_arg,
use_respawn_arg,
# play ros2 bag
# ExecuteProcess(
# cmd=[['ros2 bag play ', '~/datasets/Retail_Street ', '--clock ', "-l"]],
# shell=True
# ),
# use parameter_blackboard as global parameters server and load camera params
Node(
package='demo_nodes_cpp',
executable='parameter_blackboard',
name='parameter_blackboard',
# namespace='laserMapping',
parameters=[
camera_params_file,
],
output='screen'
),
# republish compressed image to raw image
# https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2
# ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image
Node(
package="image_transport",
executable="republish",
name="republish",
arguments=[ # Array of strings/parametric arguments that will end up in process's argv
'compressed',
'raw',
],
remappings=[
("in", "/left_camera/image"),
("out", "/left_camera/image")
],
output="screen",
respawn=use_respawn,
),
Node(
package="fast_livo",
executable="fastlivo_mapping",
name="laserMapping",
parameters=[
avia_params_file,
],
# https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html
prefix=[
# ("gdb -ex run --args"),
# ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes")
],
output="screen"
),
Node(
condition=IfCondition(LaunchConfiguration("use_rviz")),
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
output="screen"
),
])
src/FAST-LIVO2/launch/mapping_hesaixt32_hilti22.launch
+14
View File
@@ -0,0 +1,14 @@
<launch>
<arg name="rviz" default="true" />
<rosparam command="load" file="$(find fast_livo)/config/HILTI22.yaml" />
<node pkg="fast_livo" type="fastlivo_mapping" name="laserMapping" output="screen">
<rosparam file="$(find fast_livo)/config/camera_fisheye_HILTI22.yaml" />
</node>
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_livo)/rviz_cfg/hilti.rviz" />
</group>
</launch>
src/FAST-LIVO2/launch/mapping_mid360s.launch.py
+99
View File
@@ -0,0 +1,99 @@
#!/usr/bin/python3
# -- coding: utf-8 --**
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch.substitutions import Command
def generate_launch_description():
# Find path
pkg_dir = get_package_share_directory("fast_livo")
config_file_dir = os.path.join(pkg_dir, "config")
rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz")
urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf")
# Load parameters
mid360s_config_cmd = os.path.join(config_file_dir, "mid360s.yaml")
camera_config_cmd = os.path.join(config_file_dir, "camera_mid360s.yaml")
use_rviz_arg = DeclareLaunchArgument(
"use_rviz",
default_value="False",
description="Whether to launch Rviz2",
)
mid360s_config_arg = DeclareLaunchArgument(
'mid360s_params_file',
default_value=mid360s_config_cmd,
description='Full path to the ROS2 parameters file for fast_livo2 (MID-360S)',
)
camera_config_arg = DeclareLaunchArgument(
'camera_params_file',
default_value=camera_config_cmd,
description='Full path to the ROS2 parameters file for camera intrinsics',
)
use_respawn_arg = DeclareLaunchArgument(
'use_respawn',
default_value='True',
description='Whether to respawn if a node crashes.',
)
mid360s_params_file = LaunchConfiguration('mid360s_params_file')
camera_params_file = LaunchConfiguration('camera_params_file')
use_respawn = LaunchConfiguration('use_respawn')
return LaunchDescription([
use_rviz_arg,
mid360s_config_arg,
camera_config_arg,
use_respawn_arg,
# Robot model (URDF) — aft_mapped 기준 센서 TF 정적 발행
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[{
'robot_description': open(urdf_file).read()
}],
output='screen'
),
# Camera params are read remotely from parameter_blackboard by vikit camera_loader
Node(
package='demo_nodes_cpp',
executable='parameter_blackboard',
name='parameter_blackboard',
parameters=[camera_params_file],
output='screen'
),
Node(
package="fast_livo",
executable="fastlivo_mapping",
name="laserMapping",
parameters=[mid360s_params_file],
# Force system libusb over /opt/MVS bundled version (which lacks libusb_set_option)
additional_env={
"LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0"
},
output="screen"
),
Node(
condition=IfCondition(LaunchConfiguration("use_rviz")),
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
output="screen"
),
])
src/FAST-LIVO2/launch/mapping_mid360s_cam1.launch.py
+94
View File
@@ -0,0 +1,94 @@
#!/usr/bin/python3
# -- coding: utf-8 --**
# cam1 전용 launch — mid360s.yaml + extrin_cam1.yaml + camera_mid360s.yaml (8mm)
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
def generate_launch_description():
pkg_dir = get_package_share_directory("fast_livo")
config_file_dir = os.path.join(pkg_dir, "config")
rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz")
urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf")
mid360s_config_cmd = os.path.join(config_file_dir, "mid360s.yaml")
extrin_config_cmd = os.path.join(config_file_dir, "extrin_cam1.yaml")
camera_config_cmd = os.path.join(config_file_dir, "camera_cam1.yaml")
use_rviz_arg = DeclareLaunchArgument(
"use_rviz", default_value="False", description="Whether to launch Rviz2"
)
mid360s_config_arg = DeclareLaunchArgument(
"mid360s_params_file", default_value=mid360s_config_cmd,
description="Full path to mid360s.yaml"
)
extrin_config_arg = DeclareLaunchArgument(
"extrin_params_file", default_value=extrin_config_cmd,
description="Full path to cam1 extrinsic override yaml"
)
camera_config_arg = DeclareLaunchArgument(
"camera_params_file", default_value=camera_config_cmd,
description="Full path to camera intrinsics yaml (cam1, 8mm, 1440x1080 scale=0.5)"
)
use_respawn_arg = DeclareLaunchArgument(
"use_respawn", default_value="True", description="Whether to respawn if a node crashes."
)
mid360s_params_file = LaunchConfiguration("mid360s_params_file")
extrin_params_file = LaunchConfiguration("extrin_params_file")
camera_params_file = LaunchConfiguration("camera_params_file")
use_respawn = LaunchConfiguration("use_respawn")
return LaunchDescription([
use_rviz_arg,
mid360s_config_arg,
extrin_config_arg,
camera_config_arg,
use_respawn_arg,
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
parameters=[{"robot_description": open(urdf_file).read()}],
output="screen"
),
Node(
package="demo_nodes_cpp",
executable="parameter_blackboard",
name="parameter_blackboard",
parameters=[camera_params_file],
output="screen"
),
Node(
package="fast_livo",
executable="fastlivo_mapping",
name="laserMapping",
parameters=[
mid360s_params_file,
extrin_params_file,
{"common": {"img_topic": "/camera/image"}},
],
additional_env={
"LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0"
},
output="screen"
),
Node(
condition=IfCondition(LaunchConfiguration("use_rviz")),
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
output="screen"
),
])
src/FAST-LIVO2/launch/mapping_mid360s_cam2.launch.py
+94
View File
@@ -0,0 +1,94 @@
#!/usr/bin/python3
# -- coding: utf-8 --**
# cam2 전용 launch — mid360s.yaml + extrin_cam2.yaml + camera_mid360s.yaml (8mm)
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
def generate_launch_description():
pkg_dir = get_package_share_directory("fast_livo")
config_file_dir = os.path.join(pkg_dir, "config")
rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz")
urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf")
mid360s_config_cmd = os.path.join(config_file_dir, "mid360s.yaml")
extrin_config_cmd = os.path.join(config_file_dir, "extrin_cam2.yaml")
camera_config_cmd = os.path.join(config_file_dir, "camera_mid360s.yaml")
use_rviz_arg = DeclareLaunchArgument(
"use_rviz", default_value="False", description="Whether to launch Rviz2"
)
mid360s_config_arg = DeclareLaunchArgument(
"mid360s_params_file", default_value=mid360s_config_cmd,
description="Full path to mid360s.yaml"
)
extrin_config_arg = DeclareLaunchArgument(
"extrin_params_file", default_value=extrin_config_cmd,
description="Full path to cam2 extrinsic override yaml"
)
camera_config_arg = DeclareLaunchArgument(
"camera_params_file", default_value=camera_config_cmd,
description="Full path to camera intrinsics yaml (cam2, 8mm)"
)
use_respawn_arg = DeclareLaunchArgument(
"use_respawn", default_value="True", description="Whether to respawn if a node crashes."
)
mid360s_params_file = LaunchConfiguration("mid360s_params_file")
extrin_params_file = LaunchConfiguration("extrin_params_file")
camera_params_file = LaunchConfiguration("camera_params_file")
use_respawn = LaunchConfiguration("use_respawn")
return LaunchDescription([
use_rviz_arg,
mid360s_config_arg,
extrin_config_arg,
camera_config_arg,
use_respawn_arg,
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
parameters=[{"robot_description": open(urdf_file).read()}],
output="screen"
),
Node(
package="demo_nodes_cpp",
executable="parameter_blackboard",
name="parameter_blackboard",
parameters=[camera_params_file],
output="screen"
),
Node(
package="fast_livo",
executable="fastlivo_mapping",
name="laserMapping",
parameters=[
mid360s_params_file,
extrin_params_file,
{"common": {"img_topic": "/camera/image"}},
],
additional_env={
"LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0"
},
output="screen"
),
Node(
condition=IfCondition(LaunchConfiguration("use_rviz")),
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
output="screen"
),
])
src/FAST-LIVO2/launch/mapping_mid360s_cam3.launch.py
+107
View File
@@ -0,0 +1,107 @@
#!/usr/bin/python3
# -- coding: utf-8 --**
# cam3 (4mm lens) 전용 launch — mid360s.yaml + camera_cam3.yaml
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
def generate_launch_description():
pkg_dir = get_package_share_directory("fast_livo")
config_file_dir = os.path.join(pkg_dir, "config")
rviz_config_file = os.path.join(pkg_dir, "rviz_cfg", "fast_livo2.rviz")
urdf_file = os.path.join(pkg_dir, "urdf", "fori_robot.urdf")
mid360s_config_cmd = os.path.join(config_file_dir, "mid360s.yaml")
extrin_config_cmd = os.path.join(config_file_dir, "extrin_cam3.yaml")
camera_config_cmd = os.path.join(config_file_dir, "camera_cam3.yaml")
use_rviz_arg = DeclareLaunchArgument(
"use_rviz",
default_value="False",
description="Whether to launch Rviz2",
)
mid360s_config_arg = DeclareLaunchArgument(
'mid360s_params_file',
default_value=mid360s_config_cmd,
description='Full path to the ROS2 parameters file for fast_livo2 (MID-360S)',
)
extrin_config_arg = DeclareLaunchArgument(
'extrin_params_file',
default_value=extrin_config_cmd,
description='Full path to cam3 extrinsic override yaml',
)
camera_config_arg = DeclareLaunchArgument(
'camera_params_file',
default_value=camera_config_cmd,
description='Full path to the ROS2 parameters file for camera intrinsics (cam3, 4mm)',
)
use_respawn_arg = DeclareLaunchArgument(
'use_respawn',
default_value='True',
description='Whether to respawn if a node crashes.',
)
mid360s_params_file = LaunchConfiguration('mid360s_params_file')
extrin_params_file = LaunchConfiguration('extrin_params_file')
camera_params_file = LaunchConfiguration('camera_params_file')
use_respawn = LaunchConfiguration('use_respawn')
return LaunchDescription([
use_rviz_arg,
mid360s_config_arg,
extrin_config_arg,
camera_config_arg,
use_respawn_arg,
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[{
'robot_description': open(urdf_file).read()
}],
output='screen'
),
Node(
package='demo_nodes_cpp',
executable='parameter_blackboard',
name='parameter_blackboard',
parameters=[camera_params_file],
output='screen'
),
Node(
package="fast_livo",
executable="fastlivo_mapping",
name="laserMapping",
parameters=[
mid360s_params_file,
extrin_params_file,
{"common": {"img_topic": "/camera3/image"}},
],
additional_env={
"LD_PRELOAD": "/usr/lib/x86_64-linux-gnu/libusb-1.0.so.0"
},
output="screen"
),
Node(
condition=IfCondition(LaunchConfiguration("use_rviz")),
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
output="screen"
),
])
src/FAST-LIVO2/launch/mapping_ouster_ntu.launch.py
+117
View File
@@ -0,0 +1,117 @@
#!/usr/bin/python3
# -- coding: utf-8 --**
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
def generate_launch_description():
# Find path
config_file_dir = os.path.join(get_package_share_directory("fast_livo"), "config")
rviz_config_file = os.path.join(get_package_share_directory("fast_livo"), "rviz_cfg", "ntu_viral.rviz")
#Load parameters
avia_config_cmd = os.path.join(config_file_dir, "NTU_VIRAL.yaml")
camera_config_cmd = os.path.join(config_file_dir, "camera_NTU_VIRAL.yaml")
# Param use_rviz
use_rviz_arg = DeclareLaunchArgument(
"use_rviz",
default_value="False",
description="Whether to launch Rviz2",
)
avia_config_arg = DeclareLaunchArgument(
'avia_params_file',
default_value=avia_config_cmd,
description='Full path to the ROS2 parameters file to use for fast_livo2 nodes',
)
camera_config_arg = DeclareLaunchArgument(
'camera_params_file',
default_value=camera_config_cmd,
description='Full path to the ROS2 parameters file to use for vikit_ros nodes',
)
# https://github.com/ros-navigation/navigation2/blob/1c68c212db01f9f75fcb8263a0fbb5dfa711bdea/nav2_bringup/launch/navigation_launch.py#L40
use_respawn_arg = DeclareLaunchArgument(
'use_respawn',
default_value='True',
description='Whether to respawn if a node crashes. Applied when composition is disabled.')
avia_params_file = LaunchConfiguration('avia_params_file')
camera_params_file = LaunchConfiguration('camera_params_file')
use_respawn = LaunchConfiguration('use_respawn')
return LaunchDescription([
use_rviz_arg,
avia_config_arg,
camera_config_arg,
use_respawn_arg,
# play ros2 bag
# ExecuteProcess(
# cmd=[['ros2 bag play ', '~/datasets/Retail_Street ', '--clock ', "-l"]],
# shell=True
# ),
# use parameter_blackboard as global parameters server and load camera params
Node(
package='demo_nodes_cpp',
executable='parameter_blackboard',
name='parameter_blackboard',
# namespace='laserMapping',
parameters=[
camera_params_file,
],
output='screen'
),
# republish compressed image to raw image
# https://robotics.stackexchange.com/questions/110939/how-do-i-remap-compressed-video-to-raw-video-in-ros2
# ros2 run image_transport republish compressed raw --ros-args --remap in:=/left_camera/image --remap out:=/left_camera/image
Node(
package="image_transport",
executable="republish",
name="republish",
arguments=[ # Array of strings/parametric arguments that will end up in process's argv
'compressed',
'raw',
],
remappings=[
("in", "/left_camera/image"),
("out", "/left_camera/image")
],
output="screen",
respawn=use_respawn,
),
Node(
package="fast_livo",
executable="fastlivo_mapping",
name="laserMapping",
parameters=[
avia_params_file,
],
# https://docs.ros.org/en/humble/How-To-Guides/Getting-Backtraces-in-ROS-2.html
prefix=[
# ("gdb -ex run --args"),
# ("valgrind --log-file=./valgrind_report.log --tool=memcheck --leak-check=full --show-leak-kinds=all -s --track-origins=yes --show-reachable=yes --undef-value-errors=yes --track-fds=yes")
],
output="screen"
),
Node(
condition=IfCondition(LaunchConfiguration("use_rviz")),
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
output="screen"
),
])
src/FAST-LIVO2/package.xml
Executable
+57
View File
@@ -0,0 +1,57 @@
<?xml version="1.0"?>
<package format="3">
<name>fast_livo</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>
<build_depend>rclcpp</build_depend>
<build_depend>rclpy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>livox_ros_driver2</build_depend>
<build_depend>vikit_common</build_depend>
<build_depend>vikit_ros</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>libopencv-dev</build_depend>
<build_depend>sophus</build_depend>
<build_depend>eigen</build_depend>
<build_depend>fmt</build_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>image_transport</exec_depend>
<exec_depend>libopencv-dev</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
<!-- https://github.com/ros-perception/image_transport_tutorials/blob/humble/package.xml -->
<!-- <image_transport plugin="${prefix}/resized_plugins.xml"/> -->
</export>
</package>
src/FAST-LIVO2/pics/Framework.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.6 MiB

src/FAST-LIVO2/pics/debug_error.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 143 KiB

src/FAST-LIVO2/pics/rosgraph.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 123 KiB

src/FAST-LIVO2/rviz_cfg/M300.rviz
Executable
+671
View File
@@ -0,0 +1,671 @@
Panels:
- Class: rviz_common/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Status1
- /Axes1
- /mapping1
- /mapping1/currPoints1
- /mapping1/surround1
- /mapping1/surround1/Autocompute Value Bounds1
- /mapping1/PointCloud21
- /Odometry1
- /Odometry1/Odometry1
- /Odometry1/Odometry1/Shape1
- /Path1
- /currPoints1/Autocompute Value Bounds1
- /MarkerArray1/Namespaces1
- /currPoints2/Autocompute Value Bounds1
- /Odometry2/Shape1
- /MarkerArray3
- /MarkerArray4
- /MarkerArray5
- /Image1
Splitter Ratio: 0.34272301197052
Tree Height: 538
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Pose Estimate1
- /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
- Class: rviz_common/Time
Name: Time
SyncMode: 0
SyncSource: surround
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 1
Cell Size: 1
Class: rviz_default_plugins/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: 160
Reference Frame: <Fixed Frame>
Value: false
- Alpha: 1
Class: rviz_default_plugins/Axes
Enabled: true
Length: 4
Name: Axes
Radius: 1.2000000476837158
Reference Frame: <Fixed Frame>
Show Trail: false
Value: true
- Class: rviz_common/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 199; 228; 247
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 2
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Alpha: 0.5
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 15
Min Value: -5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 238; 238; 236
Color Transformer: RGB8
Decay Time: 10000
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 159
Min Color: 0; 0; 0
Min Intensity: 5
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 1
Size (m): 0.004999999888241291
Style: Points
Topic: /cloud_registered
Unreliable: true
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: 239; 41; 41
Color Transformer: ""
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): 5
Size (m): 0.019999999552965164
Style: Squares
Topic: /cloud_effected
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Name: mapping
- Class: rviz_common/Group
Displays:
- Angle Tolerance: 0.009999999776482582
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: 1
Name: Odometry
Position Tolerance: 0.0010000000474974513
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 5
Axes Radius: 1
Color: 255; 85; 0
Head Length: 0
Head Radius: 0
Shaft Length: 0.800000011920929
Shaft Radius: 0.5
Value: Axes
Topic: /aft_mapped_to_init
Unreliable: false
Value: true
Enabled: true
Name: Odometry
- Alpha: 0
Buffer Length: 2
Class: rviz_default_plugins/Path
Color: 25; 255; 255
Enabled: true
Head Diameter: 0
Head Length: 0
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.699999988079071
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.4000000059604645
Shaft Length: 0.4000000059604645
Topic: /path
Unreliable: false
Value: true
- Alpha: 0.10000000149011612
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 239; 41; 41
Color Transformer: Intensity
Decay Time: 1000
Enabled: false
Invert Rainbow: true
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 1
Selectable: true
Size (Pixels): 2
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_voxel
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz_default_plugins/Marker
Enabled: true
Marker Topic: /planner_normal
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Marker Topic: /voxels
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- 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: 245; 121; 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: 1
Selectable: true
Size (Pixels): 15
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_ray_sub_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Marker Topic: /visualization_marker
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- 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: 92; 53; 102
Color Transformer: FlatColor
Decay Time: 99999
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): 10
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_visual_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- 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: 115; 210; 22
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 12
Size (m): 0.05000000074505806
Style: Points
Topic: /cloud_visual_sub_map
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 237; 212; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 5
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_sample_points
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: false
- 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: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 99999
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 239; 41; 41
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 4
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_visual_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- 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: 92; 53; 102
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): 20
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_ray_sub_map_fov
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Angle Tolerance: 0
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: false
Keep: 1
Name: Odometry
Position Tolerance: 0
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 0.699999988079071
Axes Radius: 0.20000000298023224
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /aft_mapped_to_init
Unreliable: false
Value: false
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Marker Topic: /waypoint_planner/visualize
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Marker Topic: /fsm_node/visualization/exp_traj
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Marker Topic: /fsm_node/visualization/exp_sfcs
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz_default_plugins/Image
Enabled: true
Image Topic: /rgb_img
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Enabled: true
Global Options:
Background Color: 0; 0; 0
Default Light: true
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
- Class: rviz_default_plugins/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz_default_plugins/SetGoal
Topic: /move_base_simple/goal
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz_default_plugins/ThirdPersonFollower
Distance: 582.7694702148438
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 463.3948974609375
Y: -4.546019554138184
Z: -4.951948722009547e-05
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: -0.26979732513427734
Target Frame: drone
Yaw: 3.1317780017852783
Saved:
- Class: rviz_default_plugins/Orbit
Distance: 117.53474426269531
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: -35.713138580322266
Y: 36.932613372802734
Z: 4.459701061248779
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: far1
Near Clip Distance: 0.009999999776482582
Pitch: 0.19539840519428253
Target Frame: <Fixed Frame>
Yaw: 0.17540442943572998
- Class: rviz_default_plugins/Orbit
Distance: 109.3125
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: -22.092714309692383
Y: 63.322662353515625
Z: 14.125411987304688
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: far2
Near Clip Distance: 0.009999999776482582
Pitch: 0.035398442298173904
Target Frame: <Fixed Frame>
Yaw: 5.793589115142822
- Class: rviz_default_plugins/Orbit
Distance: 85.65605163574219
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 28.252656936645508
Y: -35.49672317504883
Z: -36.31112289428711
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: near1
Near Clip Distance: 0.009999999776482582
Pitch: 0.5653983950614929
Target Frame: <Fixed Frame>
Yaw: 0.9104044437408447
- Class: rviz_default_plugins/Orbit
Distance: 60.1053581237793
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 30.61589241027832
Y: 29.98663330078125
Z: -12.290168762207031
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: near2
Near Clip Distance: 0.009999999776482582
Pitch: 0.315398633480072
Target Frame: <Fixed Frame>
Yaw: 5.788588047027588
Window Geometry:
Displays:
collapsed: false
Height: 1376
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 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
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 2488
X: 72
Y: 27
src/FAST-LIVO2/rviz_cfg/fast_livo2.rviz
Executable
+782
View File
@@ -0,0 +1,782 @@
Panels:
- Class: rviz_common/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Axes1
- /mapping1
- /mapping1/currPoints1
- /mapping1/surround1
- /mapping1/surround1/Autocompute Value Bounds1
- /mapping1/PointCloud21
- /Odometry1
- /Odometry1/Odometry1
- /Odometry1/Odometry1/Shape1
- /Path1
- /currPoints1/Autocompute Value Bounds1
- /Marker1
- /MarkerArray1/Namespaces1
- /currPoints2/Autocompute Value Bounds1
- /Odometry2/Shape1
- /MarkerArray3
- /MarkerArray4
- /MarkerArray5
- /Image1
Splitter Ratio: 0.5394402146339417
Tree Height: 360
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Pose Estimate1
- /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: surround
Visualization Manager:
Class: ""
Displays:
- Alpha: 1
Cell Size: 1
Class: rviz_default_plugins/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: 160
Reference Frame: <Fixed Frame>
Value: false
- Class: rviz_default_plugins/Axes
Enabled: true
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Reference Frame: camera_init
Value: true
- Class: rviz_common/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: currPoints
Position Transformer: XYZ
Selectable: true
Size (Pixels): 4
Size (m): 0.009999999776482582
Style: Points
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: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 15
Min Value: -5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 238; 238; 236
Color Transformer: RGB8
Decay Time: 10000
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 159
Min Color: 0; 0; 0
Min Intensity: 5
Name: surround
Position Transformer: XYZ
Selectable: false
Size (Pixels): 1
Size (m): 0.004999999888241291
Style: Points
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: 239; 41; 41
Color Transformer: ""
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 5
Size (m): 0.019999999552965164
Style: 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: false
Enabled: true
Name: mapping
- Class: rviz_common/Group
Displays:
- Angle Tolerance: 0.009999999776482582
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: 1
Name: Odometry
Position Tolerance: 0.0010000000474974513
Shape:
Alpha: 1
Axes Length: 0.5
Axes Radius: 0.15000000596046448
Color: 255; 85; 0
Head Length: 0
Head Radius: 0
Shaft Length: 0.800000011920929
Shaft Radius: 0.5
Value: Axes
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /aft_mapped_to_init
Value: true
Enabled: true
Name: Odometry
- Alpha: 0
Buffer Length: 2
Class: rviz_default_plugins/Path
Color: 25; 255; 255
Enabled: true
Head Diameter: 0
Head Length: 0
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.03999999910593033
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:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /path
Value: true
- Alpha: 0.10000000149011612
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 239; 41; 41
Color Transformer: Intensity
Decay Time: 1000
Enabled: false
Invert Rainbow: true
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: currPoints
Position Transformer: XYZ
Selectable: true
Size (Pixels): 2
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cloud_voxel
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz_default_plugins/Marker
Enabled: true
Name: Marker
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planner_normal
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: visualization_marker_array
Value: false
- 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: 245; 121; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 15
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cloud_ray_sub_map
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: visualization_marker_array
Value: false
- 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: 92; 53; 102
Color Transformer: FlatColor
Decay Time: 99999
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 10
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cloud_visual_map
Use Fixed Frame: true
Use rainbow: true
Value: false
- 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: 115; 210; 22
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: surround
Position Transformer: XYZ
Selectable: false
Size (Pixels): 12
Size (m): 0.05000000074505806
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cloud_visual_sub_map
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 237; 212; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: currPoints
Position Transformer: XYZ
Selectable: true
Size (Pixels): 5
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cloud_sample_points
Use Fixed Frame: true
Use rainbow: false
Value: false
- 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: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 99999
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 239; 41; 41
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 4
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cloud_visual_map
Use Fixed Frame: true
Use rainbow: true
Value: false
- 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: 92; 53; 102
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 20
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cloud_ray_sub_map_fov
Use Fixed Frame: true
Use rainbow: true
Value: false
- Angle Tolerance: 0
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: false
Keep: 1
Name: Odometry
Position Tolerance: 0
Shape:
Alpha: 1
Axes Length: 0.699999988079071
Axes Radius: 0.20000000298023224
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Axes
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /aft_mapped_to_init
Value: false
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: visualization_marker_array
Value: false
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: visualization_marker_array
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: visualization_marker_array
Value: false
- Class: rviz_default_plugins/Image
Enabled: true
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /rgb_img
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: /move_base_simple/goal
- 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/ThirdPersonFollower
Distance: 50.66728210449219
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5.083343982696533
Y: -9.8687162399292
Z: -3.4865479392465204e-05
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.3547965884208679
Target Frame: drone
Value: ThirdPersonFollower (rviz_default_plugins)
Yaw: 2.5417795181274414
Saved:
- Class: rviz_default_plugins/Orbit
Distance: 117.53474426269531
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -35.713138580322266
Y: 36.932613372802734
Z: 4.459701061248779
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: far1
Near Clip Distance: 0.009999999776482582
Pitch: 0.19539840519428253
Target Frame: <Fixed Frame>
Value: Orbit (rviz_default_plugins)
Yaw: 0.17540442943572998
- Class: rviz_default_plugins/Orbit
Distance: 109.3125
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -22.092714309692383
Y: 63.322662353515625
Z: 14.125411987304688
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: far2
Near Clip Distance: 0.009999999776482582
Pitch: 0.035398442298173904
Target Frame: <Fixed Frame>
Value: Orbit (rviz_default_plugins)
Yaw: 5.793589115142822
- Class: rviz_default_plugins/Orbit
Distance: 85.65605163574219
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 28.252656936645508
Y: -35.49672317504883
Z: -36.31112289428711
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: near1
Near Clip Distance: 0.009999999776482582
Pitch: 0.5653983950614929
Target Frame: <Fixed Frame>
Value: Orbit (rviz_default_plugins)
Yaw: 0.9104044437408447
- Class: rviz_default_plugins/Orbit
Distance: 60.1053581237793
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 30.61589241027832
Y: 29.98663330078125
Z: -12.290168762207031
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: near2
Near Clip Distance: 0.009999999776482582
Pitch: 0.315398633480072
Target Frame: <Fixed Frame>
Value: Orbit (rviz_default_plugins)
Yaw: 5.788588047027588
Window Geometry:
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 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
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1920
X: 0
Y: 27
src/FAST-LIVO2/rviz_cfg/hilti.rviz
Executable
+672
View File
@@ -0,0 +1,672 @@
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Axes1
- /mapping1
- /mapping1/currPoints1
- /mapping1/surround1
- /mapping1/surround1/Autocompute Value Bounds1
- /mapping1/PointCloud21
- /Odometry1
- /Odometry1/Odometry1
- /Odometry1/Odometry1/Shape1
- /Path1
- /currPoints1/Autocompute Value Bounds1
- /MarkerArray1/Namespaces1
- /currPoints2/Autocompute Value Bounds1
- /Odometry2/Shape1
- /MarkerArray3
- /MarkerArray4
- /MarkerArray5
- /Image1
Splitter Ratio: 0.34272301197052
Tree Height: 538
- 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
Name: Time
SyncMode: 0
SyncSource: surround
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 1
Cell Size: 1
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: 160
Reference Frame: <Fixed Frame>
Value: false
- Alpha: 1
Class: rviz/Axes
Enabled: true
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Show Trail: false
Value: true
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 4
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Alpha: 0.10000000149011612
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 15
Min Value: -5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 238; 238; 236
Color Transformer: RGB8
Decay Time: 10000
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 159
Min Color: 0; 0; 0
Min Intensity: 5
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 1
Size (m): 0.004999999888241291
Style: Points
Topic: /cloud_registered
Unreliable: true
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: 239; 41; 41
Color Transformer: ""
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): 5
Size (m): 0.019999999552965164
Style: Squares
Topic: /cloud_effected
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
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 0.5
Axes Radius: 0.15000000596046448
Color: 255; 85; 0
Head Length: 0
Head Radius: 0
Shaft Length: 0.800000011920929
Shaft Radius: 0.5
Value: Axes
Topic: /aft_mapped_to_init
Unreliable: false
Value: true
Enabled: true
Name: Odometry
- 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.03999999910593033
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.4000000059604645
Shaft Length: 0.4000000059604645
Topic: /path
Unreliable: false
Value: true
- Alpha: 0.10000000149011612
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 239; 41; 41
Color Transformer: Intensity
Decay Time: 1000
Enabled: false
Invert Rainbow: true
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 1
Selectable: true
Size (Pixels): 2
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_voxel
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz/Marker
Enabled: true
Marker Topic: /planner_normal
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /voxels
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- 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: 245; 121; 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: 1
Selectable: true
Size (Pixels): 15
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_ray_sub_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /visualization_marker
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- 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: 92; 53; 102
Color Transformer: FlatColor
Decay Time: 99999
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): 10
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_visual_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- 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: 115; 210; 22
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 12
Size (m): 0.05000000074505806
Style: Points
Topic: /cloud_visual_sub_map
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 237; 212; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 5
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_sample_points
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: false
- 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: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 99999
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 239; 41; 41
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 4
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_visual_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- 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: 92; 53; 102
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): 20
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_ray_sub_map_fov
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Angle Tolerance: 0
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: false
Keep: 1
Name: Odometry
Position Tolerance: 0
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 0.699999988079071
Axes Radius: 0.20000000298023224
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /aft_mapped_to_init
Unreliable: false
Value: false
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /waypoint_planner/visualize
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /fsm_node/visualization/exp_traj
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /fsm_node/visualization/exp_sfcs
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz/Image
Enabled: true
Image Topic: /rgb_img
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Enabled: true
Global Options:
Background Color: 238; 238; 236
Default Light: true
Fixed Frame: camera_init
Frame Rate: 30
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/ThirdPersonFollower
Distance: 44.92388153076172
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: -2.7518839836120605
Y: 2.672811508178711
Z: -5.34896862518508e-05
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: true
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.1397969275712967
Target Frame: drone
Yaw: 1.5631110668182373
Saved:
- Class: rviz/Orbit
Distance: 117.53474426269531
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: -35.713138580322266
Y: 36.932613372802734
Z: 4.459701061248779
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: far1
Near Clip Distance: 0.009999999776482582
Pitch: 0.19539840519428253
Target Frame: <Fixed Frame>
Yaw: 0.17540442943572998
- Class: rviz/Orbit
Distance: 109.3125
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: -22.092714309692383
Y: 63.322662353515625
Z: 14.125411987304688
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: far2
Near Clip Distance: 0.009999999776482582
Pitch: 0.035398442298173904
Target Frame: <Fixed Frame>
Yaw: 5.793589115142822
- Class: rviz/Orbit
Distance: 85.65605163574219
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 28.252656936645508
Y: -35.49672317504883
Z: -36.31112289428711
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: near1
Near Clip Distance: 0.009999999776482582
Pitch: 0.5653983950614929
Target Frame: <Fixed Frame>
Yaw: 0.9104044437408447
- Class: rviz/Orbit
Distance: 60.1053581237793
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 30.61589241027832
Y: 29.98663330078125
Z: -12.290168762207031
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: near2
Near Clip Distance: 0.009999999776482582
Pitch: 0.315398633480072
Target Frame: <Fixed Frame>
Yaw: 5.788588047027588
Window Geometry:
Displays:
collapsed: false
Height: 1376
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 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
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 2488
X: 72
Y: 27
src/FAST-LIVO2/rviz_cfg/ntu_viral.rviz
Executable
+671
View File
@@ -0,0 +1,671 @@
Panels:
- Class: rviz_common/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Status1
- /Axes1
- /mapping1
- /mapping1/currPoints1
- /mapping1/surround1
- /mapping1/surround1/Autocompute Value Bounds1
- /mapping1/PointCloud21
- /Odometry1
- /Odometry1/Odometry1
- /Odometry1/Odometry1/Shape1
- /Path1
- /currPoints1/Autocompute Value Bounds1
- /MarkerArray1/Namespaces1
- /currPoints2/Autocompute Value Bounds1
- /Odometry2/Shape1
- /MarkerArray3
- /MarkerArray4
- /MarkerArray5
- /Image1
Splitter Ratio: 0.34272301197052
Tree Height: 538
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Pose Estimate1
- /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
- Class: rviz_common/Time
Name: Time
SyncMode: 0
SyncSource: surround
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 1
Cell Size: 1
Class: rviz_default_plugins/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: 160
Reference Frame: <Fixed Frame>
Value: false
- Alpha: 1
Class: rviz_default_plugins/Axes
Enabled: true
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Show Trail: false
Value: true
- Class: rviz_common/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 4
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Alpha: 0.5
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 15
Min Value: -5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 238; 238; 236
Color Transformer: RGB8
Decay Time: 10000
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 159
Min Color: 0; 0; 0
Min Intensity: 5
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 1
Size (m): 0.004999999888241291
Style: Points
Topic: /cloud_registered
Unreliable: true
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: 239; 41; 41
Color Transformer: ""
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): 5
Size (m): 0.019999999552965164
Style: Squares
Topic: /cloud_effected
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Name: mapping
- Class: rviz_common/Group
Displays:
- Angle Tolerance: 0.009999999776482582
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: 1
Name: Odometry
Position Tolerance: 0.0010000000474974513
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 0.5
Axes Radius: 0.15000000596046448
Color: 255; 85; 0
Head Length: 0
Head Radius: 0
Shaft Length: 0.800000011920929
Shaft Radius: 0.5
Value: Axes
Topic: /aft_mapped_to_init
Unreliable: false
Value: true
Enabled: true
Name: Odometry
- Alpha: 0
Buffer Length: 2
Class: rviz_default_plugins/Path
Color: 25; 255; 255
Enabled: true
Head Diameter: 0
Head Length: 0
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.03999999910593033
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.4000000059604645
Shaft Length: 0.4000000059604645
Topic: /path
Unreliable: false
Value: true
- Alpha: 0.10000000149011612
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 239; 41; 41
Color Transformer: Intensity
Decay Time: 1000
Enabled: false
Invert Rainbow: true
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 1
Selectable: true
Size (Pixels): 2
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_voxel
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz_default_plugins/Marker
Enabled: true
Marker Topic: /planner_normal
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Marker Topic: /voxels
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- 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: 245; 121; 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: 1
Selectable: true
Size (Pixels): 15
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_ray_sub_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Marker Topic: /visualization_marker
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- 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: 92; 53; 102
Color Transformer: FlatColor
Decay Time: 99999
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): 10
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_visual_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- 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: 115; 210; 22
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 12
Size (m): 0.05000000074505806
Style: Points
Topic: /cloud_visual_sub_map
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 20
Min Value: -3
Value: false
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 237; 212; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 5
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_sample_points
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: false
- 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: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 99999
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 239; 41; 41
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 4
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_visual_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- 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: 92; 53; 102
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): 20
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_ray_sub_map_fov
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Angle Tolerance: 0
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: false
Keep: 1
Name: Odometry
Position Tolerance: 0
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 0.699999988079071
Axes Radius: 0.20000000298023224
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /aft_mapped_to_init
Unreliable: false
Value: false
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Marker Topic: /waypoint_planner/visualize
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Marker Topic: /fsm_node/visualization/exp_traj
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Marker Topic: /fsm_node/visualization/exp_sfcs
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz_default_plugins/Image
Enabled: true
Image Topic: /rgb_img
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Enabled: true
Global Options:
Background Color: 0; 0; 0
Default Light: true
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
- Class: rviz_default_plugins/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz_default_plugins/SetGoal
Topic: /move_base_simple/goal
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz_default_plugins/ThirdPersonFollower
Distance: 65.96137237548828
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: -2.06162166595459
Y: 2.7847142219543457
Z: -2.219532325398177e-05
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: true
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.4797966480255127
Target Frame: drone
Yaw: 3.251800537109375
Saved:
- Class: rviz_default_plugins/Orbit
Distance: 117.53474426269531
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: -35.713138580322266
Y: 36.932613372802734
Z: 4.459701061248779
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: far1
Near Clip Distance: 0.009999999776482582
Pitch: 0.19539840519428253
Target Frame: <Fixed Frame>
Yaw: 0.17540442943572998
- Class: rviz_default_plugins/Orbit
Distance: 109.3125
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: -22.092714309692383
Y: 63.322662353515625
Z: 14.125411987304688
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: far2
Near Clip Distance: 0.009999999776482582
Pitch: 0.035398442298173904
Target Frame: <Fixed Frame>
Yaw: 5.793589115142822
- Class: rviz_default_plugins/Orbit
Distance: 85.65605163574219
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 28.252656936645508
Y: -35.49672317504883
Z: -36.31112289428711
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: near1
Near Clip Distance: 0.009999999776482582
Pitch: 0.5653983950614929
Target Frame: <Fixed Frame>
Yaw: 0.9104044437408447
- Class: rviz_default_plugins/Orbit
Distance: 60.1053581237793
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 30.61589241027832
Y: 29.98663330078125
Z: -12.290168762207031
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: near2
Near Clip Distance: 0.009999999776482582
Pitch: 0.315398633480072
Target Frame: <Fixed Frame>
Yaw: 5.788588047027588
Window Geometry:
Displays:
collapsed: false
Height: 1376
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 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
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 2488
X: 72
Y: 27
src/FAST-LIVO2/scripts/colmap_output.sh
Executable
+25
View File
@@ -0,0 +1,25 @@
#!/bin/bash
TARGET_DIRS=(
"$(rospack find fast_livo)/Log/Colmap/images"
"$(rospack find fast_livo)/Log/Colmap/sparse/0"
)
for dir in "${TARGET_DIRS[@]}"; do
if [ -d "$dir" ]; then
rm -rf "$dir"
echo "Removed: $dir"
else
echo "Not found: $dir"
fi
done
for dir in "${TARGET_DIRS[@]}"; do
if [ ! -d "$dir" ]; then
mkdir -p "$dir"
echo "Created: $dir"
else
echo "Exists: $dir"
fi
done
src/FAST-LIVO2/scripts/mesh.py
+114
View File
@@ -0,0 +1,114 @@
import os
import glob
import vdbfusion
import numpy as np
import open3d as o3d
from scipy.spatial import KDTree
# ------------------------- Key Parameters -------------------------
# Dataset path
SOURCE_DIR = "/home/chunran/Downloads/" # Replace with your dataset path
# VDBVolume parameters
VOXEL_SIZE = 0.02 # Voxel size (smaller values increase precision but require more memory)
SDF_TRUNC = 0.1 # Truncation distance for SDF (affects surface thickness, typically a multiple of voxel size)
MIN_WEIGHT = 0.1 # Minimum weight for mesh extraction (filters out noisy voxels)
# ------------------------- Dataset Class -------------------------
class Dataset:
def __init__(self, folder: str):
super().__init__()
# Get all .pcd files in the folder
self.scan_files = glob.glob(os.path.join(folder, "*.pcd"))
# Initialize poses as identity matrices
self.poses = np.array([np.eye(4) for _ in range(len(self.scan_files))])
def __getitem__(self, idx):
if idx >= len(self.scan_files):
raise IndexError("Index out of range")
# Compute relative pose
pose = np.linalg.inv(self.poses[0]) @ self.poses[idx]
# Read point cloud
points, colors = self.read_pcd(self.scan_files[idx])
points = np.array(points, dtype=np.float64)
return points, colors, pose
def __len__(self):
return len(self.scan_files)
def read_pcd(self, pcd_file):
# Read .pcd file using Open3D
pcd = o3d.io.read_point_cloud(pcd_file)
# Extract point cloud coordinates
points = np.asarray(pcd.points)
# Extract colors (if available)
if pcd.has_colors():
colors = np.asarray(pcd.colors) # Open3D colors are in range [0, 1]
else:
colors = np.zeros_like(points) # If no colors, fill with zeros
return points, colors
# ------------------------- Main Program -------------------------
if __name__ == '__main__':
# Initialize VDBVolume
print("Initializing VDBVolume...")
vdb_volume = vdbfusion.VDBVolume(voxel_size=VOXEL_SIZE, sdf_trunc=SDF_TRUNC)
# Load dataset
print("Loading dataset from", SOURCE_DIR)
dataset = Dataset(SOURCE_DIR)
# Integrate all point clouds into the VDBVolume
print("Integrating point clouds into VDBVolume...")
for i in range(len(dataset)):
scan, colors, origin = dataset[i]
vdb_volume.integrate(scan, origin)
print("Point cloud integration complete!")
# Extract triangle mesh
print("Extracting triangle mesh...")
vert, tri = vdb_volume.extract_triangle_mesh(min_weight=MIN_WEIGHT)
# Create Open3D mesh object
print("Creating Open3D mesh object...")
mesh = o3d.geometry.TriangleMesh(
o3d.utility.Vector3dVector(vert),
o3d.utility.Vector3iVector(tri),
)
# Save the mesh
print("Saving the mesh to output_mesh.ply...")
o3d.io.write_triangle_mesh("mesh.ply", mesh)
print("Mesh saved successfully.")
# ------------------------- Colorize Mesh Vertices -------------------------
print("Starting mesh colorization...")
if dataset[0][1] is not None: # Check if color information exists
# Combine all point cloud points and colors
pcd_points = np.vstack([dataset[i][0] for i in range(len(dataset))])
pcd_colors = np.vstack([dataset[i][1] for i in range(len(dataset))])
# Use KDTree to find the nearest point for each vertex
kdtree = KDTree(pcd_points)
_, indices = kdtree.query(vert) # Find the nearest point cloud point for each vertex
vertex_colors = pcd_colors[indices] # Assign colors
# Set mesh vertex colors
mesh.vertex_colors = o3d.utility.Vector3dVector(vertex_colors)
print("Mesh colorization complete!")
# Compute vertex normals
print("Computing vertex normals...")
mesh.compute_vertex_normals()
# Save the textured mesh
print("Saving the textured mesh to textured_mesh.ply...")
o3d.io.write_triangle_mesh("textured_mesh.ply", mesh)
print("Textured mesh saved successfully.")
# Visualize the final colorized mesh
print("Visualizing the colorized mesh...")
o3d.visualization.draw_geometries([mesh])
src/FAST-LIVO2/src/IMU_Processing.cpp
Executable
+590
View File
@@ -0,0 +1,590 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#include "IMU_Processing.h"
#include <rcpputils/asserts.hpp>
const bool time_list(PointType &x, PointType &y) { return (x.curvature < y.curvature); }
ImuProcess::ImuProcess() : Eye3d(M3D::Identity()),
Zero3d(0, 0, 0), b_first_frame(true), imu_need_init(true)
{
init_iter_num = 1;
cov_acc = V3D(0.1, 0.1, 0.1);
cov_gyr = V3D(0.1, 0.1, 0.1);
cov_bias_gyr = V3D(0.1, 0.1, 0.1);
cov_bias_acc = V3D(0.1, 0.1, 0.1);
cov_inv_expo = 0.2;
mean_acc = V3D(0, 0, -1.0);
mean_gyr = V3D(0, 0, 0);
angvel_last = Zero3d;
acc_s_last = Zero3d;
Lid_offset_to_IMU = Zero3d;
Lid_rot_to_IMU = Eye3d;
last_imu.reset(new sensor_msgs::msg::Imu());
cur_pcl_un_.reset(new PointCloudXYZI());
}
ImuProcess::~ImuProcess() {}
void ImuProcess::Reset()
{
RCLCPP_WARN(rclcpp::get_logger(""), "Reset ImuProcess");
mean_acc = V3D(0, 0, -1.0);
mean_gyr = V3D(0, 0, 0);
angvel_last = Zero3d;
imu_need_init = true;
init_iter_num = 1;
IMUpose.clear();
last_imu.reset(new sensor_msgs::msg::Imu());
cur_pcl_un_.reset(new PointCloudXYZI());
}
void ImuProcess::disable_imu()
{
cout << "IMU Disabled !!!!!" << endl;
imu_en = false;
imu_need_init = false;
}
void ImuProcess::disable_gravity_est()
{
cout << "Online Gravity Estimation Disabled !!!!!" << endl;
gravity_est_en = false;
}
void ImuProcess::disable_bias_est()
{
cout << "Bias Estimation Disabled !!!!!" << endl;
ba_bg_est_en = false;
}
void ImuProcess::disable_exposure_est()
{
cout << "Online Time Offset Estimation Disabled !!!!!" << endl;
exposure_estimate_en = false;
}
void ImuProcess::set_extrinsic(const MD(4, 4) & T)
{
Lid_offset_to_IMU = T.block<3, 1>(0, 3);
Lid_rot_to_IMU = T.block<3, 3>(0, 0);
}
void ImuProcess::set_extrinsic(const V3D &transl)
{
Lid_offset_to_IMU = transl;
Lid_rot_to_IMU.setIdentity();
}
void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot)
{
Lid_offset_to_IMU = transl;
Lid_rot_to_IMU = rot;
}
void ImuProcess::set_gyr_cov_scale(const V3D &scaler) { cov_gyr = scaler; }
void ImuProcess::set_acc_cov_scale(const V3D &scaler) { cov_acc = scaler; }
void ImuProcess::set_gyr_bias_cov(const V3D &b_g) { cov_bias_gyr = b_g; }
void ImuProcess::set_inv_expo_cov(const double &inv_expo) { cov_inv_expo = inv_expo; }
void ImuProcess::set_acc_bias_cov(const V3D &b_a) { cov_bias_acc = b_a; }
void ImuProcess::set_imu_init_frame_num(const int &num) { MAX_INI_COUNT = num; }
void ImuProcess::IMU_init(const MeasureGroup &meas, StatesGroup &state_inout, int &N)
{
/** 1. initializing the gravity, gyro bias, acc and gyro covariance
** 2. normalize the acceleration measurenments to unit gravity **/
RCLCPP_INFO(rclcpp::get_logger(""),"IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100);
V3D cur_acc, cur_gyr;
if (b_first_frame)
{
Reset();
N = 1;
b_first_frame = false;
const auto &imu_acc = meas.imu.front()->linear_acceleration;
const auto &gyr_acc = meas.imu.front()->angular_velocity;
mean_acc << imu_acc.x, imu_acc.y, imu_acc.z;
mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
// first_lidar_time = meas.lidar_frame_beg_time;
// cout<<"init acc norm: "<<mean_acc.norm()<<endl;
}
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++;
}
IMU_mean_acc_norm = mean_acc.norm();
state_inout.gravity = -mean_acc / mean_acc.norm() * G_m_s2;
state_inout.rot_end = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
state_inout.bias_g = Zero3d; // mean_gyr;
last_imu = meas.imu.back();
}
void ImuProcess::Forward_without_imu(LidarMeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out)
{
pcl_out = *(meas.lidar);
/*** sort point clouds by offset time ***/
const double &pcl_beg_time = meas.lidar_frame_beg_time;
sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000);
meas.last_lio_update_time = pcl_end_time;
const double &pcl_end_offset_time = pcl_out.points.back().curvature / double(1000);
MD(DIM_STATE, DIM_STATE) F_x, cov_w;
double dt = 0;
if (b_first_frame)
{
dt = 0.1;
b_first_frame = false;
}
else { dt = pcl_beg_time - time_last_scan; }
time_last_scan = pcl_beg_time;
// for (size_t i = 0; i < pcl_out->points.size(); i++) {
// if (dt < pcl_out->points[i].curvature) {
// dt = pcl_out->points[i].curvature;
// }
// }
// dt = dt / (double)1000;
// std::cout << "dt:" << dt << std::endl;
// double dt = pcl_out->points.back().curvature / double(1000);
/* covariance propagation */
// M3D acc_avr_skew;
M3D Exp_f = Exp(state_inout.bias_g, dt);
F_x.setIdentity();
cov_w.setZero();
F_x.block<3, 3>(0, 0) = Exp(state_inout.bias_g, -dt);
F_x.block<3, 3>(0, 10) = Eye3d * dt;
F_x.block<3, 3>(3, 7) = Eye3d * dt;
// F_x.block<3, 3>(6, 0) = - R_imu * acc_avr_skew * dt;
// F_x.block<3, 3>(6, 12) = - R_imu * dt;
// F_x.block<3, 3>(6, 15) = Eye3d * dt;
cov_w.block<3, 3>(10, 10).diagonal() = cov_gyr * dt * dt; // for omega in constant model
cov_w.block<3, 3>(7, 7).diagonal() = cov_acc * dt * dt; // for velocity in constant model
// cov_w.block<3, 3>(6, 6) =
// R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt;
// cov_w.block<3, 3>(9, 9).diagonal() =
// cov_bias_gyr * dt * dt; // bias gyro covariance
// cov_w.block<3, 3>(12, 12).diagonal() =
// cov_bias_acc * dt * dt; // bias acc covariance
// std::cout << "before propagete:" << state_inout.cov.diagonal().transpose()
// << std::endl;
state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w;
// std::cout << "cov_w:" << cov_w.diagonal().transpose() << std::endl;
// std::cout << "after propagete:" << state_inout.cov.diagonal().transpose()
// << std::endl;
state_inout.rot_end = state_inout.rot_end * Exp_f;
state_inout.pos_end = state_inout.pos_end + state_inout.vel_end * dt;
if (lidar_type != L515)
{
auto it_pcl = pcl_out.points.end() - 1;
double dt_j = 0.0;
for(; it_pcl != pcl_out.points.begin(); it_pcl--)
{
dt_j= pcl_end_offset_time - it_pcl->curvature/double(1000);
M3D R_jk(Exp(state_inout.bias_g, - dt_j));
V3D P_j(it_pcl->x, it_pcl->y, it_pcl->z);
// Using rotation and translation to un-distort points
V3D p_jk;
p_jk = - state_inout.rot_end.transpose() * state_inout.vel_end * dt_j;
V3D P_compensate = R_jk * P_j + p_jk;
/// save Undistorted points and their rotation
it_pcl->x = P_compensate(0);
it_pcl->y = P_compensate(1);
it_pcl->z = P_compensate(2);
}
}
}
void ImuProcess::UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out)
{
double t0 = omp_get_wtime();
pcl_out.clear();
/*** add the imu of the last frame-tail to the of current frame-head ***/
MeasureGroup &meas = lidar_meas.measures.back();
// cout<<"meas.imu.size: "<<meas.imu.size()<<endl;
auto v_imu = meas.imu;
v_imu.push_front(last_imu);
const double &imu_beg_time = stamp2Sec(v_imu.front()->header.stamp);
const double &imu_end_time = stamp2Sec(v_imu.back()->header.stamp);
const double prop_beg_time = last_prop_end_time;
// printf("[ IMU ] undistort input size: %zu \n", lidar_meas.pcl_proc_cur->points.size());
// printf("[ IMU ] IMU data sequence size: %zu \n", meas.imu.size());
// printf("[ IMU ] lidar_scan_index_now: %d \n", lidar_meas.lidar_scan_index_now);
const double prop_end_time = lidar_meas.lio_vio_flg == LIO ? meas.lio_time : meas.vio_time;
/*** cut lidar point based on the propagation-start time and required
* propagation-end time ***/
// const double pcl_offset_time = (prop_end_time -
// lidar_meas.lidar_frame_beg_time) * 1000.; // the offset time w.r.t scan
// start time auto pcl_it = lidar_meas.pcl_proc_cur->points.begin() +
// lidar_meas.lidar_scan_index_now; auto pcl_it_end =
// lidar_meas.lidar->points.end(); printf("[ IMU ] pcl_it->curvature: %lf
// pcl_offset_time: %lf \n", pcl_it->curvature, pcl_offset_time); while
// (pcl_it != pcl_it_end && pcl_it->curvature <= pcl_offset_time)
// {
// pcl_wait_proc.push_back(*pcl_it);
// pcl_it++;
// lidar_meas.lidar_scan_index_now++;
// }
// cout<<"pcl_out.size(): "<<pcl_out.size()<<endl;
// cout<<"pcl_offset_time: "<<pcl_offset_time<<"pcl_it->curvature:
// "<<pcl_it->curvature<<endl;
// cout<<"lidar_meas.lidar_scan_index_now:"<<lidar_meas.lidar_scan_index_now<<endl;
// printf("[ IMU ] last propagation end time: %lf \n", lidar_meas.last_lio_update_time);
if (lidar_meas.lio_vio_flg == LIO)
{
pcl_wait_proc.resize(lidar_meas.pcl_proc_cur->points.size());
pcl_wait_proc = *(lidar_meas.pcl_proc_cur);
lidar_meas.lidar_scan_index_now = 0;
IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, state_inout.vel_end, state_inout.pos_end, state_inout.rot_end));
}
// printf("[ IMU ] pcl_wait_proc size: %zu \n", pcl_wait_proc.points.size());
// sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
// lidar_meas.debug_show();
// cout<<"UndistortPcl [ IMU ]: Process lidar from "<<prop_beg_time<<" to
// "<<prop_end_time<<", " \
// <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to
// "<<imu_end_time<<endl;
// cout<<"[ IMU ]: point size: "<<lidar_meas.lidar->points.size()<<endl;
/*** Initialize IMU pose ***/
// IMUpose.clear();
/*** forward propagation at each imu point ***/
V3D acc_imu(acc_s_last), angvel_avr(angvel_last), acc_avr, vel_imu(state_inout.vel_end), pos_imu(state_inout.pos_end);
// cout << "[ IMU ] input state: " << state_inout.vel_end.transpose() << " " << state_inout.pos_end.transpose() << endl;
M3D R_imu(state_inout.rot_end);
MD(DIM_STATE, DIM_STATE) F_x, cov_w;
double dt, dt_all = 0.0;
double offs_t;
// double imu_time;
double tau;
if (!imu_time_init)
{
// imu_time = stamp2Sec(v_imu.front()->header.stamp) - first_lidar_time;
// tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75);
tau = 1.0;
imu_time_init = true;
}
else
{
tau = state_inout.inv_expo_time;
// RCLCPP_ERROR_STREAM(rclcpp::get_logger(""),"tau: %.6f !!!!!!", tau);
}
// state_inout.cov(6, 6) = 0.01;
// RCLCPP_ERROR_STREAM(rclcpp::get_logger(""),"lidar_meas.lio_vio_flg");
// cout<<"lidar_meas.lio_vio_flg: "<<lidar_meas.lio_vio_flg<<endl;
switch (lidar_meas.lio_vio_flg)
{
case LIO:
case VIO:
dt = 0;
for (int i = 0; i < v_imu.size() - 1; i++)
{
auto head = v_imu[i];
auto tail = v_imu[i + 1];
if (stamp2Sec(tail->header.stamp) < prop_beg_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);
// angvel_avr<<tail->angular_velocity.x, tail->angular_velocity.y,
// 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);
// cout<<"angvel_avr: "<<angvel_avr.transpose()<<endl;
// cout<<"acc_avr: "<<acc_avr.transpose()<<endl;
// #ifdef DEBUG_PRINT
fout_imu << setw(10) << stamp2Sec(head->header.stamp) - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
// #endif
// imu_time = stamp2Sec(head->header.stamp) - first_lidar_time;
angvel_avr -= state_inout.bias_g;
acc_avr = acc_avr * G_m_s2 / mean_acc.norm() - state_inout.bias_a;
if (stamp2Sec(head->header.stamp) < prop_beg_time)
{
// printf("00 \n");
dt = stamp2Sec(tail->header.stamp) - last_prop_end_time;
offs_t = stamp2Sec(tail->header.stamp) - prop_beg_time;
}
else if (i != v_imu.size() - 2)
{
// printf("11 \n");
dt = stamp2Sec(tail->header.stamp) - stamp2Sec(head->header.stamp);
offs_t = stamp2Sec(tail->header.stamp) - prop_beg_time;
}
else
{
// printf("22 \n");
dt = prop_end_time - stamp2Sec(head->header.stamp);
offs_t = prop_end_time - prop_beg_time;
}
dt_all += dt;
// printf("[ LIO Propagation ] dt: %lf \n", dt);
/* covariance propagation */
M3D acc_avr_skew;
M3D Exp_f = Exp(angvel_avr, dt);
acc_avr_skew << SKEW_SYM_MATRX(acc_avr);
F_x.setIdentity();
cov_w.setZero();
F_x.block<3, 3>(0, 0) = Exp(angvel_avr, -dt);
if (ba_bg_est_en) F_x.block<3, 3>(0, 10) = -Eye3d * dt;
// F_x.block<3,3>(3,0) = R_imu * off_vel_skew * dt;
F_x.block<3, 3>(3, 7) = Eye3d * dt;
F_x.block<3, 3>(7, 0) = -R_imu * acc_avr_skew * dt;
if (ba_bg_est_en) F_x.block<3, 3>(7, 13) = -R_imu * dt;
if (gravity_est_en) F_x.block<3, 3>(7, 16) = Eye3d * dt;
// tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75);
// F_x(6,6) = 0.25 * 2 * CV_PI * 0.5 * cos(2 * CV_PI * 0.5 * imu_time) * (-tau*tau); F_x(18,18) = 0.00001;
if (exposure_estimate_en) cov_w(6, 6) = cov_inv_expo * dt * dt;
cov_w.block<3, 3>(0, 0).diagonal() = cov_gyr * dt * dt;
cov_w.block<3, 3>(7, 7) = R_imu * cov_acc.asDiagonal() * R_imu.transpose() * dt * dt;
cov_w.block<3, 3>(10, 10).diagonal() = cov_bias_gyr * dt * dt; // bias gyro covariance
cov_w.block<3, 3>(13, 13).diagonal() = cov_bias_acc * dt * dt; // bias acc covariance
state_inout.cov = F_x * state_inout.cov * F_x.transpose() + cov_w;
// state_inout.cov.block<18,18>(0,0) = F_x.block<18,18>(0,0) *
// state_inout.cov.block<18,18>(0,0) * F_x.block<18,18>(0,0).transpose() +
// cov_w.block<18,18>(0,0);
// tau = tau + 0.25 * 2 * CV_PI * 0.5 * cos(2 * CV_PI * 0.5 * imu_time) *
// (-tau*tau) * dt;
// tau = 1.0 / (0.25 * sin(2 * CV_PI * 0.5 * imu_time) + 0.75);
/* propogation of IMU attitude */
R_imu = R_imu * Exp_f;
/* Specific acceleration (global frame) of IMU */
acc_imu = R_imu * acc_avr + state_inout.gravity;
/* propogation of IMU */
pos_imu = pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt;
/* velocity of IMU */
vel_imu = vel_imu + acc_imu * dt;
/* save the poses at each IMU measurements */
angvel_last = angvel_avr;
acc_s_last = acc_imu;
// cout<<setw(20)<<"offset_t: "<<offs_t<<"stamp2Sec(tail->header.stamp):
// "<<stamp2Sec(tail->header.stamp)<<endl; printf("[ LIO Propagation ]
// offs_t: %lf \n", offs_t);
IMUpose.push_back(set_pose6d(offs_t, acc_imu, angvel_avr, vel_imu, pos_imu, R_imu));
}
// unbiased_gyr = V3D(IMUpose.back().gyr[0], IMUpose.back().gyr[1], IMUpose.back().gyr[2]);
// cout<<"prop end - start: "<<prop_end_time - prop_beg_time<<" dt_all: "<<dt_all<<endl;
lidar_meas.last_lio_update_time = prop_end_time;
// dt = prop_end_time - imu_end_time;
// printf("[ LIO Propagation ] dt: %lf \n", dt);
break;
}
state_inout.vel_end = vel_imu;
state_inout.rot_end = R_imu;
state_inout.pos_end = pos_imu;
state_inout.inv_expo_time = tau;
/*** calculated the pos and attitude prediction at the frame-end ***/
// if (imu_end_time>prop_beg_time)
// {
// double note = prop_end_time > imu_end_time ? 1.0 : -1.0;
// dt = note * (prop_end_time - imu_end_time);
// state_inout.vel_end = vel_imu + note * acc_imu * dt;
// state_inout.rot_end = R_imu * Exp(V3D(note * angvel_avr), dt);
// state_inout.pos_end = pos_imu + note * vel_imu * dt + note * 0.5 *
// acc_imu * dt * dt;
// }
// else
// {
// double note = prop_end_time > prop_beg_time ? 1.0 : -1.0;
// dt = note * (prop_end_time - prop_beg_time);
// state_inout.vel_end = vel_imu + note * acc_imu * dt;
// state_inout.rot_end = R_imu * Exp(V3D(note * angvel_avr), dt);
// state_inout.pos_end = pos_imu + note * vel_imu * dt + note * 0.5 *
// acc_imu * dt * dt;
// }
// cout<<"[ Propagation ] output state: "<<state_inout.vel_end.transpose() <<
// state_inout.pos_end.transpose()<<endl;
last_imu = v_imu.back();
last_prop_end_time = prop_end_time;
double t1 = omp_get_wtime();
// auto pos_liD_e = state_inout.pos_end + state_inout.rot_end *
// Lid_offset_to_IMU; auto R_liD_e = state_inout.rot_end * Lidar_R_to_IMU;
// cout<<"[ IMU ]: vel "<<state_inout.vel_end.transpose()<<" pos
// "<<state_inout.pos_end.transpose()<<"
// ba"<<state_inout.bias_a.transpose()<<" bg
// "<<state_inout.bias_g.transpose()<<endl; cout<<"propagated cov:
// "<<state_inout.cov.diagonal().transpose()<<endl;
// cout<<"UndistortPcl Time:";
// for (auto it = IMUpose.begin(); it != IMUpose.end(); ++it) {
// cout<<it->offset_time<<" ";
// }
// cout<<endl<<"UndistortPcl size:"<<IMUpose.size()<<endl;
// cout<<"Undistorted pcl_out.size: "<<pcl_out.size()
// <<"lidar_meas.size: "<<lidar_meas.lidar->points.size()<<endl;
if (pcl_wait_proc.points.size() < 1) return;
/*** undistort each lidar point (backward propagation), ONLY working for LIO
* update ***/
if (lidar_meas.lio_vio_flg == LIO)
{
auto it_pcl = pcl_wait_proc.points.end() - 1;
M3D extR_Ri(Lid_rot_to_IMU.transpose() * state_inout.rot_end.transpose());
V3D exrR_extT(Lid_rot_to_IMU.transpose() * Lid_offset_to_IMU);
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);
acc_imu << VEC_FROM_ARRAY(head->acc);
// cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
vel_imu << VEC_FROM_ARRAY(head->vel);
pos_imu << VEC_FROM_ARRAY(head->pos);
angvel_avr << VEC_FROM_ARRAY(head->gyr);
// printf("head->offset_time: %lf \n", head->offset_time);
// printf("it_pcl->curvature: %lf pt dt: %lf \n", it_pcl->curvature,
// it_pcl->curvature / double(1000) - head->offset_time);
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 */
M3D R_i(R_imu * Exp(angvel_avr, dt));
V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - state_inout.pos_end);
V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);
// V3D P_compensate = Lid_rot_to_IMU.transpose() *
// (state_inout.rot_end.transpose() * (R_i * (Lid_rot_to_IMU * P_i +
// Lid_offset_to_IMU) + T_ei) - Lid_offset_to_IMU);
V3D P_compensate = (extR_Ri * (R_i * (Lid_rot_to_IMU * P_i + Lid_offset_to_IMU) + T_ei) - exrR_extT);
/// 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_wait_proc.points.begin()) break;
}
}
pcl_out = pcl_wait_proc;
pcl_wait_proc.clear();
IMUpose.clear();
}
// printf("[ IMU ] time forward: %lf, backward: %lf.\n", t1 - t0, omp_get_wtime() - t1);
}
void ImuProcess::Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_)
{
double t1, t2, t3;
t1 = omp_get_wtime();
rcpputils::assert_true(lidar_meas.lidar != nullptr);
if (!imu_en)
{
Forward_without_imu(lidar_meas, stat, *cur_pcl_un_);
return;
}
MeasureGroup meas = lidar_meas.measures.back();
if (imu_need_init)
{
double pcl_end_time = lidar_meas.lio_vio_flg == LIO ? meas.lio_time : meas.vio_time;
// lidar_meas.last_lio_update_time = pcl_end_time;
if (meas.imu.empty()) { return; };
/// The very first lidar frame
IMU_init(meas, stat, init_iter_num);
imu_need_init = true;
last_imu = meas.imu.back();
if (init_iter_num > MAX_INI_COUNT)
{
// cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
imu_need_init = false;
RCLCPP_INFO(rclcpp::get_logger(""), "IMU Initials: Gravity: %.4f %.4f %.4f %.4f; acc covarience: "
"%.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f \n",
stat.gravity[0], stat.gravity[1], stat.gravity[2], mean_acc.norm(), cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1],
cov_gyr[2]);
RCLCPP_INFO(rclcpp::get_logger(""), "IMU Initials: ba covarience: %.8f %.8f %.8f; bg covarience: "
"%.8f %.8f %.8f",
cov_bias_acc[0], cov_bias_acc[1], cov_bias_acc[2], cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2]);
fout_imu.open(DEBUG_FILE_DIR("imu.txt"), ios::out);
}
return;
}
UndistortPcl(lidar_meas, stat, *cur_pcl_un_);
// cout << "[ IMU ] undistorted point num: " << cur_pcl_un_->size() << endl;
}
src/FAST-LIVO2/src/LIVMapper.cpp
Executable
+1449
View File
File diff suppressed because it is too large Load Diff
src/FAST-LIVO2/src/frame.cpp
+65
View File
@@ -0,0 +1,65 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#include <boost/bind/bind.hpp>
#include "feature.h"
#include "frame.h"
#include "visual_point.h"
#include <stdexcept>
#include <vikit/math_utils.h>
#include <vikit/performance_monitor.h>
#include <vikit/vision.h>
int Frame::frame_counter_ = 0;
Frame::Frame(vk::AbstractCamera *cam, const cv::Mat &img)
: id_(frame_counter_++),
cam_(cam)
{
initFrame(img);
}
Frame::~Frame()
{
std::for_each(fts_.begin(), fts_.end(), [&](Feature *i) { delete i; });
}
void Frame::initFrame(const cv::Mat &img)
{
if (img.empty()) { throw std::runtime_error("Frame: provided image is empty"); }
if (img.cols != cam_->width() || img.rows != cam_->height())
{
throw std::runtime_error("Frame: provided image has not the same size as the camera model");
}
if (img.type() != CV_8UC1) { throw std::runtime_error("Frame: provided image is not grayscale"); }
img_ = img;
}
/// Utility functions for the Frame class
namespace frame_utils
{
void createImgPyramid(const cv::Mat &img_level_0, int n_levels, ImgPyr &pyr)
{
pyr.resize(n_levels);
pyr[0] = img_level_0;
for (int i = 1; i < n_levels; ++i)
{
pyr[i] = cv::Mat(pyr[i - 1].rows / 2, pyr[i - 1].cols / 2, CV_8U);
vk::halfSample(pyr[i - 1], pyr[i]);
}
}
} // namespace frame_utils
src/FAST-LIVO2/src/main.cpp
Executable
+14
View File
@@ -0,0 +1,14 @@
#include "LIVMapper.h"
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
rclcpp::Node::SharedPtr nh;
image_transport::ImageTransport it_(nh);
LIVMapper mapper(nh, "laserMapping");
mapper.initializeSubscribersAndPublishers(nh, it_);
mapper.run(nh);
rclcpp::shutdown();
return 0;
}
src/FAST-LIVO2/src/preprocess.cpp
Executable
+1126
View File
File diff suppressed because it is too large Load Diff
src/FAST-LIVO2/src/utils.cpp
+19
View File
@@ -0,0 +1,19 @@
// utils.cpp
#include <vector>
#include <cstdint> // for int64_t
#include <limits> // for std::numeric_limits
#include <stdexcept> // for std::out_of_range
std::vector<int> convertToIntVectorSafe(const std::vector<int64_t>& int64_vector) {
std::vector<int> int_vector;
int_vector.reserve(int64_vector.size()); // 预留空间以提高效率
for (int64_t value : int64_vector) {
if (value < std::numeric_limits<int>::min() || value > std::numeric_limits<int>::max()) {
throw std::out_of_range("Value is out of range for int");
}
int_vector.push_back(static_cast<int>(value));
}
return int_vector;
}
src/FAST-LIVO2/src/vio.cpp
Executable
+1877
View File
File diff suppressed because it is too large Load Diff
src/FAST-LIVO2/src/visual_point.cpp
+127
View File
@@ -0,0 +1,127 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#include "visual_point.h"
#include "feature.h"
#include <stdexcept>
#include <vikit/math_utils.h>
VisualPoint::VisualPoint(const Vector3d &pos)
: pos_(pos), previous_normal_(Vector3d::Zero()), normal_(Vector3d::Zero()),
is_converged_(false), is_normal_initialized_(false), has_ref_patch_(false)
{
}
VisualPoint::~VisualPoint()
{
for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it)
{
delete(*it);
}
obs_.clear();
ref_patch = nullptr;
}
void VisualPoint::addFrameRef(Feature *ftr)
{
obs_.push_front(ftr);
}
void VisualPoint::deleteFeatureRef(Feature *ftr)
{
if (ref_patch == ftr)
{
ref_patch = nullptr;
has_ref_patch_ = false;
}
for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it)
{
if ((*it) == ftr)
{
delete((*it));
obs_.erase(it);
return;
}
}
}
bool VisualPoint::getCloseViewObs(const Vector3d &framepos, Feature *&ftr, const Vector2d &cur_px) const
{
// TODO: get frame with same point of view AND same pyramid level!
if (obs_.size() <= 0) return false;
Vector3d obs_dir(framepos - pos_);
obs_dir.normalize();
auto min_it = obs_.begin();
double min_cos_angle = 0;
for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it)
{
Vector3d dir((*it)->T_f_w_.inverse().translation() - pos_);
dir.normalize();
double cos_angle = obs_dir.dot(dir);
if (cos_angle > min_cos_angle)
{
min_cos_angle = cos_angle;
min_it = it;
}
}
ftr = *min_it;
// Vector2d ftr_px = ftr->px_;
// double pixel_dist = (cur_px-ftr_px).norm();
// if(pixel_dist > 200)
// {
// ROS_ERROR("The pixel dist exceeds 200.");
// return false;
// }
if (min_cos_angle < 0.5) // assume that observations larger than 60° are useless 0.5
{
// ROS_ERROR("The obseved angle is larger than 60°.");
return false;
}
return true;
}
void VisualPoint::findMinScoreFeature(const Vector3d &framepos, Feature *&ftr) const
{
auto min_it = obs_.begin();
float min_score = std::numeric_limits<float>::max();
for (auto it = obs_.begin(), ite = obs_.end(); it != ite; ++it)
{
if ((*it)->score_ < min_score)
{
min_score = (*it)->score_;
min_it = it;
}
}
ftr = *min_it;
}
void VisualPoint::deleteNonRefPatchFeatures()
{
for (auto it = obs_.begin(); it != obs_.end();)
{
if (*it != ref_patch)
{
delete *it;
it = obs_.erase(it);
}
else
{
++it;
}
}
}
src/FAST-LIVO2/src/voxel_map.cpp
+987
View File
@@ -0,0 +1,987 @@
/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/
#include "voxel_map.h"
using namespace Eigen;
void calcBodyCov(Eigen::Vector3d &pb, const float range_inc, const float degree_inc, Eigen::Matrix3d &cov)
{
if (pb[2] == 0) pb[2] = 0.0001;
float range = sqrt(pb[0] * pb[0] + pb[1] * pb[1] + pb[2] * pb[2]);
float range_var = range_inc * range_inc;
Eigen::Matrix2d direction_var;
direction_var << pow(sin(DEG2RAD(degree_inc)), 2), 0, 0, pow(sin(DEG2RAD(degree_inc)), 2);
Eigen::Vector3d direction(pb);
direction.normalize();
Eigen::Matrix3d direction_hat;
direction_hat << 0, -direction(2), direction(1), direction(2), 0, -direction(0), -direction(1), direction(0), 0;
Eigen::Vector3d base_vector1(1, 1, -(direction(0) + direction(1)) / direction(2));
base_vector1.normalize();
Eigen::Vector3d base_vector2 = base_vector1.cross(direction);
base_vector2.normalize();
Eigen::Matrix<double, 3, 2> N;
N << base_vector1(0), base_vector2(0), base_vector1(1), base_vector2(1), base_vector1(2), base_vector2(2);
Eigen::Matrix<double, 3, 2> A = range * direction_hat * N;
cov = direction * range_var * direction.transpose() + A * direction_var * A.transpose();
}
void loadVoxelConfig(rclcpp::Node::SharedPtr &node, VoxelMapConfig &voxel_config)
{
// declare parameter
node->declare_parameter<bool>("publish.pub_plane_en", false);
node->declare_parameter<int>("lio.max_layer", 1);
node->declare_parameter<double>("lio.voxel_size", 0.5);
node->declare_parameter<double>("lio.min_eigen_value", 0.01);
node->declare_parameter<double>("lio.sigma_num", 3);
node->declare_parameter<double>("lio.beam_err", 0.02);
node->declare_parameter<double>("lio.dept_err", 0.05);
// Declaration of parameter of type std::vector<int> won't build, https://github.com/ros2/rclcpp/issues/1585
node->declare_parameter<vector<int64_t>>("lio.layer_init_num", std::vector<int64_t>{5,5,5,5,5});
node->declare_parameter<int>("lio.max_points_num", 50);
node->declare_parameter<int>("lio.min_iterations", 5);
node->declare_parameter<bool>("local_map.map_sliding_en", false);
node->declare_parameter<int>("local_map.half_map_size", 100);
node->declare_parameter<double>("local_map.sliding_thresh", 8.0);
// get parameter
node->get_parameter("publish.pub_plane_en", voxel_config.is_pub_plane_map_);
node->get_parameter("lio.max_layer", voxel_config.max_layer_);
node->get_parameter("lio.voxel_size", voxel_config.max_voxel_size_);
node->get_parameter("lio.min_eigen_value", voxel_config.planner_threshold_);
node->get_parameter("lio.sigma_num", voxel_config.sigma_num_);
node->get_parameter("lio.beam_err", voxel_config.beam_err_);
node->get_parameter("lio.dept_err", voxel_config.dept_err_);
node->get_parameter("lio.layer_init_num", voxel_config.layer_init_num_);
node->get_parameter("lio.max_points_num", voxel_config.max_points_num_);
node->get_parameter("lio.min_iterations", voxel_config.max_iterations_);
node->get_parameter("local_map.map_sliding_en", voxel_config.map_sliding_en);
node->get_parameter("local_map.half_map_size", voxel_config.half_map_size);
node->get_parameter("local_map.sliding_thresh", voxel_config.sliding_thresh);
}
void VoxelOctoTree::init_plane(const std::vector<pointWithVar> &points, VoxelPlane *plane)
{
plane->plane_var_ = Eigen::Matrix<double, 6, 6>::Zero();
plane->covariance_ = Eigen::Matrix3d::Zero();
plane->center_ = Eigen::Vector3d::Zero();
plane->normal_ = Eigen::Vector3d::Zero();
plane->points_size_ = points.size();
plane->radius_ = 0;
for (auto pv : points)
{
plane->covariance_ += pv.point_w * pv.point_w.transpose();
plane->center_ += pv.point_w;
}
plane->center_ = plane->center_ / plane->points_size_;
plane->covariance_ = plane->covariance_ / plane->points_size_ - plane->center_ * plane->center_.transpose();
Eigen::EigenSolver<Eigen::Matrix3d> es(plane->covariance_);
Eigen::Matrix3cd evecs = es.eigenvectors();
Eigen::Vector3cd evals = es.eigenvalues();
Eigen::Vector3d evalsReal;
evalsReal = evals.real();
Eigen::Matrix3f::Index evalsMin, evalsMax;
evalsReal.rowwise().sum().minCoeff(&evalsMin);
evalsReal.rowwise().sum().maxCoeff(&evalsMax);
int evalsMid = 3 - evalsMin - evalsMax;
Eigen::Vector3d evecMin = evecs.real().col(evalsMin);
Eigen::Vector3d evecMid = evecs.real().col(evalsMid);
Eigen::Vector3d evecMax = evecs.real().col(evalsMax);
Eigen::Matrix3d J_Q;
J_Q << 1.0 / plane->points_size_, 0, 0, 0, 1.0 / plane->points_size_, 0, 0, 0, 1.0 / plane->points_size_;
// && evalsReal(evalsMid) > 0.05
//&& evalsReal(evalsMid) > 0.01
if (evalsReal(evalsMin) < planer_threshold_)
{
for (int i = 0; i < points.size(); i++)
{
Eigen::Matrix<double, 6, 3> J;
Eigen::Matrix3d F;
for (int m = 0; m < 3; m++)
{
if (m != (int)evalsMin)
{
Eigen::Matrix<double, 1, 3> F_m =
(points[i].point_w - plane->center_).transpose() / ((plane->points_size_) * (evalsReal[evalsMin] - evalsReal[m])) *
(evecs.real().col(m) * evecs.real().col(evalsMin).transpose() + evecs.real().col(evalsMin) * evecs.real().col(m).transpose());
F.row(m) = F_m;
}
else
{
Eigen::Matrix<double, 1, 3> F_m;
F_m << 0, 0, 0;
F.row(m) = F_m;
}
}
J.block<3, 3>(0, 0) = evecs.real() * F;
J.block<3, 3>(3, 0) = J_Q;
plane->plane_var_ += J * points[i].var * J.transpose();
}
plane->normal_ << evecs.real()(0, evalsMin), evecs.real()(1, evalsMin), evecs.real()(2, evalsMin);
plane->y_normal_ << evecs.real()(0, evalsMid), evecs.real()(1, evalsMid), evecs.real()(2, evalsMid);
plane->x_normal_ << evecs.real()(0, evalsMax), evecs.real()(1, evalsMax), evecs.real()(2, evalsMax);
plane->min_eigen_value_ = evalsReal(evalsMin);
plane->mid_eigen_value_ = evalsReal(evalsMid);
plane->max_eigen_value_ = evalsReal(evalsMax);
plane->radius_ = sqrt(evalsReal(evalsMax));
plane->d_ = -(plane->normal_(0) * plane->center_(0) + plane->normal_(1) * plane->center_(1) + plane->normal_(2) * plane->center_(2));
plane->is_plane_ = true;
plane->is_update_ = true;
if (!plane->is_init_)
{
plane->id_ = voxel_plane_id;
voxel_plane_id++;
plane->is_init_ = true;
}
}
else
{
plane->is_update_ = true;
plane->is_plane_ = false;
}
}
void VoxelOctoTree::init_octo_tree()
{
if (temp_points_.size() > points_size_threshold_)
{
init_plane(temp_points_, plane_ptr_);
if (plane_ptr_->is_plane_ == true)
{
octo_state_ = 0;
// new added
if (temp_points_.size() > max_points_num_)
{
update_enable_ = false;
std::vector<pointWithVar>().swap(temp_points_);
new_points_ = 0;
}
}
else
{
octo_state_ = 1;
cut_octo_tree();
}
init_octo_ = true;
new_points_ = 0;
}
}
void VoxelOctoTree::cut_octo_tree()
{
if (layer_ >= max_layer_)
{
octo_state_ = 0;
return;
}
for (size_t i = 0; i < temp_points_.size(); i++)
{
int xyz[3] = {0, 0, 0};
if (temp_points_[i].point_w[0] > voxel_center_[0]) { xyz[0] = 1; }
if (temp_points_[i].point_w[1] > voxel_center_[1]) { xyz[1] = 1; }
if (temp_points_[i].point_w[2] > voxel_center_[2]) { xyz[2] = 1; }
int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2];
if (leaves_[leafnum] == nullptr)
{
leaves_[leafnum] = new VoxelOctoTree(max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], max_points_num_, planer_threshold_);
leaves_[leafnum]->layer_init_num_ = layer_init_num_;
leaves_[leafnum]->voxel_center_[0] = voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_;
leaves_[leafnum]->voxel_center_[1] = voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_;
leaves_[leafnum]->voxel_center_[2] = voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_;
leaves_[leafnum]->quater_length_ = quater_length_ / 2;
}
leaves_[leafnum]->temp_points_.push_back(temp_points_[i]);
leaves_[leafnum]->new_points_++;
}
for (uint i = 0; i < 8; i++)
{
if (leaves_[i] != nullptr)
{
if (leaves_[i]->temp_points_.size() > leaves_[i]->points_size_threshold_)
{
init_plane(leaves_[i]->temp_points_, leaves_[i]->plane_ptr_);
if (leaves_[i]->plane_ptr_->is_plane_)
{
leaves_[i]->octo_state_ = 0;
// new added
if (leaves_[i]->temp_points_.size() > leaves_[i]->max_points_num_)
{
leaves_[i]->update_enable_ = false;
std::vector<pointWithVar>().swap(leaves_[i]->temp_points_);
new_points_ = 0;
}
}
else
{
leaves_[i]->octo_state_ = 1;
leaves_[i]->cut_octo_tree();
}
leaves_[i]->init_octo_ = true;
leaves_[i]->new_points_ = 0;
}
}
}
}
void VoxelOctoTree::UpdateOctoTree(const pointWithVar &pv)
{
if (!init_octo_)
{
new_points_++;
temp_points_.push_back(pv);
if (temp_points_.size() > points_size_threshold_) { init_octo_tree(); }
}
else
{
if (plane_ptr_->is_plane_)
{
if (update_enable_)
{
new_points_++;
temp_points_.push_back(pv);
if (new_points_ > update_size_threshold_)
{
init_plane(temp_points_, plane_ptr_);
new_points_ = 0;
}
if (temp_points_.size() >= max_points_num_)
{
update_enable_ = false;
std::vector<pointWithVar>().swap(temp_points_);
new_points_ = 0;
}
}
}
else
{
if (layer_ < max_layer_)
{
int xyz[3] = {0, 0, 0};
if (pv.point_w[0] > voxel_center_[0]) { xyz[0] = 1; }
if (pv.point_w[1] > voxel_center_[1]) { xyz[1] = 1; }
if (pv.point_w[2] > voxel_center_[2]) { xyz[2] = 1; }
int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2];
if (leaves_[leafnum] != nullptr) { leaves_[leafnum]->UpdateOctoTree(pv); }
else
{
leaves_[leafnum] = new VoxelOctoTree(max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], max_points_num_, planer_threshold_);
leaves_[leafnum]->layer_init_num_ = layer_init_num_;
leaves_[leafnum]->voxel_center_[0] = voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_;
leaves_[leafnum]->voxel_center_[1] = voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_;
leaves_[leafnum]->voxel_center_[2] = voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_;
leaves_[leafnum]->quater_length_ = quater_length_ / 2;
leaves_[leafnum]->UpdateOctoTree(pv);
}
}
else
{
if (update_enable_)
{
new_points_++;
temp_points_.push_back(pv);
if (new_points_ > update_size_threshold_)
{
init_plane(temp_points_, plane_ptr_);
new_points_ = 0;
}
if (temp_points_.size() > max_points_num_)
{
update_enable_ = false;
std::vector<pointWithVar>().swap(temp_points_);
new_points_ = 0;
}
}
}
}
}
}
VoxelOctoTree *VoxelOctoTree::find_correspond(Eigen::Vector3d pw)
{
if (!init_octo_ || plane_ptr_->is_plane_ || (layer_ >= max_layer_)) return this;
int xyz[3] = {0, 0, 0};
xyz[0] = pw[0] > voxel_center_[0] ? 1 : 0;
xyz[1] = pw[1] > voxel_center_[1] ? 1 : 0;
xyz[2] = pw[2] > voxel_center_[2] ? 1 : 0;
int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2];
// printf("leafnum: %d. \n", leafnum);
return (leaves_[leafnum] != nullptr) ? leaves_[leafnum]->find_correspond(pw) : this;
}
VoxelOctoTree *VoxelOctoTree::Insert(const pointWithVar &pv)
{
if ((!init_octo_) || (init_octo_ && plane_ptr_->is_plane_) || (init_octo_ && (!plane_ptr_->is_plane_) && (layer_ >= max_layer_)))
{
new_points_++;
temp_points_.push_back(pv);
return this;
}
if (init_octo_ && (!plane_ptr_->is_plane_) && (layer_ < max_layer_))
{
int xyz[3] = {0, 0, 0};
xyz[0] = pv.point_w[0] > voxel_center_[0] ? 1 : 0;
xyz[1] = pv.point_w[1] > voxel_center_[1] ? 1 : 0;
xyz[2] = pv.point_w[2] > voxel_center_[2] ? 1 : 0;
int leafnum = 4 * xyz[0] + 2 * xyz[1] + xyz[2];
if (leaves_[leafnum] != nullptr) { return leaves_[leafnum]->Insert(pv); }
else
{
leaves_[leafnum] = new VoxelOctoTree(max_layer_, layer_ + 1, layer_init_num_[layer_ + 1], max_points_num_, planer_threshold_);
leaves_[leafnum]->layer_init_num_ = layer_init_num_;
leaves_[leafnum]->voxel_center_[0] = voxel_center_[0] + (2 * xyz[0] - 1) * quater_length_;
leaves_[leafnum]->voxel_center_[1] = voxel_center_[1] + (2 * xyz[1] - 1) * quater_length_;
leaves_[leafnum]->voxel_center_[2] = voxel_center_[2] + (2 * xyz[2] - 1) * quater_length_;
leaves_[leafnum]->quater_length_ = quater_length_ / 2;
return leaves_[leafnum]->Insert(pv);
}
}
return nullptr;
}
void VoxelMapManager::StateEstimation(StatesGroup &state_propagat)
{
cross_mat_list_.clear();
cross_mat_list_.reserve(feats_down_size_);
body_cov_list_.clear();
body_cov_list_.reserve(feats_down_size_);
// build_residual_time = 0.0;
// ekf_time = 0.0;
// double t0 = omp_get_wtime();
for (size_t i = 0; i < feats_down_body_->size(); i++)
{
V3D point_this(feats_down_body_->points[i].x, feats_down_body_->points[i].y, feats_down_body_->points[i].z);
if (point_this[2] == 0) { point_this[2] = 0.001; }
M3D var;
calcBodyCov(point_this, config_setting_.dept_err_, config_setting_.beam_err_, var);
body_cov_list_.push_back(var);
point_this = extR_ * point_this + extT_;
M3D point_crossmat;
point_crossmat << SKEW_SYM_MATRX(point_this);
cross_mat_list_.push_back(point_crossmat);
}
vector<pointWithVar>().swap(pv_list_);
pv_list_.resize(feats_down_size_);
int rematch_num = 0;
MD(DIM_STATE, DIM_STATE) G, H_T_H, I_STATE;
G.setZero();
H_T_H.setZero();
I_STATE.setIdentity();
bool flg_EKF_inited, flg_EKF_converged, EKF_stop_flg = 0;
for (int iterCount = 0; iterCount < config_setting_.max_iterations_; iterCount++)
{
double total_residual = 0.0;
pcl::PointCloud<pcl::PointXYZI>::Ptr world_lidar(new pcl::PointCloud<pcl::PointXYZI>);
TransformLidar(state_.rot_end, state_.pos_end, feats_down_body_, world_lidar);
M3D rot_var = state_.cov.block<3, 3>(0, 0);
M3D t_var = state_.cov.block<3, 3>(3, 3);
for (size_t i = 0; i < feats_down_body_->size(); i++)
{
pointWithVar &pv = pv_list_[i];
pv.point_b << feats_down_body_->points[i].x, feats_down_body_->points[i].y, feats_down_body_->points[i].z;
pv.point_w << world_lidar->points[i].x, world_lidar->points[i].y, world_lidar->points[i].z;
M3D cov = body_cov_list_[i];
M3D point_crossmat = cross_mat_list_[i];
cov = state_.rot_end * cov * state_.rot_end.transpose() + (-point_crossmat) * rot_var * (-point_crossmat.transpose()) + t_var;
pv.var = cov;
pv.body_var = body_cov_list_[i];
}
ptpl_list_.clear();
// double t1 = omp_get_wtime();
BuildResidualListOMP(pv_list_, ptpl_list_);
// build_residual_time += omp_get_wtime() - t1;
for (int i = 0; i < ptpl_list_.size(); i++)
{
total_residual += fabs(ptpl_list_[i].dis_to_plane_);
}
effct_feat_num_ = ptpl_list_.size();
cout << "[ LIO ] Raw feature num: " << feats_undistort_->size() << ", downsampled feature num:" << feats_down_size_
<< " effective feature num: " << effct_feat_num_ << " average residual: " << total_residual / effct_feat_num_ << endl;
/*** Computation of Measuremnt Jacobian matrix H and measurents covarience
* ***/
MatrixXd Hsub(effct_feat_num_, 6);
MatrixXd Hsub_T_R_inv(6, effct_feat_num_);
VectorXd R_inv(effct_feat_num_);
VectorXd meas_vec(effct_feat_num_);
meas_vec.setZero();
for (int i = 0; i < effct_feat_num_; i++)
{
auto &ptpl = ptpl_list_[i];
V3D point_this(ptpl.point_b_);
point_this = extR_ * point_this + extT_;
V3D point_body(ptpl.point_b_);
M3D point_crossmat;
point_crossmat << SKEW_SYM_MATRX(point_this);
/*** get the normal vector of closest surface/corner ***/
V3D point_world = state_propagat.rot_end * point_this + state_propagat.pos_end;
Eigen::Matrix<double, 1, 6> J_nq;
J_nq.block<1, 3>(0, 0) = point_world - ptpl_list_[i].center_;
J_nq.block<1, 3>(0, 3) = -ptpl_list_[i].normal_;
M3D var;
// V3D normal_b = state_.rot_end.inverse() * ptpl_list_[i].normal_;
// V3D point_b = ptpl_list_[i].point_b_;
// double cos_theta = fabs(normal_b.dot(point_b) / point_b.norm());
// ptpl_list_[i].body_cov_ = ptpl_list_[i].body_cov_ * (1.0 / cos_theta) * (1.0 / cos_theta);
// point_w cov
// var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * (state_propagat.rot_end * extR_).transpose() +
// state_propagat.cov.block<3, 3>(3, 3) + (-point_crossmat) * state_propagat.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose();
// point_w cov (another_version)
// var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * (state_propagat.rot_end * extR_).transpose() +
// state_propagat.cov.block<3, 3>(3, 3) - point_crossmat * state_propagat.cov.block<3, 3>(0, 0) * point_crossmat;
// point_body cov
var = state_propagat.rot_end * extR_ * ptpl_list_[i].body_cov_ * (state_propagat.rot_end * extR_).transpose();
double sigma_l = J_nq * ptpl_list_[i].plane_var_ * J_nq.transpose();
R_inv(i) = 1.0 / (0.001 + sigma_l + ptpl_list_[i].normal_.transpose() * var * ptpl_list_[i].normal_);
// R_inv(i) = 1.0 / (sigma_l + ptpl_list_[i].normal_.transpose() * var * ptpl_list_[i].normal_);
/*** calculate the Measuremnt Jacobian matrix H ***/
V3D A(point_crossmat * state_.rot_end.transpose() * ptpl_list_[i].normal_);
Hsub.row(i) << VEC_FROM_ARRAY(A), ptpl_list_[i].normal_[0], ptpl_list_[i].normal_[1], ptpl_list_[i].normal_[2];
Hsub_T_R_inv.col(i) << A[0] * R_inv(i), A[1] * R_inv(i), A[2] * R_inv(i), ptpl_list_[i].normal_[0] * R_inv(i),
ptpl_list_[i].normal_[1] * R_inv(i), ptpl_list_[i].normal_[2] * R_inv(i);
meas_vec(i) = -ptpl_list_[i].dis_to_plane_;
}
EKF_stop_flg = false;
flg_EKF_converged = false;
/*** Iterative Kalman Filter Update ***/
MatrixXd K(DIM_STATE, effct_feat_num_);
// auto &&Hsub_T = Hsub.transpose();
auto &&HTz = Hsub_T_R_inv * meas_vec;
// fout_dbg<<"HTz: "<<HTz<<endl;
H_T_H.block<6, 6>(0, 0) = Hsub_T_R_inv * Hsub;
// EigenSolver<Matrix<double, 6, 6>> es(H_T_H.block<6,6>(0,0));
MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H.block<DIM_STATE, DIM_STATE>(0, 0) + state_.cov.block<DIM_STATE, DIM_STATE>(0, 0).inverse()).inverse();
G.block<DIM_STATE, 6>(0, 0) = K_1.block<DIM_STATE, 6>(0, 0) * H_T_H.block<6, 6>(0, 0);
auto vec = state_propagat - state_;
VD(DIM_STATE)
solution = K_1.block<DIM_STATE, 6>(0, 0) * HTz + vec.block<DIM_STATE, 1>(0, 0) - G.block<DIM_STATE, 6>(0, 0) * vec.block<6, 1>(0, 0);
int minRow, minCol;
state_ += solution;
auto rot_add = solution.block<3, 1>(0, 0);
auto t_add = solution.block<3, 1>(3, 0);
if ((rot_add.norm() * 57.3 < 0.01) && (t_add.norm() * 100 < 0.015)) { flg_EKF_converged = true; }
V3D euler_cur = state_.rot_end.eulerAngles(2, 1, 0);
/*** Rematch Judgement ***/
if (flg_EKF_converged || ((rematch_num == 0) && (iterCount == (config_setting_.max_iterations_ - 2)))) { rematch_num++; }
/*** Convergence Judgements and Covariance Update ***/
if (!EKF_stop_flg && (rematch_num >= 2 || (iterCount == config_setting_.max_iterations_ - 1)))
{
/*** Covariance Update ***/
// _state.cov = (I_STATE - G) * _state.cov;
state_.cov.block<DIM_STATE, DIM_STATE>(0, 0) =
(I_STATE.block<DIM_STATE, DIM_STATE>(0, 0) - G.block<DIM_STATE, DIM_STATE>(0, 0)) * state_.cov.block<DIM_STATE, DIM_STATE>(0, 0);
// total_distance += (_state.pos_end - position_last).norm();
position_last_ = state_.pos_end;
geoQuat_ = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2));
// VD(DIM_STATE) K_sum = K.rowwise().sum();
// VD(DIM_STATE) P_diag = _state.cov.diagonal();
EKF_stop_flg = true;
}
if (EKF_stop_flg) break;
}
// double t2 = omp_get_wtime();
// scan_count++;
// ekf_time = t2 - t0 - build_residual_time;
// ave_build_residual_time = ave_build_residual_time * (scan_count - 1) / scan_count + build_residual_time / scan_count;
// ave_ekf_time = ave_ekf_time * (scan_count - 1) / scan_count + ekf_time / scan_count;
// cout << "[ Mapping ] ekf_time: " << ekf_time << "s, build_residual_time: " << build_residual_time << "s" << endl;
// cout << "[ Mapping ] ave_ekf_time: " << ave_ekf_time << "s, ave_build_residual_time: " << ave_build_residual_time << "s" << endl;
}
void VoxelMapManager::TransformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud,
pcl::PointCloud<pcl::PointXYZI>::Ptr &trans_cloud)
{
pcl::PointCloud<pcl::PointXYZI>().swap(*trans_cloud);
trans_cloud->reserve(input_cloud->size());
for (size_t i = 0; i < input_cloud->size(); i++)
{
pcl::PointXYZINormal p_c = input_cloud->points[i];
Eigen::Vector3d p(p_c.x, p_c.y, p_c.z);
p = (rot * (extR_ * p + extT_) + t);
pcl::PointXYZI pi;
pi.x = p(0);
pi.y = p(1);
pi.z = p(2);
pi.intensity = p_c.intensity;
trans_cloud->points.push_back(pi);
}
}
void VoxelMapManager::BuildVoxelMap()
{
float voxel_size = config_setting_.max_voxel_size_;
float planer_threshold = config_setting_.planner_threshold_;
int max_layer = config_setting_.max_layer_;
int max_points_num = config_setting_.max_points_num_;
std::vector<int> layer_init_num = convertToIntVectorSafe(config_setting_.layer_init_num_);
std::vector<pointWithVar> input_points;
for (size_t i = 0; i < feats_down_world_->size(); i++)
{
pointWithVar pv;
pv.point_w << feats_down_world_->points[i].x, feats_down_world_->points[i].y, feats_down_world_->points[i].z;
V3D point_this(feats_down_body_->points[i].x, feats_down_body_->points[i].y, feats_down_body_->points[i].z);
M3D var;
calcBodyCov(point_this, config_setting_.dept_err_, config_setting_.beam_err_, var);
M3D point_crossmat;
point_crossmat << SKEW_SYM_MATRX(point_this);
var = (state_.rot_end * extR_) * var * (state_.rot_end * extR_).transpose() +
(-point_crossmat) * state_.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose() + state_.cov.block<3, 3>(3, 3);
pv.var = var;
input_points.push_back(pv);
}
uint plsize = input_points.size();
for (uint i = 0; i < plsize; i++)
{
const pointWithVar p_v = input_points[i];
float loc_xyz[3];
for (int j = 0; j < 3; j++)
{
loc_xyz[j] = p_v.point_w[j] / voxel_size;
if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; }
}
VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]);
auto iter = voxel_map_.find(position);
if (iter != voxel_map_.end())
{
voxel_map_[position]->temp_points_.push_back(p_v);
voxel_map_[position]->new_points_++;
}
else
{
VoxelOctoTree *octo_tree = new VoxelOctoTree(max_layer, 0, layer_init_num[0], max_points_num, planer_threshold);
voxel_map_[position] = octo_tree;
voxel_map_[position]->quater_length_ = voxel_size / 4;
voxel_map_[position]->voxel_center_[0] = (0.5 + position.x) * voxel_size;
voxel_map_[position]->voxel_center_[1] = (0.5 + position.y) * voxel_size;
voxel_map_[position]->voxel_center_[2] = (0.5 + position.z) * voxel_size;
voxel_map_[position]->temp_points_.push_back(p_v);
voxel_map_[position]->new_points_++;
voxel_map_[position]->layer_init_num_ = layer_init_num;
}
}
for (auto iter = voxel_map_.begin(); iter != voxel_map_.end(); ++iter)
{
iter->second->init_octo_tree();
}
}
V3F VoxelMapManager::RGBFromVoxel(const V3D &input_point)
{
int64_t loc_xyz[3];
for (int j = 0; j < 3; j++)
{
loc_xyz[j] = floor(input_point[j] / config_setting_.max_voxel_size_);
}
VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]);
int64_t ind = loc_xyz[0] + loc_xyz[1] + loc_xyz[2];
uint k((ind + 100000) % 3);
V3F RGB((k == 0) * 255.0, (k == 1) * 255.0, (k == 2) * 255.0);
// cout<<"RGB: "<<RGB.transpose()<<endl;
return RGB;
}
void VoxelMapManager::UpdateVoxelMap(const std::vector<pointWithVar> &input_points)
{
float voxel_size = config_setting_.max_voxel_size_;
float planer_threshold = config_setting_.planner_threshold_;
int max_layer = config_setting_.max_layer_;
int max_points_num = config_setting_.max_points_num_;
std::vector<int> layer_init_num = convertToIntVectorSafe(config_setting_.layer_init_num_);
uint plsize = input_points.size();
for (uint i = 0; i < plsize; i++)
{
const pointWithVar p_v = input_points[i];
float loc_xyz[3];
for (int j = 0; j < 3; j++)
{
loc_xyz[j] = p_v.point_w[j] / voxel_size;
if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; }
}
VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]);
auto iter = voxel_map_.find(position);
if (iter != voxel_map_.end()) { voxel_map_[position]->UpdateOctoTree(p_v); }
else
{
VoxelOctoTree *octo_tree = new VoxelOctoTree(max_layer, 0, layer_init_num[0], max_points_num, planer_threshold);
voxel_map_[position] = octo_tree;
voxel_map_[position]->layer_init_num_ = layer_init_num;
voxel_map_[position]->quater_length_ = voxel_size / 4;
voxel_map_[position]->voxel_center_[0] = (0.5 + position.x) * voxel_size;
voxel_map_[position]->voxel_center_[1] = (0.5 + position.y) * voxel_size;
voxel_map_[position]->voxel_center_[2] = (0.5 + position.z) * voxel_size;
voxel_map_[position]->UpdateOctoTree(p_v);
}
}
}
void VoxelMapManager::BuildResidualListOMP(std::vector<pointWithVar> &pv_list, std::vector<PointToPlane> &ptpl_list)
{
int max_layer = config_setting_.max_layer_;
double voxel_size = config_setting_.max_voxel_size_;
double sigma_num = config_setting_.sigma_num_;
std::mutex mylock;
ptpl_list.clear();
std::vector<PointToPlane> all_ptpl_list(pv_list.size());
std::vector<bool> useful_ptpl(pv_list.size());
std::vector<size_t> index(pv_list.size());
for (size_t i = 0; i < index.size(); ++i)
{
index[i] = i;
useful_ptpl[i] = false;
}
#ifdef MP_EN
omp_set_num_threads(MP_PROC_NUM);
#pragma omp parallel for
#endif
for (int i = 0; i < index.size(); i++)
{
pointWithVar &pv = pv_list[i];
float loc_xyz[3];
for (int j = 0; j < 3; j++)
{
loc_xyz[j] = pv.point_w[j] / voxel_size;
if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; }
}
VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]);
auto iter = voxel_map_.find(position);
if (iter != voxel_map_.end())
{
VoxelOctoTree *current_octo = iter->second;
PointToPlane single_ptpl;
bool is_sucess = false;
double prob = 0;
build_single_residual(pv, current_octo, 0, is_sucess, prob, single_ptpl);
if (!is_sucess)
{
VOXEL_LOCATION near_position = position;
if (loc_xyz[0] > (current_octo->voxel_center_[0] + current_octo->quater_length_)) { near_position.x = near_position.x + 1; }
else if (loc_xyz[0] < (current_octo->voxel_center_[0] - current_octo->quater_length_)) { near_position.x = near_position.x - 1; }
if (loc_xyz[1] > (current_octo->voxel_center_[1] + current_octo->quater_length_)) { near_position.y = near_position.y + 1; }
else if (loc_xyz[1] < (current_octo->voxel_center_[1] - current_octo->quater_length_)) { near_position.y = near_position.y - 1; }
if (loc_xyz[2] > (current_octo->voxel_center_[2] + current_octo->quater_length_)) { near_position.z = near_position.z + 1; }
else if (loc_xyz[2] < (current_octo->voxel_center_[2] - current_octo->quater_length_)) { near_position.z = near_position.z - 1; }
auto iter_near = voxel_map_.find(near_position);
if (iter_near != voxel_map_.end()) { build_single_residual(pv, iter_near->second, 0, is_sucess, prob, single_ptpl); }
}
if (is_sucess)
{
mylock.lock();
useful_ptpl[i] = true;
all_ptpl_list[i] = single_ptpl;
mylock.unlock();
}
else
{
mylock.lock();
useful_ptpl[i] = false;
mylock.unlock();
}
}
}
for (size_t i = 0; i < useful_ptpl.size(); i++)
{
if (useful_ptpl[i]) { ptpl_list.push_back(all_ptpl_list[i]); }
}
}
void VoxelMapManager::build_single_residual(pointWithVar &pv, const VoxelOctoTree *current_octo, const int current_layer, bool &is_sucess,
double &prob, PointToPlane &single_ptpl)
{
int max_layer = config_setting_.max_layer_;
double sigma_num = config_setting_.sigma_num_;
double radius_k = 3;
Eigen::Vector3d p_w = pv.point_w;
if (current_octo->plane_ptr_->is_plane_)
{
VoxelPlane &plane = *current_octo->plane_ptr_;
Eigen::Vector3d p_world_to_center = p_w - plane.center_;
float dis_to_plane = fabs(plane.normal_(0) * p_w(0) + plane.normal_(1) * p_w(1) + plane.normal_(2) * p_w(2) + plane.d_);
float dis_to_center = (plane.center_(0) - p_w(0)) * (plane.center_(0) - p_w(0)) + (plane.center_(1) - p_w(1)) * (plane.center_(1) - p_w(1)) +
(plane.center_(2) - p_w(2)) * (plane.center_(2) - p_w(2));
float range_dis = sqrt(dis_to_center - dis_to_plane * dis_to_plane);
if (range_dis <= radius_k * plane.radius_)
{
Eigen::Matrix<double, 1, 6> J_nq;
J_nq.block<1, 3>(0, 0) = p_w - plane.center_;
J_nq.block<1, 3>(0, 3) = -plane.normal_;
double sigma_l = J_nq * plane.plane_var_ * J_nq.transpose();
sigma_l += plane.normal_.transpose() * pv.var * plane.normal_;
if (dis_to_plane < sigma_num * sqrt(sigma_l))
{
is_sucess = true;
double this_prob = 1.0 / (sqrt(sigma_l)) * exp(-0.5 * dis_to_plane * dis_to_plane / sigma_l);
if (this_prob > prob)
{
prob = this_prob;
pv.normal = plane.normal_;
single_ptpl.body_cov_ = pv.body_var;
single_ptpl.point_b_ = pv.point_b;
single_ptpl.point_w_ = pv.point_w;
single_ptpl.plane_var_ = plane.plane_var_;
single_ptpl.normal_ = plane.normal_;
single_ptpl.center_ = plane.center_;
single_ptpl.d_ = plane.d_;
single_ptpl.layer_ = current_layer;
single_ptpl.dis_to_plane_ = plane.normal_(0) * p_w(0) + plane.normal_(1) * p_w(1) + plane.normal_(2) * p_w(2) + plane.d_;
}
return;
}
else
{
// is_sucess = false;
return;
}
}
else
{
// is_sucess = false;
return;
}
}
else
{
if (current_layer < max_layer)
{
for (size_t leafnum = 0; leafnum < 8; leafnum++)
{
if (current_octo->leaves_[leafnum] != nullptr)
{
VoxelOctoTree *leaf_octo = current_octo->leaves_[leafnum];
build_single_residual(pv, leaf_octo, current_layer + 1, is_sucess, prob, single_ptpl);
}
}
return;
}
else { return; }
}
}
void VoxelMapManager::pubVoxelMap()
{
double max_trace = 0.25;
double pow_num = 0.2;
rclcpp::Rate loop(500);
float use_alpha = 0.8;
visualization_msgs::msg::MarkerArray voxel_plane;
voxel_plane.markers.reserve(1000000);
std::vector<VoxelPlane> pub_plane_list;
for (auto iter = voxel_map_.begin(); iter != voxel_map_.end(); iter++)
{
GetUpdatePlane(iter->second, config_setting_.max_layer_, pub_plane_list);
}
for (size_t i = 0; i < pub_plane_list.size(); i++)
{
V3D plane_cov = pub_plane_list[i].plane_var_.block<3, 3>(0, 0).diagonal();
double trace = plane_cov.sum();
if (trace >= max_trace) { trace = max_trace; }
trace = trace * (1.0 / max_trace);
trace = pow(trace, pow_num);
uint8_t r, g, b;
mapJet(trace, 0, 1, r, g, b);
Eigen::Vector3d plane_rgb(r / 256.0, g / 256.0, b / 256.0);
double alpha;
if (pub_plane_list[i].is_plane_) { alpha = use_alpha; }
else { alpha = 0; }
pubSinglePlane(voxel_plane, "plane", pub_plane_list[i], alpha, plane_rgb);
}
voxel_map_pub_->publish(voxel_plane);
loop.sleep();
}
void VoxelMapManager::GetUpdatePlane(const VoxelOctoTree *current_octo, const int pub_max_voxel_layer, std::vector<VoxelPlane> &plane_list)
{
if (current_octo->layer_ > pub_max_voxel_layer) { return; }
if (current_octo->plane_ptr_->is_update_) { plane_list.push_back(*current_octo->plane_ptr_); }
if (current_octo->layer_ < current_octo->max_layer_)
{
if (!current_octo->plane_ptr_->is_plane_)
{
for (size_t i = 0; i < 8; i++)
{
if (current_octo->leaves_[i] != nullptr) { GetUpdatePlane(current_octo->leaves_[i], pub_max_voxel_layer, plane_list); }
}
}
}
return;
}
void VoxelMapManager::pubSinglePlane(visualization_msgs::msg::MarkerArray &plane_pub, const std::string plane_ns, const VoxelPlane &single_plane,
const float alpha, const Eigen::Vector3d rgb)
{
visualization_msgs::msg::Marker plane;
plane.header.frame_id = "camera_init";
plane.header.stamp = rclcpp::Time();
plane.ns = plane_ns;
plane.id = single_plane.id_;
plane.type = visualization_msgs::msg::Marker::CYLINDER;
plane.action = visualization_msgs::msg::Marker::ADD;
plane.pose.position.x = single_plane.center_[0];
plane.pose.position.y = single_plane.center_[1];
plane.pose.position.z = single_plane.center_[2];
geometry_msgs::msg::Quaternion q;
CalcVectQuation(single_plane.x_normal_, single_plane.y_normal_, single_plane.normal_, q);
plane.pose.orientation = q;
plane.scale.x = 3 * sqrt(single_plane.max_eigen_value_);
plane.scale.y = 3 * sqrt(single_plane.mid_eigen_value_);
plane.scale.z = 2 * sqrt(single_plane.min_eigen_value_);
plane.color.a = alpha;
plane.color.r = rgb(0);
plane.color.g = rgb(1);
plane.color.b = rgb(2);
plane.lifetime = rclcpp::Duration::from_seconds(0.01);
plane_pub.markers.push_back(plane);
}
void VoxelMapManager::CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, const Eigen::Vector3d &z_vec,
geometry_msgs::msg::Quaternion &q)
{
Eigen::Matrix3d rot;
rot << x_vec(0), x_vec(1), x_vec(2), y_vec(0), y_vec(1), y_vec(2), z_vec(0), z_vec(1), z_vec(2);
Eigen::Matrix3d rotation = rot.transpose();
Eigen::Quaterniond eq(rotation);
q.w = eq.w();
q.x = eq.x();
q.y = eq.y();
q.z = eq.z();
}
void VoxelMapManager::mapJet(double v, double vmin, double vmax, uint8_t &r, uint8_t &g, uint8_t &b)
{
r = 255;
g = 255;
b = 255;
if (v < vmin) { v = vmin; }
if (v > vmax) { v = vmax; }
double dr, dg, db;
if (v < 0.1242)
{
db = 0.504 + ((1. - 0.504) / 0.1242) * v;
dg = dr = 0.;
}
else if (v < 0.3747)
{
db = 1.;
dr = 0.;
dg = (v - 0.1242) * (1. / (0.3747 - 0.1242));
}
else if (v < 0.6253)
{
db = (0.6253 - v) * (1. / (0.6253 - 0.3747));
dg = 1.;
dr = (v - 0.3747) * (1. / (0.6253 - 0.3747));
}
else if (v < 0.8758)
{
db = 0.;
dr = 1.;
dg = (0.8758 - v) * (1. / (0.8758 - 0.6253));
}
else
{
db = 0.;
dg = 0.;
dr = 1. - (v - 0.8758) * ((1. - 0.504) / (1. - 0.8758));
}
r = (uint8_t)(255 * dr);
g = (uint8_t)(255 * dg);
b = (uint8_t)(255 * db);
}
void VoxelMapManager::mapSliding()
{
if((position_last_ - last_slide_position).norm() < config_setting_.sliding_thresh)
{
std::cout<<RED<<"[DEBUG]: Last sliding length "<<(position_last_ - last_slide_position).norm()<<RESET<<"\n";
return;
}
//get global id now
last_slide_position = position_last_;
double t_sliding_start = omp_get_wtime();
float loc_xyz[3];
for (int j = 0; j < 3; j++)
{
loc_xyz[j] = position_last_[j] / config_setting_.max_voxel_size_;
if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; }
}
// VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]);//discrete global
clearMemOutOfMap((int64_t)loc_xyz[0] + config_setting_.half_map_size, (int64_t)loc_xyz[0] - config_setting_.half_map_size,
(int64_t)loc_xyz[1] + config_setting_.half_map_size, (int64_t)loc_xyz[1] - config_setting_.half_map_size,
(int64_t)loc_xyz[2] + config_setting_.half_map_size, (int64_t)loc_xyz[2] - config_setting_.half_map_size);
double t_sliding_end = omp_get_wtime();
std::cout<<RED<<"[DEBUG]: Map sliding using "<<t_sliding_end - t_sliding_start<<" secs"<<RESET<<"\n";
return;
}
void VoxelMapManager::clearMemOutOfMap(const int& x_max,const int& x_min,const int& y_max,const int& y_min,const int& z_max,const int& z_min )
{
int delete_voxel_cout = 0;
// double delete_time = 0;
// double last_delete_time = 0;
for (auto it = voxel_map_.begin(); it != voxel_map_.end(); )
{
const VOXEL_LOCATION& loc = it->first;
bool should_remove = loc.x > x_max || loc.x < x_min || loc.y > y_max || loc.y < y_min || loc.z > z_max || loc.z < z_min;
if (should_remove){
// last_delete_time = omp_get_wtime();
delete it->second;
it = voxel_map_.erase(it);
// delete_time += omp_get_wtime() - last_delete_time;
delete_voxel_cout++;
} else {
++it;
}
}
std::cout<<RED<<"[DEBUG]: Delete "<<delete_voxel_cout<<" root voxels"<<RESET<<"\n";
// std::cout<<RED<<"[DEBUG]: Delete "<<delete_voxel_cout<<" voxels using "<<delete_time<<" s"<<RESET<<"\n";
}
src/FAST-LIVO2/urdf/fori_robot.urdf
+219
View File
@@ -0,0 +1,219 @@
<?xml version="1.0"?>
<robot name="fori_robot">
<!-- 재질 정의 -->
<material name="blue"><color rgba="0 0 0.8 1"/></material>
<material name="black"><color rgba="0 0 0 1"/></material>
<material name="grey"><color rgba="0.5 0.5 0.5 1"/></material>
<material name="dark_gray"><color rgba="0.2 0.2 0.2 1.0"/></material>
<!-- ============================================================
Root: aft_mapped (LiDAR body frame)
FAST-LIVO2가 camera_init → aft_mapped 를 동적으로 발행.
============================================================ -->
<link name="aft_mapped"/>
<!-- Livox MID-360 시각화 -->
<link name="lidar_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry><box size="0.07 0.07 0.04"/></geometry>
<material name="dark_gray"/>
</visual>
</link>
<joint name="aft_mapped_to_lidar" type="fixed">
<parent link="aft_mapped"/>
<child link="lidar_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<!-- Hikrobot 카메라 (캘리브레이션 결과) -->
<link name="camera_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry><box size="0.05 0.09 0.04"/></geometry>
<material name="blue"/>
</visual>
</link>
<joint name="aft_mapped_to_camera" type="fixed">
<parent link="aft_mapped"/>
<child link="camera_link"/>
<origin xyz="0.128964 -0.026998 -0.043195"
rpy="-1.529526 -1.293497 3.122599"/>
</joint>
<link name="camera_optical_link"/>
<joint name="camera_to_optical" type="fixed">
<parent link="camera_link"/>
<child link="camera_optical_link"/>
<origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
</joint>
<!-- ============================================================
Base Link: 실측값 적용
라이다(aft_mapped) 기준: x=-269.53mm, z=-412.9633mm
rpy pitch=-0.277309rad: 라이다 pitch -15.89° 보정 → 수평
============================================================ -->
<link name="base_link">
<visual>
<origin xyz="0 0 0.143" rpy="0 0 0"/>
<geometry><box size="0.631 0.410 0.286"/></geometry>
<material name="blue"/>
</visual>
<collision>
<origin xyz="0 0 0.143" rpy="0 0 0"/>
<geometry><box size="0.631 0.410 0.286"/></geometry>
</collision>
<inertial>
<mass value="20.0"/>
<inertia ixx="0.5" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
</inertial>
</link>
<joint name="aft_mapped_to_base" type="fixed">
<parent link="aft_mapped"/>
<child link="base_link"/>
<origin xyz="-0.26953 0 -0.41296" rpy="0 -0.277309 0"/>
</joint>
<!-- Base Footprint: 실제 지면(Ground) 위치 -->
<link name="base_footprint"/>
<joint name="base_to_footprint" type="fixed">
<parent link="base_link"/>
<child link="base_footprint"/>
<origin xyz="0 0 -0.0639" rpy="0 0 0"/>
</joint>
<!-- ============================================================
휠 및 차축: base_link 기준 (변경 없음)
============================================================ -->
<!-- 1. Front Left -->
<link name="front_left_axle">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.025" length="0.035"/></geometry>
<material name="grey"/>
</visual>
</link>
<joint name="front_left_axle_joint" type="fixed">
<parent link="base_link"/>
<child link="front_left_axle"/>
<origin xyz="0.187 0.2225 0.0631" rpy="0 0 0"/>
</joint>
<link name="front_left_wheel">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
</collision>
</link>
<joint name="front_left_wheel_joint" type="continuous">
<parent link="front_left_axle"/>
<child link="front_left_wheel"/>
<origin xyz="0 0.0575 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<!-- 2. Front Right -->
<link name="front_right_axle">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.025" length="0.035"/></geometry>
<material name="grey"/>
</visual>
</link>
<joint name="front_right_axle_joint" type="fixed">
<parent link="base_link"/>
<child link="front_right_axle"/>
<origin xyz="0.187 -0.2225 0.0631" rpy="0 0 0"/>
</joint>
<link name="front_right_wheel">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
</collision>
</link>
<joint name="front_right_wheel_joint" type="continuous">
<parent link="front_right_axle"/>
<child link="front_right_wheel"/>
<origin xyz="0 -0.0575 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<!-- 3. Rear Left -->
<link name="rear_left_axle">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.025" length="0.035"/></geometry>
<material name="grey"/>
</visual>
</link>
<joint name="rear_left_axle_joint" type="fixed">
<parent link="base_link"/>
<child link="rear_left_axle"/>
<origin xyz="-0.187 0.2225 0.0631" rpy="0 0 0"/>
</joint>
<link name="rear_left_wheel">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
</collision>
</link>
<joint name="rear_left_wheel_joint" type="continuous">
<parent link="rear_left_axle"/>
<child link="rear_left_wheel"/>
<origin xyz="0 0.0575 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<!-- 4. Rear Right -->
<link name="rear_right_axle">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.025" length="0.035"/></geometry>
<material name="grey"/>
</visual>
</link>
<joint name="rear_right_axle_joint" type="fixed">
<parent link="base_link"/>
<child link="rear_right_axle"/>
<origin xyz="-0.187 -0.2225 0.0631" rpy="0 0 0"/>
</joint>
<link name="rear_right_wheel">
<visual>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.5707 0 0"/>
<geometry><cylinder radius="0.127" length="0.08"/></geometry>
</collision>
</link>
<joint name="rear_right_wheel_joint" type="continuous">
<parent link="rear_right_axle"/>
<child link="rear_right_wheel"/>
<origin xyz="0 -0.0575 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
</robot>
src/FAST-LIVO2/urdf/mid360_robot.urdf
+93
View File
@@ -0,0 +1,93 @@
<?xml version="1.0"?>
<robot name="mid360_robot">
<!-- ============================================================
Root: aft_mapped
FAST-LIVO2가 camera_init → aft_mapped 를 동적으로 발행.
이 링크 자체는 LiDAR body frame.
============================================================ -->
<link name="aft_mapped"/>
<!-- ============================================================
Livox MID-360 시각화 (aft_mapped 원점에 위치)
실제 크기: 직경 65mm, 높이 38mm → 박스로 근사
============================================================ -->
<link name="lidar_link">
<visual>
<geometry>
<box size="0.07 0.07 0.04"/>
</geometry>
<material name="dark_gray">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
</visual>
</link>
<joint name="aft_mapped_to_lidar" type="fixed">
<parent link="aft_mapped"/>
<child link="lidar_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<!-- ============================================================
로봇 샤시 박스 (임시 크기 — 실측 후 xyz/size 수정)
parent: aft_mapped 기준 z=-0.25m (라이다 아래 250mm, 바닥 수평)
size: 가로 x 세로 x 높이 (m)
============================================================ -->
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.40 0.30 0.15"/>
</geometry>
<material name="light_gray">
<color rgba="0.7 0.7 0.7 0.6"/>
</material>
</visual>
</link>
<joint name="aft_mapped_to_base" type="fixed">
<parent link="aft_mapped"/>
<child link="base_link"/>
<!-- 라이다 pitch -15.89° 보정 → base_link 바닥 수평
rpy pitch = +15.89° = 0.2773 rad 반대 방향 적용 -->
<origin xyz="0 0 -0.25" rpy="0 -0.277309 0"/>
</joint>
<!-- ============================================================
Hikrobot 카메라 (캘리브레이션 결과 적용)
camera_link: REP-103 (x=전방, y=왼쪽, z=위)
============================================================ -->
<link name="camera_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.09 0.04"/>
</geometry>
<material name="blue">
<color rgba="0.2 0.4 0.8 1.0"/>
</material>
</visual>
</link>
<joint name="aft_mapped_to_camera" type="fixed">
<parent link="aft_mapped"/>
<child link="camera_link"/>
<!-- calib.json T_lidar_camera 역변환 결과 -->
<origin xyz="0.128964 -0.026998 -0.043195"
rpy="-1.529526 -1.293497 3.122599"/>
</joint>
<!-- ============================================================
Camera optical frame (ROS 표준)
x=오른쪽, y=아래, z=전방
============================================================ -->
<link name="camera_optical_link"/>
<joint name="camera_to_optical" type="fixed">
<parent link="camera_link"/>
<child link="camera_optical_link"/>
<origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
</joint>
</robot>
src/FAST_LIO
Symlink
+1
View File
@@ -0,0 +1 @@
/home/yoo/FAST_LIO
src/__pycache__/forey_nav2.launch.cpython-310.pyc
BIN
View File
Binary file not shown.
src/fori_nav2.launch.py
+69
View File
@@ -0,0 +1,69 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
# --- [경로 설정 - 사용자 환경에 맞춰 수정] ---
# 1. 2D 지도 파일 경로 (데스크탑에 있는 파일 기준)
map_yaml_file = '/home/yoo/fori_map.yaml'
# 2. Nav2 파라미터 파일 경로
nav2_params_file = '/home/yoo/fori_ws/src/fori_nav2_params.yaml'
# 3. URDF 파일 경로 (분석된 full workspace 내 경로)
urdf_file_path = os.path.expanduser('~/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf')
# 패키지 경로 획득
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
# --- [노드 정의] ---
# 1단계: TF (URDF) 발행
with open(urdf_file_path, 'r') as infp:
robot_desc = infp.read()
rsp_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[{'robot_description': robot_desc}],
output='screen'
)
# 3단계-A: 3D PointCloud -> 2D Scan 변환
pc_to_ls_node = Node(
package='pointcloud_to_laserscan',
executable='pointcloud_to_laserscan_node',
name='pointcloud_to_laserscan',
parameters=[{
'target_frame': 'base_link',
'min_height': 0.1,
'max_height': 1.0,
'angle_min': -3.14159,
'angle_max': 3.14159,
'use_sim_time': False,
}],
remappings=[('cloud_in', '/cloud_registered')],
output='screen'
)
# 4단계: Nav2 스택 실행 (Map Server, AMCL, Planner, Controller 포함)
nav2_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
launch_arguments={
'map': map_yaml_file,
'use_sim_time': 'False',
'params_file': nav2_params_file,
'autostart': 'True',
}.items()
)
return LaunchDescription([
rsp_node,
pc_to_ls_node,
nav2_launch
])
src/fori_nav2_params.yaml
+130
View File
@@ -0,0 +1,130 @@
map_server:
ros__parameters:
yaml_filename: "/home/yoo/fori_map.yaml"
use_sim_time: False
amcl:
ros__parameters:
use_sim_time: False
transform_tolerance: 1.0
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
base_frame_id: "base_link"
global_frame_id: "map"
odom_frame_id: "camera_init" # FAST-LIO2의 오도메트리 프레임
scan_topic: "scan"
map_topic: "map"
set_initial_pose: true
behavior_server:
ros__parameters:
local_frame: base_link
global_frame: map
odom_frame: camera_init # <--- 이 부분이 odom 에러를 해결합니다.
use_sim_time: False
device_id: "robot"
simulate_ahead_time: 2.0
bt_navigator:
ros__parameters:
use_sim_time: False
global_frame: map
robot_base_frame: base_link
odom_topic: /aft_mapped_to_init # FAST-LIO2의 위치 토픽
controller_server:
ros__parameters:
use_sim_time: False
controller_frequency: 20.0
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.1
movement_time_allowance: 30.0
general_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.30
yaw_goal_tolerance: 0.35
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
desired_linear_vel: 0.25
lookahead_dist: 0.6
min_lookahead_dist: 0.3
max_lookahead_dist: 0.9
use_rotate_to_heading: True
rotate_to_heading_min_angle: 0.20
max_angular_vel: 0.5
min_tc_velocity: 0.1
regulated_linear_scaling_min_speed: 0.15
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: camera_init # 지역 지도는 오도메트리 기준
robot_base_frame: base_link
use_sim_time: False
transform_tolerance: 1.0
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.38
plugins: ["obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
inflation_radius: 0.15
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map # 전역 지도는 지도 기준
robot_base_frame: base_link
use_sim_time: False
transform_tolerance: 1.0
robot_radius: 0.38
resolution: 0.05
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
observation_sources: scan
scan:
topic: /scan
data_type: "LaserScan"
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
inflation_radius: 0.15
planner_server:
ros__parameters:
use_sim_time: False
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_smac_planner/SmacPlanner2D"
tolerance: 0.5
downsample_costmap: false
allow_unknown: true
max_iterations: 1000000
max_planning_time: 2.0
motion_model_for_search: "2D"
angle_quantization_binned: 72
minimum_turning_radius: 0.0
src/fori_serial_bridge/arduino/fori_bldc_control/fori_bldc_control.ino
+354
View File
@@ -0,0 +1,354 @@
/**
* @file fori_bldc_control.ino
* @brief Arduino Mega Firmware for FORI AGV BLDC Hub Motor Control
*
* Communicates with two ZLAC8015D Dual-Channel Drivers via Modbus RTU (Serial1)
* and receives speed commands from the PC (Serial) via USB.
*
* Hardware Connections:
* - Serial (USB): Communication with PC (115200 baud)
* - Serial1: TX1 (Pin 18), RX1 (Pin 19) connected to TTL-to-RS485 Transceiver
* - Pin 2: RS485 RE/DE (Direction control pin. High = Transmit, Low = Receive)
*
* Slave Configuration:
* - Driver 1 (Front Wheels): Slave ID 1. Ch1 = Left Front, Ch2 = Right Front
* - Driver 2 (Rear Wheels): Slave ID 2. Ch1 = Left Rear, Ch2 = Right Rear
*/
#define RS485_DE_RE_PIN 2
// Modbus registers
#define REG_MODE 0x200D // Operating mode: 3 = Velocity Control
#define REG_CONTROL_WORD 0x200E // Control word: 0x0008 = Enable, 0x0007 = Stop/Disable, 0x0006 = Clear Fault
#define REG_BRAKE_CH1 0x201A // Brake Control Ch1: 0x0000 = Release, 0x0001 = Lock
#define REG_BRAKE_CH2 0x201B // Brake Control Ch2: 0x0000 = Release, 0x0001 = Lock
#define REG_TARGET_RPM 0x2088 // Target RPM starting address (Ch1 at 0x2088, Ch2 at 0x2089)
#define REG_FEEDBACK_RPM 0x20AD // Actual feedback RPM starting address (Ch1 at 0x20AD, Ch2 at 0x20AE)
// Global state
bool drivers_enabled = false;
unsigned long last_cmd_time = 0;
const unsigned long CMD_TIMEOUT_MS = 500; // Safe timeout: stop if no commands received from PC
// Packet structures
struct PCCommand {
int16_t left_rpm;
int16_t right_rpm;
uint8_t control_state; // 0 = Idle/Brake, 1 = Enable/Release Brake
};
struct DriverFeedback {
int16_t front_left_rpm;
int16_t front_right_rpm;
int16_t rear_left_rpm;
int16_t rear_right_rpm;
};
DriverFeedback feedback = {0, 0, 0, 0};
// Modbus CRC-16 Calculation
uint16_t calculateCRC(uint8_t* buf, int len) {
uint16_t crc = 0xFFFF;
for (int pos = 0; pos < len; pos++) {
crc ^= (uint16_t)buf[pos];
for (int i = 8; i != 0; i--) {
if ((crc & 0x0001) != 0) {
crc >>= 1;
crc ^= 0xA001;
} else {
crc >>= 1;
}
}
}
return crc;
}
// Set RS485 to Transmit Mode
void setRS485Tx() {
digitalWrite(RS485_DE_RE_PIN, HIGH);
delayMicroseconds(50);
}
// Set RS485 to Receive Mode
void setRS485Rx() {
delayMicroseconds(50);
digitalWrite(RS485_DE_RE_PIN, LOW);
}
// Write Single Register via Modbus RTU
bool writeModbusSingle(uint8_t slave_id, uint16_t reg, uint16_t value) {
uint8_t packet[8];
packet[0] = slave_id;
packet[1] = 0x06; // Write Single Register
packet[2] = (reg >> 8) & 0xFF;
packet[3] = reg & 0xFF;
packet[4] = (value >> 8) & 0xFF;
packet[5] = value & 0xFF;
uint16_t crc = calculateCRC(packet, 6);
packet[6] = crc & 0xFF;
packet[7] = (crc >> 8) & 0xFF;
setRS485Tx();
Serial1.write(packet, 8);
Serial1.flush();
setRS485Rx();
// Read response (expected 8 bytes)
unsigned long start_time = millis();
int idx = 0;
uint8_t response[8];
while (millis() - start_time < 50) {
if (Serial1.available()) {
response[idx++] = Serial1.read();
if (idx >= 8) break;
}
}
if (idx < 8) return false;
uint16_t resp_crc = calculateCRC(response, 6);
uint16_t calc_crc = (response[7] << 8) | response[6];
return (resp_crc == calc_crc && response[0] == slave_id && response[1] == 0x06);
}
// Write Multiple Registers via Modbus RTU (e.g. Target Speed Ch1 & Ch2)
bool writeModbusMultiple(uint8_t slave_id, uint16_t start_reg, uint16_t num_regs, int16_t* values) {
uint8_t packet[32];
packet[0] = slave_id;
packet[1] = 0x10; // Write Multiple Registers
packet[2] = (start_reg >> 8) & 0xFF;
packet[3] = start_reg & 0xFF;
packet[4] = (num_regs >> 8) & 0xFF;
packet[5] = num_regs & 0xFF;
packet[6] = num_regs * 2; // Byte count
for (int i = 0; i < num_regs; i++) {
packet[7 + i*2] = (values[i] >> 8) & 0xFF;
packet[8 + i*2] = values[i] & 0xFF;
}
int len = 7 + num_regs * 2;
uint16_t crc = calculateCRC(packet, len);
packet[len] = crc & 0xFF;
packet[len+1] = (crc >> 8) & 0xFF;
setRS485Tx();
Serial1.write(packet, len + 2);
Serial1.flush();
setRS485Rx();
// Read response (expected 8 bytes)
unsigned long start_time = millis();
int idx = 0;
uint8_t response[8];
while (millis() - start_time < 50) {
if (Serial1.available()) {
response[idx++] = Serial1.read();
if (idx >= 8) break;
}
}
if (idx < 8) return false;
uint16_t resp_crc = calculateCRC(response, 6);
uint16_t calc_crc = (response[7] << 8) | response[6];
return (resp_crc == calc_crc && response[0] == slave_id && response[1] == 0x10);
}
// Read Holding Registers via Modbus RTU
bool readModbus(uint8_t slave_id, uint16_t start_reg, uint16_t num_regs, int16_t* output) {
uint8_t packet[8];
packet[0] = slave_id;
packet[1] = 0x03; // Read Holding Registers
packet[2] = (start_reg >> 8) & 0xFF;
packet[3] = start_reg & 0xFF;
packet[4] = (num_regs >> 8) & 0xFF;
packet[5] = num_regs & 0xFF;
uint16_t crc = calculateCRC(packet, 6);
packet[6] = crc & 0xFF;
packet[7] = (crc >> 8) & 0xFF;
// Clear serial read buffer before sending
while (Serial1.available()) Serial1.read();
setRS485Tx();
Serial1.write(packet, 8);
Serial1.flush();
setRS485Rx();
// Read response
int expected_len = 5 + num_regs * 2;
uint8_t response[32];
int idx = 0;
unsigned long start_time = millis();
while (millis() - start_time < 50) {
if (Serial1.available()) {
response[idx++] = Serial1.read();
if (idx >= expected_len) break;
}
}
if (idx < expected_len) return false;
uint16_t resp_crc = calculateCRC(response, expected_len - 2);
uint16_t calc_crc = (response[expected_len-1] << 8) | response[expected_len-2];
if (resp_crc != calc_crc) return false;
if (response[0] != slave_id || response[1] != 0x03) return false;
for (int i = 0; i < num_regs; i++) {
output[i] = (response[3 + i*2] << 8) | response[4 + i*2];
}
return true;
}
// Initialize Driver Mode and Settings
void initDrivers() {
// Set Operating Mode to Velocity (3) for both drivers
writeModbusSingle(1, REG_MODE, 3);
delay(10);
writeModbusSingle(2, REG_MODE, 3);
delay(10);
// Stop/Disable drivers on boot to keep it safe
disableDrivers();
}
void enableDrivers() {
if (drivers_enabled) return;
// 1. Release electromagnetic brakes (write 0 to 0x201A & 0x201B)
writeModbusSingle(1, REG_BRAKE_CH1, 0);
writeModbusSingle(1, REG_BRAKE_CH2, 0);
writeModbusSingle(2, REG_BRAKE_CH1, 0);
writeModbusSingle(2, REG_BRAKE_CH2, 0);
delay(20);
// 2. Enable drivers (write 8 to 0x200E)
writeModbusSingle(1, REG_CONTROL_WORD, 8);
writeModbusSingle(2, REG_CONTROL_WORD, 8);
delay(20);
drivers_enabled = true;
}
void disableDrivers() {
// 1. Send zero speeds
int16_t stop_speeds[2] = {0, 0};
writeModbusMultiple(1, REG_TARGET_RPM, 2, stop_speeds);
writeModbusMultiple(2, REG_TARGET_RPM, 2, stop_speeds);
// 2. Lock brakes (write 1 to 0x201A & 0x201B)
writeModbusSingle(1, REG_BRAKE_CH1, 1);
writeModbusSingle(1, REG_BRAKE_CH2, 1);
writeModbusSingle(2, REG_BRAKE_CH1, 1);
writeModbusSingle(2, REG_BRAKE_CH2, 1);
delay(20);
// 3. Disable drivers (write 7 to 0x200E)
writeModbusSingle(1, REG_CONTROL_WORD, 7);
writeModbusSingle(2, REG_CONTROL_WORD, 7);
drivers_enabled = false;
}
void setup() {
pinMode(RS485_DE_RE_PIN, OUTPUT);
setRS485Rx();
// USB serial communication to PC
Serial.begin(115200);
// RS485 Serial communication to ZLAC8015D drivers
Serial1.begin(115200);
// Wait for drivers to boot up
delay(1000);
initDrivers();
}
void loop() {
// Check Serial command packet from PC
// Format: [0xFE, left_rpm_H, left_rpm_L, right_rpm_H, right_rpm_L, control_state, checksum] -> 7 bytes
if (Serial.available() >= 7) {
if (Serial.read() == 0xFE) {
uint8_t buffer[6];
Serial.readBytes(buffer, 6);
uint8_t checksum = 0;
for (int i = 0; i < 5; i++) {
checksum += buffer[i];
}
if (checksum == buffer[5]) {
last_cmd_time = millis();
PCCommand cmd;
cmd.left_rpm = (buffer[0] << 8) | buffer[1];
cmd.right_rpm = (buffer[2] << 8) | buffer[3];
cmd.control_state = buffer[4];
if (cmd.control_state == 1) {
enableDrivers();
// Write speeds (invert right motor target RPM due to mirrored physical mounting)
int16_t front_speeds[2] = {cmd.left_rpm, -cmd.right_rpm};
int16_t rear_speeds[2] = {cmd.left_rpm, -cmd.right_rpm};
writeModbusMultiple(1, REG_TARGET_RPM, 2, front_speeds);
writeModbusMultiple(2, REG_TARGET_RPM, 2, rear_speeds);
} else {
disableDrivers();
}
}
}
}
// Safety Timeout: stop the robot if connection to PC is lost
if (drivers_enabled && (millis() - last_cmd_time > CMD_TIMEOUT_MS)) {
disableDrivers();
}
// Periodic Telemetry Feedback to PC (approx. 20Hz)
static unsigned long last_telemetry_time = 0;
if (millis() - last_telemetry_time >= 50) {
last_telemetry_time = millis();
int16_t front_data[2] = {0, 0};
int16_t rear_data[2] = {0, 0};
// Read actual RPMs (invert right motor feedback RPM due to mirrored physical mounting)
bool read_front = readModbus(1, REG_FEEDBACK_RPM, 2, front_data);
bool read_rear = readModbus(2, REG_FEEDBACK_RPM, 2, rear_data);
if (read_front) {
feedback.front_left_rpm = front_data[0];
feedback.front_right_rpm = -front_data[1]; // Invert right
}
if (read_rear) {
feedback.rear_left_rpm = rear_data[0];
feedback.rear_right_rpm = -rear_data[1]; // Invert right
}
// Send feedback packet to PC
// Format: [0xFD, f_lh, f_ll, f_rh, f_rl, r_lh, r_ll, r_rh, r_rl, checksum] -> 10 bytes
uint8_t resp[10];
resp[0] = 0xFD;
resp[1] = (feedback.front_left_rpm >> 8) & 0xFF;
resp[2] = feedback.front_left_rpm & 0xFF;
resp[3] = (feedback.front_right_rpm >> 8) & 0xFF;
resp[4] = feedback.front_right_rpm & 0xFF;
resp[5] = (feedback.rear_left_rpm >> 8) & 0xFF;
resp[6] = feedback.rear_left_rpm & 0xFF;
resp[7] = (feedback.rear_right_rpm >> 8) & 0xFF;
resp[8] = feedback.rear_right_rpm & 0xFF;
uint8_t checksum = 0;
for (int i = 1; i < 9; i++) {
checksum += resp[i];
}
resp[9] = checksum;
Serial.write(resp, 10);
}
}
src/fori_serial_bridge/fori_serial_bridge/__init__.py
View File
src/fori_serial_bridge/fori_serial_bridge/__pycache__/__init__.cpython-310.pyc
BIN
View File
Binary file not shown.
src/fori_serial_bridge/fori_serial_bridge/__pycache__/parking_controller_node.cpython-310.pyc
BIN
View File
Binary file not shown.
src/fori_serial_bridge/fori_serial_bridge/__pycache__/serial_bridge_node.cpython-310.pyc
BIN
View File
Binary file not shown.
src/fori_serial_bridge/fori_serial_bridge/__pycache__/ui_server_node.cpython-310.pyc
BIN
View File
Binary file not shown.
src/fori_serial_bridge/fori_serial_bridge/parking_controller_node.py
Executable
+1019
View File
File diff suppressed because it is too large Load Diff
src/fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py
Executable
+534
View File
@@ -0,0 +1,534 @@
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, TransformStamped, PoseWithCovarianceStamped
from sensor_msgs.msg import Imu, JointState, BatteryState
from nav_msgs.msg import Odometry
from std_msgs.msg import String
import serial
import struct
import time
import math
class ZLAC8015DModbus:
"""
Lightweight, zero-dependency Python implementation of ZLAC8015D Modbus RTU master.
"""
def __init__(self, port, baudrate=115200, timeout=0.02):
self.ser = serial.Serial(port, baudrate, timeout=timeout)
def calculate_crc(self, data):
crc = 0xFFFF
for pos in data:
crc ^= pos
for _ in range(8):
if (crc & 1) != 0:
crc >>= 1
crc ^= 0xA001
else:
crc >>= 1
return crc
def write_single(self, slave_id, reg, value):
packet = struct.pack('>BBHH', slave_id, 0x06, reg, value)
crc = self.calculate_crc(packet)
packet += struct.pack('<H', crc)
try:
self.ser.write(packet)
self.ser.flush()
# Read 8 byte echo response
self.ser.read(8)
return True
except Exception as e:
return False
def write_multiple(self, slave_id, start_reg, values):
num_regs = len(values)
header = struct.pack('>BBHHB', slave_id, 0x10, start_reg, num_regs, num_regs * 2)
data = b''
for val in values:
data += struct.pack('>h', val)
packet = header + data
crc = self.calculate_crc(packet)
packet += struct.pack('<H', crc)
try:
self.ser.write(packet)
self.ser.flush()
# Read 8 byte response
self.ser.read(8)
return True
except Exception as e:
return False
def read_registers(self, slave_id, start_reg, num_regs):
packet = struct.pack('>BBHH', slave_id, 0x03, start_reg, num_regs)
crc = self.calculate_crc(packet)
packet += struct.pack('<H', crc)
try:
# Clear input buffer
self.ser.reset_input_buffer()
self.ser.write(packet)
self.ser.flush()
expected_len = 5 + num_regs * 2
response = self.ser.read(expected_len)
if len(response) < expected_len:
return None
# Verify CRC
resp_crc = self.calculate_crc(response[:-2])
calc_crc = struct.unpack('<H', response[-2:])[0]
if resp_crc != calc_crc:
return None
values = []
for i in range(num_regs):
val = struct.unpack('>h', response[3 + i*2 : 5 + i*2])[0]
values.append(val)
return values
except Exception as e:
return None
class ForiSerialBridge(Node):
def __init__(self):
super().__init__('fori_serial_bridge')
# --- [ROS Parameters] ---
# "arduino" = PC -> Arduino Mega -> Drivers
# "direct_pc" = PC -> USB-to-RS485 -> Drivers
self.declare_parameter('control_method', 'direct_pc')
self.declare_parameter('port', '/dev/ttyUSB0') # Default port (ttyACM0 for Arduino, ttyUSB0 for direct)
self.declare_parameter('baud', 115200)
self.declare_parameter('target_linear_speed', 0.5)
self.declare_parameter('accel_limit', 0.5)
self.declare_parameter('wheel_base', 0.374)
self.declare_parameter('wheel_radius', 0.127)
self.control_method = self.get_parameter('control_method').value
self.port = self.get_parameter('port').value
self.baud = self.get_parameter('baud').value
self.limit_v = self.get_parameter('target_linear_speed').value
self.accel_limit = self.get_parameter('accel_limit').value
self.wheel_base = self.get_parameter('wheel_base').value
self.wheel_radius = self.get_parameter('wheel_radius').value
# Motor parameters
self.MAX_RPM = 250
# --- [IMU Yaw Integration / Control] ---
self.Kp_yaw = 0.50
self.Ki_yaw = 0.20
self.yaw_error_integral = 0.0
self.max_integral = 1.0
# --- [Robot states] ---
self.target_v = 0.0
self.target_w = 0.0
self.current_v = 0.0
self.current_w = 0.0
self.current_imu_w = 0.0
self.last_time = self.get_clock().now()
# Odometry states
self.odom_x = 0.0
self.odom_y = 0.0
self.odom_th = 0.0
# Wheel positions for JointState
self.left_wheel_joint_pos = 0.0
self.right_wheel_joint_pos = 0.0
# Driver state tracker (direct mode only)
self.drivers_initialized = False
self.drivers_enabled = False
self.is_parked = False # Track if the robot is fully parked
self.get_logger().info('==========================================')
self.get_logger().info(' FORI BLDC ZLAC8015D CONTROL BRIDGE ')
self.get_logger().info('==========================================')
self.get_logger().info(f' Control Method: {self.control_method}')
self.get_logger().info(f' Serial Port: {self.port} at {self.baud} baud')
self.get_logger().info('==========================================')
self.is_mock_mode = False
# --- [Serial/Modbus Connection] ---
if self.control_method == 'direct_pc':
try:
self.modbus = ZLAC8015DModbus(self.port, self.baud)
self.get_logger().info('BLDC 드라이버 초기화 대기 중...')
self.init_direct_drivers()
self.get_logger().info('BLDC 드라이버 초기화 완료!')
except Exception as e:
self.get_logger().warn(f'!!! 드라이버 연결 실패: {e} !!!')
self.get_logger().warn('실제 모터 드라이버 연결을 찾을 수 없어 [가상 모의 주행(Mock Mode)]으로 동작합니다.')
self.is_mock_mode = True
elif self.control_method == 'arduino':
try:
self.ser = serial.Serial(self.port, self.baud, timeout=0.01)
self.get_logger().info('아두이노 초기화 대기 중 (3초)...')
time.sleep(3)
self.get_logger().info('아두이노 메가 통신 준비 완료!')
except Exception as e:
self.get_logger().warn(f'!!! 아두이노 시리얼 연결 실패: {e} !!!')
self.get_logger().warn('아두이노 연결을 찾을 수 없어 [가상 모의 주행(Mock Mode)]으로 동작합니다.')
self.is_mock_mode = True
# --- [ROS Publishers & Subscribers] ---
qos_profile = rclpy.qos.QoSProfile(depth=1)
self.create_subscription(Twist, '/cmd_vel', self.cmd_vel_callback, qos_profile)
self.create_subscription(Imu, '/livox/imu', self.imu_callback, qos_profile)
self.create_subscription(PoseWithCovarianceStamped, '/initialpose', self.initial_pose_callback, 10)
self.create_subscription(String, '/robot_mode_status', self.mode_status_callback, 10)
# Telemetry/Odometry/Battery publishers
self.odom_pub = self.create_publisher(Odometry, '/odom_wheels', qos_profile)
self.joint_pub = self.create_publisher(JointState, '/joint_states', qos_profile)
self.battery_pub = self.create_publisher(BatteryState, '/battery_state', qos_profile)
# Control loop timer (20Hz)
self.create_timer(0.05, self.control_loop)
# Battery status timer (1Hz)
self.create_timer(1.0, self.publish_battery_status)
def cmd_vel_callback(self, msg):
self.target_v = max(min(msg.linear.x, self.limit_v), -self.limit_v)
self.target_w = msg.angular.z
def imu_callback(self, msg):
self.current_imu_w = msg.angular_velocity.z
def initial_pose_callback(self, msg):
self.odom_x = msg.pose.pose.position.x
self.odom_y = msg.pose.pose.position.y
q = msg.pose.pose.orientation
siny_cosp = 2 * (q.w * q.z + q.x * q.y)
cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
self.odom_th = math.atan2(siny_cosp, cosy_cosp)
self.get_logger().info(f'[MOCK] Reset robot pose to: x={self.odom_x:.2f}, y={self.odom_y:.2f}, yaw={self.odom_th:.2f}')
def mode_status_callback(self, msg):
# Checks if the robot has successfully arrived in PARKED state
self.is_parked = "State: PARKED" in msg.data
def publish_battery_status(self):
msg = BatteryState()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'base_link'
msg.design_capacity = 60.0 # 60Ah LiFePO4
msg.capacity = 60.0 # 60Ah LiFePO4
msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIFE # LiFePO4
voltage = 25.1 # Default nominal voltage for mock simulation mode
if not self.is_mock_mode and self.control_method == 'direct_pc':
try:
# Read both 0x20A0 (External Voltage) and 0x20A1 (Bus Voltage)
v_resp = self.modbus.read_registers(1, 0x20A0, 2)
if v_resp and len(v_resp) > 0:
raw_a0 = abs(v_resp[0])
raw_a1 = abs(v_resp[1]) if len(v_resp) > 1 else raw_a0
# Uncalibrated raw voltage from driver sensor
raw_v = 0.0
if 200 <= raw_a0 <= 320:
raw_v = raw_a0 * 0.1
elif 2000 <= raw_a0 <= 3200:
raw_v = raw_a0 * 0.01
elif 200 <= raw_a1 <= 320:
raw_v = raw_a1 * 0.1
elif 2000 <= raw_a1 <= 3200:
raw_v = raw_a1 * 0.01
else:
raw_v = raw_a1 * 0.01 if raw_a1 > 1000 else (raw_a1 * 0.1 if raw_a1 > 100 else float(raw_a1))
# Offset-Linear Calibration for 25.1V Multimeter Baseline
# Midpoint of driver's raw reading band for 25.1V is 28.2V
raw_baseline = 28.2
voltage = 25.1 + (raw_v - raw_baseline) * 0.88536
self.get_logger().info(f'[BATTERY TELEMETRY] Driver Raw: {raw_v:.2f}V -> Calibrated Real Battery: {voltage:.2f}V', throttle_duration_sec=3.0)
except Exception as e:
pass
# 2. Sliding Window Median + Heavy EMA Filter (eliminates IR drop jumps & Modbus spikes)
if not hasattr(self, 'voltage_history'):
import collections
self.voltage_history = collections.deque(maxlen=10)
self.voltage_history.append(voltage)
# Median filtering to reject any sudden Modbus voltage spike
sorted_v = sorted(self.voltage_history)
median_v = sorted_v[len(sorted_v) // 2]
if not hasattr(self, 'filtered_battery_voltage') or self.filtered_battery_voltage is None:
self.filtered_battery_voltage = median_v
else:
# Heavy EMA smoothing (Alpha = 0.05 for 10-second smooth transition)
self.filtered_battery_voltage = self.filtered_battery_voltage * 0.95 + median_v * 0.05
voltage = self.filtered_battery_voltage
# 3. 24V LiFePO4 8S battery voltage bounds: Full = 28.0V, Cutoff = 21.6V
v_min = 21.6
v_max = 28.0
percentage = (voltage - v_min) / (v_max - v_min)
percentage = max(0.0, min(1.0, percentage))
msg.voltage = float(voltage)
msg.percentage = float(percentage)
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
self.battery_pub.publish(msg)
def init_direct_drivers(self):
"""
Initializes ZLAC8015D parameters (Operating mode to Velocity Control).
"""
# Reg 0x200D (Operating Mode) = 3 (Velocity Mode)
self.modbus.write_single(1, 0x200D, 3)
time.sleep(0.01)
self.modbus.write_single(2, 0x200D, 3)
time.sleep(0.01)
self.disable_direct_drivers()
self.drivers_initialized = True
def enable_direct_drivers(self):
if self.drivers_enabled:
return
# Release brakes (0x201A, 0x201B) = 0
self.modbus.write_single(1, 0x201A, 0)
self.modbus.write_single(1, 0x201B, 0)
self.modbus.write_single(2, 0x201A, 0)
self.modbus.write_single(2, 0x201B, 0)
time.sleep(0.01)
# Enable drivers (0x200E) = 8
self.modbus.write_single(1, 0x200E, 8)
self.modbus.write_single(2, 0x200E, 8)
self.drivers_enabled = True
self.get_logger().info('BLDC Drivers and Brakes Released (ENABLED)')
def disable_direct_drivers(self):
# Stop velocity
self.modbus.write_multiple(1, 0x2088, [0, 0])
self.modbus.write_multiple(2, 0x2088, [0, 0])
# Lock brakes (0x201A, 0x201B) = 1
self.modbus.write_single(1, 0x201A, 1)
self.modbus.write_single(1, 0x201B, 1)
self.modbus.write_single(2, 0x201A, 1)
self.modbus.write_single(2, 0x201B, 1)
time.sleep(0.01)
# Disable drivers (0x200E) = 7
self.modbus.write_single(1, 0x200E, 7)
self.modbus.write_single(2, 0x200E, 7)
self.drivers_enabled = False
self.get_logger().info('BLDC Brakes Locked (DISABLED)')
def control_loop(self):
now = self.get_clock().now()
dt = (now - self.last_time).nanoseconds / 1e9
self.last_time = now
if dt <= 0: return
# 1. Acceleration Ramp (Smoothing)
dv = self.target_v - self.current_v
max_dv = self.accel_limit * dt
self.current_v += max(min(dv, max_dv), -max_dv)
dw = self.target_w - self.current_w
max_dw = 2.0 * dt
self.current_w += max(min(dw, max_dw), -max_dw)
# 2. Yaw rate PI Control
yaw_error = self.current_w - self.current_imu_w
# Stop condition
is_idle = (abs(self.target_v) < 0.01 and abs(self.target_w) < 0.01)
if is_idle:
self.yaw_error_integral = 0.0
correction = 0.0
self.current_v = 0.0
self.current_w = 0.0
else:
self.yaw_error_integral += yaw_error * dt
self.yaw_error_integral = max(min(self.yaw_error_integral, self.max_integral), -self.max_integral)
correction = (self.Kp_yaw * yaw_error) + (self.Ki_yaw * self.yaw_error_integral)
# 3. Inverse Kinematics (Wheel Linear Speeds)
v_left = self.current_v - ((self.current_w + correction) * self.wheel_base / 2.0)
v_right = self.current_v + ((self.current_w + correction) * self.wheel_base / 2.0)
# Convert to RPM (RPM = v / (2 * pi * r) * 60)
rpm_left = int(round(v_left / (2.0 * math.pi * self.wheel_radius) * 60.0))
rpm_right = int(round(v_right / (2.0 * math.pi * self.wheel_radius) * 60.0))
# Clip speed limits
rpm_left = max(min(rpm_left, self.MAX_RPM), -self.MAX_RPM)
rpm_right = max(min(rpm_right, self.MAX_RPM), -self.MAX_RPM)
# 4. Command the Motors
feedback_speeds = [0, 0, 0, 0] # FL, FR, RL, RR
if self.is_mock_mode:
feedback_speeds[0] = rpm_left
feedback_speeds[1] = rpm_right
feedback_speeds[2] = rpm_left
feedback_speeds[3] = rpm_right
else:
if self.control_method == 'direct_pc':
# Only disable drivers and lock brakes when idle AND fully parked
if is_idle and self.is_parked:
if self.drivers_enabled:
self.disable_direct_drivers()
else:
if not self.drivers_enabled:
self.enable_direct_drivers()
# Write velocities (invert right motor target RPM due to mirrored physical mounting)
self.modbus.write_multiple(1, 0x2088, [rpm_left, -rpm_right])
self.modbus.write_multiple(2, 0x2088, [rpm_left, -rpm_right])
# Read actual speeds (invert right motor feedback RPM due to mirrored physical mounting)
front_resp = self.modbus.read_registers(1, 0x20AD, 2)
rear_resp = self.modbus.read_registers(2, 0x20AD, 2)
if front_resp:
feedback_speeds[0] = front_resp[0]
feedback_speeds[1] = -front_resp[1]
if rear_resp:
feedback_speeds[2] = rear_resp[0]
feedback_speeds[3] = -rear_resp[1]
elif self.control_method == 'arduino':
# Send command packet
# Format: [0xFE, left_rpm_H, left_rpm_L, right_rpm_H, right_rpm_L, control_state, checksum]
packet = bytearray(7)
packet[0] = 0xFE
struct.pack_into('>h', packet, 1, rpm_left)
struct.pack_into('>h', packet, 3, rpm_right)
# control_state: 0 (disable) only when idle AND fully parked, otherwise 1 (enable)
packet[5] = 0 if (is_idle and self.is_parked) else 1
packet[6] = sum(packet[1:6]) & 0xFF
try:
self.ser.write(bytes(packet))
self.ser.flush()
except Exception as e:
self.get_logger().error(f'아두이노 송신 오류: {e}')
# Read feedback packet
# Format: [0xFD, f_lh, f_ll, f_rh, f_rl, r_lh, r_ll, r_rh, r_rl, checksum]
try:
if self.ser.in_waiting >= 10:
head = self.ser.read(1)
if head == b'\xfd':
data = self.ser.read(9)
if len(data) == 9:
checksum = sum(data[:-1]) & 0xFF
if checksum == data[-1]:
f_lh, f_rh, r_lh, r_rh = struct.unpack('>hhhh', data[:-1])
feedback_speeds[0] = f_lh
feedback_speeds[1] = f_rh
feedback_speeds[2] = r_lh
feedback_speeds[3] = r_rh
except Exception as e:
self.get_logger().error(f'아두이노 수신 오류: {e}')
# 5. Odometry & Telemetry Processing
self.process_feedback(feedback_speeds, dt)
# Print debug log (2s throttle)
self.get_logger().info(
f'[BLDC FEEDBACK] FL:{feedback_speeds[0]} FR:{feedback_speeds[1]} | '
f'RL:{feedback_speeds[2]} RR:{feedback_speeds[3]} RPM | '
f'Target_W:{self.current_w:.2f} IMU_W:{self.current_imu_w:.2f}',
throttle_duration_sec=2.0
)
def process_feedback(self, speeds, dt):
# Average front/rear left and right wheel speeds
actual_rpm_l = (speeds[0] + speeds[2]) / 2.0
actual_rpm_r = (speeds[1] + speeds[3]) / 2.0
# Convert RPM to linear speeds (v = rpm * 2 * pi * r / 60)
v_l = actual_rpm_l * 2.0 * math.pi * self.wheel_radius / 60.0
v_r = actual_rpm_r * 2.0 * math.pi * self.wheel_radius / 60.0
# Calculate robot linear and angular velocities
linear_vel = (v_r + v_l) / 2.0
angular_vel = (v_r - v_l) / self.wheel_base
# Integrate pose (Odometry)
delta_th = angular_vel * dt
self.odom_th += delta_th
self.odom_x += linear_vel * math.cos(self.odom_th) * dt
self.odom_y += linear_vel * math.sin(self.odom_th) * dt
# Update joint states
self.left_wheel_joint_pos += (v_l / self.wheel_radius) * dt
self.right_wheel_joint_pos += (v_r / self.wheel_radius) * dt
# Publish JointState
joint_state = JointState()
joint_state.header.stamp = self.get_clock().now().to_msg()
joint_state.name = ['front_left_wheel_joint', 'front_right_wheel_joint',
'rear_left_wheel_joint', 'rear_right_wheel_joint']
# Calculate velocity in rad/s
rads_l = v_l / self.wheel_radius
rads_r = v_r / self.wheel_radius
joint_state.position = [self.left_wheel_joint_pos, self.right_wheel_joint_pos,
self.left_wheel_joint_pos, self.right_wheel_joint_pos]
joint_state.velocity = [rads_l, rads_r, rads_l, rads_r]
self.joint_pub.publish(joint_state)
# Publish Odometry msg
odom = Odometry()
odom.header.stamp = joint_state.header.stamp
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_link'
# Set positions
odom.pose.pose.position.x = self.odom_x
odom.pose.pose.position.y = self.odom_y
odom.pose.pose.position.z = 0.0
# Quaternion from yaw
cy = math.cos(self.odom_th * 0.5)
sy = math.sin(self.odom_th * 0.5)
odom.pose.pose.orientation.w = cy
odom.pose.pose.orientation.z = sy
# Set velocities
odom.twist.twist.linear.x = linear_vel
odom.twist.twist.angular.z = angular_vel
self.odom_pub.publish(odom)
def main(args=None):
rclpy.init(args=args)
node = ForiSerialBridge()
try:
rclpy.spin(node)
except KeyboardInterrupt:
if node.control_method == 'direct_pc':
node.disable_direct_drivers()
elif node.control_method == 'arduino':
try:
# Send explicit stop packet
packet = bytearray(7)
packet[0] = 0xFE
packet[5] = 0
packet[6] = sum(packet[1:6]) & 0xFF
node.ser.write(bytes(packet))
node.ser.flush()
except:
pass
node.get_logger().info('ROS2 BRIDGE STOPPED')
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
src/fori_serial_bridge/fori_serial_bridge/ui_server_node.py
Executable
+78
View File
@@ -0,0 +1,78 @@
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from ament_index_python.packages import get_package_share_directory
import http.server
import socketserver
import threading
import os
import webbrowser
class ThreadingHTTPServer(socketserver.ThreadingMixIn, http.server.HTTPServer):
pass
class UIServerNode(Node):
def __init__(self):
super().__init__('ui_server_node')
# Parameters
self.declare_parameter('port', 8080)
self.port = self.get_parameter('port').value
# Locate UI directory in share space
share_dir = get_package_share_directory('fori_serial_bridge')
self.ui_dir = os.path.join(share_dir, 'ui')
if not os.path.exists(self.ui_dir):
# Fallback to source directory for development / symlink install
self.ui_dir = os.path.expanduser('~/fori_ws/src/fori_serial_bridge/ui')
self.get_logger().info(f'Serving UI files from directory: {self.ui_dir}')
# Start HTTP server in a separate daemon thread
self.server_thread = threading.Thread(target=self.start_server, daemon=True)
self.server_thread.start()
def start_server(self):
# Change working directory of the handler to UI directory
os.chdir(self.ui_dir)
handler = http.server.SimpleHTTPRequestHandler
try:
self.server = ThreadingHTTPServer(("", self.port), handler)
self.get_logger().info('==========================================')
self.get_logger().info(f' Web UI Dashboard server launched! ')
self.get_logger().info(f' Connect via: http://localhost:{self.port} ')
self.get_logger().info('==========================================')
# Automatically open default web browser
try:
webbrowser.open(f"http://localhost:{self.port}")
except Exception as browser_err:
self.get_logger().warn(f'Failed to auto-open web browser: {browser_err}')
self.server.serve_forever()
except Exception as e:
self.get_logger().error(f'Failed to start web server on port {self.port}: {e}')
def destroy_node(self):
self.get_logger().info('Shutting down Web UI server...')
if hasattr(self, 'server'):
self.server.shutdown()
self.server.server_close()
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
node = UIServerNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
if rclpy.ok():
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
src/fori_serial_bridge/launch/fori_full.launch.py
+184
View File
@@ -0,0 +1,184 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
# Check if physical port exists to toggle simulation mock mode
port_exists = os.path.exists('/dev/ttyUSB0')
mock_mode_param = True if not port_exists else False
# Package directories
serial_bridge_dir = get_package_share_directory('fori_serial_bridge')
hik_camera_dir = get_package_share_directory('hik_camera_ros2_driver')
# Map configuration file path
map_yaml_file = '/home/yoo/fori_map.yaml'
# 2. Include Hikrobot Camera launch description (pass camera config to isolate it from Nav2 parameters)
camera_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(hik_camera_dir, 'launch', 'hik_camera_launch.py')
),
launch_arguments={
'params_file': os.path.join(hik_camera_dir, 'config', 'camera_params.yaml')
}.items()
)
# 3. Launch ArUco Detector (run standalone Python script)
aruco_detector_node = Node(
executable='python3',
arguments=['/home/yoo/camera_ws/src/aruco_detector_node.py'],
name='aruco_detector_node',
output='screen'
)
# 4. Launch Motor Bridge Node (ZLAC8015D direct PC RS485 Mode by default)
motor_bridge_node = Node(
package='fori_serial_bridge',
executable='serial_bridge_node',
name='serial_bridge_node',
parameters=[{
'control_method': 'direct_pc',
'port': '/dev/ttyUSB0',
'baud': 115200,
'target_linear_speed': 0.5,
'accel_limit': 0.5,
'wheel_base': 0.374,
'wheel_radius': 0.127
}],
output='screen'
)
# 5. Launch Parking Controller Node
parking_controller_node = Node(
package='fori_serial_bridge',
executable='parking_controller_node',
name='parking_controller_node',
parameters=[{
'parking_waypoint_x': 0.0,
'parking_waypoint_y': 0.0,
'parking_waypoint_yaw': 0.0,
'parking_frame': 'map',
'target_distance': 0.5,
'kp_linear': 0.25,
'kp_bearing': 0.8,
'kp_yaw': 0.4,
'mock_mode': mock_mode_param
}],
remappings=[
('/cmd_vel', '/cmd_vel') # Keep topic clean
],
output='screen'
)
# 6. Launch ROSbridge WebSocket Server (run node directly to bypass XML syntax conflict)
rosbridge_node = Node(
package='rosbridge_server',
executable='rosbridge_websocket',
name='rosbridge_websocket',
parameters=[{
'port': 9090
}],
output='screen'
)
# 7. Launch UI Web Server Node (Serves Dashboard files on port 8080)
ui_server_node = Node(
package='fori_serial_bridge',
executable='ui_server_node',
name='ui_server_node',
parameters=[{
'port': 8080
}],
output='screen'
)
# 8. Launch web_video_server (MJPEG HTTP stream on port 8085 for optimized camera feed)
web_video_server_node = Node(
package='web_video_server',
executable='web_video_server',
name='web_video_server',
parameters=[{
'port': 8085
}],
respawn=True, # Auto-restart if the node crashes
respawn_delay=2.0, # Wait 2 seconds before restarting
output='screen'
)
# Initialize LaunchDescription
ld = LaunchDescription()
# Add core nodes that are always run
ld.add_action(camera_launch)
ld.add_action(aruco_detector_node)
ld.add_action(motor_bridge_node)
ld.add_action(parking_controller_node)
ld.add_action(rosbridge_node)
ld.add_action(ui_server_node)
ld.add_action(web_video_server_node)
# Conditionally launch localization/map components
if mock_mode_param:
# 1. Standalone map server (does not require AMCL or LIDAR sensor data)
map_server_node = Node(
package='nav2_map_server',
executable='map_server',
name='map_server',
parameters=[{
'yaml_filename': map_yaml_file,
'use_sim_time': False
}],
output='screen'
)
# 2. Lifecycle manager to configure and activate the standalone map server
lifecycle_manager_node = Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_map',
parameters=[{
'use_sim_time': False,
'autostart': True,
'node_names': ['map_server']
}],
output='screen'
)
# 3. Static transform publisher for map -> odom since AMCL is bypassed
static_tf_node = Node(
package='tf2_ros',
executable='static_transform_publisher',
name='static_tf_map_to_odom',
arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
output='screen'
)
# 4. Robot State Publisher (URDF) to define joints/frames
urdf_file_path = os.path.expanduser('~/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf')
with open(urdf_file_path, 'r') as infp:
robot_desc = infp.read()
rsp_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[{'robot_description': robot_desc}],
output='screen'
)
ld.add_action(map_server_node)
ld.add_action(lifecycle_manager_node)
ld.add_action(static_tf_node)
ld.add_action(rsp_node)
else:
# In real mode, include the full Nav2/AMCL/LIDAR localization launch
nav2_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource('/home/yoo/fori_ws/src/fori_nav2.launch.py')
)
ld.add_action(nav2_launch)
return ld
src/fori_serial_bridge/package.xml
+25
View File
@@ -0,0 +1,25 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>fori_serial_bridge</name>
<version>0.0.1</version>
<description>Serial Bridge for FORI AGV with Smoothing and Delay Optimization</description>
<maintainer email="user@todo.todo">yoo</maintainer>
<license>Apache License 2.0</license>
<depend>rclpy</depend>
<depend>geometry_msgs</depend>
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>nav2_msgs</depend>
<depend>action_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/fori_serial_bridge/resource/fori_serial_bridge
View File
src/fori_serial_bridge/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/fori_serial_bridge
[install]
install_scripts=$base/lib/fori_serial_bridge
src/fori_serial_bridge/setup.py
+30
View File
@@ -0,0 +1,30 @@
from setuptools import setup
package_name = 'fori_serial_bridge'
setup(
name=package_name,
version='0.0.1',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name + '/launch', ['launch/fori_full.launch.py']),
('share/' + package_name + '/ui', ['ui/index.html', 'ui/style.css', 'ui/app.js']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='yoo',
maintainer_email='user@todo.todo',
description='Serial Bridge for FORI AGV with Smoothing and Delay Optimization',
license='Apache License 2.0',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'serial_bridge_node = fori_serial_bridge.serial_bridge_node:main',
'parking_controller_node = fori_serial_bridge.parking_controller_node:main',
'ui_server_node = fori_serial_bridge.ui_server_node:main'
],
},
)
src/fori_serial_bridge/ui/app.js
+876
View File
@@ -0,0 +1,876 @@
// FORI AGV Dashboard Client Application
// --- [UI Performance Optimization Config] ---
// Set to true to use web_video_server for video stream, false to use standard rosbridge
const USE_WEB_VIDEO_SERVER = true;
const WEB_VIDEO_SERVER_PORT = 8085; // Port for web_video_server (avoid conflict with ui_server 8080)
// --- [ROS2 Connection Setup] ---
const rosHost = window.location.hostname || 'localhost';
const ros = new ROSLIB.Ros({
url: `ws://${rosHost}:9090`
});
const statusIndicator = document.getElementById('status-indicator');
const statusText = document.getElementById('status-text');
ros.on('connection', () => {
statusIndicator.className = 'pulse-indicator green';
statusText.innerText = 'Connected';
console.log('Connected to rosbridge WebSocket server.');
});
ros.on('error', (error) => {
statusIndicator.className = 'pulse-indicator red';
statusText.innerText = 'Connection Error';
console.error('Error connecting to rosbridge WebSocket:', error);
});
ros.on('close', () => {
statusIndicator.className = 'pulse-indicator red';
statusText.innerText = 'Disconnected';
console.log('Connection to rosbridge WebSocket closed.');
});
// --- [ROS2 Topics & Message Types] ---
// Mode Control
const robotModePub = new ROSLIB.Topic({
ros: ros,
name: '/robot_mode',
messageType: 'std_msgs/msg/String'
});
const robotModeStatusSub = new ROSLIB.Topic({
ros: ros,
name: '/robot_mode_status',
messageType: 'std_msgs/msg/String'
});
// Waypoint configuration
const waypointPub = new ROSLIB.Topic({
ros: ros,
name: '/aruco_marker_waypoint',
messageType: 'geometry_msgs/msg/PoseStamped'
});
// Nav2 goal configuration
const nav2GoalPub = new ROSLIB.Topic({
ros: ros,
name: '/goal_pose',
messageType: 'geometry_msgs/msg/PoseStamped'
});
// Initial pose publisher (2D Pose Estimate)
const initialPosePub = new ROSLIB.Topic({
ros: ros,
name: '/initialpose',
messageType: 'geometry_msgs/msg/PoseWithCovarianceStamped'
});
// Camera image stream (throttled to 10Hz if fallback is used)
const imageSub = new ROSLIB.Topic({
ros: ros,
name: '/aruco_detector/image/compressed',
messageType: 'sensor_msgs/msg/CompressedImage',
throttle_rate: 100
});
// ArUco marker pose subscriber
const arucoPoseSub = new ROSLIB.Topic({
ros: ros,
name: '/aruco_marker_pose',
messageType: 'geometry_msgs/msg/PoseStamped',
throttle_rate: 50
});
// Map subscriber (throttled to 1Hz since maps update infrequently)
const mapSub = new ROSLIB.Topic({
ros: ros,
name: '/map',
messageType: 'nav_msgs/msg/OccupancyGrid',
throttle_rate: 1000
});
// Odometry subscriber (throttled to 10Hz for smoother rendering without CPU spikes)
const odomSub = new ROSLIB.Topic({
ros: ros,
name: '/odom_wheels',
messageType: 'nav_msgs/msg/Odometry',
throttle_rate: 100
});
// Joint State subscriber (throttled to 5Hz to update RPM gauges efficiently)
const jointStateSub = new ROSLIB.Topic({
ros: ros,
name: '/joint_states',
messageType: 'sensor_msgs/msg/JointState',
throttle_rate: 200
});
// Battery State subscriber
const batterySub = new ROSLIB.Topic({
ros: ros,
name: '/battery_state',
messageType: 'sensor_msgs/msg/BatteryState',
throttle_rate: 500
});
batterySub.subscribe(function(msg) {
const voltage = msg.voltage ? msg.voltage.toFixed(1) : '26.4';
const pct = Math.round((msg.percentage !== undefined ? msg.percentage : 0.85) * 100);
const badge = document.getElementById('battery-badge');
const bar = document.getElementById('battery-bar');
if (badge) {
badge.innerText = `${pct}% (${voltage}V)`;
if (pct >= 50) {
badge.style.background = 'rgba(34, 197, 94, 0.2)';
badge.style.color = '#4ade80';
badge.style.borderColor = 'rgba(74, 222, 128, 0.4)';
} else if (pct >= 20) {
badge.style.background = 'rgba(234, 179, 8, 0.2)';
badge.style.color = '#facc15';
badge.style.borderColor = 'rgba(250, 204, 21, 0.4)';
} else {
badge.style.background = 'rgba(239, 68, 68, 0.2)';
badge.style.color = '#f87171';
badge.style.borderColor = 'rgba(248, 113, 113, 0.4)';
}
}
if (bar) {
bar.style.width = `${pct}%`;
if (pct >= 50) {
bar.style.background = 'linear-gradient(90deg, #22c55e, #4ade80)';
} else if (pct >= 20) {
bar.style.background = 'linear-gradient(90deg, #eab308, #facc15)';
} else {
bar.style.background = 'linear-gradient(90deg, #ef4444, #f87171)';
}
}
});
// --- [Global State] ---
let robotPose = { x: 0.0, y: 0.0, yaw: 0.0 };
let parkingWaypoint = null; // Start as null so no marker is drawn at (0,0) by default
let nav2Goal = { x: 0.0, y: 0.0, yaw: 0.0 };
let nav2GoalActive = false;
let currentMode = 'nav2'; // 'nav2' or 'parking'
let mapData = null;
let mapInfo = null;
let feedbackRPMs = [0, 0, 0, 0]; // FL, FR, RL, RR
// Offscreen canvas for map pre-rendering performance and zoom transforms
const offscreenCanvas = document.createElement('canvas');
const offscreenCtx = offscreenCanvas.getContext('2d');
// Zoom and Pan states
let zoom = 1.0;
let panX = 0.0;
let panY = 0.0;
let isPanning = false;
let panStart = { x: 0, y: 0 };
// --- [Canvas Map Rendering] ---
const canvas = document.getElementById('map-canvas');
const ctx = canvas.getContext('2d');
function drawMap() {
if (!mapData || !mapInfo) return;
const w = mapInfo.width;
const h = mapInfo.height;
// Resize main canvas dimensions to match map aspect ratio
if (canvas.width !== w || canvas.height !== h) {
canvas.width = w;
canvas.height = h;
}
// Clear main canvas
ctx.clearRect(0, 0, canvas.width, canvas.height);
ctx.save();
// Apply zoom & pan transforms relative to center of canvas
ctx.translate(canvas.width / 2 + panX, canvas.height / 2 + panY);
ctx.scale(zoom, zoom);
ctx.translate(-canvas.width / 2, -canvas.height / 2);
// Draw the offscreen map
ctx.drawImage(offscreenCanvas, 0, 0);
ctx.restore();
// Draw Parking Waypoint (Target)
if (currentMode === 'parking' && parkingWaypoint) {
drawTarget(parkingWaypoint.x, parkingWaypoint.y, parkingWaypoint.yaw, '#00d2ff');
}
// Draw Nav2 Goal Target (Yellow)
if (currentMode === 'nav2' && nav2GoalActive) {
drawTarget(nav2Goal.x, nav2Goal.y, nav2Goal.yaw, '#eab308');
}
// Draw Robot Pose (Triangle)
drawRobot(robotPose.x, robotPose.y, robotPose.yaw, '#10b981');
}
// Convert ROS meters coordinate (x,y) to Canvas pixels index (u,v)
defRosToCanvas = (rx, ry) => {
if (!mapInfo) return { u: 0, v: 0 };
const res = mapInfo.resolution;
const originX = mapInfo.origin.position.x;
const originY = mapInfo.origin.position.y;
// Raw pixels coordinates
const u_raw = (rx - originX) / res;
const v_raw = canvas.height - ((ry - originY) / res);
// Apply zoom & pan transformations
const u = (u_raw - canvas.width / 2) * zoom + canvas.width / 2 + panX;
const v = (v_raw - canvas.height / 2) * zoom + canvas.height / 2 + panY;
return { u, v };
};
// Convert Canvas pixels index (u,v) back to ROS meters coordinate (x,y)
defCanvasToRos = (u, v) => {
if (!mapInfo) return { rx: 0, ry: 0 };
const res = mapInfo.resolution;
const originX = mapInfo.origin.position.x;
const originY = mapInfo.origin.position.y;
// Reverse zoom & pan transformations
const u_raw = (u - panX - canvas.width / 2) / zoom + canvas.width / 2;
const v_raw = (v - panY - canvas.height / 2) / zoom + canvas.height / 2;
const rx = (u_raw * res) + originX;
const ry = ((canvas.height - v_raw) * res) + originY;
return { rx, ry };
};
function drawRobot(rx, ry, ryaw, color) {
const pt = defRosToCanvas(rx, ry);
ctx.save();
ctx.translate(pt.u, pt.v);
// Draw robot pointing relative to Yaw
// Canvas rotation is clockwise. In ROS, yaw increases counter-clockwise.
// So we negate yaw to align correctly.
ctx.rotate(-ryaw);
// Draw triangle
ctx.fillStyle = color;
ctx.shadowBlur = 10;
ctx.shadowColor = color;
ctx.beginPath();
ctx.moveTo(10, 0);
ctx.lineTo(-8, -6);
ctx.lineTo(-4, 0);
ctx.lineTo(-8, 6);
ctx.closePath();
ctx.fill();
ctx.restore();
}
function drawTarget(rx, ry, ryaw, color) {
const pt = defRosToCanvas(rx, ry);
ctx.save();
ctx.translate(pt.u, pt.v);
ctx.rotate(-ryaw);
// Draw target marker
ctx.strokeStyle = color;
ctx.lineWidth = 2;
ctx.shadowBlur = 8;
ctx.shadowColor = color;
ctx.beginPath();
ctx.arc(0, 0, 7, 0, 2 * Math.PI);
ctx.stroke();
// Draw heading line
ctx.beginPath();
ctx.moveTo(0, 0);
ctx.lineTo(12, 0);
ctx.stroke();
ctx.restore();
}
// --- [Subscribe Listeners] ---
// Map listener
mapSub.subscribe((message) => {
mapData = message.data;
mapInfo = message.info;
const w = mapInfo.width;
const h = mapInfo.height;
// Re-initialize offscreen canvas sizes if map sizes change
if (offscreenCanvas.width !== w || offscreenCanvas.height !== h) {
offscreenCanvas.width = w;
offscreenCanvas.height = h;
}
// Draw occupancy grid grid-by-grid onto offscreen canvas
const imgData = offscreenCtx.createImageData(w, h);
for (let i = 0; i < mapData.length; i++) {
const val = mapData[i];
let r, g, b, a;
if (val === 0) {
r = 15; g = 18; b = 32; a = 255;
} else if (val === 100) {
r = 157; g = 78; b = 221; a = 255;
} else {
r = 6; g = 6; b = 10; a = 255;
}
const col = i % w;
const row = h - 1 - Math.floor(i / w);
const pixelIdx = (row * w + col) * 4;
imgData.data[pixelIdx] = r;
imgData.data[pixelIdx + 1] = g;
imgData.data[pixelIdx + 2] = b;
imgData.data[pixelIdx + 3] = a;
}
offscreenCtx.putImageData(imgData, 0, 0);
drawMap();
});
// Odom listener
odomSub.subscribe((message) => {
const pose = message.pose.pose;
robotPose.x = pose.position.x;
robotPose.y = pose.position.y;
// Quaternion to Euler yaw
const q = pose.orientation;
const siny_cosp = 2 * (q.w * q.z + q.x * q.y);
const cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
robotPose.yaw = Math.atan2(siny_cosp, cosy_cosp);
// Update speeds
const twist = message.twist.twist;
document.getElementById('val-linear').innerText = `${twist.linear.x.toFixed(2)} m/s`;
document.getElementById('val-angular').innerText = `${twist.angular.z.toFixed(2)} rad/s`;
// Update real-time pose metrics
const poseVal = document.getElementById('val-pose');
if (poseVal) {
const yawDeg = Math.round(robotPose.yaw * 180 / Math.PI);
poseVal.innerHTML = `X: ${robotPose.x.toFixed(2)}m &nbsp;|&nbsp; Y: ${robotPose.y.toFixed(2)}m &nbsp;|&nbsp; Yaw: ${yawDeg}°`;
}
drawMap();
});
// Camera stream setup with automatic fallback
const cameraStreamImg = document.getElementById('camera-stream');
let rosbridgeCameraActive = false;
function startRosbridgeCameraFallback() {
if (rosbridgeCameraActive) return;
rosbridgeCameraActive = true;
console.warn('Falling back to ROS Bridge WebSocket camera stream (base64).');
imageSub.subscribe((message) => {
cameraStreamImg.src = "data:image/jpeg;base64," + message.data;
});
}
function startWebVideoStream() {
const videoStreamUrl = `http://${rosHost}:${WEB_VIDEO_SERVER_PORT}/stream?topic=/aruco_detector/image&type=mjpeg&transport=compressed`;
cameraStreamImg.src = videoStreamUrl;
cameraStreamImg.onerror = () => {
// web_video_server is down — retry after 3s, then fall back to rosbridge
console.warn(`web_video_server unreachable. Retrying in 3s...`);
cameraStreamImg.src = '';
setTimeout(() => {
// Try once more before giving up and switching to rosbridge
const retryImg = new Image();
retryImg.onload = () => {
// Server is back up — restore the stream
console.log('web_video_server recovered. Restoring MJPEG stream.');
cameraStreamImg.src = videoStreamUrl;
cameraStreamImg.onerror = startWebVideoStream; // Re-attach error handler
};
retryImg.onerror = () => {
console.warn('web_video_server still down. Switching to ROS Bridge fallback.');
startRosbridgeCameraFallback();
};
retryImg.src = `http://${rosHost}:${WEB_VIDEO_SERVER_PORT}/`;
}, 3000);
};
console.log(`Subscribed to camera stream via web_video_server: ${videoStreamUrl}`);
}
if (USE_WEB_VIDEO_SERVER) {
startWebVideoStream();
} else {
console.log("Subscribing to camera stream via ROS Bridge WebSocket (base64 fallback).");
startRosbridgeCameraFallback();
}
// ArUco pose subscription for real-time UI feedback
let arucoTimeout = null;
arucoPoseSub.subscribe((message) => {
if (arucoTimeout) {
clearTimeout(arucoTimeout);
}
const statusBadge = document.getElementById('aruco-status');
statusBadge.innerText = '인식됨 (Detected)';
statusBadge.className = 'aruco-status-badge connected';
// Parse ID from frame_id
const markerId = message.header.frame_id.replace('aruco_marker_', '');
const x = message.pose.position.x;
const y = message.pose.position.y;
const z = message.pose.position.z;
// Convert quaternion to Euler angles (Roll, Pitch, Yaw)
const q = message.pose.orientation;
// roll (x-axis rotation)
const sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
const cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
const roll = Math.atan2(sinr_cosp, cosr_cosp) * 180 / Math.PI;
// pitch (y-axis rotation)
const sinp = 2 * (q.w * q.y - q.z * q.x);
let pitch = 0;
if (Math.abs(sinp) >= 1) {
pitch = Math.sign(sinp) * 90;
} else {
pitch = Math.asin(sinp) * 180 / Math.PI;
}
// yaw (z-axis rotation)
const siny_cosp = 2 * (q.w * q.z + q.x * q.y);
const cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
const yaw = Math.atan2(siny_cosp, cosy_cosp) * 180 / Math.PI;
document.getElementById('aruco-id').innerText = markerId;
document.getElementById('aruco-x').innerText = x.toFixed(3) + ' m';
document.getElementById('aruco-y').innerText = y.toFixed(3) + ' m';
document.getElementById('aruco-z').innerText = z.toFixed(3) + ' m';
document.getElementById('aruco-yaw').innerText = yaw.toFixed(1) + '°';
document.getElementById('aruco-rp').innerText = roll.toFixed(1) + '° / ' + pitch.toFixed(1) + '°';
// Reset UI if no new message within 1 second
arucoTimeout = setTimeout(() => {
statusBadge.innerText = '미인식 (No Marker)';
statusBadge.className = 'aruco-status-badge disconnected';
document.getElementById('aruco-id').innerText = '-';
document.getElementById('aruco-x').innerText = '-';
document.getElementById('aruco-y').innerText = '-';
document.getElementById('aruco-z').innerText = '-';
document.getElementById('aruco-yaw').innerText = '-';
document.getElementById('aruco-rp').innerText = '-';
}, 1000);
});
// Robot state listener
robotModeStatusSub.subscribe((message) => {
document.getElementById('val-state').innerText = message.data;
const status = message.data.toLowerCase();
// Update Environment Badge
const envBadge = document.getElementById('env-badge');
if (status.includes('simulation')) {
envBadge.innerText = 'Simulation Mode';
envBadge.className = 'env-badge sim';
} else if (status.includes('real')) {
envBadge.innerText = 'Real AGV Mode';
envBadge.className = 'env-badge real';
}
if (status.includes('mode: nav2')) {
currentMode = 'nav2';
document.getElementById('btn-mode-nav2').className = 'btn btn-primary active';
document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
document.getElementById('btn-mode-parking').className = 'btn btn-primary';
document.getElementById('btn-emergency-stop').className = 'btn btn-danger';
} else if (status.includes('mode: patrol')) {
currentMode = 'patrol';
document.getElementById('btn-mode-patrol').className = 'btn btn-primary active';
document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
document.getElementById('btn-mode-parking').className = 'btn btn-primary';
document.getElementById('btn-emergency-stop').className = 'btn btn-danger';
} else if (status.includes('mode: parking')) {
currentMode = 'parking';
document.getElementById('btn-mode-parking').className = 'btn btn-primary active';
document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
document.getElementById('btn-emergency-stop').className = 'btn btn-danger';
} else if (status.includes('mode: stop') || status.includes('state: estop')) {
currentMode = 'stop';
document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
document.getElementById('btn-mode-parking').className = 'btn btn-primary';
document.getElementById('btn-emergency-stop').className = 'btn btn-danger active';
}
});
// Joint State listener (RPM updates)
jointStateSub.subscribe((message) => {
// Get velocities in rad/s, convert to RPM (RPM = rad/s * 60 / (2 * pi))
// Joint indices might vary, we average or read them based on names
const names = message.name;
const vel = message.velocity;
names.forEach((name, idx) => {
const rpm = Math.round(vel[idx] * 60 / (2 * Math.PI));
let barId = '';
let valId = '';
if (name.includes('front_left')) { barId = 'bar-fl'; valId = 'val-fl'; }
else if (name.includes('front_right')) { barId = 'bar-fr'; valId = 'val-fr'; }
else if (name.includes('rear_left')) { barId = 'bar-rl'; valId = 'val-rl'; }
else if (name.includes('rear_right')) { barId = 'bar-rr'; valId = 'val-rr'; }
if (barId && valId) {
document.getElementById(valId).innerText = `${rpm} RPM`;
// Scale bar width (max 250 RPM is 100% width)
const pct = Math.min(Math.abs(rpm) / 250 * 100, 100);
document.getElementById(barId).style.width = `${pct}%`;
}
});
});
// --- [UI Interaction Actions] ---
// Toggle Nav2 mode
document.getElementById('btn-mode-nav2').addEventListener('click', () => {
currentMode = 'nav2';
nav2GoalActive = false;
parkingWaypoint = null; // Clear waypoint on UI
document.getElementById('btn-mode-nav2').className = 'btn btn-primary active';
document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
document.getElementById('btn-mode-parking').className = 'btn btn-primary';
const msg = new ROSLIB.Message({ data: 'nav2' });
robotModePub.publish(msg);
});
// Toggle Patrol mode
document.getElementById('btn-mode-patrol').addEventListener('click', () => {
currentMode = 'patrol';
parkingWaypoint = null;
nav2GoalActive = false;
document.getElementById('btn-mode-patrol').className = 'btn btn-primary active';
document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
document.getElementById('btn-mode-parking').className = 'btn btn-primary';
const msg = new ROSLIB.Message({ data: 'patrol' });
robotModePub.publish(msg);
});
// Toggle Parking mode
document.getElementById('btn-mode-parking').addEventListener('click', () => {
currentMode = 'parking';
parkingWaypoint = null; // Clear waypoint on UI until set by user
document.getElementById('btn-mode-parking').className = 'btn btn-primary active';
document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
const msg = new ROSLIB.Message({ data: 'parking' });
robotModePub.publish(msg);
});
// Toggle 2D Pose Estimate Mode
let poseEstimateMode = false;
document.getElementById('btn-pose-estimate').addEventListener('click', () => {
poseEstimateMode = true;
document.getElementById('btn-pose-estimate').className = 'btn btn-secondary active';
document.getElementById('btn-pose-estimate').innerText = '📍 지도를 클릭/드래그하여 로봇 위치 지정...';
nav2GoalActive = false;
drawMap();
});
// Trigger Emergency Stop (ESTOP)
document.getElementById('btn-emergency-stop').addEventListener('click', () => {
currentMode = 'stop';
nav2GoalActive = false;
parkingWaypoint = null; // Clear waypoint on UI
document.getElementById('btn-mode-nav2').className = 'btn btn-primary';
document.getElementById('btn-mode-patrol').className = 'btn btn-primary';
document.getElementById('btn-mode-parking').className = 'btn btn-primary';
document.getElementById('btn-emergency-stop').className = 'btn btn-danger active';
const msg = new ROSLIB.Message({ data: 'stop' });
robotModePub.publish(msg);
});
// Send updated Waypoint manually from inputs
document.getElementById('btn-set-wp').addEventListener('click', () => {
const x = parseFloat(document.getElementById('wp-x').value);
const y = parseFloat(document.getElementById('wp-y').value);
const yawDeg = parseFloat(document.getElementById('wp-yaw').value);
const yawRad = yawDeg * Math.PI / 180.0;
if (currentMode === 'nav2') {
nav2Goal.x = x;
nav2Goal.y = y;
nav2Goal.yaw = yawRad;
nav2GoalActive = true;
publishNav2Goal();
} else {
parkingWaypoint = { x: x, y: y, yaw: yawRad };
publishWaypoint();
}
});
// Drag-to-steer (Rviz2 style) mouse handlers
// Drag-to-steer and Pan/Zoom mouse handlers
let isDragging = false;
let dragStartCoords = { x: 0, y: 0 };
let dragStartRos = { x: 0, y: 0 };
let currentDragYaw = 0.0;
// Prevent standard context menu on canvas to allow right-click panning
canvas.addEventListener('contextmenu', (event) => {
event.preventDefault();
});
canvas.addEventListener('mousedown', (event) => {
if (!mapInfo) return;
// Right click OR Shift + Left click starts Panning
if (event.button === 2 || (event.button === 0 && event.shiftKey)) {
isPanning = true;
panStart.x = event.clientX;
panStart.y = event.clientY;
return;
}
// Standard Left Click starts Drag-to-steer
if (event.button === 0) {
isDragging = true;
const rect = canvas.getBoundingClientRect();
const scale = Math.min(rect.width / canvas.width, rect.height / canvas.height);
const dx_padding = (rect.width - canvas.width * scale) / 2;
const dy_padding = (rect.height - canvas.height * scale) / 2;
const clickU = (event.clientX - rect.left - dx_padding) / scale;
const clickV = (event.clientY - rect.top - dy_padding) / scale;
dragStartCoords.x = clickU;
dragStartCoords.y = clickV;
const coords = defCanvasToRos(clickU, clickV);
dragStartRos.x = coords.rx;
dragStartRos.y = coords.ry;
document.getElementById('wp-x').value = coords.rx.toFixed(2);
document.getElementById('wp-y').value = coords.ry.toFixed(2);
const yawDeg = parseFloat(document.getElementById('wp-yaw').value) || 0.0;
currentDragYaw = yawDeg * Math.PI / 180.0;
if (poseEstimateMode) {
// Update local robotPose estimate representation
robotPose.x = coords.rx;
robotPose.y = coords.ry;
robotPose.yaw = currentDragYaw;
} else if (currentMode === 'nav2') {
nav2Goal.x = coords.rx;
nav2Goal.y = coords.ry;
nav2Goal.yaw = currentDragYaw;
nav2GoalActive = true;
} else {
parkingWaypoint = { x: coords.rx, y: coords.ry, yaw: currentDragYaw };
}
drawMap();
}
});
canvas.addEventListener('mousemove', (event) => {
const rect = canvas.getBoundingClientRect();
const scale = Math.min(rect.width / canvas.width, rect.height / canvas.height);
if (isPanning) {
const dx = event.clientX - panStart.x;
const dy = event.clientY - panStart.y;
// Pan dynamically mapped to canvas pixels and zoom factor
panX += dx / (scale * zoom);
panY += dy / (scale * zoom);
panStart.x = event.clientX;
panStart.y = event.clientY;
drawMap();
return;
}
if (!isDragging || !mapInfo) return;
const dx_padding = (rect.width - canvas.width * scale) / 2;
const dy_padding = (rect.height - canvas.height * scale) / 2;
const clickU = (event.clientX - rect.left - dx_padding) / scale;
const clickV = (event.clientY - rect.top - dy_padding) / scale;
const dx = clickU - dragStartCoords.x;
const dy = clickV - dragStartCoords.y;
if (Math.sqrt(dx * dx + dy * dy) > 8) { // Drag threshold
currentDragYaw = Math.atan2(-dy, dx);
let yawDeg = Math.round(currentDragYaw * 180.0 / Math.PI);
if (yawDeg < 0) yawDeg += 360;
document.getElementById('wp-yaw').value = yawDeg;
if (poseEstimateMode) {
robotPose.yaw = currentDragYaw;
} else if (currentMode === 'nav2') {
nav2Goal.yaw = currentDragYaw;
} else {
if (!parkingWaypoint) {
parkingWaypoint = { x: dragStartRos.x, y: dragStartRos.y, yaw: currentDragYaw };
}
parkingWaypoint.yaw = currentDragYaw;
}
drawMap();
// Render drag line (drawn in screen space)
ctx.strokeStyle = '#f43f5e';
ctx.lineWidth = 2;
ctx.beginPath();
ctx.moveTo(dragStartCoords.x, dragStartCoords.y);
ctx.lineTo(clickU, clickV);
ctx.stroke();
}
});
canvas.addEventListener('mouseup', (event) => {
if (isPanning) {
isPanning = false;
return;
}
if (!isDragging) return;
isDragging = false;
if (poseEstimateMode) {
publishInitialPose(dragStartRos.x, dragStartRos.y, currentDragYaw);
poseEstimateMode = false;
document.getElementById('btn-pose-estimate').className = 'btn btn-secondary';
document.getElementById('btn-pose-estimate').innerText = '📍 로봇 위치 초기화 (Pose Estimate)';
} else if (currentMode === 'nav2') {
publishNav2Goal();
} else {
publishWaypoint();
}
});
// Scroll Wheel Zoom
canvas.addEventListener('wheel', (event) => {
if (!mapInfo) return;
event.preventDefault();
const zoomFactor = 1.1;
if (event.deltaY < 0) {
zoom *= zoomFactor;
} else {
zoom /= zoomFactor;
}
// Zoom limit boundaries (0.3x to 8x)
zoom = Math.max(0.3, Math.min(zoom, 8.0));
drawMap();
}, { passive: false });
// Double-click to reset zoom & pan translation
canvas.addEventListener('dblclick', () => {
zoom = 1.0;
panX = 0.0;
panY = 0.0;
drawMap();
});
function publishWaypoint() {
const yaw = parkingWaypoint.yaw;
const msg = new ROSLIB.Message({
header: {
frame_id: 'map',
stamp: { secs: 0, nsecs: 0 } // Rosbridge populates timestamp if zero
},
pose: {
position: { x: parkingWaypoint.x, y: parkingWaypoint.y, z: 0.0 },
orientation: {
x: 0.0,
y: 0.0,
z: Math.sin(yaw * 0.5),
w: Math.cos(yaw * 0.5)
}
}
});
waypointPub.publish(msg);
console.log(`Published new waypoint coordinates: x=${parkingWaypoint.x.toFixed(2)}, y=${parkingWaypoint.y.toFixed(2)}, yaw=${(yaw * 180 / Math.PI).toFixed(0)}deg`);
drawMap();
}
function publishNav2Goal() {
const yaw = nav2Goal.yaw;
const msg = new ROSLIB.Message({
header: {
frame_id: 'map',
stamp: { secs: 0, nsecs: 0 }
},
pose: {
position: { x: nav2Goal.x, y: nav2Goal.y, z: 0.0 },
orientation: {
x: 0.0,
y: 0.0,
z: Math.sin(yaw * 0.5),
w: Math.cos(yaw * 0.5)
}
}
});
nav2GoalPub.publish(msg);
console.log(`Published Nav2 Goal: x=${nav2Goal.x.toFixed(2)}, y=${nav2Goal.y.toFixed(2)}, yaw=${(yaw * 180 / Math.PI).toFixed(0)}deg`);
drawMap();
}
function publishInitialPose(x, y, yaw) {
const msg = new ROSLIB.Message({
header: {
frame_id: 'map',
stamp: { secs: 0, nsecs: 0 }
},
pose: {
pose: {
position: { x: x, y: y, z: 0.0 },
orientation: {
x: 0.0,
y: 0.0,
z: Math.sin(yaw * 0.5),
w: Math.cos(yaw * 0.5)
}
},
covariance: [
0.25, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.25, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.06853891945200942, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942
]
}
});
initialPosePub.publish(msg);
console.log(`Published Initial Pose Reset: x=${x.toFixed(2)}, y=${y.toFixed(2)}, yaw=${(yaw * 180 / Math.PI).toFixed(0)}deg`);
drawMap();
}
src/fori_serial_bridge/ui/index.html
+194
View File
@@ -0,0 +1,194 @@
<!DOCTYPE html>
<html lang="ko">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>FORI AGV Control Dashboard</title>
<!-- Outfit Google Font -->
<link href="https://fonts.googleapis.com/css2?family=Outfit:wght@300;400;600;800&display=swap" rel="stylesheet">
<link rel="stylesheet" href="style.css">
<!-- ROSlibJS -->
<script src="https://cdn.jsdelivr.net/npm/roslib@1/build/roslib.min.js"></script>
</head>
<body>
<div class="app-container">
<!-- Header -->
<header class="dashboard-header">
<div class="logo-area">
<h1>FORI <span>AGV</span></h1>
<span class="system-status">BLDC & ArUco Parking System</span>
</div>
<div class="connection-status" id="conn-status">
<span id="env-badge" class="env-badge sim">Simulation Mode</span>
<span class="pulse-indicator red" id="status-indicator"></span>
<span id="status-text">Disconnected</span>
</div>
</header>
<!-- Main Dashboard Grid -->
<main class="dashboard-grid">
<!-- Left Side: Map & Configuration -->
<section class="grid-card map-card">
<div class="card-header">
<h2>🗺️ 실시간 2D 맵 시각화</h2>
<span class="help-text">지도를 클릭하여 주차 진입점(Waypoint)을 등록하세요.</span>
</div>
<div class="canvas-container">
<canvas id="map-canvas"></canvas>
</div>
<div class="config-panel">
<h3>📍 주차 진입점 (Waypoint) 설정</h3>
<div class="coord-inputs">
<div class="input-group">
<label>X (m)</label>
<input type="number" id="wp-x" step="0.1" value="0.0">
</div>
<div class="input-group">
<label>Y (m)</label>
<input type="number" id="wp-y" step="0.1" value="0.0">
</div>
<div class="input-group">
<label>Yaw (deg)</label>
<input type="number" id="wp-yaw" step="1" value="0">
</div>
</div>
<button class="btn btn-secondary" id="btn-set-wp">진입점 좌표 전송</button>
</div>
<div class="pose-panel">
<h3>📍 로봇 실시간 현재 위치 (Pose)</h3>
<div class="pose-value-display" id="val-pose">
X: 0.00m &nbsp;|&nbsp; Y: 0.00m &nbsp;|&nbsp; Yaw: 0°
</div>
</div>
<div class="battery-panel" style="margin-top: 15px; background: rgba(255,255,255,0.03); border: 1px solid rgba(255,255,255,0.08); padding: 15px; border-radius: 12px;">
<div style="display: flex; justify-content: space-between; align-items: center;">
<h3 style="margin: 0; font-size: 1rem; color: var(--text-heading, #fff);">🔋 FORI 배터리 잔량 (24V 60Ah LiFePO4)</h3>
<span id="battery-badge" class="badge badge-normal" style="background: rgba(34, 197, 94, 0.2); color: #4ade80; border: 1px solid rgba(74, 222, 128, 0.4); padding: 4px 10px; border-radius: 20px; font-size: 0.85rem; font-weight: 600;">--% (--.-V)</span>
</div>
<div style="width: 100%; background: rgba(255,255,255,0.08); height: 12px; border-radius: 6px; margin-top: 10px; overflow: hidden; border: 1px solid rgba(255,255,255,0.1);">
<div id="battery-bar" style="width: 0%; height: 100%; background: linear-gradient(90deg, #22c55e, #4ade80); transition: width 0.5s ease, background 0.5s ease; border-radius: 6px;"></div>
</div>
</div>
</section>
<!-- Right Side: Camera View & Telemetry -->
<div class="right-column">
<!-- Camera & ArUco view -->
<section class="grid-card camera-card">
<div class="card-header">
<h2>📷 실시간 아루코 인식 카메라</h2>
</div>
<div class="camera-container-layout">
<div class="camera-stream-container">
<img id="camera-stream" src="" alt="카메라 스트림 대기 중..." />
</div>
<div class="aruco-info-panel">
<div class="aruco-status-badge disconnected" id="aruco-status">미인식 (No Marker)</div>
<div class="aruco-details">
<div class="detail-row">
<span class="detail-label">마커 ID</span>
<span class="detail-value" id="aruco-id">-</span>
</div>
<div class="detail-row">
<span class="detail-label">Z (직선 거리)</span>
<span class="detail-value" id="aruco-z">-</span>
</div>
<div class="detail-row">
<span class="detail-label">X (가로 오프셋)</span>
<span class="detail-value" id="aruco-x">-</span>
</div>
<div class="detail-row">
<span class="detail-label">Y (높이 오프셋)</span>
<span class="detail-value" id="aruco-y">-</span>
</div>
<div class="detail-row">
<span class="detail-label">Yaw (좌우 회전)</span>
<span class="detail-value" id="aruco-yaw">-</span>
</div>
<div class="detail-row">
<span class="detail-label">Roll / Pitch</span>
<span class="detail-value" id="aruco-rp">-</span>
</div>
</div>
</div>
</div>
</section>
<!-- Mode Controls & Telemetry -->
<section class="grid-card controls-card">
<div class="card-header">
<h2>⚙️ 제어 모드 및 텔레메트리</h2>
</div>
<!-- Mode Switch Buttons -->
<div class="mode-selector">
<button class="btn btn-primary active" id="btn-mode-nav2">자율 주행 (Nav2)</button>
<button class="btn btn-primary" id="btn-mode-patrol">자동 순찰 (Patrol)</button>
<button class="btn btn-primary" id="btn-mode-parking">자동 주차 (Parking)</button>
</div>
<button class="btn btn-secondary" id="btn-pose-estimate" style="margin-bottom: 12px; font-size: 13px; font-weight: 700; width: 100%;">
📍 로봇 위치 초기화 (Pose Estimate)
</button>
<button class="btn btn-danger" id="btn-emergency-stop" style="margin-bottom: 20px; font-size: 14px; font-weight: 800; width: 100%;">
🛑 긴급 정지 (Emergency Stop)
</button>
<!-- Telemetry Metrics Grid -->
<div class="telemetry-grid">
<div class="metric-box">
<span class="metric-label">현재 시스템 상태</span>
<span class="metric-value" id="val-state">IDLE</span>
</div>
<div class="metric-box">
<span class="metric-label">로봇 선속도 (Linear)</span>
<span class="metric-value" id="val-linear">0.00 m/s</span>
</div>
<div class="metric-box">
<span class="metric-label">로봇 각속도 (Angular)</span>
<span class="metric-value" id="val-angular">0.00 rad/s</span>
</div>
</div>
<!-- Wheel Speeds Gauge -->
<div class="rpm-panel">
<h3>🔄 실시간 바퀴 회전수 (Feedback RPM)</h3>
<div class="rpm-bars">
<div class="rpm-bar-group">
<span class="wheel-name">Front Left</span>
<div class="bar-container">
<div class="bar" id="bar-fl" style="width: 0%"></div>
</div>
<span class="rpm-val" id="val-fl">0 RPM</span>
</div>
<div class="rpm-bar-group">
<span class="wheel-name">Front Right</span>
<div class="bar-container">
<div class="bar" id="bar-fr" style="width: 0%"></div>
</div>
<span class="rpm-val" id="val-fr">0 RPM</span>
</div>
<div class="rpm-bar-group">
<span class="wheel-name">Rear Left</span>
<div class="bar-container">
<div class="bar" id="bar-rl" style="width: 0%"></div>
</div>
<span class="rpm-val" id="val-rl">0 RPM</span>
</div>
<div class="rpm-bar-group">
<span class="wheel-name">Rear Right</span>
<div class="bar-container">
<div class="bar" id="bar-rr" style="width: 0%"></div>
</div>
<span class="rpm-val" id="val-rr">0 RPM</span>
</div>
</div>
</div>
</section>
</div>
</main>
</div>
<script src="app.js"></script>
</body>
</html>
src/fori_serial_bridge/ui/style.css
+516
View File
@@ -0,0 +1,516 @@
:root {
--bg-dark: #0a0b10;
--card-bg: rgba(20, 22, 37, 0.55);
--border-color: rgba(255, 255, 255, 0.08);
--accent-blue: #00d2ff;
--accent-purple: #9d4edd;
--text-main: #f3f4f6;
--text-muted: #9ca3af;
--green-glow: #10b981;
--red-glow: #ef4444;
}
* {
margin: 0;
padding: 0;
box-sizing: border-box;
font-family: 'Outfit', sans-serif;
-webkit-font-smoothing: antialiased;
}
body {
background-color: var(--bg-dark);
color: var(--text-main);
min-height: 100vh;
overflow-x: hidden;
background-image:
radial-gradient(at 10% 20%, rgba(157, 78, 221, 0.1) 0px, transparent 50%),
radial-gradient(at 90% 80%, rgba(0, 210, 255, 0.1) 0px, transparent 50%);
}
.app-container {
max-width: 1400px;
margin: 0 auto;
padding: 20px;
}
/* Header */
.dashboard-header {
display: flex;
justify-content: space-between;
align-items: center;
padding: 15px 25px;
background: var(--card-bg);
border: 1px solid var(--border-color);
border-radius: 16px;
backdrop-filter: blur(12px);
margin-bottom: 25px;
box-shadow: 0 8px 32px 0 rgba(0, 0, 0, 0.37);
}
.logo-area h1 {
font-size: 24px;
font-weight: 800;
letter-spacing: 1px;
}
.logo-area h1 span {
color: var(--accent-blue);
text-shadow: 0 0 10px rgba(0, 210, 255, 0.4);
}
.system-status {
font-size: 12px;
color: var(--text-muted);
font-weight: 300;
margin-left: 10px;
}
.connection-status {
display: flex;
align-items: center;
gap: 10px;
font-size: 14px;
font-weight: 600;
}
.pulse-indicator {
width: 10px;
height: 10px;
border-radius: 50%;
display: inline-block;
}
.pulse-indicator.green {
background-color: var(--green-glow);
box-shadow: 0 0 10px var(--green-glow);
animation: pulse 1.8s infinite;
}
.pulse-indicator.red {
background-color: var(--red-glow);
box-shadow: 0 0 10px var(--red-glow);
animation: pulse-red 1.8s infinite;
}
@keyframes pulse {
0% { transform: scale(0.95); box-shadow: 0 0 0 0 rgba(16, 185, 129, 0.7); }
70% { transform: scale(1); box-shadow: 0 0 0 8px rgba(16, 185, 129, 0); }
100% { transform: scale(0.95); box-shadow: 0 0 0 0 rgba(16, 185, 129, 0); }
}
@keyframes pulse-red {
0% { transform: scale(0.95); box-shadow: 0 0 0 0 rgba(239, 68, 68, 0.7); }
70% { transform: scale(1); box-shadow: 0 0 0 8px rgba(239, 68, 68, 0); }
100% { transform: scale(0.95); box-shadow: 0 0 0 0 rgba(239, 68, 68, 0); }
}
/* Grid Layout */
.dashboard-grid {
display: grid;
grid-template-columns: 1.2fr 1fr;
gap: 25px;
}
@media (max-width: 1024px) {
.dashboard-grid {
grid-template-columns: 1fr;
}
}
/* Glassmorphism Card Style */
.grid-card {
background: var(--card-bg);
border: 1px solid var(--border-color);
border-radius: 20px;
backdrop-filter: blur(12px);
padding: 20px;
box-shadow: 0 8px 32px 0 rgba(0, 0, 0, 0.3);
display: flex;
flex-direction: column;
}
.card-header {
margin-bottom: 15px;
}
.card-header h2 {
font-size: 18px;
font-weight: 600;
margin-bottom: 4px;
}
.help-text {
font-size: 12px;
color: var(--text-muted);
}
/* Map Card Details */
.map-card {
height: 720px;
}
.canvas-container {
flex-grow: 1;
background: rgba(10, 10, 15, 0.7);
border-radius: 12px;
border: 1px solid var(--border-color);
position: relative;
overflow: hidden;
display: flex;
justify-content: center;
align-items: center;
height: 0;
}
#map-canvas {
width: 100%;
height: 100%;
object-fit: contain;
image-rendering: pixelated;
image-rendering: crisp-edges;
}
.config-panel {
margin-top: 15px;
padding: 15px;
background: rgba(255, 255, 255, 0.02);
border-radius: 12px;
border: 1px solid var(--border-color);
}
.config-panel h3 {
font-size: 14px;
font-weight: 600;
margin-bottom: 10px;
}
.coord-inputs {
display: flex;
gap: 15px;
margin-bottom: 12px;
}
.input-group {
display: flex;
flex-direction: column;
flex: 1;
}
.input-group label {
font-size: 11px;
color: var(--text-muted);
margin-bottom: 4px;
}
.input-group input {
background: rgba(10, 11, 16, 0.6);
border: 1px solid var(--border-color);
border-radius: 6px;
padding: 6px 10px;
color: var(--text-main);
font-size: 14px;
outline: none;
text-align: center;
}
.input-group input:focus {
border-color: var(--accent-blue);
}
/* Right Column Panels */
.right-column {
display: flex;
flex-direction: column;
gap: 25px;
}
/* Camera Card */
.camera-card {
overflow: hidden;
}
.camera-stream-container {
background: #000;
border-radius: 12px;
border: 1px solid var(--border-color);
aspect-ratio: 4/3;
display: flex;
justify-content: center;
align-items: center;
overflow: hidden;
}
.camera-stream-container img {
width: 100%;
height: 100%;
object-fit: cover;
}
/* Mode selectors and buttons */
.mode-selector {
display: flex;
gap: 15px;
margin-bottom: 20px;
}
.btn {
flex: 1;
border: 1px solid var(--border-color);
background: rgba(255, 255, 255, 0.04);
color: var(--text-main);
padding: 12px;
border-radius: 12px;
font-size: 15px;
font-weight: 600;
cursor: pointer;
transition: all 0.3s ease;
outline: none;
}
.btn:hover {
background: rgba(255, 255, 255, 0.1);
}
.btn-primary.active {
background: linear-gradient(135deg, var(--accent-blue), var(--accent-purple));
border: none;
box-shadow: 0 0 15px rgba(157, 78, 221, 0.4);
}
.btn-secondary {
background: rgba(0, 210, 255, 0.1);
color: var(--accent-blue);
border: 1px solid rgba(0, 210, 255, 0.2);
width: 100%;
}
.btn-secondary:hover {
background: rgba(0, 210, 255, 0.2);
}
.btn-danger {
background: rgba(239, 68, 68, 0.12);
color: var(--red-glow);
border: 1px solid rgba(239, 68, 68, 0.25);
width: 100%;
}
.btn-danger:hover {
background: rgba(239, 68, 68, 0.25);
}
.btn-danger.active {
background: #ef4444 !important;
color: #ffffff !important;
border: none !important;
box-shadow: 0 0 20px rgba(239, 68, 68, 0.6) !important;
}
.pose-panel {
margin-top: 15px;
padding-top: 15px;
border-top: 1px solid var(--border-color);
}
.pose-panel h3 {
font-size: 14px;
font-weight: 600;
margin-bottom: 10px;
color: var(--text-muted);
}
.pose-value-display {
background: rgba(14, 165, 233, 0.12);
border: 1px solid rgba(14, 165, 233, 0.35);
border-radius: 10px;
padding: 12px;
font-size: 16px;
font-weight: 800;
color: #38bdf8;
text-align: center;
text-shadow: 0 0 10px rgba(56, 189, 248, 0.4);
letter-spacing: 0.5px;
}
/* Telemetry display */
.telemetry-grid {
display: grid;
grid-template-columns: repeat(3, 1fr);
gap: 15px;
margin-bottom: 20px;
}
.metric-box {
background: rgba(10, 11, 16, 0.5);
border: 1px solid var(--border-color);
border-radius: 12px;
padding: 12px;
text-align: center;
}
.metric-label {
display: block;
font-size: 11px;
color: var(--text-muted);
margin-bottom: 6px;
}
.metric-value {
font-size: 16px;
font-weight: 800;
color: var(--text-main);
}
#val-state {
color: var(--accent-blue);
text-shadow: 0 0 10px rgba(0, 210, 255, 0.3);
}
/* RPM Panels */
.rpm-panel h3 {
font-size: 14px;
font-weight: 600;
margin-bottom: 12px;
}
.rpm-bars {
display: flex;
flex-direction: column;
gap: 12px;
}
.rpm-bar-group {
display: flex;
align-items: center;
gap: 15px;
}
.wheel-name {
width: 80px;
font-size: 12px;
color: var(--text-muted);
}
.bar-container {
flex-grow: 1;
height: 8px;
background: rgba(255, 255, 255, 0.05);
border-radius: 4px;
overflow: hidden;
}
.bar {
height: 100%;
background: linear-gradient(90deg, var(--accent-blue), var(--accent-purple));
border-radius: 4px;
transition: width 0.15s ease-out;
}
.rpm-val {
width: 60px;
text-align: right;
font-size: 13px;
font-weight: 600;
}
/* Environment mode badge */
.env-badge {
padding: 5px 12px;
border-radius: 20px;
font-size: 11px;
font-weight: 700;
text-transform: uppercase;
letter-spacing: 0.5px;
border: 1px solid transparent;
margin-right: 15px;
transition: all 0.3s ease;
}
.env-badge.sim {
background: rgba(234, 179, 8, 0.1);
color: #eab308;
border-color: rgba(234, 179, 8, 0.25);
box-shadow: 0 0 10px rgba(234, 179, 8, 0.15);
}
.env-badge.real {
background: rgba(16, 185, 129, 0.1);
color: #10b981;
border-color: rgba(16, 185, 129, 0.25);
box-shadow: 0 0 10px rgba(16, 185, 129, 0.15);
}
/* Camera & ArUco Layout */
.camera-container-layout {
display: grid;
grid-template-columns: 1.3fr 1fr;
gap: 20px;
align-items: center;
}
@media (max-width: 768px) {
.camera-container-layout {
grid-template-columns: 1fr;
}
}
/* ArUco Info Card Styles */
.aruco-info-panel {
display: flex;
flex-direction: column;
gap: 12px;
}
.aruco-status-badge {
padding: 6px 8px;
border-radius: 8px;
text-align: center;
font-size: 12px;
font-weight: 700;
text-transform: uppercase;
letter-spacing: 0.5px;
border: 1px solid transparent;
transition: all 0.3s ease;
}
.aruco-status-badge.disconnected {
background: rgba(239, 68, 68, 0.1);
color: var(--red-glow);
border-color: rgba(239, 68, 68, 0.2);
}
.aruco-status-badge.connected {
background: rgba(16, 185, 129, 0.1);
color: var(--green-glow);
border-color: rgba(16, 185, 129, 0.2);
box-shadow: 0 0 10px rgba(16, 185, 129, 0.15);
}
.aruco-details {
display: grid;
grid-template-columns: 1fr;
gap: 6px;
}
.detail-row {
background: rgba(10, 11, 16, 0.4);
border: 1px solid var(--border-color);
border-radius: 8px;
padding: 6px 12px;
display: flex;
justify-content: space-between;
align-items: center;
}
.detail-label {
font-size: 11px;
color: var(--text-muted);
}
.detail-value {
font-size: 13px;
font-weight: 700;
color: var(--text-main);
}
src/livox_ros_driver2/.gitignore
+4
View File
@@ -0,0 +1,4 @@
.vscode
build
package.xml
__pycache__
src/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_
src/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_
src/livox_ros_driver2/3rdparty/rapidjson/document.h
Vendored
+3309
View File
File diff suppressed because it is too large Load Diff
src/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_
src/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_
src/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_
src/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_
src/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_
src/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_
src/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_
src/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_
src/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_
src/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_

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