commit 0b8f64d39e850bdd99b09b8962e8e0856f4245fb Author: robin Date: Fri Aug 7 14:22:54 2026 +0900 Initial commit: workspace configuration and src diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..838cb2d --- /dev/null +++ b/.gitignore @@ -0,0 +1,10 @@ +# ROS 2 build artifacts +build/ +install/ +log/ + +# IDE / System files +.vscode/ +.idea/ +*.swp +.DS_Store diff --git a/src/FAST-LIVO2/.gitignore b/src/FAST-LIVO2/.gitignore new file mode 100644 index 0000000..e7a814b --- /dev/null +++ b/src/FAST-LIVO2/.gitignore @@ -0,0 +1,2 @@ +Log/* +build/* diff --git a/src/FAST-LIVO2/CMakeLists.txt b/src/FAST-LIVO2/CMakeLists.txt new file mode 100755 index 0000000..e811137 --- /dev/null +++ b/src/FAST-LIVO2/CMakeLists.txt @@ -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() \ No newline at end of file diff --git a/src/FAST-LIVO2/LICENSE b/src/FAST-LIVO2/LICENSE new file mode 100644 index 0000000..d159169 --- /dev/null +++ b/src/FAST-LIVO2/LICENSE @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + 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. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/src/FAST-LIVO2/README.md b/src/FAST-LIVO2/README.md new file mode 100644 index 0000000..c2ec79d --- /dev/null +++ b/src/FAST-LIVO2/README.md @@ -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) + +
+ +
+ +### 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 and Prof. Fu Zhang at to discuss an alternative license. \ No newline at end of file diff --git a/src/FAST-LIVO2/Supplementary/LIVO2_supplementary.pdf b/src/FAST-LIVO2/Supplementary/LIVO2_supplementary.pdf new file mode 100644 index 0000000..34c81aa Binary files /dev/null and b/src/FAST-LIVO2/Supplementary/LIVO2_supplementary.pdf differ diff --git a/src/FAST-LIVO2/config/HILTI22.yaml b/src/FAST-LIVO2/config/HILTI22.yaml new file mode 100644 index 0000000..c8cddbc --- /dev/null +++ b/src/FAST-LIVO2/config/HILTI22.yaml @@ -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 diff --git a/src/FAST-LIVO2/config/MARS_LVIG.yaml b/src/FAST-LIVO2/config/MARS_LVIG.yaml new file mode 100644 index 0000000..cdff828 --- /dev/null +++ b/src/FAST-LIVO2/config/MARS_LVIG.yaml @@ -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 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/NTU_VIRAL.yaml b/src/FAST-LIVO2/config/NTU_VIRAL.yaml new file mode 100644 index 0000000..a3db092 --- /dev/null +++ b/src/FAST-LIVO2/config/NTU_VIRAL.yaml @@ -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 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/avia.yaml b/src/FAST-LIVO2/config/avia.yaml new file mode 100755 index 0000000..00ae725 --- /dev/null +++ b/src/FAST-LIVO2/config/avia.yaml @@ -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 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/camera_MARS_LVIG.yaml b/src/FAST-LIVO2/config/camera_MARS_LVIG.yaml new file mode 100644 index 0000000..4b4e09f --- /dev/null +++ b/src/FAST-LIVO2/config/camera_MARS_LVIG.yaml @@ -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 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/camera_NTU_VIRAL.yaml b/src/FAST-LIVO2/config/camera_NTU_VIRAL.yaml new file mode 100644 index 0000000..bf1308d --- /dev/null +++ b/src/FAST-LIVO2/config/camera_NTU_VIRAL.yaml @@ -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 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/camera_cam1.yaml b/src/FAST-LIVO2/config/camera_cam1.yaml new file mode 100644 index 0000000..01c5048 --- /dev/null +++ b/src/FAST-LIVO2/config/camera_cam1.yaml @@ -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 diff --git a/src/FAST-LIVO2/config/camera_cam3.yaml b/src/FAST-LIVO2/config/camera_cam3.yaml new file mode 100644 index 0000000..869085d --- /dev/null +++ b/src/FAST-LIVO2/config/camera_cam3.yaml @@ -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 diff --git a/src/FAST-LIVO2/config/camera_fisheye_HILTI22.yaml b/src/FAST-LIVO2/config/camera_fisheye_HILTI22.yaml new file mode 100644 index 0000000..c9314ee --- /dev/null +++ b/src/FAST-LIVO2/config/camera_fisheye_HILTI22.yaml @@ -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 diff --git a/src/FAST-LIVO2/config/camera_mid360s.yaml b/src/FAST-LIVO2/config/camera_mid360s.yaml new file mode 100644 index 0000000..e099c73 --- /dev/null +++ b/src/FAST-LIVO2/config/camera_mid360s.yaml @@ -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 diff --git a/src/FAST-LIVO2/config/camera_pinhole.yaml b/src/FAST-LIVO2/config/camera_pinhole.yaml new file mode 100644 index 0000000..e14d3fa --- /dev/null +++ b/src/FAST-LIVO2/config/camera_pinhole.yaml @@ -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 \ No newline at end of file diff --git a/src/FAST-LIVO2/config/extrin_cam1.yaml b/src/FAST-LIVO2/config/extrin_cam1.yaml new file mode 100644 index 0000000..e4506f2 --- /dev/null +++ b/src/FAST-LIVO2/config/extrin_cam1.yaml @@ -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] diff --git a/src/FAST-LIVO2/config/extrin_cam2.yaml b/src/FAST-LIVO2/config/extrin_cam2.yaml new file mode 100644 index 0000000..97927f0 --- /dev/null +++ b/src/FAST-LIVO2/config/extrin_cam2.yaml @@ -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] diff --git a/src/FAST-LIVO2/config/extrin_cam3.yaml b/src/FAST-LIVO2/config/extrin_cam3.yaml new file mode 100644 index 0000000..d04ea13 --- /dev/null +++ b/src/FAST-LIVO2/config/extrin_cam3.yaml @@ -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] diff --git a/src/FAST-LIVO2/config/mid360s.yaml b/src/FAST-LIVO2/config/mid360s.yaml new file mode 100644 index 0000000..986f51e --- /dev/null +++ b/src/FAST-LIVO2/config/mid360s.yaml @@ -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 diff --git a/src/FAST-LIVO2/include/IMU_Processing.h b/src/FAST-LIVO2/include/IMU_Processing.h new file mode 100644 index 0000000..cf09609 --- /dev/null +++ b/src/FAST-LIVO2/include/IMU_Processing.h @@ -0,0 +1,91 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 +#include +#include "common_lib.h" +#include +#include +#include +#include +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 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 ImuProcessPtr; +#endif \ No newline at end of file diff --git a/src/FAST-LIVO2/include/LIVMapper.h b/src/FAST-LIVO2/include/LIVMapper.h new file mode 100644 index 0000000..7477e2f --- /dev/null +++ b/src/FAST-LIVO2/include/LIVMapper.h @@ -0,0 +1,194 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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() +#include +#else +#include +#endif +#include +#include +#include +#include +#include + +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::SharedPtr &pubLaserCloudFullRes, VIOManagerPtr vio_manager); + void publish_visual_sub_map(const rclcpp::Publisher::SharedPtr &pubSubVisualMap); + void publish_effect_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudEffect, const std::vector &ptpl_list); + void publish_odometry(const rclcpp::Publisher::SharedPtr &pmavros_pose_publisherubOdomAftMapped); + void publish_mavros(const rclcpp::Publisher::SharedPtr &mavros_pose_publisher); + void publish_path(const rclcpp::Publisher::SharedPtr &pubPath); + void readParameters(rclcpp::Node::SharedPtr &node); + template void set_posestamp(T &out); + template void pointBodyToWorld(const Eigen::Matrix &pi, Eigen::Matrix &po); + template Eigen::Matrix pointBodyToWorld(const Eigen::Matrix &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_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 prop_imu_buffer; + sensor_msgs::msg::Imu newest_imu; + double latest_ekf_time; + nav_msgs::msg::Odometry imu_prop_odom; + rclcpp::Publisher::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 lid_raw_data_buffer; + deque lid_header_time_buffer; + deque imu_buffer; + deque img_buffer; + deque img_time_buffer; + vector _pv_list; + vector extrinT; + vector extrinR; + vector cameraextrinT; + vector 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 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::SharedPtr plane_pub; + rclcpp::Publisher::SharedPtr voxel_pub; + std::shared_ptr sub_pcl; + rclcpp::Subscription::SharedPtr sub_imu; + rclcpp::Subscription::SharedPtr sub_img; + rclcpp::Publisher::SharedPtr pubLaserCloudFullRes; + rclcpp::Publisher::SharedPtr pubNormal; + rclcpp::Publisher::SharedPtr pubSubVisualMap; + rclcpp::Publisher::SharedPtr pubLaserCloudEffect; + rclcpp::Publisher::SharedPtr pubLaserCloudMap; + rclcpp::Publisher::SharedPtr pubOdomAftMapped; + rclcpp::Publisher::SharedPtr pubPath; + rclcpp::Publisher::SharedPtr pubLaserCloudDyn; + rclcpp::Publisher::SharedPtr pubLaserCloudDynRmed; + rclcpp::Publisher::SharedPtr pubLaserCloudDynDbg; + image_transport::Publisher pubImage; + rclcpp::Publisher::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 \ No newline at end of file diff --git a/src/FAST-LIVO2/include/common_lib.h b/src/FAST-LIVO2/include/common_lib.h new file mode 100755 index 0000000..6202918 --- /dev/null +++ b/src/FAST-LIVO2/include/common_lib.h @@ -0,0 +1,247 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 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 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 &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 &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 operator-(const StatesGroup &b) + { + Matrix 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 cov; // states covariance +}; + +template +auto set_pose6d(const double t, const Matrix &a, const Matrix &g, const Matrix &v, const Matrix &p, + const Matrix &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(rot_kp.rot, 3,3) = R; + return move(rot_kp); +} + +#endif \ No newline at end of file diff --git a/src/FAST-LIVO2/include/feature.h b/src/FAST-LIVO2/include/feature.h new file mode 100644 index 0000000..a94eea7 --- /dev/null +++ b/src/FAST-LIVO2/include/feature.h @@ -0,0 +1,56 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 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 &_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_ diff --git a/src/FAST-LIVO2/include/frame.h b/src/FAST-LIVO2/include/frame.h new file mode 100644 index 0000000..ea8a534 --- /dev/null +++ b/src/FAST-LIVO2/include/frame.h @@ -0,0 +1,84 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 +#include + +class VisualPoint; +struct Feature; + +typedef list Features; +typedef vector 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 T_f_w_; //!< Transform (f)rame from (w)orld. + SE3 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 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_ diff --git a/src/FAST-LIVO2/include/livox_ros_driver/CustomMsg.h b/src/FAST-LIVO2/include/livox_ros_driver/CustomMsg.h new file mode 100644 index 0000000..144d800 --- /dev/null +++ b/src/FAST-LIVO2/include/livox_ros_driver/CustomMsg.h @@ -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 +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace livox_ros_driver +{ +template +struct CustomMsg_ +{ + typedef CustomMsg_ 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_ _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 _rsvd_type; + _rsvd_type rsvd; + + typedef std::vector< ::livox_ros_driver::CustomPoint_ , typename std::allocator_traits::template rebind_alloc< ::livox_ros_driver::CustomPoint_ >> _points_type; + _points_type points; + + + + + + typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg_ > Ptr; + typedef boost::shared_ptr< ::livox_ros_driver::CustomMsg_ const> ConstPtr; + +}; // struct CustomMsg_ + +typedef ::livox_ros_driver::CustomMsg_ > 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 +std::ostream& operator<<(std::ostream& s, const ::livox_ros_driver::CustomMsg_ & v) +{ +ros::message_operations::Printer< ::livox_ros_driver::CustomMsg_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::livox_ros_driver::CustomMsg_ & lhs, const ::livox_ros_driver::CustomMsg_ & 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 +bool operator!=(const ::livox_ros_driver::CustomMsg_ & lhs, const ::livox_ros_driver::CustomMsg_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace livox_ros_driver + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::livox_ros_driver::CustomMsg_ > + : TrueType + { }; + +template +struct IsMessage< ::livox_ros_driver::CustomMsg_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::livox_ros_driver::CustomMsg_ > + : FalseType + { }; + +template +struct IsFixedSize< ::livox_ros_driver::CustomMsg_ const> + : FalseType + { }; + +template +struct HasHeader< ::livox_ros_driver::CustomMsg_ > + : TrueType + { }; + +template +struct HasHeader< ::livox_ros_driver::CustomMsg_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::livox_ros_driver::CustomMsg_ > +{ + static const char* value() + { + return "e4d6829bdfe657cb6c21a746c86b21a6"; + } + + static const char* value(const ::livox_ros_driver::CustomMsg_&) { return value(); } + static const uint64_t static_value1 = 0xe4d6829bdfe657cbULL; + static const uint64_t static_value2 = 0x6c21a746c86b21a6ULL; +}; + +template +struct DataType< ::livox_ros_driver::CustomMsg_ > +{ + static const char* value() + { + return "livox_ros_driver/CustomMsg"; + } + + static const char* value(const ::livox_ros_driver::CustomMsg_&) { return value(); } +}; + +template +struct Definition< ::livox_ros_driver::CustomMsg_ > +{ + 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_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::livox_ros_driver::CustomMsg_ > + { + template 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 +struct Printer< ::livox_ros_driver::CustomMsg_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::livox_ros_driver::CustomMsg_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "timebase: "; + Printer::stream(s, indent + " ", v.timebase); + s << indent << "point_num: "; + Printer::stream(s, indent + " ", v.point_num); + s << indent << "lidar_id: "; + Printer::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::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_ >::stream(s, indent + " ", v.points[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // LIVOX_ROS_DRIVER_MESSAGE_CUSTOMMSG_H diff --git a/src/FAST-LIVO2/include/livox_ros_driver/CustomPoint.h b/src/FAST-LIVO2/include/livox_ros_driver/CustomPoint.h new file mode 100644 index 0000000..36ae394 --- /dev/null +++ b/src/FAST-LIVO2/include/livox_ros_driver/CustomPoint.h @@ -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 +#include +#include + +#include +#include +#include +#include + + +namespace livox_ros_driver +{ +template +struct CustomPoint_ +{ + typedef CustomPoint_ 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_ > Ptr; + typedef boost::shared_ptr< ::livox_ros_driver::CustomPoint_ const> ConstPtr; + +}; // struct CustomPoint_ + +typedef ::livox_ros_driver::CustomPoint_ > 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 +std::ostream& operator<<(std::ostream& s, const ::livox_ros_driver::CustomPoint_ & v) +{ +ros::message_operations::Printer< ::livox_ros_driver::CustomPoint_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::livox_ros_driver::CustomPoint_ & lhs, const ::livox_ros_driver::CustomPoint_ & 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 +bool operator!=(const ::livox_ros_driver::CustomPoint_ & lhs, const ::livox_ros_driver::CustomPoint_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace livox_ros_driver + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::livox_ros_driver::CustomPoint_ > + : TrueType + { }; + +template +struct IsMessage< ::livox_ros_driver::CustomPoint_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::livox_ros_driver::CustomPoint_ > + : TrueType + { }; + +template +struct IsFixedSize< ::livox_ros_driver::CustomPoint_ const> + : TrueType + { }; + +template +struct HasHeader< ::livox_ros_driver::CustomPoint_ > + : FalseType + { }; + +template +struct HasHeader< ::livox_ros_driver::CustomPoint_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::livox_ros_driver::CustomPoint_ > +{ + static const char* value() + { + return "109a3cc548bb1f96626be89a5008bd6d"; + } + + static const char* value(const ::livox_ros_driver::CustomPoint_&) { return value(); } + static const uint64_t static_value1 = 0x109a3cc548bb1f96ULL; + static const uint64_t static_value2 = 0x626be89a5008bd6dULL; +}; + +template +struct DataType< ::livox_ros_driver::CustomPoint_ > +{ + static const char* value() + { + return "livox_ros_driver/CustomPoint"; + } + + static const char* value(const ::livox_ros_driver::CustomPoint_&) { return value(); } +}; + +template +struct Definition< ::livox_ros_driver::CustomPoint_ > +{ + 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_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::livox_ros_driver::CustomPoint_ > + { + template 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 +struct Printer< ::livox_ros_driver::CustomPoint_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::livox_ros_driver::CustomPoint_& v) + { + s << indent << "offset_time: "; + Printer::stream(s, indent + " ", v.offset_time); + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "z: "; + Printer::stream(s, indent + " ", v.z); + s << indent << "reflectivity: "; + Printer::stream(s, indent + " ", v.reflectivity); + s << indent << "tag: "; + Printer::stream(s, indent + " ", v.tag); + s << indent << "line: "; + Printer::stream(s, indent + " ", v.line); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // LIVOX_ROS_DRIVER_MESSAGE_CUSTOMPOINT_H diff --git a/src/FAST-LIVO2/include/preprocess.h b/src/FAST-LIVO2/include/preprocess.h new file mode 100755 index 0000000..bb550cf --- /dev/null +++ b/src/FAST-LIVO2/include/preprocess.h @@ -0,0 +1,200 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 +#include + +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 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> pub_full; + std::shared_ptr> pub_surf; + std::shared_ptr> 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 &types); + void pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct); + int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); + bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); + bool edge_jump_judge(const PointCloudXYZI &pl, vector &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 PreprocessPtr; + +#endif // PREPROCESS_H_ \ No newline at end of file diff --git a/src/FAST-LIVO2/include/utils/color.h b/src/FAST-LIVO2/include/utils/color.h new file mode 100644 index 0000000..6ffc24f --- /dev/null +++ b/src/FAST-LIVO2/include/utils/color.h @@ -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 diff --git a/src/FAST-LIVO2/include/utils/so3_math.h b/src/FAST-LIVO2/include/utils/so3_math.h new file mode 100755 index 0000000..a759489 --- /dev/null +++ b/src/FAST-LIVO2/include/utils/so3_math.h @@ -0,0 +1,89 @@ +#ifndef SO3_MATH_H +#define SO3_MATH_H + +#include +#include + +#define SKEW_SYM_MATRX(v) 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0 + +template Eigen::Matrix Exp(const Eigen::Matrix &&ang) +{ + T ang_norm = ang.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (ang_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang / ang_norm; + Eigen::Matrix 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 Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) +{ + T ang_vel_norm = ang_vel.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + + if (ang_vel_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang_vel / ang_vel_norm; + Eigen::Matrix 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 Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) +{ + T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (norm > 0.00001) + { + T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; + Eigen::Matrix 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 Eigen::Matrix Log(const Eigen::Matrix &R) +{ + T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); + Eigen::Matrix 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 Eigen::Matrix RotMtoEuler(const Eigen::Matrix &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 ang(x, y, z); + return ang; +} + +#endif diff --git a/src/FAST-LIVO2/include/utils/types.h b/src/FAST-LIVO2/include/utils/types.h new file mode 100644 index 0000000..4e0564a --- /dev/null +++ b/src/FAST-LIVO2/include/utils/types.h @@ -0,0 +1,39 @@ +#ifndef TYPES_H +#define TYPES_H + +#include +#include +#include + +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointXYZRGB PointTypeRGB; +typedef pcl::PointXYZRGBA PointTypeRGBA; +typedef pcl::PointCloud PointCloudXYZI; +typedef std::vector> PointVector; +typedef pcl::PointCloud PointCloudXYZRGB; +typedef pcl::PointCloud 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 +#define VD(a) Eigen::Matrix +#define MF(a, b) Eigen::Matrix +#define VF(a) Eigen::Matrix + +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 \ No newline at end of file diff --git a/src/FAST-LIVO2/include/utils/utils.h b/src/FAST-LIVO2/include/utils/utils.h new file mode 100644 index 0000000..9555f70 --- /dev/null +++ b/src/FAST-LIVO2/include/utils/utils.h @@ -0,0 +1,76 @@ +#ifndef UTILS_H +#define UTILS_H + +#include +#include // for int64_t +#include // for std::numeric_limits +#include // for std::out_of_range +#include +#include +#include +#include +#include +#include + +std::vector convertToIntVectorSafe(const std::vector& 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 \ No newline at end of file diff --git a/src/FAST-LIVO2/include/vio.h b/src/FAST-LIVO2/include/vio.h new file mode 100755 index 0000000..21f2d9d --- /dev/null +++ b/src/FAST-LIVO2/include/vio.h @@ -0,0 +1,187 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 +#include +#include +#include +#include +#include +#include + +struct SubSparseMap +{ + vector propa_errors; + vector errors; + vector> warp_patch; + vector search_levels; + vector voxel_points; + vector inv_expo_list; + vector 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 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 grid_num; + vector map_index; + vector border_flag; + vector update_flag; + vector map_dist; + vector scan_value; + vector 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> 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 G, H_T_H; + Eigen::MatrixXd K, H_sub_inv; + + ofstream fout_camera, fout_colmap; + unordered_map feat_map; + unordered_map sub_feat_map; + unordered_map warp_map; + vector retrieve_voxel_points; + vector 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 &pg, const unordered_map &feat_map, double img_time); + void retrieveFromVisualSparseMap(cv::Mat img, vector &pg, const unordered_map &plane_map); + void generateVisualMapPoints(cv::Mat img, vector &pg); + void setImuToLidarExtrinsic(const V3D &transl, const M3D &rot); + void setLidarToCameraExtrinsic(vector &R, vector &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 &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 &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 &plane_map); + void updateReferencePatch(const unordered_map &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 map_cur_frame; + // deque sub_map_ray; + // deque sub_map_ray_fov; + // deque visual_sub_map_cur; + // deque visual_converged_point; + // std::vector> sample_points; + + // PointCloudXYZI::Ptr pg_down; + // pcl::VoxelGrid downSizeFilter; +}; +typedef std::shared_ptr VIOManagerPtr; + +#endif // VIO_H_ \ No newline at end of file diff --git a/src/FAST-LIVO2/include/visual_point.h b/src/FAST-LIVO2/include/visual_point.h new file mode 100644 index 0000000..494cc72 --- /dev/null +++ b/src/FAST-LIVO2/include/visual_point.h @@ -0,0 +1,48 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 +#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 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_ diff --git a/src/FAST-LIVO2/include/voxel_map.h b/src/FAST-LIVO2/include/voxel_map.h new file mode 100644 index 0000000..2f28d80 --- /dev/null +++ b/src/FAST-LIVO2/include/voxel_map.h @@ -0,0 +1,259 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 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 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 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::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 +{ + 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 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 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 &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::SharedPtr voxel_map_pub_; + std::unordered_map 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 cross_mat_list_; + std::vector body_cov_list_; + std::vector pv_list_; + std::vector ptpl_list_; + + VoxelMapManager(VoxelMapConfig &config_setting, std::unordered_map &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::Ptr &trans_cloud); + + void BuildVoxelMap(); + V3F RGBFromVoxel(const V3D &input_point); + + void UpdateVoxelMap(const std::vector &input_points); + + void BuildResidualListOMP(std::vector &pv_list, std::vector &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 &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 VoxelMapManagerPtr; + +#endif // VOXEL_MAP_H_ \ No newline at end of file diff --git a/src/FAST-LIVO2/launch/mapping_avia_marslvig.launch.py b/src/FAST-LIVO2/launch/mapping_avia_marslvig.launch.py new file mode 100755 index 0000000..7e78750 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_avia_marslvig.launch.py @@ -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" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_aviz.launch.py b/src/FAST-LIVO2/launch/mapping_aviz.launch.py new file mode 100644 index 0000000..a9b17a5 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_aviz.launch.py @@ -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" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_hesaixt32_hilti22.launch b/src/FAST-LIVO2/launch/mapping_hesaixt32_hilti22.launch new file mode 100644 index 0000000..59257b0 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_hesaixt32_hilti22.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/src/FAST-LIVO2/launch/mapping_mid360s.launch.py b/src/FAST-LIVO2/launch/mapping_mid360s.launch.py new file mode 100644 index 0000000..2694640 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_mid360s.launch.py @@ -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" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_mid360s_cam1.launch.py b/src/FAST-LIVO2/launch/mapping_mid360s_cam1.launch.py new file mode 100644 index 0000000..3316ee5 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_mid360s_cam1.launch.py @@ -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" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_mid360s_cam2.launch.py b/src/FAST-LIVO2/launch/mapping_mid360s_cam2.launch.py new file mode 100644 index 0000000..5752507 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_mid360s_cam2.launch.py @@ -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" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_mid360s_cam3.launch.py b/src/FAST-LIVO2/launch/mapping_mid360s_cam3.launch.py new file mode 100644 index 0000000..8501974 --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_mid360s_cam3.launch.py @@ -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" + ), + ]) diff --git a/src/FAST-LIVO2/launch/mapping_ouster_ntu.launch.py b/src/FAST-LIVO2/launch/mapping_ouster_ntu.launch.py new file mode 100644 index 0000000..1b1328f --- /dev/null +++ b/src/FAST-LIVO2/launch/mapping_ouster_ntu.launch.py @@ -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" + ), + ]) diff --git a/src/FAST-LIVO2/package.xml b/src/FAST-LIVO2/package.xml new file mode 100755 index 0000000..f38b1f1 --- /dev/null +++ b/src/FAST-LIVO2/package.xml @@ -0,0 +1,57 @@ + + + fast_livo + 0.0.0 + + + 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. + + + claydergc + + BSD + + Ji Zhang + + ament_cmake + + rclcpp + rclpy + sensor_msgs + geometry_msgs + visualization_msgs + nav_msgs + std_msgs + tf2_ros + pcl_ros + pcl_conversions + livox_ros_driver2 + vikit_common + vikit_ros + cv_bridge + image_transport + libopencv-dev + sophus + eigen + fmt + + cv_bridge + image_transport + libopencv-dev + sensor_msgs + std_msgs + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + + diff --git a/src/FAST-LIVO2/pics/Framework.png b/src/FAST-LIVO2/pics/Framework.png new file mode 100644 index 0000000..c2f1479 Binary files /dev/null and b/src/FAST-LIVO2/pics/Framework.png differ diff --git a/src/FAST-LIVO2/pics/debug_error.png b/src/FAST-LIVO2/pics/debug_error.png new file mode 100644 index 0000000..3baa521 Binary files /dev/null and b/src/FAST-LIVO2/pics/debug_error.png differ diff --git a/src/FAST-LIVO2/pics/rosgraph.png b/src/FAST-LIVO2/pics/rosgraph.png new file mode 100644 index 0000000..26ba96f Binary files /dev/null and b/src/FAST-LIVO2/pics/rosgraph.png differ diff --git a/src/FAST-LIVO2/rviz_cfg/M300.rviz b/src/FAST-LIVO2/rviz_cfg/M300.rviz new file mode 100755 index 0000000..84a33a6 --- /dev/null +++ b/src/FAST-LIVO2/rviz_cfg/M300.rviz @@ -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: + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Axes + Enabled: true + Length: 4 + Name: Axes + Radius: 1.2000000476837158 + Reference 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: + 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: + 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: + 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: + 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 diff --git a/src/FAST-LIVO2/rviz_cfg/fast_livo2.rviz b/src/FAST-LIVO2/rviz_cfg/fast_livo2.rviz new file mode 100755 index 0000000..f89870f --- /dev/null +++ b/src/FAST-LIVO2/rviz_cfg/fast_livo2.rviz @@ -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: + 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: + 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: + 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: + 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: + 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 diff --git a/src/FAST-LIVO2/rviz_cfg/hilti.rviz b/src/FAST-LIVO2/rviz_cfg/hilti.rviz new file mode 100755 index 0000000..b84c64c --- /dev/null +++ b/src/FAST-LIVO2/rviz_cfg/hilti.rviz @@ -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: + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.10000000149011612 + Reference 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: + 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: + 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: + 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: + 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 diff --git a/src/FAST-LIVO2/rviz_cfg/ntu_viral.rviz b/src/FAST-LIVO2/rviz_cfg/ntu_viral.rviz new file mode 100755 index 0000000..265b245 --- /dev/null +++ b/src/FAST-LIVO2/rviz_cfg/ntu_viral.rviz @@ -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: + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.10000000149011612 + Reference 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: + 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: + 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: + 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: + 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 diff --git a/src/FAST-LIVO2/scripts/colmap_output.sh b/src/FAST-LIVO2/scripts/colmap_output.sh new file mode 100755 index 0000000..f0b35d5 --- /dev/null +++ b/src/FAST-LIVO2/scripts/colmap_output.sh @@ -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 + diff --git a/src/FAST-LIVO2/scripts/mesh.py b/src/FAST-LIVO2/scripts/mesh.py new file mode 100644 index 0000000..f53428e --- /dev/null +++ b/src/FAST-LIVO2/scripts/mesh.py @@ -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]) diff --git a/src/FAST-LIVO2/src/IMU_Processing.cpp b/src/FAST-LIVO2/src/IMU_Processing.cpp new file mode 100755 index 0000000..7985552 --- /dev/null +++ b/src/FAST-LIVO2/src/IMU_Processing.cpp @@ -0,0 +1,590 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 + +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: "<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: "<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: "<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(): "<curvature: + // "<curvature<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 "<points.size()<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: "<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<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: "<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<header.stamp): + // "<header.stamp)<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: "<offset_time<<" "; + // } + // cout<points.size()<rot); + acc_imu << VEC_FROM_ARRAY(head->acc); + // cout<<"head imu acc: "<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; +} \ No newline at end of file diff --git a/src/FAST-LIVO2/src/LIVMapper.cpp b/src/FAST-LIVO2/src/LIVMapper.cpp new file mode 100755 index 0000000..d67d140 --- /dev/null +++ b/src/FAST-LIVO2/src/LIVMapper.cpp @@ -0,0 +1,1449 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 "LIVMapper.h" +#include + +using namespace Sophus; +LIVMapper::LIVMapper(rclcpp::Node::SharedPtr &node, std::string node_name) + : node(std::make_shared(node_name)), + extT(0, 0, 0), + extR(M3D::Identity()) +{ + extrinT.assign(3, 0.0); + extrinR.assign(9, 0.0); + cameraextrinT.assign(3, 0.0); + cameraextrinR.assign(9, 0.0); + + p_pre.reset(new Preprocess()); + p_imu.reset(new ImuProcess()); + + readParameters(this->node); + VoxelMapConfig voxel_config; + loadVoxelConfig(this->node, voxel_config); + + visual_sub_map.reset(new PointCloudXYZI()); + feats_undistort.reset(new PointCloudXYZI()); + feats_down_body.reset(new PointCloudXYZI()); + feats_down_world.reset(new PointCloudXYZI()); + pcl_w_wait_pub.reset(new PointCloudXYZI()); + pcl_wait_pub.reset(new PointCloudXYZI()); + pcl_wait_save.reset(new PointCloudXYZRGB()); + pcl_wait_save_intensity.reset(new PointCloudXYZI()); + voxelmap_manager.reset(new VoxelMapManager(voxel_config, voxel_map)); + vio_manager.reset(new VIOManager()); + root_dir = ROOT_DIR; + initializeFiles(); + initializeComponents(this->node); // initialize components errors + path.header.stamp = this->node->now(); + path.header.frame_id = "camera_init"; +} + +LIVMapper::~LIVMapper() {} + +void LIVMapper::readParameters(rclcpp::Node::SharedPtr &node) +{ + // declare parameters + this->node->declare_parameter("common.lid_topic", "/livox/lidar"); + this->node->declare_parameter("common.imu_topic", "/livox/imu"); + this->node->declare_parameter("common.ros_driver_bug_fix", false); + this->node->declare_parameter("common.img_en", 1); + this->node->declare_parameter("common.lidar_en", 1); + this->node->declare_parameter("common.img_topic", "/left_camera/image"); + + this->node->declare_parameter("vio.normal_en", true); + this->node->declare_parameter("vio.inverse_composition_en", false); + this->node->declare_parameter("vio.max_iterations", 5); + this->node->declare_parameter("vio.img_point_cov", 100); + this->node->declare_parameter("vio.raycast_en", false); + this->node->declare_parameter("vio.exposure_estimate_en", true); + this->node->declare_parameter("vio.inv_expo_cov", 0.1); + this->node->declare_parameter("vio.grid_size", 5); + this->node->declare_parameter("vio.grid_n_height", 17); + this->node->declare_parameter("vio.patch_pyrimid_level", 4); + this->node->declare_parameter("vio.patch_size", 8); + this->node->declare_parameter("vio.outlier_threshold", 100); + this->node->declare_parameter("time_offset.exposure_time_init", 0.0); + this->node->declare_parameter("time_offset.img_time_offset", 0.0); + this->node->declare_parameter("time_offset.imu_time_offset", 0.0); + this->node->declare_parameter("time_offset.lidar_time_offset", 0.0); + this->node->declare_parameter("uav.imu_rate_odom", false); + this->node->declare_parameter("uav.gravity_align_en", false); + + this->node->declare_parameter("evo.seq_name", "01"); + this->node->declare_parameter("evo.pose_output_en", false); + this->node->declare_parameter("imu.gyr_cov", 1.0); + this->node->declare_parameter("imu.acc_cov", 1.0); + this->node->declare_parameter("imu.imu_int_frame", 30); + this->node->declare_parameter("imu.imu_en", true); + this->node->declare_parameter("imu.gravity_est_en", true); + this->node->declare_parameter("imu.ba_bg_est_en", true); + + this->node->declare_parameter("preprocess.blind", 0.01); + this->node->declare_parameter("preprocess.hilti_en", false); + this->node->declare_parameter("preprocess.filter_size_surf", 0.5); + this->node->declare_parameter("preprocess.lidar_type", AVIA); + this->node->declare_parameter("preprocess.scan_line",6); + this->node->declare_parameter("preprocess.point_filter_num", 3); + this->node->declare_parameter("preprocess.feature_extract_enabled", false); + + this->node->declare_parameter("pcd_save.interval", -1); + this->node->declare_parameter("pcd_save.pcd_save_en", false); + this->node->declare_parameter("image_save.img_save_en", false); + this->node->declare_parameter("image_save.interval", 1); + + this->node->declare_parameter("pcd_save.type", 0); + this->node->declare_parameter("pcd_save.colmap_output_en", false); + this->node->declare_parameter("pcd_save.filter_size_pcd", 0.5); + this->node->declare_parameter>("extrin_calib.extrinsic_T", vector{}); + this->node->declare_parameter>("extrin_calib.extrinsic_R", vector{}); + this->node->declare_parameter>("extrin_calib.Pcl", vector{}); + this->node->declare_parameter>("extrin_calib.Rcl", vector{}); + this->node->declare_parameter("debug.plot_time", -10); + this->node->declare_parameter("debug.frame_cnt", 6); + + this->node->declare_parameter("publish.blind_rgb_points", 0.01); + this->node->declare_parameter("publish.pub_scan_num", 1); + this->node->declare_parameter("publish.pub_effect_point_en", false); + this->node->declare_parameter("publish.dense_map_en", false); + + // get parameter + this->node->get_parameter("common.lid_topic", lid_topic); + this->node->get_parameter("common.imu_topic", imu_topic); + this->node->get_parameter("common.ros_driver_bug_fix", ros_driver_fix_en); + this->node->get_parameter("common.img_en", img_en); + this->node->get_parameter("common.lidar_en", lidar_en); + this->node->get_parameter("common.img_topic", img_topic); + + this->node->get_parameter("vio.normal_en", normal_en); + this->node->get_parameter("vio.inverse_composition_en", inverse_composition_en); + this->node->get_parameter("vio.max_iterations", max_iterations); + this->node->get_parameter("vio.img_point_cov", IMG_POINT_COV); + this->node->get_parameter("vio.raycast_en", raycast_en); + this->node->get_parameter("vio.exposure_estimate_en", exposure_estimate_en); + this->node->get_parameter("vio.inv_expo_cov", inv_expo_cov); + this->node->get_parameter("vio.grid_size", grid_size); + this->node->get_parameter("vio.grid_n_height", grid_n_height); + this->node->get_parameter("vio.patch_pyrimid_level", patch_pyrimid_level); + this->node->get_parameter("vio.patch_size", patch_size); + this->node->get_parameter("vio.outlier_threshold", outlier_threshold); + this->node->get_parameter("time_offset.exposure_time_init", exposure_time_init); + this->node->get_parameter("time_offset.img_time_offset", img_time_offset); + this->node->get_parameter("time_offset.imu_time_offset", imu_time_offset); + this->node->get_parameter("time_offset.lidar_time_offset", lidar_time_offset); + this->node->get_parameter("uav.imu_rate_odom", imu_prop_enable); + this->node->get_parameter("uav.gravity_align_en", gravity_align_en); + + this->node->get_parameter("evo.seq_name", seq_name); + this->node->get_parameter("evo.pose_output_en", pose_output_en); + this->node->get_parameter("imu.gyr_cov", gyr_cov); + this->node->get_parameter("imu.acc_cov", acc_cov); + this->node->get_parameter("imu.imu_int_frame", imu_int_frame); + this->node->get_parameter("imu.imu_en", imu_en); + this->node->get_parameter("imu.gravity_est_en", gravity_est_en); + this->node->get_parameter("imu.ba_bg_est_en", ba_bg_est_en); + + this->node->get_parameter("preprocess.blind", p_pre->blind); + this->node->get_parameter("preprocess.filter_size_surf", filter_size_surf_min); + this->node->get_parameter("preprocess.lidar_type", p_pre->lidar_type); + this->node->get_parameter("preprocess.scan_line", p_pre->N_SCANS); + this->node->get_parameter("preprocess.point_filter_num", p_pre->point_filter_num); + this->node->get_parameter("preprocess.feature_extract_enabled", p_pre->feature_enabled); + + this->node->get_parameter("pcd_save.interval", pcd_save_interval); + this->node->get_parameter("pcd_save.pcd_save_en", pcd_save_en); + this->node->get_parameter("pcd_save.colmap_output_en", colmap_output_en); + this->node->get_parameter("pcd_save.filter_size_pcd", filter_size_pcd); + this->node->get_parameter("extrin_calib.extrinsic_T", extrinT); + this->node->get_parameter("extrin_calib.extrinsic_R", extrinR); + this->node->get_parameter("extrin_calib.Pcl", cameraextrinT); + this->node->get_parameter("extrin_calib.Rcl", cameraextrinR); + this->node->get_parameter("debug.plot_time", plot_time); + this->node->get_parameter("debug.frame_cnt", frame_cnt); + + this->node->get_parameter("publish.blind_rgb_points", blind_rgb_points); + this->node->get_parameter("publish.pub_scan_num", pub_scan_num); + this->node->get_parameter("publish.pub_effect_point_en", pub_effect_point_en); + this->node->get_parameter("publish.dense_map_en", dense_map_en); +} + +void LIVMapper::initializeComponents(rclcpp::Node::SharedPtr &node) +{ + downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); + + // extrinT.assign({0.04165, 0.02326, -0.0284}); + // extrinR.assign({1, 0, 0, 0, 1, 0, 0, 0, 1}); + // cameraextrinT.assign({0.0194384, 0.104689,-0.0251952}); + // cameraextrinR.assign({0.00610193,-0.999863,-0.0154172,-0.00615449,0.0153796,-0.999863,0.999962,0.00619598,-0.0060598}); + + extT << VEC_FROM_ARRAY(extrinT); + extR << MAT_FROM_ARRAY(extrinR); + + voxelmap_manager->extT_ << VEC_FROM_ARRAY(extrinT); + voxelmap_manager->extR_ << MAT_FROM_ARRAY(extrinR); + + if (!vk::camera_loader::loadFromRosNs(this->node, "parameter_blackboard", vio_manager->cam)) throw std::runtime_error("Camera model not correctly specified."); + + vio_manager->grid_size = grid_size; + vio_manager->patch_size = patch_size; + vio_manager->outlier_threshold = outlier_threshold; + vio_manager->setImuToLidarExtrinsic(extT, extR); + vio_manager->setLidarToCameraExtrinsic(cameraextrinR, cameraextrinT); + vio_manager->state = &_state; + vio_manager->state_propagat = &state_propagat; + vio_manager->max_iterations = max_iterations; + vio_manager->img_point_cov = IMG_POINT_COV; + vio_manager->normal_en = normal_en; + vio_manager->inverse_composition_en = inverse_composition_en; + vio_manager->raycast_en = raycast_en; + vio_manager->grid_n_width = grid_n_width; + vio_manager->grid_n_height = grid_n_height; + vio_manager->patch_pyrimid_level = patch_pyrimid_level; + vio_manager->exposure_estimate_en = exposure_estimate_en; + vio_manager->colmap_output_en = colmap_output_en; + vio_manager->initializeVIO(); + + p_imu->set_extrinsic(extT, extR); + p_imu->set_gyr_cov_scale(V3D(gyr_cov, gyr_cov, gyr_cov)); + p_imu->set_acc_cov_scale(V3D(acc_cov, acc_cov, acc_cov)); + p_imu->set_inv_expo_cov(inv_expo_cov); + p_imu->set_gyr_bias_cov(V3D(0.0001, 0.0001, 0.0001)); + p_imu->set_acc_bias_cov(V3D(0.0001, 0.0001, 0.0001)); + p_imu->set_imu_init_frame_num(imu_int_frame); + + if (!imu_en) p_imu->disable_imu(); + if (!gravity_est_en) p_imu->disable_gravity_est(); + if (!ba_bg_est_en) p_imu->disable_bias_est(); + if (!exposure_estimate_en) p_imu->disable_exposure_est(); + + slam_mode_ = (img_en && lidar_en) ? LIVO : imu_en ? ONLY_LIO : ONLY_LO; +} + +void LIVMapper::initializeFiles() +{ + if (pcd_save_en && colmap_output_en) + { + const std::string folderPath = std::string(ROOT_DIR) + "/scripts/colmap_output.sh"; + + std::string chmodCommand = "chmod +x " + folderPath; + + int chmodRet = system(chmodCommand.c_str()); + if (chmodRet != 0) { + std::cerr << "Failed to set execute permissions for the script." << std::endl; + return; + } + + int executionRet = system(folderPath.c_str()); + if (executionRet != 0) { + std::cerr << "Failed to execute the script." << std::endl; + return; + } + } + if(colmap_output_en) fout_points.open(std::string(ROOT_DIR) + "Log/Colmap/sparse/0/points3D.txt", std::ios::out); + if(pcd_save_en) fout_lidar_pos.open(std::string(ROOT_DIR) + "Log/pcd/lidar_poses.txt", std::ios::out); + if(img_save_en) fout_visual_pos.open(std::string(ROOT_DIR) + "Log/image/image_poses.txt", std::ios::out); + fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"), std::ios::out); + fout_out.open(DEBUG_FILE_DIR("mat_out.txt"), std::ios::out); +} + +void LIVMapper::initializeSubscribersAndPublishers(rclcpp::Node::SharedPtr &node, image_transport::ImageTransport &it_) +{ + image_transport::ImageTransport it(this->node); + if (p_pre->lidar_type == AVIA) { + sub_pcl = this->node->create_subscription(lid_topic, 200000, std::bind(&LIVMapper::livox_pcl_cbk, this, std::placeholders::_1)); + } else { + sub_pcl = this->node->create_subscription(lid_topic, 200000, std::bind(&LIVMapper::standard_pcl_cbk, this, std::placeholders::_1)); + } + sub_imu = this->node->create_subscription(imu_topic, 200000, std::bind(&LIVMapper::imu_cbk, this, std::placeholders::_1)); + sub_img = this->node->create_subscription(img_topic, 200000, std::bind(&LIVMapper::img_cbk, this, std::placeholders::_1)); + + pubLaserCloudFullRes = this->node->create_publisher("/cloud_registered", 100); + pubNormal = this->node->create_publisher("/visualization_marker", 100); + pubSubVisualMap = this->node->create_publisher("/cloud_visual_sub_map_before", 100); + pubLaserCloudEffect = this->node->create_publisher("/cloud_effected", 100); + pubLaserCloudMap = this->node->create_publisher("/Laser_map", 100); + pubOdomAftMapped = this->node->create_publisher("/aft_mapped_to_init", 10); + pubPath = this->node->create_publisher("/path", 10); + plane_pub = this->node->create_publisher("/planner_normal", 1); + voxel_pub = this->node->create_publisher("/voxels", 1); + pubLaserCloudDyn = this->node->create_publisher("/dyn_obj", 100); + pubLaserCloudDynRmed = this->node->create_publisher("/dyn_obj_removed", 100); + pubLaserCloudDynDbg = this->node->create_publisher("/dyn_obj_dbg_hist", 100); + mavros_pose_publisher = this->node->create_publisher("/mavros/vision_pose/pose", 10); + pubImage = it.advertise("/rgb_img", 1); + pubImuPropOdom = this->node->create_publisher("/LIVO2/imu_propagate", 10000); + imu_prop_timer = this->node->create_wall_timer(0.004s, std::bind(&LIVMapper::imu_prop_callback, this)); + voxelmap_manager->voxel_map_pub_= this->node->create_publisher("/planes", 10000); +} + +void LIVMapper::handleFirstFrame() +{ + if (!is_first_frame) + { + _first_lidar_time = LidarMeasures.last_lio_update_time; + p_imu->first_lidar_time = _first_lidar_time; // Only for IMU data log + is_first_frame = true; + cout << "FIRST LIDAR FRAME!" << endl; + } +} + +void LIVMapper::gravityAlignment() +{ + if (!p_imu->imu_need_init && !gravity_align_finished) + { + std::cout << "Gravity Alignment Starts" << std::endl; + V3D ez(0, 0, -1), gz(_state.gravity); + Eigen::Quaterniond G_q_I0 = Eigen::Quaterniond::FromTwoVectors(gz, ez); + M3D G_R_I0 = G_q_I0.toRotationMatrix(); + + _state.pos_end = G_R_I0 * _state.pos_end; + _state.rot_end = G_R_I0 * _state.rot_end; + _state.vel_end = G_R_I0 * _state.vel_end; + _state.gravity = G_R_I0 * _state.gravity; + gravity_align_finished = true; + std::cout << "Gravity Alignment Finished" << std::endl; + } +} + +void LIVMapper::processImu() +{ + // double t0 = omp_get_wtime(); + + p_imu->Process2(LidarMeasures, _state, feats_undistort); + + if (gravity_align_en) gravityAlignment(); + + state_propagat = _state; + voxelmap_manager->state_ = _state; + voxelmap_manager->feats_undistort_ = feats_undistort; + + // double t_prop = omp_get_wtime(); + + // std::cout << "[ Mapping ] feats_undistort: " << feats_undistort->size() << std::endl; + // std::cout << "[ Mapping ] predict cov: " << _state.cov.diagonal().transpose() << std::endl; + // std::cout << "[ Mapping ] predict sta: " << state_propagat.pos_end.transpose() << state_propagat.vel_end.transpose() << std::endl; +} + +void LIVMapper::stateEstimationAndMapping() +{ + switch (LidarMeasures.lio_vio_flg) + { + case VIO: + handleVIO(); + break; + case LIO: + case LO: + handleLIO(); + break; + } +} + +void LIVMapper::handleVIO() +{ + euler_cur = RotMtoEuler(_state.rot_end); + fout_pre << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << std::endl; + + if (pcl_w_wait_pub->empty() || (pcl_w_wait_pub == nullptr)) + { + std::cout << "[ VIO ] No point!!!" << std::endl; + return; + } + + std::cout << "[ VIO ] Raw feature num: " << pcl_w_wait_pub->points.size() << std::endl; + + if (fabs((LidarMeasures.last_lio_update_time - _first_lidar_time) - plot_time) < (frame_cnt / 2 * 0.1)) + { + vio_manager->plot_flag = true; + } + else + { + vio_manager->plot_flag = false; + } + + vio_manager->processFrame(LidarMeasures.measures.back().img, _pv_list, voxelmap_manager->voxel_map_, LidarMeasures.last_lio_update_time - _first_lidar_time); + + if (imu_prop_enable) + { + ekf_finish_once = true; + latest_ekf_state = _state; + latest_ekf_time = LidarMeasures.last_lio_update_time; + state_update_flg = true; + } + + // int size_sub_map = vio_manager->visual_sub_map_cur.size(); + // visual_sub_map->reserve(size_sub_map); + // for (int i = 0; i < size_sub_map; i++) + // { + // PointType temp_map; + // temp_map.x = vio_manager->visual_sub_map_cur[i]->pos_[0]; + // temp_map.y = vio_manager->visual_sub_map_cur[i]->pos_[1]; + // temp_map.z = vio_manager->visual_sub_map_cur[i]->pos_[2]; + // temp_map.intensity = 0.; + // visual_sub_map->push_back(temp_map); + // } + + publish_frame_world(pubLaserCloudFullRes, vio_manager); + publish_img_rgb(pubImage, vio_manager); + + euler_cur = RotMtoEuler(_state.rot_end); + fout_out << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << " " << feats_undistort->points.size() << std::endl; +} + +void LIVMapper::handleLIO() +{ + euler_cur = RotMtoEuler(_state.rot_end); + fout_pre << setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << endl; + + if (feats_undistort->empty() || (feats_undistort == nullptr)) + { + std::cout << "[ LIO ]: No point!!!" << std::endl; + return; + } + + double t0 = omp_get_wtime(); + + downSizeFilterSurf.setInputCloud(feats_undistort); + downSizeFilterSurf.filter(*feats_down_body); + + double t_down = omp_get_wtime(); + + feats_down_size = feats_down_body->points.size(); + voxelmap_manager->feats_down_body_ = feats_down_body; + transformLidar(_state.rot_end, _state.pos_end, feats_down_body, feats_down_world); + voxelmap_manager->feats_down_world_ = feats_down_world; + voxelmap_manager->feats_down_size_ = feats_down_size; + + if (!lidar_map_inited) + { + lidar_map_inited = true; + voxelmap_manager->BuildVoxelMap(); + } + + double t1 = omp_get_wtime(); + + voxelmap_manager->StateEstimation(state_propagat); + _state = voxelmap_manager->state_; + _pv_list = voxelmap_manager->pv_list_; + + double t2 = omp_get_wtime(); + + if (imu_prop_enable) + { + ekf_finish_once = true; + latest_ekf_state = _state; + latest_ekf_time = LidarMeasures.last_lio_update_time; + state_update_flg = true; + } + + if (pose_output_en) + { + static bool pos_opend = false; + static int ocount = 0; + std::ofstream outFile, evoFile; + if (!pos_opend) + { + evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::out); + pos_opend = true; + if (!evoFile.is_open()) RCLCPP_ERROR(this->node->get_logger(), "open fail\n"); + } + else + { + evoFile.open(std::string(ROOT_DIR) + "Log/result/" + seq_name + ".txt", std::ios::app); + if (!evoFile.is_open()) RCLCPP_ERROR(this->node->get_logger(), "open fail\n"); + } + Eigen::Matrix4d outT; + Eigen::Quaterniond q(_state.rot_end); + evoFile << std::fixed; + evoFile << LidarMeasures.last_lio_update_time << " " << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " + << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl; + } + + euler_cur = RotMtoEuler(_state.rot_end); + geoQuat = tf::createQuaternionMsgFromRollPitchYaw(euler_cur(0), euler_cur(1), euler_cur(2)); + publish_odometry(pubOdomAftMapped); + + double t3 = omp_get_wtime(); + + PointCloudXYZI::Ptr world_lidar(new PointCloudXYZI()); + transformLidar(_state.rot_end, _state.pos_end, feats_down_body, world_lidar); + for (size_t i = 0; i < world_lidar->points.size(); i++) + { + voxelmap_manager->pv_list_[i].point_w << world_lidar->points[i].x, world_lidar->points[i].y, world_lidar->points[i].z; + M3D point_crossmat = voxelmap_manager->cross_mat_list_[i]; + M3D var = voxelmap_manager->body_cov_list_[i]; + 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); + voxelmap_manager->pv_list_[i].var = var; + } + voxelmap_manager->UpdateVoxelMap(voxelmap_manager->pv_list_); + std::cout << "[ LIO ] Update Voxel Map" << std::endl; + _pv_list = voxelmap_manager->pv_list_; + + double t4 = omp_get_wtime(); + + if(voxelmap_manager->config_setting_.map_sliding_en) + { + voxelmap_manager->mapSliding(); + } + + PointCloudXYZI::Ptr laserCloudFullRes(dense_map_en ? feats_undistort : feats_down_body); + int size = laserCloudFullRes->points.size(); + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + RGBpointBodyToWorld(&laserCloudFullRes->points[i], &laserCloudWorld->points[i]); + } + *pcl_w_wait_pub = *laserCloudWorld; + + publish_frame_world(pubLaserCloudFullRes, vio_manager); + if (pub_effect_point_en) publish_effect_world(pubLaserCloudEffect, voxelmap_manager->ptpl_list_); + if (voxelmap_manager->config_setting_.is_pub_plane_map_) voxelmap_manager->pubVoxelMap(); + publish_path(pubPath); + publish_mavros(mavros_pose_publisher); + + frame_num++; + aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t4 - t0) / frame_num; + + // aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + (t2 - t1) / frame_num; + // aver_time_map_inre = aver_time_map_inre * (frame_num - 1) / frame_num + (t4 - t3) / frame_num; + // aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + (solve_time) / frame_num; + // aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1) / frame_num + solve_const_H_time / frame_num; + // printf("[ mapping time ]: per scan: propagation %0.6f downsample: %0.6f match: %0.6f solve: %0.6f ICP: %0.6f map incre: %0.6f total: %0.6f \n" + // "[ mapping time ]: average: icp: %0.6f construct H: %0.6f, total: %0.6f \n", + // t_prop - t0, t1 - t_prop, match_time, solve_time, t3 - t1, t5 - t3, t5 - t0, aver_time_icp, aver_time_const_H_time, aver_time_consu); + + // printf("\033[1;36m[ LIO mapping time ]: current scan: icp: %0.6f secs, map incre: %0.6f secs, total: %0.6f secs.\033[0m\n" + // "\033[1;36m[ LIO mapping time ]: average: icp: %0.6f secs, map incre: %0.6f secs, total: %0.6f secs.\033[0m\n", + // t2 - t1, t4 - t3, t4 - t0, aver_time_icp, aver_time_map_inre, aver_time_consu); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| LIO Mapping Time |\033[0m\n"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| %-29s | %-27s |\033[0m\n", "Algorithm Stage", "Time (secs)"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "DownSample", t_down - t0); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "ICP", t2 - t1); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "updateVoxelMap", t4 - t3); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "Current Total Time", t4 - t0); + printf("\033[1;36m| %-29s | %-27f |\033[0m\n", "Average Total Time", aver_time_consu); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + + euler_cur = RotMtoEuler(_state.rot_end); + fout_out << std::setw(20) << LidarMeasures.last_lio_update_time - _first_lidar_time << " " << euler_cur.transpose() * 57.3 << " " + << _state.pos_end.transpose() << " " << _state.vel_end.transpose() << " " << _state.bias_g.transpose() << " " + << _state.bias_a.transpose() << " " << V3D(_state.inv_expo_time, 0, 0).transpose() << " " << feats_undistort->points.size() << std::endl; +} + +void LIVMapper::savePCD() +{ + if (pcd_save_en && (pcl_wait_save->points.size() > 0 || pcl_wait_save_intensity->points.size() > 0) && pcd_save_interval < 0) + { + std::string raw_points_dir = std::string(ROOT_DIR) + "Log/pcd/all_raw_points.pcd"; + std::string downsampled_points_dir = std::string(ROOT_DIR) + "Log/pcd/all_downsampled_points.pcd"; + pcl::PCDWriter pcd_writer; + + if (img_en) + { + pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); + pcl::VoxelGrid voxel_filter; + voxel_filter.setInputCloud(pcl_wait_save); + voxel_filter.setLeafSize(filter_size_pcd, filter_size_pcd, filter_size_pcd); + voxel_filter.filter(*downsampled_cloud); + + pcd_writer.writeBinary(raw_points_dir, *pcl_wait_save); // Save the raw point cloud data + std::cout << GREEN << "Raw point cloud data saved to: " << raw_points_dir + << " with point count: " << pcl_wait_save->points.size() << RESET << std::endl; + + pcd_writer.writeBinary(downsampled_points_dir, *downsampled_cloud); // Save the downsampled point cloud data + std::cout << GREEN << "Downsampled point cloud data saved to: " << downsampled_points_dir + << " with point count after filtering: " << downsampled_cloud->points.size() << RESET << std::endl; + + if(colmap_output_en) + { + fout_points << "# 3D point list with one line of data per point\n"; + fout_points << "# POINT_ID, X, Y, Z, R, G, B, ERROR\n"; + for (size_t i = 0; i < downsampled_cloud->size(); ++i) + { + const auto& point = downsampled_cloud->points[i]; + fout_points << i << " " + << std::fixed << std::setprecision(6) + << point.x << " " << point.y << " " << point.z << " " + << static_cast(point.r) << " " + << static_cast(point.g) << " " + << static_cast(point.b) << " " + << 0 << std::endl; + } + } + } + else + { + pcd_writer.writeBinary(raw_points_dir, *pcl_wait_save_intensity); + std::cout << GREEN << "Raw point cloud data saved to: " << raw_points_dir + << " with point count: " << pcl_wait_save_intensity->points.size() << RESET << std::endl; + } + } +} + +void LIVMapper::run(rclcpp::Node::SharedPtr &node) +{ + rclcpp::Rate rate(5000); + while (rclcpp::ok()) + { + rclcpp::spin_some(this->node); + if (!sync_packages(LidarMeasures)) + { + rate.sleep(); + continue; + } + handleFirstFrame(); + + processImu(); + + // if (!p_imu->imu_time_init) continue; + + stateEstimationAndMapping(); + } + savePCD(); +} + +void LIVMapper::prop_imu_once(StatesGroup &imu_prop_state, const double dt, V3D acc_avr, V3D angvel_avr) +{ + double mean_acc_norm = p_imu->IMU_mean_acc_norm; + acc_avr = acc_avr * G_m_s2 / mean_acc_norm - imu_prop_state.bias_a; + angvel_avr -= imu_prop_state.bias_g; + + M3D Exp_f = Exp(angvel_avr, dt); + /* propogation of IMU attitude */ + imu_prop_state.rot_end = imu_prop_state.rot_end * Exp_f; + + /* Specific acceleration (global frame) of IMU */ + V3D acc_imu = imu_prop_state.rot_end * acc_avr + V3D(imu_prop_state.gravity[0], imu_prop_state.gravity[1], imu_prop_state.gravity[2]); + + /* propogation of IMU */ + imu_prop_state.pos_end = imu_prop_state.pos_end + imu_prop_state.vel_end * dt + 0.5 * acc_imu * dt * dt; + + /* velocity of IMU */ + imu_prop_state.vel_end = imu_prop_state.vel_end + acc_imu * dt; +} + +void LIVMapper::imu_prop_callback() +{ + if (p_imu->imu_need_init || !new_imu || !ekf_finish_once) { return; } + mtx_buffer_imu_prop.lock(); + new_imu = false; // 控制 propagate 频率和 IMU 频率一致 + if (imu_prop_enable && !prop_imu_buffer.empty()) + { + static double last_t_from_lidar_end_time = 0; + if (state_update_flg) + { + imu_propagate = latest_ekf_state; + // drop all useless imu pkg + while ((!prop_imu_buffer.empty() && stamp2Sec(prop_imu_buffer.front().header.stamp) < latest_ekf_time)) + { + prop_imu_buffer.pop_front(); + } + last_t_from_lidar_end_time = 0; + for (int i = 0; i < prop_imu_buffer.size(); i++) + { + double t_from_lidar_end_time = stamp2Sec(prop_imu_buffer[i].header.stamp) - latest_ekf_time; + double dt = t_from_lidar_end_time - last_t_from_lidar_end_time; + // cout << "prop dt" << dt << ", " << t_from_lidar_end_time << ", " << last_t_from_lidar_end_time << endl; + V3D acc_imu(prop_imu_buffer[i].linear_acceleration.x, prop_imu_buffer[i].linear_acceleration.y, prop_imu_buffer[i].linear_acceleration.z); + V3D omg_imu(prop_imu_buffer[i].angular_velocity.x, prop_imu_buffer[i].angular_velocity.y, prop_imu_buffer[i].angular_velocity.z); + prop_imu_once(imu_propagate, dt, acc_imu, omg_imu); + last_t_from_lidar_end_time = t_from_lidar_end_time; + } + state_update_flg = false; + } + else + { + V3D acc_imu(newest_imu.linear_acceleration.x, newest_imu.linear_acceleration.y, newest_imu.linear_acceleration.z); + V3D omg_imu(newest_imu.angular_velocity.x, newest_imu.angular_velocity.y, newest_imu.angular_velocity.z); + double t_from_lidar_end_time = stamp2Sec(newest_imu.header.stamp) - latest_ekf_time; + double dt = t_from_lidar_end_time - last_t_from_lidar_end_time; + prop_imu_once(imu_propagate, dt, acc_imu, omg_imu); + last_t_from_lidar_end_time = t_from_lidar_end_time; + } + + V3D posi, vel_i; + Eigen::Quaterniond q; + posi = imu_propagate.pos_end; + vel_i = imu_propagate.vel_end; + q = Eigen::Quaterniond(imu_propagate.rot_end); + imu_prop_odom.header.frame_id = "world"; + imu_prop_odom.header.stamp = newest_imu.header.stamp; + imu_prop_odom.pose.pose.position.x = posi.x(); + imu_prop_odom.pose.pose.position.y = posi.y(); + imu_prop_odom.pose.pose.position.z = posi.z(); + imu_prop_odom.pose.pose.orientation.w = q.w(); + imu_prop_odom.pose.pose.orientation.x = q.x(); + imu_prop_odom.pose.pose.orientation.y = q.y(); + imu_prop_odom.pose.pose.orientation.z = q.z(); + imu_prop_odom.twist.twist.linear.x = vel_i.x(); + imu_prop_odom.twist.twist.linear.y = vel_i.y(); + imu_prop_odom.twist.twist.linear.z = vel_i.z(); + pubImuPropOdom->publish(imu_prop_odom); + } + mtx_buffer_imu_prop.unlock(); +} + +void LIVMapper::transformLidar(const Eigen::Matrix3d rot, const Eigen::Vector3d t, const PointCloudXYZI::Ptr &input_cloud, PointCloudXYZI::Ptr &trans_cloud) +{ + PointCloudXYZI().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); + PointType 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 LIVMapper::pointBodyToWorld(const PointType &pi, PointType &po) +{ + V3D p_body(pi.x, pi.y, pi.z); + V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + po.x = p_global(0); + po.y = p_global(1); + po.z = p_global(2); + po.intensity = pi.intensity; +} + +template void LIVMapper::pointBodyToWorld(const Matrix &pi, Matrix &po) +{ + V3D p_body(pi[0], pi[1], pi[2]); + V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + po[0] = p_global(0); + po[1] = p_global(1); + po[2] = p_global(2); +} + +template Matrix LIVMapper::pointBodyToWorld(const Matrix &pi) +{ + V3D p(pi[0], pi[1], pi[2]); + p = (_state.rot_end * (extR * p + extT) + _state.pos_end); + Eigen::Matrix po(p[0], p[1], p[2]); + return po; +} + +void LIVMapper::RGBpointBodyToWorld(PointType const *const pi, PointType *const po) +{ + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(_state.rot_end * (extR * p_body + extT) + _state.pos_end); + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; + po->curvature = pi->curvature; + po->normal_x = pi->normal_x; + po->normal_y = pi->normal_y; + po->normal_z = pi->normal_z; +} + +void LIVMapper::RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po) +{ + V3D p_body_lidar(pi->x, pi->y, pi->z); + V3D p_body_imu(extR * p_body_lidar + extT); + + po->x = p_body_imu(0); + po->y = p_body_imu(1); + po->z = p_body_imu(2); + po->intensity = pi->intensity; + po->curvature = pi->curvature; + po->normal_x = pi->normal_x; + po->normal_y = pi->normal_y; + po->normal_z = pi->normal_z; +} + +void LIVMapper::standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + if (!lidar_en) return; + mtx_buffer.lock(); + + double cur_head_time = stamp2Sec(msg->header.stamp) + lidar_time_offset; + // cout<<"got feature"<node->get_logger(),"lidar loop back, clear buffer"); + lid_raw_data_buffer.clear(); + } + // ROS_INFO("get point cloud at time: %.6f", stamp2Sec(msg->header.stamp)); + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); + lid_raw_data_buffer.push_back(ptr); + lid_header_time_buffer.push_back(cur_head_time); + last_timestamp_lidar = cur_head_time; + + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +void LIVMapper::livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::ConstSharedPtr &msg_in) +{ + if (!lidar_en) return; + mtx_buffer.lock(); + livox_ros_driver2::msg::CustomMsg::SharedPtr msg(new livox_ros_driver2::msg::CustomMsg(*msg_in)); + // if ((abs(stamp2Sec(msg->header.stamp) - last_timestamp_lidar) > 0.2 && last_timestamp_lidar > 0) || sync_jump_flag) + // { + // ROS_WARN("lidar jumps %.3f\n", stamp2Sec(msg->header.stamp) - last_timestamp_lidar); + // sync_jump_flag = true; + // msg->header.stamp = rclcpp::Time().fromSec(last_timestamp_lidar + 0.1); + // } + if (abs(last_timestamp_imu - stamp2Sec(msg->header.stamp)) > 1.0 && !imu_buffer.empty()) + { + double timediff_imu_wrt_lidar = last_timestamp_imu - stamp2Sec(msg->header.stamp); + RCLCPP_INFO(this->node->get_logger(), "\033[95mSelf sync IMU and LiDAR, HARD time lag is %.10lf \n\033[0m", timediff_imu_wrt_lidar - 0.100); + // imu_time_offset = timediff_imu_wrt_lidar; + } + + double cur_head_time = stamp2Sec(msg->header.stamp); + RCLCPP_INFO(this->node->get_logger(), "Get LiDAR, its header time: %.6f", cur_head_time); + if (cur_head_time < last_timestamp_lidar) + { + RCLCPP_ERROR(this->node->get_logger(), "lidar loop back, clear buffer"); + lid_raw_data_buffer.clear(); + } + RCLCPP_INFO(this->node->get_logger(), "get point cloud at time: %.6f", stamp2Sec(msg->header.stamp)); + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); + + if (!ptr || ptr->empty()) { + RCLCPP_ERROR(this->node->get_logger(), "Received an empty point cloud"); + mtx_buffer.unlock(); + return; + } + + lid_raw_data_buffer.push_back(ptr); + lid_header_time_buffer.push_back(cur_head_time); + last_timestamp_lidar = cur_head_time; + + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +void LIVMapper::imu_cbk(const sensor_msgs::msg::Imu::ConstSharedPtr &msg_in) +{ + if (!imu_en) return; + + if (last_timestamp_lidar < 0.0) return; + RCLCPP_INFO(this->node->get_logger(), "get imu at time: %.6f", stamp2Sec(msg_in->header.stamp)); + sensor_msgs::msg::Imu::SharedPtr msg(new sensor_msgs::msg::Imu(*msg_in)); + msg->header.stamp = sec2Stamp(stamp2Sec(msg->header.stamp) - imu_time_offset); + double timestamp = stamp2Sec(msg->header.stamp); + + if (fabs(last_timestamp_lidar - timestamp) > 0.5 && (!ros_driver_fix_en)) + { + RCLCPP_WARN(this->node->get_logger(), "IMU and LiDAR not synced! delta time: %lf .\n", last_timestamp_lidar - timestamp); + } + + if (ros_driver_fix_en) timestamp += std::round(last_timestamp_lidar - timestamp); + msg->header.stamp = sec2Stamp(timestamp); + + mtx_buffer.lock(); + + if (last_timestamp_imu > 0.0 && timestamp < last_timestamp_imu) + { + mtx_buffer.unlock(); + sig_buffer.notify_all(); + RCLCPP_ERROR(this->node->get_logger(), "imu loop back, offset: %lf \n", last_timestamp_imu - timestamp); + return; + } + + if (last_timestamp_imu > 0.0 && timestamp > last_timestamp_imu + 0.2) + { + RCLCPP_WARN(this->node->get_logger(), "imu time stamp Jumps %0.4lf seconds \n", timestamp - last_timestamp_imu); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + last_timestamp_imu = timestamp; + + imu_buffer.push_back(msg); + cout<<"got imu: "<imu_need_init) { prop_imu_buffer.push_back(*msg); } + newest_imu = *msg; + new_imu = true; + mtx_buffer_imu_prop.unlock(); + } + sig_buffer.notify_all(); +} + +cv::Mat LIVMapper::getImageFromMsg(const sensor_msgs::msg::Image::ConstSharedPtr &img_msg) +{ + cv::Mat img; + img = cv_bridge::toCvShare(img_msg, "bgr8")->image; + return img; +} + +// static int i = 0; +void LIVMapper::img_cbk(const sensor_msgs::msg::Image::ConstSharedPtr &msg_in) +{ + if (!img_en) return; + sensor_msgs::msg::Image::SharedPtr msg(new sensor_msgs::msg::Image(*msg_in)); + // if ((abs(stamp2Sec(msg->header.stamp) - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag) + // { + // RCLCPP_WARN(this->node->get_logger(), "img jumps %.3f\n", stamp2Sec(msg->header.stamp) - last_timestamp_img); + // sync_jump_flag = true; + // msg->header.stamp = rclcpp::Time().fromSec(last_timestamp_img + 0.1); + // } + + // Hiliti2022 40Hz + if (hilti_en) + { + static int frame_counter = 0; + if (++frame_counter % 4 != 0) return; + } + // double msg_header_time = stamp2Sec(msg->header.stamp); + double msg_header_time = stamp2Sec(msg->header.stamp) + img_time_offset; + if (abs(msg_header_time - last_timestamp_img) < 0.001) return; + RCLCPP_INFO(this->node->get_logger(), "Get image, its header time: %.6f", msg_header_time); + if (last_timestamp_lidar < 0) return; + + if (msg_header_time < last_timestamp_img) + { + RCLCPP_ERROR(this->node->get_logger(), "image loop back. \n"); + return; + } + + mtx_buffer.lock(); + + double img_time_correct = msg_header_time; // last_timestamp_lidar + 0.105; + + if (img_time_correct - last_timestamp_img < 0.02) + { + RCLCPP_WARN(this->node->get_logger(), "Image need Jumps: %.6f", img_time_correct); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + cv::Mat img_cur = getImageFromMsg(msg); + img_buffer.push_back(img_cur); + img_time_buffer.push_back(img_time_correct); + + // ROS_INFO("Correct Image time: %.6f", img_time_correct); + + last_timestamp_img = img_time_correct; + // cv::imshow("img", img); + // cv::waitKey(1); + // cout<<"last_timestamp_img:::"<points.size() <= 1) return false; + + meas.lidar_frame_beg_time = lid_header_time_buffer.front(); // generate lidar_frame_beg_time + meas.lidar_frame_end_time = meas.lidar_frame_beg_time + meas.lidar->points.back().curvature / double(1000); // calc lidar scan end time + meas.pcl_proc_cur = meas.lidar; + lidar_pushed = true; // flag + } + + if (imu_en && last_timestamp_imu < meas.lidar_frame_end_time) + { // waiting imu message needs to be + // larger than _lidar_frame_end_time, + // make sure complete propagate. + // ROS_ERROR("out sync"); + return false; + } + + struct MeasureGroup m; // standard method to keep imu message. + + m.imu.clear(); + m.lio_time = meas.lidar_frame_end_time; + mtx_buffer.lock(); + while (!imu_buffer.empty()) + { + if (stamp2Sec(imu_buffer.front()->header.stamp) > meas.lidar_frame_end_time) break; + m.imu.push_back(imu_buffer.front()); + imu_buffer.pop_front(); + } + lid_raw_data_buffer.pop_front(); + lid_header_time_buffer.pop_front(); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + + meas.lio_vio_flg = LIO; // process lidar topic, so timestamp should be lidar scan end. + meas.measures.push_back(m); + // ROS_INFO("ONlY HAS LiDAR and IMU, NO IMAGE!"); + lidar_pushed = false; // sync one whole lidar scan. + return true; + + break; + } + + case LIVO: + { + /*** For LIVO mode, the time of LIO update is set to be the same as VIO, LIO + * first than VIO imediatly ***/ + EKF_STATE last_lio_vio_flg = meas.lio_vio_flg; + // double t0 = omp_get_wtime(); + switch (last_lio_vio_flg) + { + // double img_capture_time = meas.lidar_frame_beg_time + exposure_time_init; + case WAIT: + case VIO: + { + // printf("!!! meas.lio_vio_flg: %d \n", meas.lio_vio_flg); + double img_capture_time = img_time_buffer.front() + exposure_time_init; + /*** has img topic, but img topic timestamp larger than lidar end time, + * process lidar topic. After LIO update, the meas.lidar_frame_end_time + * will be refresh. ***/ + if (meas.last_lio_update_time < 0.0) meas.last_lio_update_time = lid_header_time_buffer.front(); + // printf("[ Data Cut ] wait \n"); + // printf("[ Data Cut ] last_lio_update_time: %lf \n", + // meas.last_lio_update_time); + + double lid_newest_time = lid_header_time_buffer.back() + lid_raw_data_buffer.back()->points.back().curvature / double(1000); + double imu_newest_time = stamp2Sec(imu_buffer.back()->header.stamp); + + if (img_capture_time < meas.last_lio_update_time + 0.00001) + { + img_buffer.pop_front(); + img_time_buffer.pop_front(); + RCLCPP_ERROR(this->node->get_logger(), "[ Data Cut ] Throw one image frame! \n"); + return false; + } + + if (img_capture_time > lid_newest_time || img_capture_time > imu_newest_time) + { + // RCLCPP_ERROR(this->node->get_logger(), "lost first camera frame"); + // printf("img_capture_time, lid_newest_time, imu_newest_time: %lf , %lf + // , %lf \n", img_capture_time, lid_newest_time, imu_newest_time); + return false; + } + + struct MeasureGroup m; + + // printf("[ Data Cut ] LIO \n"); + // printf("[ Data Cut ] img_capture_time: %lf \n", img_capture_time); + m.imu.clear(); + m.lio_time = img_capture_time; + mtx_buffer.lock(); + while (!imu_buffer.empty()) + { + if (stamp2Sec(imu_buffer.front()->header.stamp) > m.lio_time) break; + + if (stamp2Sec(imu_buffer.front()->header.stamp) > meas.last_lio_update_time) m.imu.push_back(imu_buffer.front()); + + imu_buffer.pop_front(); + // printf("[ Data Cut ] imu time: %lf \n", + // stamp2Sec(imu_buffer.front()->header.stamp)); + } + mtx_buffer.unlock(); + sig_buffer.notify_all(); + + *(meas.pcl_proc_cur) = *(meas.pcl_proc_next); + PointCloudXYZI().swap(*meas.pcl_proc_next); + + int lid_frame_num = lid_raw_data_buffer.size(); + int max_size = meas.pcl_proc_cur->size() + 24000 * lid_frame_num; + meas.pcl_proc_cur->reserve(max_size); + meas.pcl_proc_next->reserve(max_size); + // deque lidar_buffer_tmp; + + while (!lid_raw_data_buffer.empty()) + { + if (lid_header_time_buffer.front() > img_capture_time) break; + auto pcl(lid_raw_data_buffer.front()->points); + double frame_header_time(lid_header_time_buffer.front()); + float max_offs_time_ms = (m.lio_time - frame_header_time) * 1000.0f; + + for (int i = 0; i < pcl.size(); i++) + { + auto pt = pcl[i]; + if (pcl[i].curvature < max_offs_time_ms) + { + pt.curvature += (frame_header_time - meas.last_lio_update_time) * 1000.0f; + meas.pcl_proc_cur->points.push_back(pt); + } + else + { + pt.curvature += (frame_header_time - m.lio_time) * 1000.0f; + meas.pcl_proc_next->points.push_back(pt); + } + } + lid_raw_data_buffer.pop_front(); + lid_header_time_buffer.pop_front(); + } + + meas.measures.push_back(m); + meas.lio_vio_flg = LIO; + // meas.last_lio_update_time = m.lio_time; + // printf("!!! meas.lio_vio_flg: %d \n", meas.lio_vio_flg); + // printf("[ Data Cut ] pcl_proc_cur number: %d \n", meas.pcl_proc_cur + // ->points.size()); printf("[ Data Cut ] LIO process time: %lf \n", + // omp_get_wtime() - t0); + return true; + } + + case LIO: + { + double img_capture_time = img_time_buffer.front() + exposure_time_init; + meas.lio_vio_flg = VIO; + // printf("[ Data Cut ] VIO \n"); + meas.measures.clear(); + double imu_time = stamp2Sec(imu_buffer.front()->header.stamp); + + struct MeasureGroup m; + m.vio_time = img_capture_time; + m.lio_time = meas.last_lio_update_time; + m.img = img_buffer.front(); + mtx_buffer.lock(); + // while ((!imu_buffer.empty() && (imu_time < img_capture_time))) + // { + // imu_time = stamp2Sec(imu_buffer.front()->header.stamp); + // if (imu_time > img_capture_time) break; + // m.imu.push_back(imu_buffer.front()); + // imu_buffer.pop_front(); + // printf("[ Data Cut ] imu time: %lf \n", + // stamp2Sec(imu_buffer.front()->header.stamp)); + // } + img_buffer.pop_front(); + img_time_buffer.pop_front(); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + meas.measures.push_back(m); + lidar_pushed = false; // after VIO update, the _lidar_frame_end_time will be refresh. + // printf("[ Data Cut ] VIO process time: %lf \n", omp_get_wtime() - t0); + return true; + } + + default: + { + // printf("!! WRONG EKF STATE !!"); + return false; + } + // return false; + } + break; + } + + case ONLY_LO: + { + if (!lidar_pushed) + { + // If not in lidar scan, need to generate new meas + if (lid_raw_data_buffer.empty()) return false; + meas.lidar = lid_raw_data_buffer.front(); // push the first lidar topic + meas.lidar_frame_beg_time = lid_header_time_buffer.front(); // generate lidar_beg_time + meas.lidar_frame_end_time = meas.lidar_frame_beg_time + meas.lidar->points.back().curvature / double(1000); // calc lidar scan end time + lidar_pushed = true; + } + struct MeasureGroup m; // standard method to keep imu message. + m.lio_time = meas.lidar_frame_end_time; + mtx_buffer.lock(); + lid_raw_data_buffer.pop_front(); + lid_header_time_buffer.pop_front(); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + lidar_pushed = false; // sync one whole lidar scan. + meas.lio_vio_flg = LO; // process lidar topic, so timestamp should be lidar scan end. + meas.measures.push_back(m); + return true; + break; + } + + default: + { + printf("!! WRONG SLAM TYPE !!"); + return false; + } + } + RCLCPP_ERROR(this->node->get_logger(), "out sync"); +} + +void LIVMapper::publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager) +{ + cv::Mat img_rgb = vio_manager->img_cp; + cv_bridge::CvImage out_msg; + out_msg.header.stamp = this->node->get_clock()->now(); + // out_msg.header.frame_id = "camera_init"; + out_msg.encoding = sensor_msgs::image_encodings::BGR8; + out_msg.image = img_rgb; + pubImage.publish(out_msg.toImageMsg()); +} + +// Provide output format for LiDAR-visual BA +void LIVMapper::publish_frame_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudFullRes, VIOManagerPtr vio_manager) +{ + if (pcl_w_wait_pub->empty()) return; + PointCloudXYZRGB::Ptr laserCloudWorldRGB(new PointCloudXYZRGB()); + static int pub_num = 1; + pub_num++; + + if (LidarMeasures.lio_vio_flg == VIO) + { + *pcl_wait_pub += *pcl_w_wait_pub; + if(pub_num >= pub_scan_num) + { + pub_num = 1; + size_t size = pcl_wait_pub->points.size(); + laserCloudWorldRGB->reserve(size); + // double inv_expo = _state.inv_expo_time; + cv::Mat img_rgb = vio_manager->img_rgb; + for (size_t i = 0; i < size; i++) + { + PointTypeRGB pointRGB; + pointRGB.x = pcl_wait_pub->points[i].x; + pointRGB.y = pcl_wait_pub->points[i].y; + pointRGB.z = pcl_wait_pub->points[i].z; + + V3D p_w(pcl_wait_pub->points[i].x, pcl_wait_pub->points[i].y, pcl_wait_pub->points[i].z); + V3D pf(vio_manager->new_frame_->w2f(p_w)); if (pf[2] < 0) continue; + V2D pc(vio_manager->new_frame_->w2c(p_w)); + + if (vio_manager->new_frame_->cam_->isInFrame(pc.cast(), 3)) // 100 + { + V3F pixel = vio_manager->getInterpolatedPixel(img_rgb, pc); + pointRGB.r = pixel[2]; + pointRGB.g = pixel[1]; + pointRGB.b = pixel[0]; + // pointRGB.r = pixel[2] * inv_expo; pointRGB.g = pixel[1] * inv_expo; pointRGB.b = pixel[0] * inv_expo; + // if (pointRGB.r > 255) pointRGB.r = 255; else if (pointRGB.r < 0) pointRGB.r = 0; + // if (pointRGB.g > 255) pointRGB.g = 255; else if (pointRGB.g < 0) pointRGB.g = 0; + // if (pointRGB.b > 255) pointRGB.b = 255; else if (pointRGB.b < 0) pointRGB.b = 0; + if (pf.norm() > blind_rgb_points) laserCloudWorldRGB->push_back(pointRGB); + } + } + } + } + + /*** Publish Frame ***/ + sensor_msgs::msg::PointCloud2 laserCloudmsg; + if (slam_mode_ == LIVO && LidarMeasures.lio_vio_flg == VIO) + { + pcl::toROSMsg(*laserCloudWorldRGB, laserCloudmsg); + } + if (slam_mode_ == ONLY_LIO || slam_mode_ == ONLY_LO) + { + pcl::toROSMsg(*pcl_w_wait_pub, laserCloudmsg); + } + laserCloudmsg.header.stamp = this->node->get_clock()->now(); //.fromSec(last_timestamp_lidar); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFullRes->publish(laserCloudmsg); + + /**************** save map ****************/ + /* 1. make sure you have enough memories + /* 2. noted that pcd save will influence the real-time performences **/ + double update_time = 0.0; + if (LidarMeasures.lio_vio_flg == VIO) { + update_time = LidarMeasures.measures.back().vio_time; + } else { // LIO / LO + update_time = LidarMeasures.measures.back().lio_time; + } + std::stringstream ss_time; + ss_time << std::fixed << std::setprecision(6) << update_time; + + if (pcd_save_en) + { + static int scan_wait_num = 0; + + switch (pcd_save_type) + { + case 0: /** world frame **/ + if (slam_mode_ == LIVO) + { + *pcl_wait_save += *laserCloudWorldRGB; + } + else + { + *pcl_wait_save_intensity += *pcl_w_wait_pub; + } + if(LidarMeasures.lio_vio_flg == LIO || LidarMeasures.lio_vio_flg == LO) scan_wait_num++; + break; + + case 1: /** body frame **/ + if (LidarMeasures.lio_vio_flg == LIO || LidarMeasures.lio_vio_flg == LO) + { + int size = feats_undistort->points.size(); + PointCloudXYZI::Ptr laserCloudBody(new PointCloudXYZI(size, 1)); + for (int i = 0; i < size; i++) + { + RGBpointBodyLidarToIMU(&feats_undistort->points[i], &laserCloudBody->points[i]); + } + *pcl_wait_save_intensity += *laserCloudBody; + scan_wait_num++; + cout << "save body frame points: " << pcl_wait_save_intensity->points.size() << endl; + } + pcd_save_interval = 1; + + break; + + default: + pcd_save_interval = 1; + scan_wait_num++; + break; + } + if ((pcl_wait_save->size() > 0 || pcl_wait_save_intensity->size() > 0) && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) + { + string all_points_dir(string(string(ROOT_DIR) + "Log/pcd/") + ss_time.str() + string(".pcd")); + + pcl::PCDWriter pcd_writer; + + cout << "current scan saved to " << all_points_dir << endl; + if (pcl_wait_save->points.size() > 0) + { + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); // pcl::io::savePCDFileASCII(all_points_dir, *pcl_wait_save); + PointCloudXYZRGB().swap(*pcl_wait_save); + } + if(pcl_wait_save_intensity->points.size() > 0) + { + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save_intensity); + PointCloudXYZI().swap(*pcl_wait_save_intensity); + } + scan_wait_num = 0; + } + + if(LidarMeasures.lio_vio_flg == LIO || LidarMeasures.lio_vio_flg == LO) + { + Eigen::Quaterniond q(_state.rot_end); + fout_lidar_pos << std::fixed << std::setprecision(6); + fout_lidar_pos << LidarMeasures.measures.back().lio_time << " " << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " << q.x() << " " << q.y() << " " << q.z() + << " " << q.w() << " " << endl; + } + } + if (img_save_en && LidarMeasures.lio_vio_flg == VIO) + { + static int img_wait_num = 0; + img_wait_num++; + + if (img_save_interval > 0 && img_wait_num >= img_save_interval) + { + imwrite(string(string(ROOT_DIR) + "Log/image/") + ss_time.str() + string(".png"), vio_manager->img_rgb); + + Eigen::Quaterniond q(_state.rot_end); + fout_visual_pos << std::fixed << std::setprecision(6); + fout_visual_pos << LidarMeasures.measures.back().vio_time << " " << _state.pos_end[0] << " " << _state.pos_end[1] << " " << _state.pos_end[2] << " " + << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl; + img_wait_num = 0; + } + } + + if(laserCloudWorldRGB->size() > 0) PointCloudXYZI().swap(*pcl_wait_pub); + if(LidarMeasures.lio_vio_flg == VIO) PointCloudXYZI().swap(*pcl_w_wait_pub); +} + +void LIVMapper::publish_visual_sub_map(const rclcpp::Publisher::SharedPtr &pubSubVisualMap) +{ + PointCloudXYZI::Ptr laserCloudFullRes(visual_sub_map); + int size = laserCloudFullRes->points.size(); if (size == 0) return; + PointCloudXYZI::Ptr sub_pcl_visual_map_pub(new PointCloudXYZI()); + *sub_pcl_visual_map_pub = *laserCloudFullRes; + if (1) + { + sensor_msgs::msg::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*sub_pcl_visual_map_pub, laserCloudmsg); + laserCloudmsg.header.stamp = this->node->get_clock()->now(); + laserCloudmsg.header.frame_id = "camera_init"; + pubSubVisualMap->publish(laserCloudmsg); + } +} + +void LIVMapper::publish_effect_world(const rclcpp::Publisher::SharedPtr &pubLaserCloudEffect, const std::vector &ptpl_list) +{ + int effect_feat_num = ptpl_list.size(); + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(effect_feat_num, 1)); + for (int i = 0; i < effect_feat_num; i++) + { + laserCloudWorld->points[i].x = ptpl_list[i].point_w_[0]; + laserCloudWorld->points[i].y = ptpl_list[i].point_w_[1]; + laserCloudWorld->points[i].z = ptpl_list[i].point_w_[2]; + } + sensor_msgs::msg::PointCloud2 laserCloudFullRes3; + pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); + laserCloudFullRes3.header.stamp = this->node->get_clock()->now(); + laserCloudFullRes3.header.frame_id = "camera_init"; + pubLaserCloudEffect->publish(laserCloudFullRes3); +} + +template void LIVMapper::set_posestamp(T &out) +{ + out.position.x = _state.pos_end(0); + out.position.y = _state.pos_end(1); + out.position.z = _state.pos_end(2); + out.orientation.x = geoQuat.x; + out.orientation.y = geoQuat.y; + out.orientation.z = geoQuat.z; + out.orientation.w = geoQuat.w; +} + +void LIVMapper::publish_odometry(const rclcpp::Publisher::SharedPtr &pubOdomAftMapped) +{ + odomAftMapped.header.frame_id = "camera_init"; + odomAftMapped.child_frame_id = "aft_mapped"; + odomAftMapped.header.stamp = this->node->get_clock()->now(); //.ros::Time()fromSec(last_timestamp_lidar); + set_posestamp(odomAftMapped.pose.pose); + + static std::shared_ptr br; + br = std::make_shared(this->node); + tf2::Transform transform; + tf2::Quaternion q; + transform.setOrigin(tf2::Vector3(_state.pos_end(0), _state.pos_end(1), _state.pos_end(2))); + q.setW(geoQuat.w); + q.setX(geoQuat.x); + q.setY(geoQuat.y); + q.setZ(geoQuat.z); + transform.setRotation(q); + br->sendTransform(geometry_msgs::msg::TransformStamped(createTransformStamped(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped"))); + pubOdomAftMapped->publish(odomAftMapped); +} + +void LIVMapper::publish_mavros(const rclcpp::Publisher::SharedPtr &mavros_pose_publisher) +{ + msg_body_pose.header.stamp = this->node->get_clock()->now(); + msg_body_pose.header.frame_id = "camera_init"; + set_posestamp(msg_body_pose.pose); + mavros_pose_publisher->publish(msg_body_pose); +} + +void LIVMapper::publish_path(const rclcpp::Publisher::SharedPtr &pubPath) +{ + set_posestamp(msg_body_pose.pose); + msg_body_pose.header.stamp = this->node->get_clock()->now(); + msg_body_pose.header.frame_id = "camera_init"; + path.poses.push_back(msg_body_pose); + pubPath->publish(path); +} \ No newline at end of file diff --git a/src/FAST-LIVO2/src/frame.cpp b/src/FAST-LIVO2/src/frame.cpp new file mode 100644 index 0000000..643dd10 --- /dev/null +++ b/src/FAST-LIVO2/src/frame.cpp @@ -0,0 +1,65 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 +#include "feature.h" +#include "frame.h" +#include "visual_point.h" +#include +#include +#include +#include + +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 diff --git a/src/FAST-LIVO2/src/main.cpp b/src/FAST-LIVO2/src/main.cpp new file mode 100755 index 0000000..61dd592 --- /dev/null +++ b/src/FAST-LIVO2/src/main.cpp @@ -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; +} \ No newline at end of file diff --git a/src/FAST-LIVO2/src/preprocess.cpp b/src/FAST-LIVO2/src/preprocess.cpp new file mode 100755 index 0000000..bf0d497 --- /dev/null +++ b/src/FAST-LIVO2/src/preprocess.cpp @@ -0,0 +1,1126 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 "preprocess.h" + +#define RETURN0 0x00 +#define RETURN0AND1 0x10 + +Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1) +{ + inf_bound = 10; + N_SCANS = 6; + group_size = 8; + disA = 0.01; + disA = 0.1; // B? + p2l_ratio = 225; + limit_maxmid = 6.25; + limit_midmin = 6.25; + limit_maxmin = 3.24; + jump_up_limit = 170.0; + jump_down_limit = 8.0; + cos160 = 160.0; + edgea = 2; + edgeb = 0.1; + smallp_intersect = 172.5; + smallp_ratio = 1.2; + given_offset_time = false; + + jump_up_limit = cos(jump_up_limit / 180 * M_PI); + jump_down_limit = cos(jump_down_limit / 180 * M_PI); + cos160 = cos(cos160 / 180 * M_PI); + smallp_intersect = cos(smallp_intersect / 180 * M_PI); +} + +Preprocess::~Preprocess() {} + +void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) +{ + feature_enabled = feat_en; + lidar_type = lid_type; + blind = bld; + point_filter_num = pfilt_num; +} + +void Preprocess::process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) +{ + avia_handler(msg); + *pcl_out = pl_surf; +} + +void Preprocess::process(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg, PointCloudXYZI::Ptr &pcl_out) +{ + switch (lidar_type) + { + case OUST64: + oust64_handler(msg); + break; + + case VELO16: + velodyne_handler(msg); + break; + + case L515: + l515_handler(msg); + break; + + case XT32: + xt32_handler(msg); + break; + + case PANDAR128: + Pandar128_handler(msg); + break; + + case ROBOSENSE: + robosense_handler(msg); + break; + + default: + printf("Error LiDAR Type: %d \n", lidar_type); + break; + } + *pcl_out = pl_surf; +} + +void Preprocess::avia_handler(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + double t1 = omp_get_wtime(); + int plsize = msg->point_num; + printf("[ Preprocess ] Input point number: %d \n", plsize); + // printf("point_filter_num: %d\n", point_filter_num); + + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + pl_full.resize(plsize); + + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + uint valid_num = 0; + + if (feature_enabled) + { + for (uint i = 1; i < plsize; i++) + { + if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10)) + { + pl_full[i].x = msg->points[i].x; + pl_full[i].y = msg->points[i].y; + pl_full[i].z = msg->points[i].z; + pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points + + bool is_new = false; + if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || + (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) + { + pl_buff[msg->points[i].line].push_back(pl_full[i]); + } + } + } + static int count = 0; + static double time = 0.0; + count++; + double t0 = omp_get_wtime(); + for (int j = 0; j < N_SCANS; j++) + { + if (pl_buff[j].size() <= 5) continue; + pcl::PointCloud &pl = pl_buff[j]; + plsize = pl.size(); + vector &types = typess[j]; + types.clear(); + types.resize(plsize); + plsize--; + for (uint i = 0; i < plsize; i++) + { + types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y; + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y; + give_feature(pl, types); + // pl_surf += pl; + } + time += omp_get_wtime() - t0; + printf("Feature extraction time: %lf \n", time / count); + } + else + { + for (uint i = 0; i < plsize; i++) + { + if ((msg->points[i].line < N_SCANS)) // && ((msg->points[i].tag & 0x30) == 0x10)) + { + valid_num++; + + pl_full[i].x = msg->points[i].x; + pl_full[i].y = msg->points[i].y; + pl_full[i].z = msg->points[i].z; + pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points + + if (i == 0) + pl_full[i].curvature = fabs(pl_full[i].curvature) < 1.0 ? pl_full[i].curvature : 0.0; + else + { + // if(fabs(pl_full[i].curvature - pl_full[i - 1].curvature) > 1.0) ROS_ERROR("time jump: %f", fabs(pl_full[i].curvature - pl_full[i - 1].curvature)); + pl_full[i].curvature = fabs(pl_full[i].curvature - pl_full[i - 1].curvature) < 1.0 + ? pl_full[i].curvature + : pl_full[i - 1].curvature + 0.004166667f; // float(100/24000) + } + + if (valid_num % point_filter_num == 0) + { + if (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z >= blind_sqr) + { + pl_surf.push_back(pl_full[i]); + // if (i % 100 == 0 || i == 0) printf("pl_full[i].curvature: %f \n", + // pl_full[i].curvature); + } + } + } + } + } + printf("[ Preprocess ] Output point number: %zu \n", pl_surf.points.size()); +} + +void Preprocess::l515_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + + double time_stamp = stamp2Sec(msg->header.stamp); + // cout << "===================================" << endl; + // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); + for (int i = 0; i < pl_orig.points.size(); i++) + { + if (i % point_filter_num != 0) continue; + + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + + if (range < blind_sqr) continue; + + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.normal_x = pl_orig.points[i].r; + added_pt.normal_y = pl_orig.points[i].g; + added_pt.normal_z = pl_orig.points[i].b; + + added_pt.curvature = 0.0; + pl_surf.points.push_back(added_pt); + } + + cout << "pl size:: " << pl_orig.points.size() << endl; + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +void Preprocess::oust64_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + if (feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (uint i = 0; i < plsize; i++) + { + double range = + pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + if (range < blind_sqr) continue; + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; + if (yaw_angle >= 180.0) yaw_angle -= 360.0; + if (yaw_angle <= -180.0) yaw_angle += 360.0; + + added_pt.curvature = pl_orig.points[i].t / 1e6; + if (pl_orig.points[i].ring < N_SCANS) { pl_buff[pl_orig.points[i].ring].push_back(added_pt); } + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI &pl = pl_buff[j]; + int linesize = pl.size(); + vector &types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + double time_stamp = stamp2Sec(msg->header.stamp); + // cout << "===================================" << endl; + // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); + for (int i = 0; i < pl_orig.points.size(); i++) + { + if (i % point_filter_num != 0) continue; + + double range = + pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + + if (range < blind_sqr) continue; + + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; + if (yaw_angle >= 180.0) yaw_angle -= 360.0; + if (yaw_angle <= -180.0) yaw_angle += 360.0; + + added_pt.curvature = pl_orig.points[i].t / 1e6; + + // cout<header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +#define MAX_LINE_NUM 64 + +void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) return; + pl_surf.reserve(plsize); + + bool is_first[MAX_LINE_NUM]; + double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point + double omega_l = 3.61; // scan angular velocity + float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point + float time_last[MAX_LINE_NUM] = {0.0}; // last offset time + + if (pl_orig.points[plsize - 1].time > 0) { given_offset_time = true; } + else + { + given_offset_time = false; + memset(is_first, true, sizeof(is_first)); + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + if (feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + int layer = pl_orig.points[i].ring; + if (layer >= N_SCANS) continue; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms + + if (!given_offset_time) + { + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; + if (is_first[layer]) + { + // printf("layer: %d; is first: %d", layer, is_first[layer]); + yaw_fp[layer] = yaw_angle; + is_first[layer] = false; + added_pt.curvature = 0.0; + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + continue; + } + + if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; } + else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } + + if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; + + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + } + + pl_buff[layer].points.push_back(added_pt); + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI &pl = pl_buff[j]; + int linesize = pl.size(); + if (linesize < 2) continue; + vector &types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< blind_sqr) + { + pl_surf.points.push_back(added_pt); + // printf("time mode: %d time: %d \n", given_offset_time, + // pl_orig.points[i].t); + } + } + } + } + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_surf, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +void Preprocess::Pandar128_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + pl_surf.reserve(plsize); + + double time_head = pl_orig.points[0].timestamp; + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = static_cast(pl_orig.points[i].intensity) / 255.0f; + added_pt.curvature = (pl_orig.points[i].timestamp - time_head) * 1000.f; + + if (i % point_filter_num == 0) + { + if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind_sqr) + { + pl_surf.points.push_back(added_pt); + // printf("time mode: %d time: %d \n", given_offset_time, + // pl_orig.points[i].t); + } + } + } + + // define a lambda function for the comparison + auto comparePoints = [](const PointType& a, const PointType& b) -> bool + { + return a.curvature < b.curvature; + }; + + // sort the points using the comparison function + std::sort(pl_surf.points.begin(), pl_surf.points.end(), comparePoints); + + // cout << GREEN << "pl_surf.points[0].timestamp: " << pl_surf.points[0].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[1000].timestamp: " << pl_surf.points[1000].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[5000].timestamp: " << pl_surf.points[5000].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[10000].timestamp: " << pl_surf.points[10000].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[20000].timestamp: " << pl_surf.points[20000].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[30000].timestamp: " << pl_surf.points[30000].curvature << RESET << endl; + // cout << GREEN << "pl_surf.points[31000].timestamp: " << pl_surf.points[31000].curvature << RESET << endl; +} + +void Preprocess::xt32_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + pl_surf.reserve(plsize); + + bool is_first[MAX_LINE_NUM]; + double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point + double omega_l = 3.61; // scan angular velocity + float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point + float time_last[MAX_LINE_NUM] = {0.0}; // last offset time + + if (pl_orig.points[plsize - 1].timestamp > 0) { given_offset_time = true; } + else + { + given_offset_time = false; + memset(is_first, true, sizeof(is_first)); + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + double time_head = pl_orig.points[0].timestamp; + + if (feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + int layer = pl_orig.points[i].ring; + if (layer >= N_SCANS) continue; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.curvature = pl_orig.points[i].timestamp / 1000.0; // units: ms + + if (!given_offset_time) + { + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; + if (is_first[layer]) + { + // printf("layer: %d; is first: %d", layer, is_first[layer]); + yaw_fp[layer] = yaw_angle; + is_first[layer] = false; + added_pt.curvature = 0.0; + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + continue; + } + + if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; } + else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } + + if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; + + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + } + + pl_buff[layer].points.push_back(added_pt); + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI &pl = pl_buff[j]; + int linesize = pl.size(); + if (linesize < 2) continue; + vector &types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< blind_sqr) + { + pl_surf.points.push_back(added_pt); + // printf("time mode: %d time: %d \n", given_offset_time, + // pl_orig.points[i].t); + } + } + } + } + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_surf, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +void Preprocess::robosense_handler(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) +{ + pl_surf.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_surf.reserve(plsize); + + double time_head = pl_orig.points[0].timestamp; + for (int i = 0; i < plsize; ++i) + { + if (i % point_filter_num != 0) continue; + + const auto& pt = pl_orig.points[i]; + const double x = pt.x, y = pt.y, z = pt.z; + const double dist_sqr = x * x + y * y + z * z; + const bool is_valid = (dist_sqr >= blind_sqr) && !std::isnan(x) && !std::isnan(y) && !std::isnan(z); + if (!is_valid) continue; + + PointType added_pt; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.x = pt.x; + added_pt.y = pt.y; + added_pt.z = pt.z; + added_pt.intensity = pt.intensity; + added_pt.curvature = (pt.timestamp - time_head) * 1000.0; + pl_surf.points.push_back(added_pt); + } + std::sort(pl_surf.points.begin(), pl_surf.points.end(), [](const PointType &a, const PointType &b) { + return a.curvature < b.curvature; + }); +} + +void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) +{ + int plsize = pl.size(); + int plsize2; + if (plsize == 0) + { + printf("something wrong\n"); + return; + } + uint head = 0; + + while (types[head].range < blind_sqr) + { + head++; + } + + // Surf + plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; + + Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); + Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); + + uint i_nex = 0, i2; + uint last_i = 0; + uint last_i_nex = 0; + int last_state = 0; + int plane_type; + + for (uint i = head; i < plsize2; i++) + { + if (types[i].range < blind_sqr) { continue; } + + i2 = i; + + plane_type = plane_judge(pl, types, i, i_nex, curr_direct); + + if (plane_type == 1) + { + for (uint j = i; j <= i_nex; j++) + { + if (j != i && j != i_nex) { types[j].ftype = Real_Plane; } + else { types[j].ftype = Poss_Plane; } + } + + // if(last_state==1 && fabs(last_direct.sum())>0.5) + if (last_state == 1 && last_direct.norm() > 0.1) + { + double mod = last_direct.transpose() * curr_direct; + if (mod > -0.707 && mod < 0.707) { types[i].ftype = Edge_Plane; } + else { types[i].ftype = Real_Plane; } + } + + i = i_nex - 1; + last_state = 1; + } + else // if(plane_type == 2) + { + i = i_nex; + last_state = 0; + } + // else if(plane_type == 0) + // { + // if(last_state == 1) + // { + // uint i_nex_tem; + // uint j; + // for(j=last_i+1; j<=last_i_nex; j++) + // { + // uint i_nex_tem2 = i_nex_tem; + // Eigen::Vector3d curr_direct2; + + // uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2); + + // if(ttem != 1) + // { + // i_nex_tem = i_nex_tem2; + // break; + // } + // curr_direct = curr_direct2; + // } + + // if(j == last_i+1) + // { + // last_state = 0; + // } + // else + // { + // for(uint k=last_i_nex; k<=i_nex_tem; k++) + // { + // if(k != i_nex_tem) + // { + // types[k].ftype = Real_Plane; + // } + // else + // { + // types[k].ftype = Poss_Plane; + // } + // } + // i = i_nex_tem-1; + // i_nex = i_nex_tem; + // i2 = j-1; + // last_state = 1; + // } + + // } + // } + + last_i = i2; + last_i_nex = i_nex; + last_direct = curr_direct; + } + + plsize2 = plsize > 3 ? plsize - 3 : 0; + for (uint i = head + 3; i < plsize2; i++) + { + if (types[i].range < blind_sqr || types[i].ftype >= Real_Plane) { continue; } + + if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16) { continue; } + + Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); + Eigen::Vector3d vecs[2]; + + for (int j = 0; j < 2; j++) + { + int m = -1; + if (j == 1) { m = 1; } + + if (types[i + m].range < blind_sqr) + { + if (types[i].range > inf_bound) { types[i].edj[j] = Nr_inf; } + else { types[i].edj[j] = Nr_blind; } + continue; + } + + vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z); + vecs[j] = vecs[j] - vec_a; + + types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); + if (types[i].angle[j] < jump_up_limit) { types[i].edj[j] = Nr_180; } + else if (types[i].angle[j] > jump_down_limit) { types[i].edj[j] = Nr_zero; } + } + + types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); + if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 && types[i].dista > 4 * types[i - 1].dista) + { + if (types[i].intersect > cos160) + { + if (edge_jump_judge(pl, types, i, Prev)) { types[i].ftype = Edge_Jump; } + } + } + else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 && types[i - 1].dista > 4 * types[i].dista) + { + if (types[i].intersect > cos160) + { + if (edge_jump_judge(pl, types, i, Next)) { types[i].ftype = Edge_Jump; } + } + } + else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf) + { + if (edge_jump_judge(pl, types, i, Prev)) { types[i].ftype = Edge_Jump; } + } + else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor) + { + if (edge_jump_judge(pl, types, i, Next)) { types[i].ftype = Edge_Jump; } + } + else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor) + { + if (types[i].ftype == Nor) { types[i].ftype = Wire; } + } + } + + plsize2 = plsize - 1; + double ratio; + for (uint i = head + 1; i < plsize2; i++) + { + if (types[i].range < blind_sqr || types[i - 1].range < blind_sqr || types[i + 1].range < blind_sqr) { continue; } + + if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8) { continue; } + + if (types[i].ftype == Nor) + { + if (types[i - 1].dista > types[i].dista) { ratio = types[i - 1].dista / types[i].dista; } + else { ratio = types[i].dista / types[i - 1].dista; } + + if (types[i].intersect < smallp_intersect && ratio < smallp_ratio) + { + if (types[i - 1].ftype == Nor) { types[i - 1].ftype = Real_Plane; } + if (types[i + 1].ftype == Nor) { types[i + 1].ftype = Real_Plane; } + types[i].ftype = Real_Plane; + } + } + } + + int last_surface = -1; + for (uint j = head; j < plsize; j++) + { + if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane) + { + if (last_surface == -1) { last_surface = j; } + + if (j == uint(last_surface + point_filter_num - 1)) + { + PointType ap; + ap.x = pl[j].x; + ap.y = pl[j].y; + ap.z = pl[j].z; + ap.curvature = pl[j].curvature; + pl_surf.push_back(ap); + + last_surface = -1; + } + } + else + { + if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane) { pl_corn.push_back(pl[j]); } + if (last_surface != -1) + { + PointType ap; + for (uint k = last_surface; k < j; k++) + { + ap.x += pl[k].x; + ap.y += pl[k].y; + ap.z += pl[k].z; + ap.curvature += pl[k].curvature; + } + ap.x /= (j - last_surface); + ap.y /= (j - last_surface); + ap.z /= (j - last_surface); + ap.curvature /= (j - last_surface); + pl_surf.push_back(ap); + } + last_surface = -1; + } + } +} + +void Preprocess::pub_func(PointCloudXYZI &pl, const rclcpp::Time &ct) +{ + pl.height = 1; + pl.width = pl.size(); + sensor_msgs::msg::PointCloud2 output; + pcl::toROSMsg(pl, output); + output.header.frame_id = "livox"; + output.header.stamp = ct; +} + +int Preprocess::plane_judge(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) +{ + double group_dis = disA * types[i_cur].range + disB; + group_dis = group_dis * group_dis; + // i_nex = i_cur; + + double two_dis; + vector disarr; + disarr.reserve(20); + + for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++) + { + if (types[i_nex].range < blind_sqr) + { + curr_direct.setZero(); + return 2; + } + disarr.push_back(types[i_nex].dista); + } + + for (;;) + { + if ((i_cur >= pl.size()) || (i_nex >= pl.size())) break; + + if (types[i_nex].range < blind_sqr) + { + curr_direct.setZero(); + return 2; + } + vx = pl[i_nex].x - pl[i_cur].x; + vy = pl[i_nex].y - pl[i_cur].y; + vz = pl[i_nex].z - pl[i_cur].z; + two_dis = vx * vx + vy * vy + vz * vz; + if (two_dis >= group_dis) { break; } + disarr.push_back(types[i_nex].dista); + i_nex++; + } + + double leng_wid = 0; + double v1[3], v2[3]; + for (uint j = i_cur + 1; j < i_nex; j++) + { + if ((j >= pl.size()) || (i_cur >= pl.size())) break; + v1[0] = pl[j].x - pl[i_cur].x; + v1[1] = pl[j].y - pl[i_cur].y; + v1[2] = pl[j].z - pl[i_cur].z; + + v2[0] = v1[1] * vz - vy * v1[2]; + v2[1] = v1[2] * vx - v1[0] * vz; + v2[2] = v1[0] * vy - vx * v1[1]; + + double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2]; + if (lw > leng_wid) { leng_wid = lw; } + } + + if ((two_dis * two_dis / leng_wid) < p2l_ratio) + { + curr_direct.setZero(); + return 0; + } + + uint disarrsize = disarr.size(); + for (uint j = 0; j < disarrsize - 1; j++) + { + for (uint k = j + 1; k < disarrsize; k++) + { + if (disarr[j] < disarr[k]) + { + leng_wid = disarr[j]; + disarr[j] = disarr[k]; + disarr[k] = leng_wid; + } + } + } + + if (disarr[disarr.size() - 2] < 1e-16) + { + curr_direct.setZero(); + return 0; + } + + if (lidar_type == AVIA) + { + double dismax_mid = disarr[0] / disarr[disarrsize / 2]; + double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2]; + + if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin) + { + curr_direct.setZero(); + return 0; + } + } + else + { + double dismax_min = disarr[0] / disarr[disarrsize - 2]; + if (dismax_min >= limit_maxmin) + { + curr_direct.setZero(); + return 0; + } + } + + curr_direct << vx, vy, vz; + curr_direct.normalize(); + return 1; +} + +bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir) +{ + if (nor_dir == 0) + { + if (types[i - 1].range < blind_sqr || types[i - 2].range < blind_sqr) { return false; } + } + else if (nor_dir == 1) + { + if (types[i + 1].range < blind_sqr || types[i + 2].range < blind_sqr) { return false; } + } + double d1 = types[i + nor_dir - 1].dista; + double d2 = types[i + 3 * nor_dir - 2].dista; + double d; + + if (d1 < d2) + { + d = d1; + d1 = d2; + d2 = d; + } + + d1 = sqrt(d1); + d2 = sqrt(d2); + + if (d1 > edgea * d2 || (d1 - d2) > edgeb) { return false; } + + return true; +} \ No newline at end of file diff --git a/src/FAST-LIVO2/src/utils.cpp b/src/FAST-LIVO2/src/utils.cpp new file mode 100644 index 0000000..1e453c8 --- /dev/null +++ b/src/FAST-LIVO2/src/utils.cpp @@ -0,0 +1,19 @@ +// utils.cpp +#include +#include // for int64_t +#include // for std::numeric_limits +#include // for std::out_of_range + +std::vector convertToIntVectorSafe(const std::vector& int64_vector) { + std::vector int_vector; + int_vector.reserve(int64_vector.size()); // 预留空间以提高效率 + + for (int64_t value : int64_vector) { + if (value < std::numeric_limits::min() || value > std::numeric_limits::max()) { + throw std::out_of_range("Value is out of range for int"); + } + int_vector.push_back(static_cast(value)); + } + + return int_vector; +} diff --git a/src/FAST-LIVO2/src/vio.cpp b/src/FAST-LIVO2/src/vio.cpp new file mode 100755 index 0000000..6e14fcf --- /dev/null +++ b/src/FAST-LIVO2/src/vio.cpp @@ -0,0 +1,1877 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 "vio.h" + +using namespace Eigen; +VIOManager::VIOManager() +{ + // downSizeFilter.setLeafSize(0.2, 0.2, 0.2); +} + +VIOManager::~VIOManager() +{ + delete visual_submap; + for (auto& pair : warp_map) delete pair.second; + warp_map.clear(); + for (auto& pair : feat_map) delete pair.second; + feat_map.clear(); +} + +void VIOManager::setImuToLidarExtrinsic(const V3D &transl, const M3D &rot) +{ + Pli = -rot.transpose() * transl; + Rli = rot.transpose(); +} + +void VIOManager::setLidarToCameraExtrinsic(vector &R, vector &P) +{ + Rcl << MAT_FROM_ARRAY(R); + Pcl << VEC_FROM_ARRAY(P); +} + +void VIOManager::initializeVIO() +{ + visual_submap = new SubSparseMap; + + fx = cam->fx(); + fy = cam->fy(); + cx = cam->cx(); + cy = cam->cy(); + image_resize_factor = cam->scale(); + + printf("intrinsic: %.6lf, %.6lf, %.6lf, %.6lf\n", fx, fy, cx, cy); + + width = cam->width(); + height = cam->height(); + + printf("width: %d, height: %d, scale: %f\n", width, height, image_resize_factor); + Rci = Rcl * Rli; + Pci = Rcl * Pli + Pcl; + + V3D Pic; + M3D tmp; + Jdphi_dR = Rci; + Pic = -Rci.transpose() * Pci; + tmp << SKEW_SYM_MATRX(Pic); + Jdp_dR = -Rci * tmp; + + if (grid_size > 10) + { + grid_n_width = ceil(static_cast(width / grid_size)); + grid_n_height = ceil(static_cast(height / grid_size)); + } + else + { + grid_size = static_cast(height / grid_n_height); + grid_n_height = ceil(static_cast(height / grid_size)); + grid_n_width = ceil(static_cast(width / grid_size)); + } + length = grid_n_width * grid_n_height; + + if(raycast_en) + { + // cv::Mat img_test = cv::Mat::zeros(height, width, CV_8UC1); + // uchar* it = (uchar*)img_test.data; + + border_flag.resize(length, 0); + + std::vector>().swap(rays_with_sample_points); + rays_with_sample_points.reserve(length); + printf("grid_size: %d, grid_n_height: %d, grid_n_width: %d, length: %d\n", grid_size, grid_n_height, grid_n_width, length); + + float d_min = 0.1; + float d_max = 3.0; + float step = 0.2; + for (int grid_row = 1; grid_row <= grid_n_height; grid_row++) + { + for (int grid_col = 1; grid_col <= grid_n_width; grid_col++) + { + std::vector SamplePointsEachGrid; + int index = (grid_row - 1) * grid_n_width + grid_col - 1; + + if (grid_row == 1 || grid_col == 1 || grid_row == grid_n_height || grid_col == grid_n_width) border_flag[index] = 1; + + int u = grid_size / 2 + (grid_col - 1) * grid_size; + int v = grid_size / 2 + (grid_row - 1) * grid_size; + // it[ u + v * width ] = 255; + for (float d_temp = d_min; d_temp <= d_max; d_temp += step) + { + V3D xyz; + xyz = cam->cam2world(u, v); + xyz *= d_temp / xyz[2]; + // xyz[0] = (u - cx) / fx * d_temp; + // xyz[1] = (v - cy) / fy * d_temp; + // xyz[2] = d_temp; + SamplePointsEachGrid.push_back(xyz); + } + rays_with_sample_points.push_back(SamplePointsEachGrid); + } + } + // printf("rays_with_sample_points: %d, RaysWithSamplePointsCapacity: %d, + // rays_with_sample_points[0].capacity(): %d, rays_with_sample_points[0]: %d\n", + // rays_with_sample_points.size(), rays_with_sample_points.capacity(), + // rays_with_sample_points[0].capacity(), rays_with_sample_points[0].size()); for + // (const auto & it : rays_with_sample_points[0]) cout << it.transpose() << endl; + // cv::imshow("img_test", img_test); + // cv::waitKey(1); + } + + if(colmap_output_en) + { + pinhole_cam = dynamic_cast(cam); + fout_colmap.open(DEBUG_FILE_DIR("Colmap/sparse/0/images.txt"), ios::out); + fout_colmap << "# Image list with two lines of data per image:\n"; + fout_colmap << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n"; + fout_colmap << "# POINTS2D[] as (X, Y, POINT3D_ID)\n"; + fout_camera.open(DEBUG_FILE_DIR("Colmap/sparse/0/cameras.txt"), ios::out); + fout_camera << "# Camera list with one line of data per camera:\n"; + fout_camera << "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n"; + fout_camera << "1 PINHOLE " << width << " " << height << " " + << std::fixed << std::setprecision(6) // 控制浮点数精度为10位 + << fx << " " << fy << " " + << cx << " " << cy << std::endl; + fout_camera.close(); + } + grid_num.resize(length); + map_index.resize(length); + map_dist.resize(length); + update_flag.resize(length); + scan_value.resize(length); + + patch_size_total = patch_size * patch_size; + patch_size_half = static_cast(patch_size / 2); + patch_buffer.resize(patch_size_total); + warp_len = patch_size_total * patch_pyrimid_level; + border = (patch_size_half + 1) * (1 << patch_pyrimid_level); + + retrieve_voxel_points.reserve(length); + append_voxel_points.reserve(length); + + sub_feat_map.clear(); +} + +void VIOManager::resetGrid() +{ + fill(grid_num.begin(), grid_num.end(), TYPE_UNKNOWN); + fill(map_index.begin(), map_index.end(), 0); + fill(map_dist.begin(), map_dist.end(), 10000.0f); + fill(update_flag.begin(), update_flag.end(), 0); + fill(scan_value.begin(), scan_value.end(), 0.0f); + + retrieve_voxel_points.clear(); + retrieve_voxel_points.resize(length); + + append_voxel_points.clear(); + append_voxel_points.resize(length); + + total_points = 0; +} + +// void VIOManager::resetRvizDisplay() +// { + // sub_map_ray.clear(); + // sub_map_ray_fov.clear(); + // visual_sub_map_cur.clear(); + // visual_converged_point.clear(); + // map_cur_frame.clear(); + // sample_points.clear(); +// } + +void VIOManager::computeProjectionJacobian(V3D p, MD(2, 3) & J) +{ + const double x = p[0]; + const double y = p[1]; + const double z_inv = 1. / p[2]; + const double z_inv_2 = z_inv * z_inv; + J(0, 0) = fx * z_inv; + J(0, 1) = 0.0; + J(0, 2) = -fx * x * z_inv_2; + J(1, 0) = 0.0; + J(1, 1) = fy * z_inv; + J(1, 2) = -fy * y * z_inv_2; +} + +void VIOManager::getImagePatch(cv::Mat img, V2D pc, float *patch_tmp, int level) +{ + const float u_ref = pc[0]; + const float v_ref = pc[1]; + const int scale = (1 << level); + const int u_ref_i = floorf(pc[0] / scale) * scale; + const int v_ref_i = floorf(pc[1] / scale) * scale; + const float subpix_u_ref = (u_ref - u_ref_i) / scale; + const float subpix_v_ref = (v_ref - v_ref_i) / scale; + const float w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref); + const float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref); + const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; + const float w_ref_br = subpix_u_ref * subpix_v_ref; + for (int x = 0; x < patch_size; x++) + { + uint8_t *img_ptr = (uint8_t *)img.data + (v_ref_i - patch_size_half * scale + x * scale) * width + (u_ref_i - patch_size_half * scale); + for (int y = 0; y < patch_size; y++, img_ptr += scale) + { + patch_tmp[patch_size_total * level + x * patch_size + y] = + w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * width] + w_ref_br * img_ptr[scale * width + scale]; + } + } +} + +void VIOManager::insertPointIntoVoxelMap(VisualPoint *pt_new) +{ + V3D pt_w(pt_new->pos_[0], pt_new->pos_[1], pt_new->pos_[2]); + double voxel_size = 0.5; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = pt_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 = feat_map.find(position); + if (iter != feat_map.end()) + { + iter->second->voxel_points.push_back(pt_new); + iter->second->count++; + } + else + { + VOXEL_POINTS *ot = new VOXEL_POINTS(0); + ot->voxel_points.push_back(pt_new); + feat_map[position] = ot; + } +} + +void VIOManager::getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref, const V3D &xyz_ref, const V3D &normal_ref, + const SE3 &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref) +{ + // create homography matrix + const V3D t = T_cur_ref.inverse().translation(); + const Eigen::Matrix3d H_cur_ref = + T_cur_ref.rotationMatrix() * (normal_ref.dot(xyz_ref) * Eigen::Matrix3d::Identity() - t * normal_ref.transpose()); + // Compute affine warp matrix A_ref_cur using homography projection + const int kHalfPatchSize = 4; + V3D f_du_ref(cam.cam2world(px_ref + Eigen::Vector2d(kHalfPatchSize, 0) * (1 << level_ref))); + V3D f_dv_ref(cam.cam2world(px_ref + Eigen::Vector2d(0, kHalfPatchSize) * (1 << level_ref))); + // f_du_ref = f_du_ref/f_du_ref[2]; + // f_dv_ref = f_dv_ref/f_dv_ref[2]; + const V3D f_cur(H_cur_ref * xyz_ref); + const V3D f_du_cur = H_cur_ref * f_du_ref; + const V3D f_dv_cur = H_cur_ref * f_dv_ref; + V2D px_cur(cam.world2cam(f_cur)); + V2D px_du_cur(cam.world2cam(f_du_cur)); + V2D px_dv_cur(cam.world2cam(f_dv_cur)); + A_cur_ref.col(0) = (px_du_cur - px_cur) / kHalfPatchSize; + A_cur_ref.col(1) = (px_dv_cur - px_cur) / kHalfPatchSize; +} + +void VIOManager::getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref, + const SE3 &T_cur_ref, const int level_ref, const int pyramid_level, const int halfpatch_size, + Matrix2d &A_cur_ref) +{ + // Compute affine warp matrix A_ref_cur + const Vector3d xyz_ref(f_ref * depth_ref); + Vector3d xyz_du_ref(cam.cam2world(px_ref + Vector2d(halfpatch_size, 0) * (1 << level_ref) * (1 << pyramid_level))); + Vector3d xyz_dv_ref(cam.cam2world(px_ref + Vector2d(0, halfpatch_size) * (1 << level_ref) * (1 << pyramid_level))); + xyz_du_ref *= xyz_ref[2] / xyz_du_ref[2]; + xyz_dv_ref *= xyz_ref[2] / xyz_dv_ref[2]; + const Vector2d px_cur(cam.world2cam(T_cur_ref * (xyz_ref))); + const Vector2d px_du(cam.world2cam(T_cur_ref * (xyz_du_ref))); + const Vector2d px_dv(cam.world2cam(T_cur_ref * (xyz_dv_ref))); + A_cur_ref.col(0) = (px_du - px_cur) / halfpatch_size; + A_cur_ref.col(1) = (px_dv - px_cur) / halfpatch_size; +} + +void VIOManager::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) +{ + const int patch_size = halfpatch_size * 2; + const Matrix2f A_ref_cur = A_cur_ref.inverse().cast(); + if (isnan(A_ref_cur(0, 0))) + { + printf("Affine warp is NaN, probably camera has no translation\n"); // TODO + return; + } + + float *patch_ptr = patch; + for (int y = 0; y < patch_size; ++y) + { + for (int x = 0; x < patch_size; ++x) //, ++patch_ptr) + { + Vector2f px_patch(x - halfpatch_size, y - halfpatch_size); + px_patch *= (1 << search_level); + px_patch *= (1 << pyramid_level); + const Vector2f px(A_ref_cur * px_patch + px_ref.cast()); + if (px[0] < 0 || px[1] < 0 || px[0] >= img_ref.cols - 1 || px[1] >= img_ref.rows - 1) + patch_ptr[patch_size_total * pyramid_level + y * patch_size + x] = 0; + else + patch_ptr[patch_size_total * pyramid_level + y * patch_size + x] = (float)vk::interpolateMat_8u(img_ref, px[0], px[1]); + } + } +} + +int VIOManager::getBestSearchLevel(const Matrix2d &A_cur_ref, const int max_level) +{ + // Compute patch level in other image + int search_level = 0; + double D = A_cur_ref.determinant(); + while (D > 3.0 && search_level < max_level) + { + search_level += 1; + D *= 0.25; + } + return search_level; +} + +double VIOManager::calculateNCC(float *ref_patch, float *cur_patch, int patch_size) +{ + double sum_ref = std::accumulate(ref_patch, ref_patch + patch_size, 0.0); + double mean_ref = sum_ref / patch_size; + + double sum_cur = std::accumulate(cur_patch, cur_patch + patch_size, 0.0); + double mean_curr = sum_cur / patch_size; + + double numerator = 0, demoniator1 = 0, demoniator2 = 0; + for (int i = 0; i < patch_size; i++) + { + double n = (ref_patch[i] - mean_ref) * (cur_patch[i] - mean_curr); + numerator += n; + demoniator1 += (ref_patch[i] - mean_ref) * (ref_patch[i] - mean_ref); + demoniator2 += (cur_patch[i] - mean_curr) * (cur_patch[i] - mean_curr); + } + return numerator / sqrt(demoniator1 * demoniator2 + 1e-10); +} + +void VIOManager::retrieveFromVisualSparseMap(cv::Mat img, vector &pg, const unordered_map &plane_map) +{ + if (feat_map.size() <= 0) return; + double ts0 = omp_get_wtime(); + + // pg_down->reserve(feat_map.size()); + // downSizeFilter.setInputCloud(pg); + // downSizeFilter.filter(*pg_down); + + // resetRvizDisplay(); + visual_submap->reset(); + + // Controls whether to include the visual submap from the previous frame. + sub_feat_map.clear(); + + float voxel_size = 0.5; + + if (!normal_en) warp_map.clear(); + + cv::Mat depth_img = cv::Mat::zeros(height, width, CV_32FC1); + float *it = (float *)depth_img.data; + + // float it[height * width] = {0.0}; + + // double t_insert, t_depth, t_position; + // t_insert=t_depth=t_position=0; + + int loc_xyz[3]; + + // printf("A0. initial depthmap: %.6lf \n", omp_get_wtime() - ts0); + // double ts1 = omp_get_wtime(); + + // printf("pg size: %zu \n", pg.size()); + + for (int i = 0; i < pg.size(); i++) + { + // double t0 = omp_get_wtime(); + + V3D pt_w = pg[i].point_w; + + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = floor(pt_w[j] / voxel_size); + if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + } + VOXEL_LOCATION position(loc_xyz[0], loc_xyz[1], loc_xyz[2]); + + // t_position += omp_get_wtime()-t0; + // double t1 = omp_get_wtime(); + + auto iter = sub_feat_map.find(position); + if (iter == sub_feat_map.end()) { sub_feat_map[position] = 0; } + else { iter->second = 0; } + + // t_insert += omp_get_wtime()-t1; + // double t2 = omp_get_wtime(); + + V3D pt_c(new_frame_->w2f(pt_w)); + + if (pt_c[2] > 0) + { + V2D px; + // px[0] = fx * pt_c[0]/pt_c[2] + cx; + // px[1] = fy * pt_c[1]/pt_c[2]+ cy; + px = new_frame_->cam_->world2cam(pt_c); + + if (new_frame_->cam_->isInFrame(px.cast(), border)) + { + // cv::circle(img_cp, cv::Point2f(px[0], px[1]), 3, cv::Scalar(0, 0, 255), -1, 8); + float depth = pt_c[2]; + int col = int(px[0]); + int row = int(px[1]); + it[width * row + col] = depth; + } + } + // t_depth += omp_get_wtime()-t2; + } + + // imshow("depth_img", depth_img); + // printf("A1: %.6lf \n", omp_get_wtime() - ts1); + // printf("A11. calculate pt position: %.6lf \n", t_position); + // printf("A12. sub_postion.insert(position): %.6lf \n", t_insert); + // printf("A13. generate depth map: %.6lf \n", t_depth); + // printf("A. projection: %.6lf \n", omp_get_wtime() - ts0); + + // double t1 = omp_get_wtime(); + vector DeleteKeyList; + + for (auto &iter : sub_feat_map) + { + VOXEL_LOCATION position = iter.first; + + // double t4 = omp_get_wtime(); + auto corre_voxel = feat_map.find(position); + // double t5 = omp_get_wtime(); + + if (corre_voxel != feat_map.end()) + { + bool voxel_in_fov = false; + std::vector &voxel_points = corre_voxel->second->voxel_points; + int voxel_num = voxel_points.size(); + + for (int i = 0; i < voxel_num; i++) + { + VisualPoint *pt = voxel_points[i]; + if (pt == nullptr) continue; + if (pt->obs_.size() == 0) continue; + + V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * pt->normal_); + V3D dir(new_frame_->T_f_w_ * pt->pos_); + if (dir[2] < 0) continue; + // dir.normalize(); + // if (dir.dot(norm_vec) <= 0.17) continue; // 0.34 70 degree 0.17 80 degree 0.08 85 degree + + V2D pc(new_frame_->w2c(pt->pos_)); + if (new_frame_->cam_->isInFrame(pc.cast(), border)) + { + // cv::circle(img_cp, cv::Point2f(pc[0], pc[1]), 3, cv::Scalar(0, 255, 255), -1, 8); + voxel_in_fov = true; + int index = static_cast(pc[1] / grid_size) * grid_n_width + static_cast(pc[0] / grid_size); + grid_num[index] = TYPE_MAP; + Vector3d obs_vec(new_frame_->pos() - pt->pos_); + float cur_dist = obs_vec.norm(); + if (cur_dist <= map_dist[index]) + { + map_dist[index] = cur_dist; + retrieve_voxel_points[index] = pt; + } + } + } + if (!voxel_in_fov) { DeleteKeyList.push_back(position); } + } + } + + // RayCasting Module + if (raycast_en) + { + for (int i = 0; i < length; i++) + { + if (grid_num[i] == TYPE_MAP || border_flag[i] == 1) continue; + + // int row = static_cast(i / grid_n_width) * grid_size + grid_size / + // 2; int col = (i - static_cast(i / grid_n_width) * grid_n_width) * + // grid_size + grid_size / 2; + + // cv::circle(img_cp, cv::Point2f(col, row), 3, cv::Scalar(255, 255, 0), + // -1, 8); + + // vector sample_points_temp; + // bool add_sample = false; + + for (const auto &it : rays_with_sample_points[i]) + { + V3D sample_point_w = new_frame_->f2w(it); + // sample_points_temp.push_back(sample_point_w); + + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = floor(sample_point_w[j] / voxel_size); + if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; } + } + + VOXEL_LOCATION sample_pos(loc_xyz[0], loc_xyz[1], loc_xyz[2]); + + auto corre_sub_feat_map = sub_feat_map.find(sample_pos); + if (corre_sub_feat_map != sub_feat_map.end()) break; + + auto corre_feat_map = feat_map.find(sample_pos); + if (corre_feat_map != feat_map.end()) + { + bool voxel_in_fov = false; + + std::vector &voxel_points = corre_feat_map->second->voxel_points; + int voxel_num = voxel_points.size(); + if (voxel_num == 0) continue; + + for (int j = 0; j < voxel_num; j++) + { + VisualPoint *pt = voxel_points[j]; + + if (pt == nullptr) continue; + if (pt->obs_.size() == 0) continue; + + // sub_map_ray.push_back(pt); // cloud_visual_sub_map + // add_sample = true; + + V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * pt->normal_); + V3D dir(new_frame_->T_f_w_ * pt->pos_); + if (dir[2] < 0) continue; + dir.normalize(); + // if (dir.dot(norm_vec) <= 0.17) continue; // 0.34 70 degree 0.17 80 degree 0.08 85 degree + + V2D pc(new_frame_->w2c(pt->pos_)); + + if (new_frame_->cam_->isInFrame(pc.cast(), border)) + { + // cv::circle(img_cp, cv::Point2f(pc[0], pc[1]), 3, cv::Scalar(255, 255, 0), -1, 8); + // sub_map_ray_fov.push_back(pt); + + voxel_in_fov = true; + int index = static_cast(pc[1] / grid_size) * grid_n_width + static_cast(pc[0] / grid_size); + grid_num[index] = TYPE_MAP; + Vector3d obs_vec(new_frame_->pos() - pt->pos_); + + float cur_dist = obs_vec.norm(); + + if (cur_dist <= map_dist[index]) + { + map_dist[index] = cur_dist; + retrieve_voxel_points[index] = pt; + } + } + } + + if (voxel_in_fov) sub_feat_map[sample_pos] = 0; + break; + } + else + { + VOXEL_LOCATION sample_pos(loc_xyz[0], loc_xyz[1], loc_xyz[2]); + auto iter = plane_map.find(sample_pos); + if (iter != plane_map.end()) + { + VoxelOctoTree *current_octo; + current_octo = iter->second->find_correspond(sample_point_w); + if (current_octo->plane_ptr_->is_plane_) + { + pointWithVar plane_center; + VoxelPlane &plane = *current_octo->plane_ptr_; + plane_center.point_w = plane.center_; + plane_center.normal = plane.normal_; + visual_submap->add_from_voxel_map.push_back(plane_center); + break; + } + } + } + } + // if(add_sample) sample_points.push_back(sample_points_temp); + } + } + + for (auto &key : DeleteKeyList) + { + sub_feat_map.erase(key); + } + + // double t2 = omp_get_wtime(); + + // cout<<"B. feat_map.find: "<w2c(pt->pos_)); + + // cv::circle(img_cp, cv::Point2f(pc[0], pc[1]), 3, cv::Scalar(0, 0, 255), -1, 8); // Green Sparse Align tracked + + V3D pt_cam(new_frame_->w2f(pt->pos_)); + bool depth_continous = false; + for (int u = -patch_size_half; u <= patch_size_half; u++) + { + for (int v = -patch_size_half; v <= patch_size_half; v++) + { + if (u == 0 && v == 0) continue; + + float depth = it[width * (v + int(pc[1])) + u + int(pc[0])]; + + if (depth == 0.) continue; + + double delta_dist = abs(pt_cam[2] - depth); + + if (delta_dist > 0.5) + { + depth_continous = true; + break; + } + } + if (depth_continous) break; + } + if (depth_continous) continue; + + // t_2 += omp_get_wtime() - t_1; + + // t_1 = omp_get_wtime(); + Feature *ref_ftr; + std::vector patch_wrap(warp_len); + + int search_level; + Matrix2d A_cur_ref_zero; + + if (!pt->is_normal_initialized_) continue; + + if (normal_en) + { + float phtometric_errors_min = std::numeric_limits::max(); + + if (pt->obs_.size() == 1) + { + ref_ftr = *pt->obs_.begin(); + pt->ref_patch = ref_ftr; + pt->has_ref_patch_ = true; + } + else if (!pt->has_ref_patch_) + { + for (auto it = pt->obs_.begin(), ite = pt->obs_.end(); it != ite; ++it) + { + Feature *ref_patch_temp = *it; + float *patch_temp = ref_patch_temp->patch_; + float phtometric_errors = 0.0; + int count = 0; + for (auto itm = pt->obs_.begin(), itme = pt->obs_.end(); itm != itme; ++itm) + { + if ((*itm)->id_ == ref_patch_temp->id_) continue; + float *patch_cache = (*itm)->patch_; + + for (int ind = 0; ind < patch_size_total; ind++) + { + phtometric_errors += (patch_temp[ind] - patch_cache[ind]) * (patch_temp[ind] - patch_cache[ind]); + } + count++; + } + phtometric_errors = phtometric_errors / count; + if (phtometric_errors < phtometric_errors_min) + { + phtometric_errors_min = phtometric_errors; + ref_ftr = ref_patch_temp; + } + } + pt->ref_patch = ref_ftr; + pt->has_ref_patch_ = true; + } + else { ref_ftr = pt->ref_patch; } + } + else + { + if (!pt->getCloseViewObs(new_frame_->pos(), ref_ftr, pc)) continue; + } + + if (normal_en) + { + V3D norm_vec = (ref_ftr->T_f_w_.rotationMatrix() * pt->normal_).normalized(); + + V3D pf(ref_ftr->T_f_w_ * pt->pos_); + // V3D pf_norm = pf.normalized(); + + // double cos_theta = norm_vec.dot(pf_norm); + // if(cos_theta < 0) norm_vec = -norm_vec; + // if (abs(cos_theta) < 0.08) continue; // 0.5 60 degree 0.34 70 degree 0.17 80 degree 0.08 85 degree + + SE3 T_cur_ref = new_frame_->T_f_w_ * ref_ftr->T_f_w_.inverse(); + + getWarpMatrixAffineHomography(*cam, ref_ftr->px_, pf, norm_vec, T_cur_ref, 0, A_cur_ref_zero); + + search_level = getBestSearchLevel(A_cur_ref_zero, 2); + } + else + { + auto iter_warp = warp_map.find(ref_ftr->id_); + if (iter_warp != warp_map.end()) + { + search_level = iter_warp->second->search_level; + A_cur_ref_zero = iter_warp->second->A_cur_ref; + } + else + { + getWarpMatrixAffine(*cam, ref_ftr->px_, ref_ftr->f_, (ref_ftr->pos() - pt->pos_).norm(), new_frame_->T_f_w_ * ref_ftr->T_f_w_.inverse(), + ref_ftr->level_, 0, patch_size_half, A_cur_ref_zero); + + search_level = getBestSearchLevel(A_cur_ref_zero, 2); + + Warp *ot = new Warp(search_level, A_cur_ref_zero); + warp_map[ref_ftr->id_] = ot; + } + } + // t_4 += omp_get_wtime() - t_1; + + // t_1 = omp_get_wtime(); + + for (int pyramid_level = 0; pyramid_level <= patch_pyrimid_level - 1; pyramid_level++) + { + warpAffine(A_cur_ref_zero, ref_ftr->img_, ref_ftr->px_, ref_ftr->level_, search_level, pyramid_level, patch_size_half, patch_wrap.data()); + } + + getImagePatch(img, pc, patch_buffer.data(), 0); + + float error = 0.0; + for (int ind = 0; ind < patch_size_total; ind++) + { + error += (ref_ftr->inv_expo_time_ * patch_wrap[ind] - state->inv_expo_time * patch_buffer[ind]) * + (ref_ftr->inv_expo_time_ * patch_wrap[ind] - state->inv_expo_time * patch_buffer[ind]); + } + + if (ncc_en) + { + double ncc = calculateNCC(patch_wrap.data(), patch_buffer.data(), patch_size_total); + if (ncc < ncc_thre) + { + // grid_num[i] = TYPE_UNKNOWN; + continue; + } + } + + if (error > outlier_threshold * patch_size_total) continue; + + visual_submap->voxel_points.push_back(pt); + visual_submap->propa_errors.push_back(error); + visual_submap->search_levels.push_back(search_level); + visual_submap->errors.push_back(error); + visual_submap->warp_patch.push_back(patch_wrap); + visual_submap->inv_expo_list.push_back(ref_ftr->inv_expo_time_); + + // t_5 += omp_get_wtime() - t_1; + } + } + total_points = visual_submap->voxel_points.size(); + + // double t3 = omp_get_wtime(); + // cout<<"C. addSubSparseMap: "<= 0; level--) + { + if (inverse_composition_en) + { + has_ref_patch_cache = false; + updateStateInverse(img, level); + } + else + updateState(img, level); + } + state->cov -= G * state->cov; + updateFrameState(*state); +} + +void VIOManager::generateVisualMapPoints(cv::Mat img, vector &pg) +{ + if (pg.size() <= 10) return; + + // double t0 = omp_get_wtime(); + for (int i = 0; i < pg.size(); i++) + { + if (pg[i].normal == V3D(0, 0, 0)) continue; + + V3D pt = pg[i].point_w; + V2D pc(new_frame_->w2c(pt)); + + if (new_frame_->cam_->isInFrame(pc.cast(), border)) // 20px is the patch size in the matcher + { + int index = static_cast(pc[1] / grid_size) * grid_n_width + static_cast(pc[0] / grid_size); + + if (grid_num[index] != TYPE_MAP) + { + float cur_value = vk::shiTomasiScore(img, pc[0], pc[1]); + // if (cur_value < 5) continue; + if (cur_value > scan_value[index]) + { + scan_value[index] = cur_value; + append_voxel_points[index] = pg[i]; + grid_num[index] = TYPE_POINTCLOUD; + } + } + } + } + + for (int j = 0; j < visual_submap->add_from_voxel_map.size(); j++) + { + V3D pt = visual_submap->add_from_voxel_map[j].point_w; + V2D pc(new_frame_->w2c(pt)); + + if (new_frame_->cam_->isInFrame(pc.cast(), border)) // 20px is the patch size in the matcher + { + int index = static_cast(pc[1] / grid_size) * grid_n_width + static_cast(pc[0] / grid_size); + + if (grid_num[index] != TYPE_MAP) + { + float cur_value = vk::shiTomasiScore(img, pc[0], pc[1]); + if (cur_value > scan_value[index]) + { + scan_value[index] = cur_value; + append_voxel_points[index] = visual_submap->add_from_voxel_map[j]; + grid_num[index] = TYPE_POINTCLOUD; + } + } + } + } + + // double t_b1 = omp_get_wtime() - t0; + // t0 = omp_get_wtime(); + + int add = 0; + for (int i = 0; i < length; i++) + { + if (grid_num[i] == TYPE_POINTCLOUD) // && (scan_value[i]>=50)) + { + pointWithVar pt_var = append_voxel_points[i]; + V3D pt = pt_var.point_w; + + V3D norm_vec(new_frame_->T_f_w_.rotationMatrix() * pt_var.normal); + V3D dir(new_frame_->T_f_w_ * pt); + dir.normalize(); + double cos_theta = dir.dot(norm_vec); + // if(std::fabs(cos_theta)<0.34) continue; // 70 degree + V2D pc(new_frame_->w2c(pt)); + + float *patch = new float[patch_size_total]; + getImagePatch(img, pc, patch, 0); + + VisualPoint *pt_new = new VisualPoint(pt); + + Vector3d f = cam->cam2world(pc); + Feature *ftr_new = new Feature(pt_new, patch, pc, f, new_frame_->T_f_w_, 0); + ftr_new->img_ = img; + ftr_new->id_ = new_frame_->id_; + ftr_new->inv_expo_time_ = state->inv_expo_time; + + pt_new->addFrameRef(ftr_new); + pt_new->covariance_ = pt_var.var; + pt_new->is_normal_initialized_ = true; + + if (cos_theta < 0) { pt_new->normal_ = -pt_var.normal; } + else { pt_new->normal_ = pt_var.normal; } + + pt_new->previous_normal_ = pt_new->normal_; + + insertPointIntoVoxelMap(pt_new); + add += 1; + // map_cur_frame.push_back(pt_new); + } + } + + // double t_b2 = omp_get_wtime() - t0; + + printf("[ VIO ] Append %d new visual map points\n", add); + // printf("pg.size: %d \n", pg.size()); + // printf("B1. : %.6lf \n", t_b1); + // printf("B2. : %.6lf \n", t_b2); +} + +void VIOManager::updateVisualMapPoints(cv::Mat img) +{ + if (total_points == 0) return; + + int update_num = 0; + SE3 pose_cur = new_frame_->T_f_w_; + for (int i = 0; i < total_points; i++) + { + VisualPoint *pt = visual_submap->voxel_points[i]; + if (pt == nullptr) continue; + if (pt->is_converged_) + { + pt->deleteNonRefPatchFeatures(); + continue; + } + + V2D pc(new_frame_->w2c(pt->pos_)); + bool add_flag = false; + + float *patch_temp = new float[patch_size_total]; + getImagePatch(img, pc, patch_temp, 0); + // TODO: condition: distance and view_angle + // Step 1: time + Feature *last_feature = pt->obs_.back(); + // if(new_frame_->id_ >= last_feature->id_ + 10) add_flag = true; // 10 + + // Step 2: delta_pose + SE3 pose_ref = last_feature->T_f_w_; + SE3 delta_pose = pose_ref * pose_cur.inverse(); + double delta_p = delta_pose.translation().norm(); + double delta_theta = (delta_pose.rotationMatrix().trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (delta_pose.rotationMatrix().trace() - 1)); + if (delta_p > 0.5 || delta_theta > 0.3) add_flag = true; // 0.5 || 0.3 + + // Step 3: pixel distance + Vector2d last_px = last_feature->px_; + double pixel_dist = (pc - last_px).norm(); + if (pixel_dist > 40) add_flag = true; + + // Maintain the size of 3D point observation features. + if (pt->obs_.size() >= 30) + { + Feature *ref_ftr; + pt->findMinScoreFeature(new_frame_->pos(), ref_ftr); + pt->deleteFeatureRef(ref_ftr); + // cout<<"pt->obs_.size() exceed 20 !!!!!!"<cam2world(pc); + Feature *ftr_new = new Feature(pt, patch_temp, pc, f, new_frame_->T_f_w_, visual_submap->search_levels[i]); + ftr_new->img_ = img; + ftr_new->id_ = new_frame_->id_; + ftr_new->inv_expo_time_ = state->inv_expo_time; + pt->addFrameRef(ftr_new); + } + } + printf("[ VIO ] Update %d points in visual submap\n", update_num); +} + +void VIOManager::updateReferencePatch(const unordered_map &plane_map) +{ + if (total_points == 0) return; + + for (int i = 0; i < visual_submap->voxel_points.size(); i++) + { + VisualPoint *pt = visual_submap->voxel_points[i]; + + if (!pt->is_normal_initialized_) continue; + if (pt->is_converged_) continue; + if (pt->obs_.size() <= 5) continue; + if (update_flag[i] == 0) continue; + + const V3D &p_w = pt->pos_; + float loc_xyz[3]; + for (int j = 0; j < 3; j++) + { + loc_xyz[j] = p_w[j] / 0.5; + 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 = plane_map.find(position); + if (iter != plane_map.end()) + { + VoxelOctoTree *current_octo; + current_octo = iter->second->find_correspond(p_w); + if (current_octo->plane_ptr_->is_plane_) + { + VoxelPlane &plane = *current_octo->plane_ptr_; + float dis_to_plane = plane.normal_(0) * p_w(0) + plane.normal_(1) * p_w(1) + plane.normal_(2) * p_w(2) + plane.d_; + float dis_to_plane_abs = fabs(dis_to_plane); + 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 <= 3 * plane.radius_) + { + Eigen::Matrix 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() * pt->covariance_ * plane.normal_; + + if (dis_to_plane_abs < 3 * sqrt(sigma_l)) + { + // V3D norm_vec(new_frame_->T_f_w_.rotation_matrix() * plane.normal_); + // V3D pf(new_frame_->T_f_w_ * pt->pos_); + // V3D pf_ref(pt->ref_patch->T_f_w_ * pt->pos_); + // V3D norm_vec_ref(pt->ref_patch->T_f_w_.rotation_matrix() * + // plane.normal); double cos_ref = pf_ref.dot(norm_vec_ref); + + if (pt->previous_normal_.dot(plane.normal_) < 0) { pt->normal_ = -plane.normal_; } + else { pt->normal_ = plane.normal_; } + + double normal_update = (pt->normal_ - pt->previous_normal_).norm(); + + pt->previous_normal_ = pt->normal_; + + if (normal_update < 0.0001 && pt->obs_.size() > 10) + { + pt->is_converged_ = true; + // visual_converged_point.push_back(pt); + } + } + } + } + } + + float score_max = -1000.; + for (auto it = pt->obs_.begin(), ite = pt->obs_.end(); it != ite; ++it) + { + Feature *ref_patch_temp = *it; + float *patch_temp = ref_patch_temp->patch_; + float NCC_up = 0.0; + float NCC_down1 = 0.0; + float NCC_down2 = 0.0; + float NCC = 0.0; + float score = 0.0; + int count = 0; + + V3D pf = ref_patch_temp->T_f_w_ * pt->pos_; + V3D norm_vec = ref_patch_temp->T_f_w_.rotationMatrix() * pt->normal_; + pf.normalize(); + double cos_angle = pf.dot(norm_vec); + // if(fabs(cos_angle) < 0.86) continue; // 20 degree + + float ref_mean; + if (abs(ref_patch_temp->mean_) < 1e-6) + { + float ref_sum = std::accumulate(patch_temp, patch_temp + patch_size_total, 0.0); + ref_mean = ref_sum / patch_size_total; + ref_patch_temp->mean_ = ref_mean; + } + + for (auto itm = pt->obs_.begin(), itme = pt->obs_.end(); itm != itme; ++itm) + { + if ((*itm)->id_ == ref_patch_temp->id_) continue; + float *patch_cache = (*itm)->patch_; + + float other_mean; + if (abs((*itm)->mean_) < 1e-6) + { + float other_sum = std::accumulate(patch_cache, patch_cache + patch_size_total, 0.0); + other_mean = other_sum / patch_size_total; + (*itm)->mean_ = other_mean; + } + + for (int ind = 0; ind < patch_size_total; ind++) + { + NCC_up += (patch_temp[ind] - ref_mean) * (patch_cache[ind] - other_mean); + NCC_down1 += (patch_temp[ind] - ref_mean) * (patch_temp[ind] - ref_mean); + NCC_down2 += (patch_cache[ind] - other_mean) * (patch_cache[ind] - other_mean); + } + NCC += fabs(NCC_up / sqrt(NCC_down1 * NCC_down2)); + count++; + } + + NCC = NCC / count; + + score = NCC + cos_angle; + + ref_patch_temp->score_ = score; + + if (score > score_max) + { + score_max = score; + pt->ref_patch = ref_patch_temp; + pt->has_ref_patch_ = true; + } + } + + } +} + +void VIOManager::projectPatchFromRefToCur(const unordered_map &plane_map) +{ + if (total_points == 0) return; + // if(new_frame_->id_ != 2) return; //124 + + int patch_size = 25; + string dir = string(ROOT_DIR) + "Log/ref_cur_combine/"; + + cv::Mat result = cv::Mat::zeros(height, width, CV_8UC1); + cv::Mat result_normal = cv::Mat::zeros(height, width, CV_8UC1); + cv::Mat result_dense = cv::Mat::zeros(height, width, CV_8UC1); + + cv::Mat img_photometric_error = new_frame_->img_.clone(); + + uchar *it = (uchar *)result.data; + uchar *it_normal = (uchar *)result_normal.data; + uchar *it_dense = (uchar *)result_dense.data; + + struct pixel_member + { + Vector2f pixel_pos; + uint8_t pixel_value; + }; + + int num = 0; + for (int i = 0; i < visual_submap->voxel_points.size(); i++) + { + VisualPoint *pt = visual_submap->voxel_points[i]; + + if (pt->is_normal_initialized_) + { + Feature *ref_ftr; + ref_ftr = pt->ref_patch; + // Feature* ref_ftr; + V2D pc(new_frame_->w2c(pt->pos_)); + V2D pc_prior(new_frame_->w2c_prior(pt->pos_)); + + V3D norm_vec(ref_ftr->T_f_w_.rotationMatrix() * pt->normal_); + V3D pf(ref_ftr->T_f_w_ * pt->pos_); + + if (pf.dot(norm_vec) < 0) norm_vec = -norm_vec; + + // norm_vec << norm_vec(1), norm_vec(0), norm_vec(2); + cv::Mat img_cur = new_frame_->img_; + cv::Mat img_ref = ref_ftr->img_; + + SE3 T_cur_ref = new_frame_->T_f_w_ * ref_ftr->T_f_w_.inverse(); + Matrix2d A_cur_ref; + getWarpMatrixAffineHomography(*cam, ref_ftr->px_, pf, norm_vec, T_cur_ref, 0, A_cur_ref); + + // const Matrix2f A_ref_cur = A_cur_ref.inverse().cast(); + int search_level = getBestSearchLevel(A_cur_ref.inverse(), 2); + + double D = A_cur_ref.determinant(); + if (D > 3) continue; + + num++; + + cv::Mat ref_cur_combine_temp; + int radius = 20; + cv::hconcat(img_cur, img_ref, ref_cur_combine_temp); + cv::cvtColor(ref_cur_combine_temp, ref_cur_combine_temp, CV_GRAY2BGR); + + getImagePatch(img_cur, pc, patch_buffer.data(), 0); + + float error_est = 0.0; + float error_gt = 0.0; + + for (int ind = 0; ind < patch_size_total; ind++) + { + error_est += (ref_ftr->inv_expo_time_ * visual_submap->warp_patch[i][ind] - state->inv_expo_time * patch_buffer[ind]) * + (ref_ftr->inv_expo_time_ * visual_submap->warp_patch[i][ind] - state->inv_expo_time * patch_buffer[ind]); + } + std::string ref_est = "ref_est " + std::to_string(1.0 / ref_ftr->inv_expo_time_); + std::string cur_est = "cur_est " + std::to_string(1.0 / state->inv_expo_time); + std::string cur_propa = "cur_gt " + std::to_string(error_gt); + std::string cur_optimize = "cur_est " + std::to_string(error_est); + + cv::putText(ref_cur_combine_temp, ref_est, cv::Point2f(ref_ftr->px_[0] + img_cur.cols - 40, ref_ftr->px_[1] + 40), cv::FONT_HERSHEY_COMPLEX, 0.4, + cv::Scalar(0, 255, 0), 1, 8, 0); + + cv::putText(ref_cur_combine_temp, cur_est, cv::Point2f(pc[0] - 40, pc[1] + 40), cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0, 255, 0), 1, 8, 0); + cv::putText(ref_cur_combine_temp, cur_propa, cv::Point2f(pc[0] - 40, pc[1] + 60), cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0, 0, 255), 1, 8, + 0); + cv::putText(ref_cur_combine_temp, cur_optimize, cv::Point2f(pc[0] - 40, pc[1] + 80), cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0, 255, 0), 1, 8, + 0); + + cv::rectangle(ref_cur_combine_temp, cv::Point2f(ref_ftr->px_[0] + img_cur.cols - radius, ref_ftr->px_[1] - radius), + cv::Point2f(ref_ftr->px_[0] + img_cur.cols + radius, ref_ftr->px_[1] + radius), cv::Scalar(0, 0, 255), 1); + cv::rectangle(ref_cur_combine_temp, cv::Point2f(pc[0] - radius, pc[1] - radius), cv::Point2f(pc[0] + radius, pc[1] + radius), + cv::Scalar(0, 255, 0), 1); + cv::rectangle(ref_cur_combine_temp, cv::Point2f(pc_prior[0] - radius, pc_prior[1] - radius), + cv::Point2f(pc_prior[0] + radius, pc_prior[1] + radius), cv::Scalar(255, 255, 255), 1); + cv::circle(ref_cur_combine_temp, cv::Point2f(ref_ftr->px_[0] + img_cur.cols, ref_ftr->px_[1]), 1, cv::Scalar(0, 0, 255), -1, 8); + cv::circle(ref_cur_combine_temp, cv::Point2f(pc[0], pc[1]), 1, cv::Scalar(0, 255, 0), -1, 8); + cv::circle(ref_cur_combine_temp, cv::Point2f(pc_prior[0], pc_prior[1]), 1, cv::Scalar(255, 255, 255), -1, 8); + cv::imwrite(dir + std::to_string(new_frame_->id_) + "_" + std::to_string(ref_ftr->id_) + "_" + std::to_string(num) + ".png", + ref_cur_combine_temp); + + std::vector> pixel_warp_matrix; + + for (int y = 0; y < patch_size; ++y) + { + vector pixel_warp_vec; + for (int x = 0; x < patch_size; ++x) //, ++patch_ptr) + { + Vector2f px_patch(x - patch_size / 2, y - patch_size / 2); + px_patch *= (1 << search_level); + const Vector2f px_ref(px_patch + ref_ftr->px_.cast()); + uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); + + const Vector2f px(A_cur_ref.cast() * px_patch + pc.cast()); + if (px[0] < 0 || px[1] < 0 || px[0] >= img_cur.cols - 1 || px[1] >= img_cur.rows - 1) + continue; + else + { + pixel_member pixel_warp; + pixel_warp.pixel_pos << px[0], px[1]; + pixel_warp.pixel_value = pixel_value; + pixel_warp_vec.push_back(pixel_warp); + } + } + pixel_warp_matrix.push_back(pixel_warp_vec); + } + + float x_min = 1000; + float y_min = 1000; + float x_max = 0; + float y_max = 0; + + for (int i = 0; i < pixel_warp_matrix.size(); i++) + { + vector pixel_warp_row = pixel_warp_matrix[i]; + for (int j = 0; j < pixel_warp_row.size(); j++) + { + float x_temp = pixel_warp_row[j].pixel_pos[0]; + float y_temp = pixel_warp_row[j].pixel_pos[1]; + if (x_temp < x_min) x_min = x_temp; + if (y_temp < y_min) y_min = y_temp; + if (x_temp > x_max) x_max = x_temp; + if (y_temp > y_max) y_max = y_temp; + } + } + int x_min_i = floor(x_min); + int y_min_i = floor(y_min); + int x_max_i = ceil(x_max); + int y_max_i = ceil(y_max); + Matrix2f A_cur_ref_Inv = A_cur_ref.inverse().cast(); + for (int i = x_min_i; i < x_max_i; i++) + { + for (int j = y_min_i; j < y_max_i; j++) + { + Eigen::Vector2f pc_temp(i, j); + Vector2f px_patch = A_cur_ref_Inv * (pc_temp - pc.cast()); + if (px_patch[0] > (-patch_size / 2 * (1 << search_level)) && px_patch[0] < (patch_size / 2 * (1 << search_level)) && + px_patch[1] > (-patch_size / 2 * (1 << search_level)) && px_patch[1] < (patch_size / 2 * (1 << search_level))) + { + const Vector2f px_ref(px_patch + ref_ftr->px_.cast()); + uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); + it_normal[width * j + i] = pixel_value; + } + } + } + } + } + for (int i = 0; i < visual_submap->voxel_points.size(); i++) + { + VisualPoint *pt = visual_submap->voxel_points[i]; + + if (!pt->is_normal_initialized_) continue; + + Feature *ref_ftr; + V2D pc(new_frame_->w2c(pt->pos_)); + ref_ftr = pt->ref_patch; + + Matrix2d A_cur_ref; + getWarpMatrixAffine(*cam, ref_ftr->px_, ref_ftr->f_, (ref_ftr->pos() - pt->pos_).norm(), new_frame_->T_f_w_ * ref_ftr->T_f_w_.inverse(), 0, 0, + patch_size_half, A_cur_ref); + int search_level = getBestSearchLevel(A_cur_ref.inverse(), 2); + double D = A_cur_ref.determinant(); + if (D > 3) continue; + + cv::Mat img_cur = new_frame_->img_; + cv::Mat img_ref = ref_ftr->img_; + for (int y = 0; y < patch_size; ++y) + { + for (int x = 0; x < patch_size; ++x) //, ++patch_ptr) + { + Vector2f px_patch(x - patch_size / 2, y - patch_size / 2); + px_patch *= (1 << search_level); + const Vector2f px_ref(px_patch + ref_ftr->px_.cast()); + uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]); + + const Vector2f px(A_cur_ref.cast() * px_patch + pc.cast()); + if (px[0] < 0 || px[1] < 0 || px[0] >= img_cur.cols - 1 || px[1] >= img_cur.rows - 1) + continue; + else + { + int col = int(px[0]); + int row = int(px[1]); + it[width * row + col] = pixel_value; + } + } + } + } + cv::Mat ref_cur_combine; + cv::Mat ref_cur_combine_normal; + cv::Mat ref_cur_combine_error; + + cv::hconcat(result, new_frame_->img_, ref_cur_combine); + cv::hconcat(result_normal, new_frame_->img_, ref_cur_combine_normal); + + cv::cvtColor(ref_cur_combine, ref_cur_combine, CV_GRAY2BGR); + cv::cvtColor(ref_cur_combine_normal, ref_cur_combine_normal, CV_GRAY2BGR); + cv::absdiff(img_photometric_error, result_normal, img_photometric_error); + cv::hconcat(img_photometric_error, new_frame_->img_, ref_cur_combine_error); + + cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_" + ".png", ref_cur_combine); + cv::imwrite(dir + std::to_string(new_frame_->id_) + +"_0_" + + "photometric" + ".png", + ref_cur_combine_error); + cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_" + "normal" + ".png", ref_cur_combine_normal); +} + +void VIOManager::precomputeReferencePatches(int level) +{ + double t1 = omp_get_wtime(); + if (total_points == 0) return; + MD(1, 2) Jimg; + MD(2, 3) Jdpi; + MD(1, 3) Jdphi, Jdp, JdR, Jdt; + + const int H_DIM = total_points * patch_size_total; + + H_sub_inv.resize(H_DIM, 6); + H_sub_inv.setZero(); + M3D p_w_hat; + + for (int i = 0; i < total_points; i++) + { + const int scale = (1 << level); + + VisualPoint *pt = visual_submap->voxel_points[i]; + cv::Mat img = pt->ref_patch->img_; + + if (pt == nullptr) continue; + + double depth((pt->pos_ - pt->ref_patch->pos()).norm()); + V3D pf = pt->ref_patch->f_ * depth; + V2D pc = pt->ref_patch->px_; + M3D R_ref_w = pt->ref_patch->T_f_w_.rotationMatrix(); + + computeProjectionJacobian(pf, Jdpi); + p_w_hat << SKEW_SYM_MATRX(pt->pos_); + + const float u_ref = pc[0]; + const float v_ref = pc[1]; + const int u_ref_i = floorf(pc[0] / scale) * scale; + const int v_ref_i = floorf(pc[1] / scale) * scale; + const float subpix_u_ref = (u_ref - u_ref_i) / scale; + const float subpix_v_ref = (v_ref - v_ref_i) / scale; + const float w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref); + const float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref); + const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; + const float w_ref_br = subpix_u_ref * subpix_v_ref; + + for (int x = 0; x < patch_size; x++) + { + uint8_t *img_ptr = (uint8_t *)img.data + (v_ref_i + x * scale - patch_size_half * scale) * width + u_ref_i - patch_size_half * scale; + for (int y = 0; y < patch_size; ++y, img_ptr += scale) + { + float du = + 0.5f * + ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + w_ref_bl * img_ptr[scale * width + scale] + + w_ref_br * img_ptr[scale * width + scale * 2]) - + (w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + w_ref_bl * img_ptr[scale * width - scale] + w_ref_br * img_ptr[scale * width])); + float dv = + 0.5f * + ((w_ref_tl * img_ptr[scale * width] + w_ref_tr * img_ptr[scale + scale * width] + w_ref_bl * img_ptr[width * scale * 2] + + w_ref_br * img_ptr[width * scale * 2 + scale]) - + (w_ref_tl * img_ptr[-scale * width] + w_ref_tr * img_ptr[-scale * width + scale] + w_ref_bl * img_ptr[0] + w_ref_br * img_ptr[scale])); + + Jimg << du, dv; + Jimg = Jimg * (1.0 / scale); + + JdR = Jimg * Jdpi * R_ref_w * p_w_hat; + Jdt = -Jimg * Jdpi * R_ref_w; + + H_sub_inv.block<1, 6>(i * patch_size_total + x * patch_size + y, 0) << JdR, Jdt; + } + } + } + has_ref_patch_cache = true; +} + +void VIOManager::updateStateInverse(cv::Mat img, int level) +{ + if (total_points == 0) return; + StatesGroup old_state = (*state); + V2D pc; + MD(1, 2) Jimg; + MD(2, 3) Jdpi; + MD(1, 3) Jdphi, Jdp, JdR, Jdt; + VectorXd z; + MatrixXd H_sub; + bool EKF_end = false; + float last_error = std::numeric_limits::max(); + compute_jacobian_time = update_ekf_time = 0.0; + M3D P_wi_hat; + bool z_init = true; + const int H_DIM = total_points * patch_size_total; + + z.resize(H_DIM); + z.setZero(); + + H_sub.resize(H_DIM, 6); + H_sub.setZero(); + + for (int iteration = 0; iteration < max_iterations; iteration++) + { + double t1 = omp_get_wtime(); + double count_outlier = 0; + if (has_ref_patch_cache == false) precomputeReferencePatches(level); + int n_meas = 0; + float error = 0.0; + M3D Rwi(state->rot_end); + V3D Pwi(state->pos_end); + P_wi_hat << SKEW_SYM_MATRX(Pwi); + Rcw = Rci * Rwi.transpose(); + Pcw = -Rci * Rwi.transpose() * Pwi + Pci; + + M3D p_hat; + + for (int i = 0; i < total_points; i++) + { + float patch_error = 0.0; + + const int scale = (1 << level); + + VisualPoint *pt = visual_submap->voxel_points[i]; + + if (pt == nullptr) continue; + + V3D pf = Rcw * pt->pos_ + Pcw; + pc = cam->world2cam(pf); + + const float u_ref = pc[0]; + const float v_ref = pc[1]; + const int u_ref_i = floorf(pc[0] / scale) * scale; + const int v_ref_i = floorf(pc[1] / scale) * scale; + const float subpix_u_ref = (u_ref - u_ref_i) / scale; + const float subpix_v_ref = (v_ref - v_ref_i) / scale; + const float w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref); + const float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref); + const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; + const float w_ref_br = subpix_u_ref * subpix_v_ref; + + vector P = visual_submap->warp_patch[i]; + for (int x = 0; x < patch_size; x++) + { + uint8_t *img_ptr = (uint8_t *)img.data + (v_ref_i + x * scale - patch_size_half * scale) * width + u_ref_i - patch_size_half * scale; + for (int y = 0; y < patch_size; ++y, img_ptr += scale) + { + double res = w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * width] + + w_ref_br * img_ptr[scale * width + scale] - P[patch_size_total * level + x * patch_size + y]; + z(i * patch_size_total + x * patch_size + y) = res; + patch_error += res * res; + MD(1, 3) J_dR = H_sub_inv.block<1, 3>(i * patch_size_total + x * patch_size + y, 0); + MD(1, 3) J_dt = H_sub_inv.block<1, 3>(i * patch_size_total + x * patch_size + y, 3); + JdR = J_dR * Rwi + J_dt * P_wi_hat * Rwi; + Jdt = J_dt * Rwi; + H_sub.block<1, 6>(i * patch_size_total + x * patch_size + y, 0) << JdR, Jdt; + n_meas++; + } + } + visual_submap->errors[i] = patch_error; + error += patch_error; + } + + error = error / n_meas; + + compute_jacobian_time += omp_get_wtime() - t1; + + double t3 = omp_get_wtime(); + + if (error <= last_error) + { + old_state = (*state); + last_error = error; + + auto &&H_sub_T = H_sub.transpose(); + H_T_H.setZero(); + G.setZero(); + H_T_H.block<6, 6>(0, 0) = H_sub_T * H_sub; + MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H + (state->cov / img_point_cov).inverse()).inverse(); + auto &&HTz = H_sub_T * z; + auto vec = (*state_propagat) - (*state); + G.block(0, 0) = K_1.block(0, 0) * H_T_H.block<6, 6>(0, 0); + auto solution = -K_1.block(0, 0) * HTz + vec - G.block(0, 0) * vec.block<6, 1>(0, 0); + (*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.3f < 0.001f) && (t_add.norm() * 100.0f < 0.001f)) { EKF_end = true; } + } + else + { + (*state) = old_state; + EKF_end = true; + } + + update_ekf_time += omp_get_wtime() - t3; + + if (iteration == max_iterations || EKF_end) break; + } +} + +void VIOManager::updateState(cv::Mat img, int level) +{ + if (total_points == 0) return; + StatesGroup old_state = (*state); + + VectorXd z; + MatrixXd H_sub; + bool EKF_end = false; + float last_error = std::numeric_limits::max(); + + const int H_DIM = total_points * patch_size_total; + z.resize(H_DIM); + z.setZero(); + H_sub.resize(H_DIM, 7); + H_sub.setZero(); + + for (int iteration = 0; iteration < max_iterations; iteration++) + { + double t1 = omp_get_wtime(); + + M3D Rwi(state->rot_end); + V3D Pwi(state->pos_end); + Rcw = Rci * Rwi.transpose(); + Pcw = -Rci * Rwi.transpose() * Pwi + Pci; + Jdp_dt = Rci * Rwi.transpose(); + + float error = 0.0; + int n_meas = 0; + // int max_threads = omp_get_max_threads(); + // int desired_threads = std::min(max_threads, total_points); + // omp_set_num_threads(desired_threads); + + #ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); + #pragma omp parallel for reduction(+:error, n_meas) + #endif + for (int i = 0; i < total_points; i++) + { + // printf("thread is %d, i=%d, i address is %p\n", omp_get_thread_num(), i, &i); + MD(1, 2) Jimg; + MD(2, 3) Jdpi; + MD(1, 3) Jdphi, Jdp, JdR, Jdt; + + float patch_error = 0.0; + int search_level = visual_submap->search_levels[i]; + int pyramid_level = level + search_level; + int scale = (1 << pyramid_level); + float inv_scale = 1.0f / scale; + + VisualPoint *pt = visual_submap->voxel_points[i]; + + if (pt == nullptr) continue; + + V3D pf = Rcw * pt->pos_ + Pcw; + V2D pc = cam->world2cam(pf); + + computeProjectionJacobian(pf, Jdpi); + M3D p_hat; + p_hat << SKEW_SYM_MATRX(pf); + + float u_ref = pc[0]; + float v_ref = pc[1]; + int u_ref_i = floorf(pc[0] / scale) * scale; + int v_ref_i = floorf(pc[1] / scale) * scale; + float subpix_u_ref = (u_ref - u_ref_i) / scale; + float subpix_v_ref = (v_ref - v_ref_i) / scale; + float w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref); + float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref); + float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; + float w_ref_br = subpix_u_ref * subpix_v_ref; + + vector P = visual_submap->warp_patch[i]; + double inv_ref_expo = visual_submap->inv_expo_list[i]; + // ROS_ERROR("inv_ref_expo: %.3lf, state->inv_expo_time: %.3lf\n", inv_ref_expo, state->inv_expo_time); + + for (int x = 0; x < patch_size; x++) + { + uint8_t *img_ptr = (uint8_t *)img.data + (v_ref_i + x * scale - patch_size_half * scale) * width + u_ref_i - patch_size_half * scale; + for (int y = 0; y < patch_size; ++y, img_ptr += scale) + { + float du = + 0.5f * + ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + w_ref_bl * img_ptr[scale * width + scale] + + w_ref_br * img_ptr[scale * width + scale * 2]) - + (w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + w_ref_bl * img_ptr[scale * width - scale] + w_ref_br * img_ptr[scale * width])); + float dv = + 0.5f * + ((w_ref_tl * img_ptr[scale * width] + w_ref_tr * img_ptr[scale + scale * width] + w_ref_bl * img_ptr[width * scale * 2] + + w_ref_br * img_ptr[width * scale * 2 + scale]) - + (w_ref_tl * img_ptr[-scale * width] + w_ref_tr * img_ptr[-scale * width + scale] + w_ref_bl * img_ptr[0] + w_ref_br * img_ptr[scale])); + + Jimg << du, dv; + Jimg = Jimg * state->inv_expo_time; + Jimg = Jimg * inv_scale; + Jdphi = Jimg * Jdpi * p_hat; + Jdp = -Jimg * Jdpi; + JdR = Jdphi * Jdphi_dR + Jdp * Jdp_dR; + Jdt = Jdp * Jdp_dt; + + double cur_value = + w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * width] + w_ref_br * img_ptr[scale * width + scale]; + double res = state->inv_expo_time * cur_value - inv_ref_expo * P[patch_size_total * level + x * patch_size + y]; + + z(i * patch_size_total + x * patch_size + y) = res; + + patch_error += res * res; + n_meas += 1; + + if (exposure_estimate_en) { H_sub.block<1, 7>(i * patch_size_total + x * patch_size + y, 0) << JdR, Jdt, cur_value; } + else { H_sub.block<1, 6>(i * patch_size_total + x * patch_size + y, 0) << JdR, Jdt; } + } + } + visual_submap->errors[i] = patch_error; + error += patch_error; + } + + error = error / n_meas; + + compute_jacobian_time += omp_get_wtime() - t1; + + // printf("\nPYRAMID LEVEL %i\n---------------\n", level); + // std::cout << "It. " << iteration + // << "\t last_error = " << last_error + // << "\t new_error = " << error + // << std::endl; + + double t3 = omp_get_wtime(); + + if (error <= last_error) + { + old_state = (*state); + last_error = error; + + // K = (H.transpose() / img_point_cov * H + state->cov.inverse()).inverse() * H.transpose() / img_point_cov; auto + // vec = (*state_propagat) - (*state); G = K*H; + // (*state) += (-K*z + vec - G*vec); + + auto &&H_sub_T = H_sub.transpose(); + H_T_H.setZero(); + G.setZero(); + H_T_H.block<7, 7>(0, 0) = H_sub_T * H_sub; + MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H + (state->cov / img_point_cov).inverse()).inverse(); + auto &&HTz = H_sub_T * z; + // K = K_1.block(0,0) * H_sub_T; + auto vec = (*state_propagat) - (*state); + G.block(0, 0) = K_1.block(0, 0) * H_T_H.block<7, 7>(0, 0); + MD(DIM_STATE, 1) + solution = -K_1.block(0, 0) * HTz + vec - G.block(0, 0) * vec.block<7, 1>(0, 0); + + (*state) += solution; + auto &&rot_add = solution.block<3, 1>(0, 0); + auto &&t_add = solution.block<3, 1>(3, 0); + + auto &&expo_add = solution.block<1, 1>(6, 0); + // if ((rot_add.norm() * 57.3f < 0.001f) && (t_add.norm() * 100.0f < 0.001f) && (expo_add.norm() < 0.001f)) EKF_end = true; + if ((rot_add.norm() * 57.3f < 0.001f) && (t_add.norm() * 100.0f < 0.001f)) EKF_end = true; + } + else + { + (*state) = old_state; + EKF_end = true; + } + + update_ekf_time += omp_get_wtime() - t3; + + if (iteration == max_iterations || EKF_end) break; + } + // if (state->inv_expo_time < 0.0) {ROS_ERROR("reset expo time!!!!!!!!!!\n"); state->inv_expo_time = 0.0;} +} + +void VIOManager::updateFrameState(StatesGroup state) +{ + M3D Rwi(state.rot_end); + V3D Pwi(state.pos_end); + Rcw = Rci * Rwi.transpose(); + Pcw = -Rci * Rwi.transpose() * Pwi + Pci; + new_frame_->T_f_w_ = SE3(Eigen::Quaterniond(Rcw).normalized().toRotationMatrix(), Pcw); // avoid R is not orthogonal +} + +void VIOManager::plotTrackedPoints() +{ + int total_points = visual_submap->voxel_points.size(); + if (total_points == 0) return; + // int inlier_count = 0; + // for (int i = 0; i < img_cp.rows / grid_size; i++) + // { + // cv::line(img_cp, cv::Poaint2f(0, grid_size * i), cv::Point2f(img_cp.cols, grid_size * i), cv::Scalar(255, 255, 255), 1, CV_AA); + // } + // for (int i = 0; i < img_cp.cols / grid_size; i++) + // { + // cv::line(img_cp, cv::Point2f(grid_size * i, 0), cv::Point2f(grid_size * i, img_cp.rows), cv::Scalar(255, 255, 255), 1, CV_AA); + // } + // for (int i = 0; i < img_cp.rows / grid_size; i++) + // { + // cv::line(img_cp, cv::Point2f(0, grid_size * i), cv::Point2f(img_cp.cols, grid_size * i), cv::Scalar(255, 255, 255), 1, CV_AA); + // } + // for (int i = 0; i < img_cp.cols / grid_size; i++) + // { + // cv::line(img_cp, cv::Point2f(grid_size * i, 0), cv::Point2f(grid_size * i, img_cp.rows), cv::Scalar(255, 255, 255), 1, CV_AA); + // } + for (int i = 0; i < total_points; i++) + { + VisualPoint *pt = visual_submap->voxel_points[i]; + V2D pc(new_frame_->w2c(pt->pos_)); + + if (visual_submap->errors[i] <= visual_submap->propa_errors[i]) + { + // inlier_count++; + cv::circle(img_cp, cv::Point2f(pc[0], pc[1]), 7, cv::Scalar(0, 255, 0), -1, 8); // Green Sparse Align tracked + } + else + { + cv::circle(img_cp, cv::Point2f(pc[0], pc[1]), 7, cv::Scalar(255, 0, 0), -1, 8); // Blue Sparse Align tracked + } + } + // std::string text = std::to_string(inlier_count) + " " + std::to_string(total_points); + // cv::Point2f origin; + // origin.x = img_cp.cols - 110; + // origin.y = 20; + // cv::putText(img_cp, text, origin, cv::FONT_HERSHEY_COMPLEX, 0.7, cv::Scalar(0, 255, 0), 2, 8, 0); +} + +V3F VIOManager::getInterpolatedPixel(cv::Mat img, V2D pc) +{ + const float u_ref = pc[0]; + const float v_ref = pc[1]; + const int u_ref_i = floorf(pc[0]); + const int v_ref_i = floorf(pc[1]); + const float subpix_u_ref = (u_ref - u_ref_i); + const float subpix_v_ref = (v_ref - v_ref_i); + const float w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref); + const float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref); + const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref; + const float w_ref_br = subpix_u_ref * subpix_v_ref; + uint8_t *img_ptr = (uint8_t *)img.data + ((v_ref_i)*width + (u_ref_i)) * 3; + float B = w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[0 + 3] + w_ref_bl * img_ptr[width * 3] + w_ref_br * img_ptr[width * 3 + 0 + 3]; + float G = w_ref_tl * img_ptr[1] + w_ref_tr * img_ptr[1 + 3] + w_ref_bl * img_ptr[1 + width * 3] + w_ref_br * img_ptr[width * 3 + 1 + 3]; + float R = w_ref_tl * img_ptr[2] + w_ref_tr * img_ptr[2 + 3] + w_ref_bl * img_ptr[2 + width * 3] + w_ref_br * img_ptr[width * 3 + 2 + 3]; + V3F pixel(B, G, R); + return pixel; +} + +void VIOManager::dumpDataForColmap() +{ + static int cnt = 1; + std::ostringstream ss; + ss << std::setw(5) << std::setfill('0') << cnt; + std::string cnt_str = ss.str(); + std::string image_path = std::string(ROOT_DIR) + "Log/Colmap/images/" + cnt_str + ".png"; + + cv::Mat img_rgb_undistort; + pinhole_cam->undistortImage(img_rgb, img_rgb_undistort); + cv::imwrite(image_path, img_rgb_undistort); + + Eigen::Quaterniond q(new_frame_->T_f_w_.rotationMatrix()); + Eigen::Vector3d t = new_frame_->T_f_w_.translation(); + fout_colmap << cnt << " " + << std::fixed << std::setprecision(6) // 保证浮点数精度为6位 + << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " " + << t.x() << " " << t.y() << " " << t.z() << " " + << 1 << " " // CAMERA_ID (假设相机ID为1) + << cnt_str << ".png" << std::endl; + fout_colmap << "0.0 0.0 -1" << std::endl; + cnt++; +} + +void VIOManager::processFrame(cv::Mat &img, vector &pg, const unordered_map &feat_map, double img_time) +{ + if (width != img.cols || height != img.rows) + { + if (img.empty()) printf("[ VIO ] Empty Image!\n"); + cv::resize(img, img, cv::Size(img.cols * image_resize_factor, img.rows * image_resize_factor), 0, 0, CV_INTER_LINEAR); + } + img_rgb = img.clone(); + img_cp = img.clone(); + // img_test = img.clone(); + + if (img.channels() == 3) cv::cvtColor(img, img, CV_BGR2GRAY); + + new_frame_.reset(new Frame(cam, img)); + updateFrameState(*state); + + resetGrid(); + + double t1 = omp_get_wtime(); + + retrieveFromVisualSparseMap(img, pg, feat_map); + + double t2 = omp_get_wtime(); + + computeJacobianAndUpdateEKF(img); + + double t3 = omp_get_wtime(); + + generateVisualMapPoints(img, pg); + + double t4 = omp_get_wtime(); + + plotTrackedPoints(); + + if (plot_flag) projectPatchFromRefToCur(feat_map); + + double t5 = omp_get_wtime(); + + updateVisualMapPoints(img); + + double t6 = omp_get_wtime(); + + updateReferencePatch(feat_map); + + double t7 = omp_get_wtime(); + + if(colmap_output_en) dumpDataForColmap(); + + frame_count++; + ave_total = ave_total * (frame_count - 1) / frame_count + (t7 - t1 - (t5 - t4)) / frame_count; + + // printf("[ VIO ] feat_map.size(): %zu\n", feat_map.size()); + // printf("\033[1;32m[ VIO time ]: current frame: retrieveFromVisualSparseMap time: %.6lf secs.\033[0m\n", t2 - t1); + // printf("\033[1;32m[ VIO time ]: current frame: computeJacobianAndUpdateEKF time: %.6lf secs, comp H: %.6lf secs, ekf: %.6lf secs.\033[0m\n", t3 - t2, computeH, ekf_time); + // printf("\033[1;32m[ VIO time ]: current frame: generateVisualMapPoints time: %.6lf secs.\033[0m\n", t4 - t3); + // printf("\033[1;32m[ VIO time ]: current frame: updateVisualMapPoints time: %.6lf secs.\033[0m\n", t6 - t5); + // printf("\033[1;32m[ VIO time ]: current frame: updateReferencePatch time: %.6lf secs.\033[0m\n", t7 - t6); + // printf("\033[1;32m[ VIO time ]: current total time: %.6lf, average total time: %.6lf secs.\033[0m\n", t7 - t1 - (t5 - t4), ave_total); + + // ave_build_residual_time = ave_build_residual_time * (frame_count - 1) / frame_count + (t2 - t1) / frame_count; + // ave_ekf_time = ave_ekf_time * (frame_count - 1) / frame_count + (t3 - t2) / frame_count; + + // cout << BLUE << "ave_build_residual_time: " << ave_build_residual_time << RESET << endl; + // cout << BLUE << "ave_ekf_time: " << ave_ekf_time << RESET << endl; + + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| VIO Time |\033[0m\n"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| %-29s | %-27zu |\033[0m\n", "Sparse Map Size", feat_map.size()); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;34m| %-29s | %-27s |\033[0m\n", "Algorithm Stage", "Time (secs)"); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "retrieveFromVisualSparseMap", t2 - t1); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "computeJacobianAndUpdateEKF", t3 - t2); + printf("\033[1;32m| %-27s | %-27lf |\033[0m\n", "-> computeJacobian", compute_jacobian_time); + printf("\033[1;32m| %-27s | %-27lf |\033[0m\n", "-> updateEKF", update_ekf_time); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "generateVisualMapPoints", t4 - t3); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "updateVisualMapPoints", t6 - t5); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "updateReferencePatch", t7 - t6); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "Current Total Time", t7 - t1 - (t5 - t4)); + printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "Average Total Time", ave_total); + printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n"); + + // std::string text = std::to_string(int(1 / (t7 - t1 - (t5 - t4)))) + " HZ"; + // cv::Point2f origin; + // origin.x = 20; + // origin.y = 20; + // cv::putText(img_cp, text, origin, cv::FONT_HERSHEY_COMPLEX, 0.6, cv::Scalar(255, 255, 255), 1, 8, 0); + // cv::imwrite("/home/chunran/Desktop/raycasting/" + std::to_string(new_frame_->id_) + ".png", img_cp); +} \ No newline at end of file diff --git a/src/FAST-LIVO2/src/visual_point.cpp b/src/FAST-LIVO2/src/visual_point.cpp new file mode 100644 index 0000000..1b74650 --- /dev/null +++ b/src/FAST-LIVO2/src/visual_point.cpp @@ -0,0 +1,127 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 +#include + +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::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; + } + } +} \ No newline at end of file diff --git a/src/FAST-LIVO2/src/voxel_map.cpp b/src/FAST-LIVO2/src/voxel_map.cpp new file mode 100644 index 0000000..731222c --- /dev/null +++ b/src/FAST-LIVO2/src/voxel_map.cpp @@ -0,0 +1,987 @@ +/* +This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry. + +Developer: Chunran Zheng + +For commercial use, please contact me at or +Prof. Fu Zhang at . + +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 N; + N << base_vector1(0), base_vector2(0), base_vector1(1), base_vector2(1), base_vector1(2), base_vector2(2); + Eigen::Matrix 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("publish.pub_plane_en", false); + node->declare_parameter("lio.max_layer", 1); + node->declare_parameter("lio.voxel_size", 0.5); + node->declare_parameter("lio.min_eigen_value", 0.01); + node->declare_parameter("lio.sigma_num", 3); + node->declare_parameter("lio.beam_err", 0.02); + node->declare_parameter("lio.dept_err", 0.05); + + // Declaration of parameter of type std::vector won't build, https://github.com/ros2/rclcpp/issues/1585 + node->declare_parameter>("lio.layer_init_num", std::vector{5,5,5,5,5}); + node->declare_parameter("lio.max_points_num", 50); + node->declare_parameter("lio.min_iterations", 5); + node->declare_parameter("local_map.map_sliding_en", false); + node->declare_parameter("local_map.half_map_size", 100); + node->declare_parameter("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 &points, VoxelPlane *plane) +{ + plane->plane_var_ = Eigen::Matrix::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 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 J; + Eigen::Matrix3d F; + for (int m = 0; m < 3; m++) + { + if (m != (int)evalsMin) + { + Eigen::Matrix 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 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().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().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().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().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().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::Ptr world_lidar(new pcl::PointCloud); + 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 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: "<(0, 0) = Hsub_T_R_inv * Hsub; + // EigenSolver> es(H_T_H.block<6,6>(0,0)); + MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H.block(0, 0) + state_.cov.block(0, 0).inverse()).inverse(); + G.block(0, 0) = K_1.block(0, 0) * H_T_H.block<6, 6>(0, 0); + auto vec = state_propagat - state_; + VD(DIM_STATE) + solution = K_1.block(0, 0) * HTz + vec.block(0, 0) - G.block(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(0, 0) = + (I_STATE.block(0, 0) - G.block(0, 0)) * state_.cov.block(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::Ptr &trans_cloud) +{ + pcl::PointCloud().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 layer_init_num = convertToIntVectorSafe(config_setting_.layer_init_num_); + + std::vector 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: "< &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 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 &pv_list, std::vector &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 all_ptpl_list(pv_list.size()); + std::vector useful_ptpl(pv_list.size()); + std::vector 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 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 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 &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<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< + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/FAST-LIVO2/urdf/mid360_robot.urdf b/src/FAST-LIVO2/urdf/mid360_robot.urdf new file mode 100644 index 0000000..2ac23fe --- /dev/null +++ b/src/FAST-LIVO2/urdf/mid360_robot.urdf @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/FAST_LIO b/src/FAST_LIO new file mode 120000 index 0000000..35d558a --- /dev/null +++ b/src/FAST_LIO @@ -0,0 +1 @@ +/home/yoo/FAST_LIO \ No newline at end of file diff --git a/src/__pycache__/forey_nav2.launch.cpython-310.pyc b/src/__pycache__/forey_nav2.launch.cpython-310.pyc new file mode 100644 index 0000000..74179b9 Binary files /dev/null and b/src/__pycache__/forey_nav2.launch.cpython-310.pyc differ diff --git a/src/fori_nav2.launch.py b/src/fori_nav2.launch.py new file mode 100644 index 0000000..5162bd8 --- /dev/null +++ b/src/fori_nav2.launch.py @@ -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 + ]) diff --git a/src/fori_nav2_params.yaml b/src/fori_nav2_params.yaml new file mode 100644 index 0000000..bb2bd7e --- /dev/null +++ b/src/fori_nav2_params.yaml @@ -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 diff --git a/src/fori_serial_bridge/arduino/fori_bldc_control/fori_bldc_control.ino b/src/fori_serial_bridge/arduino/fori_bldc_control/fori_bldc_control.ino new file mode 100644 index 0000000..40baf6a --- /dev/null +++ b/src/fori_serial_bridge/arduino/fori_bldc_control/fori_bldc_control.ino @@ -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); + } +} diff --git a/src/fori_serial_bridge/fori_serial_bridge/__init__.py b/src/fori_serial_bridge/fori_serial_bridge/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/fori_serial_bridge/fori_serial_bridge/__pycache__/__init__.cpython-310.pyc b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..a19a2f2 Binary files /dev/null and b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/__init__.cpython-310.pyc differ diff --git a/src/fori_serial_bridge/fori_serial_bridge/__pycache__/parking_controller_node.cpython-310.pyc b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/parking_controller_node.cpython-310.pyc new file mode 100644 index 0000000..3be71fd Binary files /dev/null and b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/parking_controller_node.cpython-310.pyc differ diff --git a/src/fori_serial_bridge/fori_serial_bridge/__pycache__/serial_bridge_node.cpython-310.pyc b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/serial_bridge_node.cpython-310.pyc new file mode 100644 index 0000000..2015449 Binary files /dev/null and b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/serial_bridge_node.cpython-310.pyc differ diff --git a/src/fori_serial_bridge/fori_serial_bridge/__pycache__/ui_server_node.cpython-310.pyc b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/ui_server_node.cpython-310.pyc new file mode 100644 index 0000000..be3c52b Binary files /dev/null and b/src/fori_serial_bridge/fori_serial_bridge/__pycache__/ui_server_node.cpython-310.pyc differ diff --git a/src/fori_serial_bridge/fori_serial_bridge/parking_controller_node.py b/src/fori_serial_bridge/fori_serial_bridge/parking_controller_node.py new file mode 100755 index 0000000..89a5e59 --- /dev/null +++ b/src/fori_serial_bridge/fori_serial_bridge/parking_controller_node.py @@ -0,0 +1,1019 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from rclpy.qos import qos_profile_sensor_data, QoSProfile, DurabilityPolicy +from std_msgs.msg import String +from geometry_msgs.msg import Twist, PoseStamped, PoseWithCovarianceStamped +from sensor_msgs.msg import LaserScan +from nav_msgs.msg import Odometry, OccupancyGrid +from nav2_msgs.action import NavigateToPose +from action_msgs.msg import GoalStatus +import math + +class ParkingControllerNode(Node): + def __init__(self): + super().__init__('parking_controller_node') + + # --- [Parameters] --- + # Map waypoint to navigate to before starting ArUco parking + self.declare_parameter('parking_waypoint_x', 0.0) # Edit this to match map coordinate + self.declare_parameter('parking_waypoint_y', 0.0) # Edit this to match map coordinate + self.declare_parameter('parking_waypoint_yaw', 0.0) # Edit this to match map coordinate + self.declare_parameter('parking_frame', 'map') + + self.declare_parameter('target_distance', 0.5) # Stop 0.5m in front of marker + self.declare_parameter('kp_linear', 0.25) + self.declare_parameter('kp_bearing', 0.8) + self.declare_parameter('kp_yaw', 0.4) + + self.waypoint_x = self.get_parameter('parking_waypoint_x').value + self.waypoint_y = self.get_parameter('parking_waypoint_y').value + self.waypoint_yaw = self.get_parameter('parking_waypoint_yaw').value + self.waypoint_frame = self.get_parameter('parking_frame').value + self.waypoint_active = False # Tracks if user has set a valid parking waypoint + + self.target_dist = self.get_parameter('target_distance').value + self.kp_linear = self.get_parameter('kp_linear').value + self.kp_bearing = self.get_parameter('kp_bearing').value + self.kp_yaw = self.get_parameter('kp_yaw').value + + # Declare patrol waypoints as parameter (flat list of x, y, yaw) + # Default patrol coordinates following the 100% obstacle-free map circuit + default_patrol = [ + 1.10, -1.35, 2.35, + -2.20, 2.00, 3.14, + -3.00, 0.50, -1.57, + -1.80, 0.80, -0.78, + -0.50, 0.00, -0.78 + ] + self.declare_parameter('patrol_waypoints', default_patrol) + self.patrol_waypoints_raw = self.get_parameter('patrol_waypoints').value + self.patrol_waypoints = [] + for i in range(0, len(self.patrol_waypoints_raw), 3): + if i + 2 < len(self.patrol_waypoints_raw): + self.patrol_waypoints.append({ + 'x': self.patrol_waypoints_raw[i], + 'y': self.patrol_waypoints_raw[i+1], + 'yaw': self.patrol_waypoints_raw[i+2] + }) + + self.patrol_targets = [] + self.patrol_index = 0 + self.patrol_start_x = 0.0 + self.patrol_start_y = 0.0 + self.patrol_start_yaw = 0.0 + + # States: "IDLE", "NAV2_WAYPOINT", "WAITING_FOR_MARKER", "VISUAL_SERVOING", "PARKED", "PATROL_NAVIGATING", "PATROL_FINISHED" + self.state = "IDLE" + self.current_mode = "nav2" + + # Mock mode parameters and simulation variables + self.declare_parameter('mock_mode', False) + self.mock_mode = self.get_parameter('mock_mode').value + + self.sim_x = 0.0 + self.sim_y = 0.0 + self.sim_yaw = 0.0 + self.sim_marker_z = 2.5 + self.sim_marker_x = 0.2 + self.sim_marker_yaw = 0.3 + self.mock_scan_start_time = None + self.virtual_pose_pub = self.create_publisher(PoseStamped, '/aruco_marker_pose', 10) + + # Mock Nav2 goal variables + self.mock_nav2_active = False + self.mock_nav2_x = 0.0 + self.mock_nav2_y = 0.0 + self.mock_nav2_yaw = 0.0 + + # Map data for mock collision avoidance + self.map_data = None + self.map_info = None + + # Damping momentum for mock angular velocity (prevents wobble oscillations) + self.mock_prev_w = 0.0 + + # Backing up (undocking) state variables + self.backup_start_x = 0.0 + self.backup_start_y = 0.0 + + # Patrol 360-degree scan state variables + self.scan_accumulated_yaw = 0.0 + self.scan_last_yaw = 0.0 + + # Stuck detection and recovery variables + self.stuck_check_time = None + self.stuck_start_pose = None + self.is_recovering = False + self.recovery_state = None # "BACKUP", "SPIN" + self.recovery_start_time = None + self.recovery_start_x = 0.0 + self.recovery_start_y = 0.0 + self.recovery_start_yaw = 0.0 + + # Last detected marker pose + self.last_marker_pose = None + self.last_marker_time = self.get_clock().now() + self.marker_timeout = 1.0 # seconds + + # LiDAR obstacle avoidance parameters + self.obstacle_detected = False + self.declare_parameter('min_safety_dist', 0.45) + self.declare_parameter('safety_cone_angle', 15.0) + self.min_safety_dist = self.get_parameter('min_safety_dist').value + self.safety_cone_angle = self.get_parameter('safety_cone_angle').value + + # --- [ROS Publishers & Subscribers] --- + self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10) + self.mode_pub = self.create_publisher(String, '/robot_mode_status', 10) + + self.create_subscription(String, '/robot_mode', self.mode_callback, 10) + self.create_subscription(Twist, '/cmd_vel_nav2', self.nav2_cmd_vel_callback, 10) + self.create_subscription(PoseStamped, '/aruco_detector/pose', self.aruco_callback, 10) + self.create_subscription(PoseStamped, '/aruco_marker_waypoint', self.waypoint_callback, 10) + self.create_subscription(LaserScan, '/scan', self.scan_callback, qos_profile_sensor_data) + self.create_subscription(Odometry, '/odom_wheels', self.odom_callback, qos_profile_sensor_data) + self.create_subscription(PoseStamped, '/goal_pose', self.mock_nav2_goal_callback, 10) + self.create_subscription(PoseWithCovarianceStamped, '/initialpose', self.initial_pose_callback, 10) + + # ROS2 OccupancyGrid map server uses TRANSIENT_LOCAL durability. + map_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self.create_subscription(OccupancyGrid, '/map', self.map_callback, map_qos) + + # Nav2 Action Client + self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + self.goal_handle = None + + # Control loop timer (20Hz) + self.create_timer(0.05, self.control_loop) + + self.get_logger().info('==========================================') + self.get_logger().info(' FORI AGV PARKING CONTROLLER ') + self.get_logger().info(' Status: Ready (Idle mode) ') + self.get_logger().info('==========================================') + + def waypoint_callback(self, msg): + self.waypoint_x = msg.pose.position.x + self.waypoint_y = msg.pose.position.y + + # Quaternion to yaw + q = msg.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.waypoint_yaw = math.atan2(siny_cosp, cosy_cosp) + self.waypoint_frame = msg.header.frame_id + + self.waypoint_active = True + self.get_logger().info(f'Updated parking waypoint from Web UI: x={self.waypoint_x:.2f}, y={self.waypoint_y:.2f}, yaw={self.waypoint_yaw:.2f}') + + # If in parking mode and waiting for a waypoint, trigger drive now + if self.current_mode == 'parking' and self.state == "WAITING_FOR_WAYPOINT": + self.state = "NAV2_WAYPOINT" + self.send_nav2_goal() + + def odom_callback(self, msg): + self.sim_x = msg.pose.pose.position.x + self.sim_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.sim_yaw = math.atan2(siny_cosp, cosy_cosp) + + def initial_pose_callback(self, msg): + self.sim_x = msg.pose.pose.position.x + self.sim_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.sim_yaw = math.atan2(siny_cosp, cosy_cosp) + self.get_logger().info(f'[INITIALPOSE] Reset controller sim pose to: x={self.sim_x:.2f}, y={self.sim_y:.2f}, yaw={self.sim_yaw:.2f}') + + def mock_nav2_goal_callback(self, msg): + if not self.mock_mode: + return + self.mock_nav2_x = msg.pose.position.x + self.mock_nav2_y = msg.pose.position.y + q = msg.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.mock_nav2_yaw = math.atan2(siny_cosp, cosy_cosp) + self.mock_nav2_active = True + self.get_logger().info(f'[MOCK NAV2] Received target goal: x={self.mock_nav2_x:.2f}, y={self.mock_nav2_y:.2f}, yaw={self.mock_nav2_yaw:.2f}') + + def map_callback(self, msg): + self.map_data = msg.data + self.map_info = msg.info + + def scan_callback(self, msg): + # Check for obstacles within frontal cone + angle_min = msg.angle_min + angle_increment = msg.angle_increment + cone_rad = self.safety_cone_angle * math.pi / 180.0 + + # Relax safety limits during fine visual docking to allow approaching the target wall (0.5m) + safety_limit = 0.25 if self.state == "ALIGNING_AND_PARKING" else self.min_safety_dist + + obstacle_found = False + for idx, dist in enumerate(msg.ranges): + if math.isnan(dist) or math.isinf(dist) or dist <= 0.05: + continue + + # Calculate angle of current ray and normalize + angle = angle_min + idx * angle_increment + angle = math.atan2(math.sin(angle), math.cos(angle)) + + if abs(angle) < cone_rad: + if dist < safety_limit: + obstacle_found = True + break + + self.obstacle_detected = obstacle_found + + def mode_callback(self, msg): + target_mode = msg.data.lower() + + # Handle emergency stop transition immediately + if target_mode == 'stop': + self.current_mode = 'stop' + self.state = "ESTOP" + self.cancel_nav2_goal() + self.stop_robot() + self.mock_nav2_active = False + self.get_logger().warn('!!! EMERGENCY STOP DETECTED FROM WEB UI !!!') + return + + if target_mode == self.current_mode: + return + + self.get_logger().info(f'Mode transition requested: {self.current_mode} -> {target_mode}') + + # Reset mock nav2 active status + self.mock_nav2_active = False + + # If transitioning to nav2 from any active parking docking state, initiate undock backing-up sequence first + if self.state in ["WAITING_FOR_MARKER", "ALIGNING_AND_PARKING", "PARKED"] and target_mode == "nav2": + self.get_logger().info('[UNDOCK] Initiating backing-up sequence (0.5m) to undock from charger...') + self.state = "BACKING_UP" + self.backup_start_x = self.sim_x + self.backup_start_y = self.sim_y + self.current_mode = "nav2" + return + + self.current_mode = target_mode + self.waypoint_active = False # Reset active waypoint status on mode switch + + if target_mode == 'parking': + # Wait for user to specify a waypoint from Web UI + self.state = "WAITING_FOR_WAYPOINT" + self.get_logger().info('Switched to parking mode. Waiting for user to send waypoint coordinates...') + elif target_mode == 'patrol': + self.state = "PATROL_INIT" + # Start position (captures current initialized pose sim_x, sim_y, sim_yaw) + self.patrol_start_x = self.sim_x + self.patrol_start_y = self.sim_y + self.patrol_start_yaw = self.sim_yaw + + # Setup targets (predefined safe map waypoints + Home start position) + self.patrol_targets = list(self.patrol_waypoints) + self.patrol_targets.append({ + 'x': self.patrol_start_x, + 'y': self.patrol_start_y, + 'yaw': self.patrol_start_yaw + }) + + self.patrol_index = 0 + self.state = "PATROL_NAVIGATING" + self.get_logger().info(f'Starting patrol from Home ({self.patrol_start_x:.2f}, {self.patrol_start_y:.2f}). Total waypoints: {len(self.patrol_targets)}') + self.send_patrol_goal() + else: + self.cancel_nav2_goal() + self.state = "IDLE" + + def nav2_cmd_vel_callback(self, msg): + # In normal nav2 mode, forward command directly + if self.current_mode == 'nav2' and self.state == "IDLE": + self.cmd_vel_pub.publish(msg) + + def aruco_callback(self, msg): + if self.mock_mode: + # In simulation mock mode, only forward real camera detections to update the Web UI card if we are NOT actively visual servoing + if self.state != "ALIGNING_AND_PARKING": + self.virtual_pose_pub.publish(msg) + return + self.last_marker_pose = msg.pose + self.last_marker_time = self.get_clock().now() + # In real mode, act as the single publisher for /aruco_marker_pose to update the Web UI + self.virtual_pose_pub.publish(msg) + + # If in real mode and currently performing 360-degree scan after patrol return, auto-transition to Parking! + if not self.mock_mode and self.current_mode == 'patrol' and self.state == "SCANNING_FOR_MARKER": + self.get_logger().info('!!! [PATROL -> AUTO PARKING] ArUco marker detected during 360-degree scan! Initiating Automatic Parking docking... !!!') + self.current_mode = 'parking' + self.state = "ALIGNING_AND_PARKING" + + def send_nav2_goal(self): + if self.mock_mode: + self.get_logger().info('[MOCK] Bypassing Nav2 Action Server. Simulating drive to waypoint...') + return + self.get_logger().info('Waiting for Nav2 NavigateToPose action server...') + self.nav_to_pose_client.wait_for_server() + + goal_msg = NavigateToPose.Goal() + goal_msg.pose.header.frame_id = self.waypoint_frame + goal_msg.pose.header.stamp = self.get_clock().now().to_msg() + + goal_msg.pose.pose.position.x = self.waypoint_x + goal_msg.pose.pose.position.y = self.waypoint_y + goal_msg.pose.pose.position.z = 0.0 + + # Euler to Quaternion for yaw + cy = math.cos(self.waypoint_yaw * 0.5) + sy = math.sin(self.waypoint_yaw * 0.5) + goal_msg.pose.pose.orientation.w = cy + goal_msg.pose.pose.orientation.z = sy + + self.get_logger().info(f'Sending Nav2 goal to pre-parking waypoint: x={self.waypoint_x:.2f}, y={self.waypoint_y:.2f}, yaw={self.waypoint_yaw:.2f}') + + send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg) + send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + self.goal_handle = future.result() + if not self.goal_handle.accepted: + self.get_logger().warn('Nav2 goal rejected by server! Switching to WAITING_FOR_MARKER.') + self.state = "WAITING_FOR_MARKER" + return + + self.get_logger().info('Nav2 goal accepted. Navigating...') + get_result_future = self.goal_handle.get_result_async() + get_result_future.add_done_callback(self.goal_result_callback) + + def goal_result_callback(self, future): + status = future.result().status + if status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info('Successfully arrived at pre-parking waypoint. Waiting for ArUco marker...') + self.state = "WAITING_FOR_MARKER" + else: + self.get_logger().warn(f'Nav2 navigation failed with status {status}. Scanning for ArUco marker directly.') + self.state = "WAITING_FOR_MARKER" + + def cancel_nav2_goal(self): + if self.goal_handle is not None: + self.get_logger().info('Canceling active Nav2 goal...') + self.goal_handle.cancel_goal_async() + self.goal_handle = None + + def check_stuck_status(self, cmd_v, cmd_w): + if not self.mock_mode or self.is_recovering: + return + + # We only check for stuck state if we are in an active navigation mode + is_navigating = False + if self.current_mode == 'nav2' and self.mock_nav2_active: + is_navigating = True + elif self.current_mode == 'patrol' and self.state == "PATROL_NAVIGATING": + is_navigating = True + elif self.current_mode == 'parking' and self.state == "NAV2_WAYPOINT": + is_navigating = True + + if not is_navigating: + self.stuck_check_time = None + self.stuck_start_pose = None + return + + now = self.get_clock().now() + if self.stuck_check_time is None: + self.stuck_check_time = now + self.stuck_start_pose = (self.sim_x, self.sim_y, self.sim_yaw) + else: + elapsed = (now - self.stuck_check_time).nanoseconds / 1e9 + if elapsed > 2.0: + dx = self.sim_x - self.stuck_start_pose[0] + dy = self.sim_y - self.stuck_start_pose[1] + dist_moved = math.sqrt(dx*dx + dy*dy) + + # Recovery triggers only when attempting FORWARD linear motion (cmd_v > 0.03) but stuck at same spot (<0.05m) for 2 seconds + if abs(cmd_v) > 0.03 and dist_moved < 0.05: + self.get_logger().warn('!!! [교착 감지] 2초간 전진 시도 불가(막다른 길): 0.6m 후진 탈출(BACKUP) 가동 !!!') + self.is_recovering = True + self.recovery_state = "BACKUP" + self.recovery_start_time = now + self.recovery_start_x = self.sim_x + self.recovery_start_y = self.sim_y + self.recovery_start_yaw = self.sim_yaw + + self.stuck_check_time = now + self.stuck_start_pose = (self.sim_x, self.sim_y, self.sim_yaw) + + def publish_mock_cmd_vel(self, v, w): + twist = Twist() + twist.linear.x = v + twist.angular.z = w + self.cmd_vel_pub.publish(twist) + self.check_stuck_status(v, w) + + def send_patrol_goal(self): + target = self.patrol_targets[self.patrol_index] + self.get_logger().info(f'[PATROL] Sending goal {self.patrol_index + 1}/{len(self.patrol_targets)}: x={target["x"]:.2f}, y={target["y"]:.2f}') + + if self.mock_mode: + # In mock mode, we use potential field simulation. + # We set the target coordinates for the potential field navigation. + self.waypoint_x = target['x'] + self.waypoint_y = target['y'] + self.waypoint_yaw = target['yaw'] + return + + self.nav_to_pose_client.wait_for_server() + goal_msg = NavigateToPose.Goal() + goal_msg.pose.header.frame_id = self.waypoint_frame + goal_msg.pose.header.stamp = self.get_clock().now().to_msg() + + goal_msg.pose.pose.position.x = target['x'] + goal_msg.pose.pose.position.y = target['y'] + goal_msg.pose.pose.position.z = 0.0 + + cy = math.cos(target['yaw'] * 0.5) + sy = math.sin(target['yaw'] * 0.5) + goal_msg.pose.pose.orientation.w = cy + goal_msg.pose.pose.orientation.z = sy + + send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg) + send_goal_future.add_done_callback(self.patrol_goal_response_callback) + + def patrol_goal_response_callback(self, future): + self.goal_handle = future.result() + if not self.goal_handle.accepted: + self.get_logger().warn('Patrol goal rejected by Nav2 server! Retrying next waypoint...') + self.advance_patrol() + return + + self.get_logger().info('Patrol goal accepted. Navigating...') + get_result_future = self.goal_handle.get_result_async() + get_result_future.add_done_callback(self.patrol_goal_result_callback) + + def patrol_goal_result_callback(self, future): + # Prevent callback execution if goal_handle was reset (canceled) + if self.goal_handle is None: + return + status = future.result().status + if status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info(f'Arrived at patrol waypoint {self.patrol_index + 1}!') + self.advance_patrol() + else: + self.get_logger().warn(f'Patrol navigation to waypoint {self.patrol_index + 1} failed with status {status}. Moving to next.') + self.advance_patrol() + + def advance_patrol(self): + self.patrol_index += 1 + if self.patrol_index < len(self.patrol_targets): + self.send_patrol_goal() + else: + self.get_logger().info('Returned to starting position! Initiating 360-degree scan for ArUco marker...') + self.state = "SCANNING_FOR_MARKER" + self.scan_accumulated_yaw = 0.0 + self.scan_last_yaw = self.sim_yaw + + def calculate_steering_target(self, tx, ty): + # Calculate goal direction vector + dx = tx - self.sim_x + dy = ty - self.sim_y + dist = math.sqrt(dx*dx + dy*dy) + if dist < 0.05: + return math.atan2(dy, dx), dist + + att_x = dx / dist + att_y = dy / dist + + # Calculate repulsive forces from map obstacles + rep_x = 0.0 + rep_y = 0.0 + + if self.map_data is not None and self.map_info is not None: + res = self.map_info.resolution + ox = self.map_info.origin.position.x + oy = self.map_info.origin.position.y + w = self.map_info.width + h = self.map_info.height + + r_col = int((self.sim_x - ox) / res) + r_row = int((self.sim_y - oy) / res) + + # Scan a 12x12 cell region around the robot (approx +/- 0.6 meters) + scan_range = 12 + for di in range(-scan_range, scan_range + 1): + for dj in range(-scan_range, scan_range + 1): + col = r_col + di + row = r_row + dj + if 0 <= col < w and 0 <= row < h: + idx = row * w + col + if self.map_data[idx] == 100: # Obstacle cell + obs_x = col * res + ox + obs_y = row * res + oy + odx = self.sim_x - obs_x + ody = self.sim_y - obs_y + o_dist = math.sqrt(odx*odx + ody*ody) + + d0 = 0.55 # Influence range (0.55 meters) + if o_dist < d0: + # Calculate relative bearing of the obstacle from the robot's current heading + vector_to_obs_x = obs_x - self.sim_x + vector_to_obs_y = obs_y - self.sim_y + angle_to_obs = math.atan2(vector_to_obs_y, vector_to_obs_x) + relative_angle = angle_to_obs - self.sim_yaw + relative_angle = math.atan2(math.sin(relative_angle), math.cos(relative_angle)) + + # Continuous angle weighting: only repel if obstacle is in front semicircle (+/- 90 deg) + if abs(relative_angle) < (math.pi / 2.0): + # Weight goes from 1.0 (directly in front) to 0.0 (at 90 degrees) continuously + angle_weight = math.cos(relative_angle) + + o_dist = max(o_dist, 0.05) + # Standard potential field force formula: force = eta * (1/d - 1/d0) * (1/d^2) * angle_weight + eta = 0.40 + force = eta * (1.0 / o_dist - 1.0 / d0) * (1.0 / (o_dist * o_dist)) * angle_weight + rep_x += (odx / o_dist) * force + rep_y += (ody / o_dist) * force + + # Clamp repulsive force to prevent it from completely overwhelming goal attraction (spinning in place) + rep_mag = math.sqrt(rep_x*rep_x + rep_y*rep_y) + max_rep = 1.3 # Slightly larger than attractive force (1.0) to allow pushing away, but limits spinning + if rep_mag > max_rep: + rep_x = (rep_x / rep_mag) * max_rep + rep_y = (rep_y / rep_mag) * max_rep + + # Combined force vector + net_x = att_x + rep_x + net_y = att_y + rep_y + + return math.atan2(net_y, net_x), dist + + def control_loop(self): + # Handle Emergency Stop (ESTOP) state first (both mock and real modes) + if self.state == "ESTOP" or self.current_mode == "stop": + self.stop_robot() + status_msg = String() + env_suffix = " (Simulation)" if self.mock_mode else " (Real)" + status_msg.data = f"Mode: stop | State: ESTOP{env_suffix}" + self.mode_pub.publish(status_msg) + return + + # Handle Backing Up state first (both mock and real modes) + if self.state == "BACKING_UP": + dx = self.sim_x - self.backup_start_x + dy = self.sim_y - self.backup_start_y + dist_traveled = math.sqrt(dx*dx + dy*dy) + + if dist_traveled >= 0.50: + self.get_logger().info('[UNDOCK] Backing-up completed (0.5m). Transitioning to IDLE.') + self.stop_robot() + self.state = "IDLE" + return + + twist = Twist() + twist.linear.x = -0.12 # Back up slowly + twist.angular.z = 0.0 + self.cmd_vel_pub.publish(twist) + + # Publish BACKING_UP status + status_msg = String() + env_suffix = " (Simulation)" if self.mock_mode else " (Real)" + status_msg.data = f"Mode: {self.current_mode} | State: BACKING_UP{env_suffix}" + self.mode_pub.publish(status_msg) + return + + # Handle Stuck Recovery (primarily for mock mode navigation) + if self.mock_mode and self.is_recovering: + now = self.get_clock().now() + v = 0.0 + w = 0.0 + + if self.recovery_state == "BACKUP": + dx = self.sim_x - self.recovery_start_x + dy = self.sim_y - self.recovery_start_y + dist = math.sqrt(dx*dx + dy*dy) + duration = (now - self.recovery_start_time).nanoseconds / 1e9 + + # Back up 0.6 meters or for 3.0 seconds max + if dist < 0.6 and duration < 3.0: + v = -0.1 + w = 0.0 + else: + self.get_logger().info('[RECOVERY] Backup finished. Initiating spin to turn away...') + self.recovery_state = "SPIN" + self.recovery_start_time = now + self.recovery_start_yaw = self.sim_yaw + + elif self.recovery_state == "SPIN": + yaw_diff = self.sim_yaw - self.recovery_start_yaw + yaw_diff = math.atan2(math.sin(yaw_diff), math.cos(yaw_diff)) + duration = (now - self.recovery_start_time).nanoseconds / 1e9 + + # Spin by 90 degrees (1.57 rad) or for 3.0 seconds max + if abs(yaw_diff) < 1.57 and duration < 3.0: + v = 0.0 + w = 0.5 # Rotate in place + else: + self.get_logger().info('[RECOVERY] Escape maneuver finished.') + self.is_recovering = False + self.recovery_state = None + self.stuck_check_time = None # Reset stuck timer + + if self.current_mode == 'patrol' and self.state == "PATROL_NAVIGATING": + # If the current target is the final Home return target, do NOT skip Home! Retry returning Home. + if self.patrol_index == len(self.patrol_targets) - 1: + home_target = self.patrol_targets[self.patrol_index] + dx = home_target['x'] - self.sim_x + dy = home_target['y'] - self.sim_y + dist_to_home = math.sqrt(dx*dx + dy*dy) + if dist_to_home < 0.18: + self.get_logger().info(f'[PATROL] Arrived near Home ({dist_to_home:.2f}m margin). Patrol completed successfully!') + self.advance_patrol() + else: + self.get_logger().warn('[PATROL] Obstacle on path to Home. Retrying navigation back to Home...') + self.send_patrol_goal() + else: + self.get_logger().warn('[PATROL] Current patrol waypoint is blocked. Skipping to the next waypoint...') + self.advance_patrol() + elif self.current_mode == 'nav2': + self.mock_nav2_active = False + self.stop_robot() + self.get_logger().warn('[MOCK NAV2] Goal is blocked! Stopping robot.') + elif self.current_mode == 'parking' and self.state == "NAV2_WAYPOINT": + self.state = "WAITING_FOR_WAYPOINT" + self.stop_robot() + self.get_logger().warn('[MOCK PARKING] Pre-parking waypoint is blocked! Stopping robot.') + + twist = Twist() + twist.linear.x = v + twist.angular.z = w + self.cmd_vel_pub.publish(twist) + + # Publish status + status_msg = String() + status_msg.data = f"Mode: {self.current_mode} | State: RECOVERY_{self.recovery_state} (Simulation)" + self.mode_pub.publish(status_msg) + return + + # Publish current mode status + status_msg = String() + env_suffix = " (Simulation)" if self.mock_mode else " (Real)" + status_msg.data = f"Mode: {self.current_mode} | State: {self.state}{env_suffix}" + self.mode_pub.publish(status_msg) + + # In Mock Mode, perform collision checking on the OccupancyGrid map + if self.mock_mode and self.map_data is not None and self.map_info is not None: + obstacle_in_path = False + # Relax safety limits during visual docking (allows approaching 0.5m target) + safety_limit = 0.25 if self.state == "ALIGNING_AND_PARKING" else self.min_safety_dist + check_dists = [0.15, 0.2, 0.25] if self.state == "ALIGNING_AND_PARKING" else [0.2, 0.3, 0.4, 0.5, 0.6] + + for check_dist in check_dists: + check_x = self.sim_x + check_dist * math.cos(self.sim_yaw) + check_y = self.sim_y + check_dist * math.sin(self.sim_yaw) + + res = self.map_info.resolution + ox = self.map_info.origin.position.x + oy = self.map_info.origin.position.y + w = self.map_info.width + h = self.map_info.height + + col = int((check_x - ox) / res) + row = int((check_y - oy) / res) + + if 0 <= col < w and 0 <= row < h: + idx = row * w + col + val = self.map_data[idx] + if val == 100: # Occupied + obstacle_in_path = True + break + + self.obstacle_detected = obstacle_in_path + + if self.current_mode == 'nav2': + if self.mock_mode and self.mock_nav2_active: + # Steer around obstacles using artificial potential field + target_heading, dist = self.calculate_steering_target(self.mock_nav2_x, self.mock_nav2_y) + + if dist > 0.08: + heading_error = target_heading - self.sim_yaw + heading_error = math.atan2(math.sin(heading_error), math.cos(heading_error)) + + # Turn in place if heading error is large to avoid driving forward into obstacles on the side + if abs(heading_error) > 0.8: + v = 0.0 + w = 0.8 * heading_error + w = max(min(w, 0.4), -0.4) + else: + v = 0.25 * dist + v = max(min(v, 0.15), -0.15) + w = 0.8 * heading_error + w = max(min(w, 0.4), -0.4) + else: + yaw_error = self.mock_nav2_yaw - self.sim_yaw + yaw_error = math.atan2(math.sin(yaw_error), math.cos(yaw_error)) + if abs(yaw_error) > 0.08: + v = 0.0 + w = 0.6 * yaw_error + w = max(min(w, 0.3), -0.3) + else: + v = 0.0 + w = 0.0 + self.mock_nav2_active = False + self.get_logger().info('[MOCK NAV2] Arrived at Nav2 Goal!') + + # Emergency safety brake: zero forward speed but allow rotation to steer away + if self.obstacle_detected: + if v > 0.0: + self.get_logger().warn('!!! LiDAR 전방 장애물 감지: 전진 차단, 회전 허용 !!!', throttle_duration_sec=1.0) + v = 0.0 # Block forward motion only + w = w * 0.5 # Damp rotation near obstacles to prevent chattering + + # Apply low-pass filter to prevent oscillations (corridor wobble) + if v == 0.0 and w == 0.0: + self.mock_prev_w = 0.0 + else: + self.mock_prev_w = self.mock_prev_w * 0.75 + w * 0.25 + w = self.mock_prev_w + + self.publish_mock_cmd_vel(v, w) + return + + if self.current_mode == 'patrol': + # Handle patrol navigation state in mock mode + if self.state == "PATROL_NAVIGATING": + if self.mock_mode: + target = self.patrol_targets[self.patrol_index] + target_heading, dist = self.calculate_steering_target(target['x'], target['y']) + + is_home_target = (self.patrol_index == len(self.patrol_targets) - 1) + arrival_dist = 0.18 if is_home_target else 0.35 + + if dist > arrival_dist: + heading_error = target_heading - self.sim_yaw + heading_error = math.atan2(math.sin(heading_error), math.cos(heading_error)) + + if abs(heading_error) < 0.06: + # Deadband: drive straight without micro-oscillations (wobble) + w = 0.0 + v = 0.25 * dist + v = max(min(v, 0.15), 0.05) + elif abs(heading_error) > 0.8: + v = 0.0 + w = 0.6 * heading_error + w = max(min(w, 0.35), -0.35) + else: + v = 0.25 * dist + v = max(min(v, 0.15), 0.05) + w = 0.5 * heading_error + w = max(min(w, 0.3), -0.3) + else: + # Arrived at waypoint - transition smoothly to next without stopping/oscillating! + self.get_logger().info(f'[MOCK] Arrived at patrol waypoint {self.patrol_index + 1}/{len(self.patrol_targets)}!') + self.advance_patrol() + return + + # Obstacle safety check: stop completely without left-right chattering + if self.obstacle_detected: + v = 0.0 + w = 0.0 # Zero out angular velocity to stop left-right wobbling in front of obstacle + + # Low pass filter + if v == 0.0 and w == 0.0: + self.mock_prev_w = 0.0 + else: + self.mock_prev_w = self.mock_prev_w * 0.75 + w * 0.25 + w = self.mock_prev_w + + self.publish_mock_cmd_vel(v, w) + elif self.state == "SCANNING_FOR_MARKER": + w = 0.30 # Smooth 360-degree rotation speed (rad/s) + v = 0.0 + + # Measure actual physical/simulated orientation change from self.sim_yaw + yaw_diff = self.sim_yaw - self.scan_last_yaw + yaw_diff = math.atan2(math.sin(yaw_diff), math.cos(yaw_diff)) + self.scan_accumulated_yaw += abs(yaw_diff) + self.scan_last_yaw = self.sim_yaw + + # In real mode, check if ArUco marker is fresh and detected during the scan + if not self.mock_mode and self.last_marker_pose is not None: + now = self.get_clock().now() + if (now - self.last_marker_time).nanoseconds / 1e9 < 1.0: + self.get_logger().info('!!! [PATROL -> AUTO PARKING] ArUco marker detected during 360-degree scan! Initiating Automatic Parking docking... !!!') + self.current_mode = 'parking' + self.state = "ALIGNING_AND_PARKING" + return + + if self.scan_accumulated_yaw >= (2.0 * math.pi - 0.05): + self.get_logger().info('[PATROL SCAN] 360-degree scan complete. Stopping at Home position.') + self.stop_robot() + self.state = "PATROL_FINISHED" + return + + # Publish direct un-damped velocity command for smooth 360-degree rotation + twist = Twist() + twist.linear.x = 0.0 + twist.angular.z = w + self.cmd_vel_pub.publish(twist) + return + elif self.state == "PATROL_FINISHED": + self.stop_robot() + return + + if self.current_mode != 'parking': + return + + # Check ArUco marker fresh status + now = self.get_clock().now() + marker_timeout = (now - self.last_marker_time).nanoseconds / 1e9 > self.marker_timeout + if marker_timeout: + self.last_marker_pose = None + + if self.state == "NAV2_WAYPOINT": + if self.mock_mode: + # Steer around obstacles using artificial potential field + target_heading, dist = self.calculate_steering_target(self.waypoint_x, self.waypoint_y) + + if dist > 0.08: + heading_error = target_heading - self.sim_yaw + heading_error = math.atan2(math.sin(heading_error), math.cos(heading_error)) + + # Turn in place if heading error is large to avoid driving forward into obstacles on the side + if abs(heading_error) > 0.8: + v = 0.0 + w = 0.8 * heading_error + w = max(min(w, 0.4), -0.4) + else: + v = 0.25 * dist + v = max(min(v, 0.15), -0.15) + w = 0.8 * heading_error + w = max(min(w, 0.4), -0.4) + else: + yaw_error = self.waypoint_yaw - self.sim_yaw + yaw_error = math.atan2(math.sin(yaw_error), math.cos(yaw_error)) + if abs(yaw_error) > 0.08: + v = 0.0 + w = 0.6 * yaw_error + w = max(min(w, 0.3), -0.3) + else: + v = 0.0 + w = 0.0 + self.get_logger().info('[MOCK] Waypoint reached! Scanning for ArUco marker...') + self.state = "WAITING_FOR_MARKER" + self.mock_scan_start_time = now + + # Emergency safety brake: zero forward speed but allow rotation to steer away + if self.obstacle_detected: + if v > 0.0: + self.get_logger().warn('!!! LiDAR 전방 장애물 감지: 전진 차단, 회전 허용 !!!', throttle_duration_sec=1.0) + v = 0.0 # Block forward motion only + w = w * 0.5 # Damp rotation near obstacles to prevent chattering + + # Apply low-pass filter to prevent oscillations (corridor wobble) + if v == 0.0 and w == 0.0: + self.mock_prev_w = 0.0 + else: + self.mock_prev_w = self.mock_prev_w * 0.75 + w * 0.25 + w = self.mock_prev_w + + self.publish_mock_cmd_vel(v, w) + else: + pass + + elif self.state == "WAITING_FOR_MARKER": + if self.mock_mode: + if self.mock_scan_start_time is None: + self.mock_scan_start_time = now + + # Scan for 2 seconds then "find" the virtual marker + elapsed = (now - self.mock_scan_start_time).nanoseconds / 1e9 + if elapsed > 2.0: + self.sim_marker_z = 2.5 + self.sim_marker_x = 0.2 + self.sim_marker_yaw = 0.3 + + mock_pose = PoseStamped() + mock_pose.header.stamp = now.to_msg() + mock_pose.header.frame_id = 'camera_optical_link' + mock_pose.pose.position.x = self.sim_marker_x + mock_pose.pose.position.z = self.sim_marker_z + mock_pose.pose.orientation.z = math.sin(self.sim_marker_yaw * 0.5) + mock_pose.pose.orientation.w = math.cos(self.sim_marker_yaw * 0.5) + self.virtual_pose_pub.publish(mock_pose) + + self.last_marker_pose = mock_pose.pose + self.last_marker_time = now + self.state = "ALIGNING_AND_PARKING" + self.get_logger().info('[MOCK] Virtual ArUco marker detected!') + else: + self.stop_robot() + else: + if self.last_marker_pose is not None: + self.get_logger().info('ArUco marker detected! Commencing visual servoing parking sequence.') + self.state = "ALIGNING_AND_PARKING" + else: + # Stop and scan + self.stop_robot() + + elif self.state == "ALIGNING_AND_PARKING": + # LiDAR Safety Halt Check + if self.obstacle_detected: + self.get_logger().warn('!!! LiDAR 전방 장애물 감지: 비상 제동 가동 !!!', throttle_duration_sec=1.0) + self.stop_robot() + return + + if self.last_marker_pose is None: + self.get_logger().warn('ArUco marker lost! Halting robot to scan.') + self.state = "WAITING_FOR_MARKER" + self.stop_robot() + return + + # Visual Servoing Kinematics + # Optical frame: Z forward, X right, Y down + x = self.last_marker_pose.position.x + z = self.last_marker_pose.position.z + + # Calculate marker yaw relative to camera + # Normal vector in camera frame from pose quaternion + qx = self.last_marker_pose.orientation.x + qy = self.last_marker_pose.orientation.y + qz = self.last_marker_pose.orientation.z + qw = self.last_marker_pose.orientation.w + + # Rotation matrix components + r02 = 2 * (qx * qz + qw * qy) + r22 = qw * qw - qx * qx - qy * qy + qz * qz + yaw_error = math.atan2(r02, r22) + + # Bearing angle to marker + bearing_error = math.atan2(x, z) + + # Target distance error + dist_error = z - self.target_dist + + self.get_logger().info(f'VS Feedback: dist_err={dist_error:.2f}m, bearing_err={bearing_error:.2f}rad, yaw_err={yaw_error:.2f}rad', throttle_duration_sec=0.5) + + # Check if parking is complete + if abs(dist_error) < 0.04 and abs(bearing_error) < 0.05 and abs(yaw_error) < 0.06: + self.get_logger().info('AUTOMATIC PARKING SUCCESSFUL! Braking applied.') + self.state = "PARKED" + self.stop_robot() + return + + # Proportional speed calculations + # Linear velocity (forward/backward) + v = self.kp_linear * dist_error + # Clip linear velocity (limit to max 0.15 m/s for safe parking) + v = max(min(v, 0.15), -0.15) + + # Angular velocity (steering) + # Combine bearing alignment and heading correction (facing parallel to marker normal) + w = (self.kp_bearing * bearing_error) + (self.kp_yaw * yaw_error) + w = max(min(w, 0.4), -0.4) + + if self.mock_mode: + dt = 0.05 + self.sim_marker_z -= v * dt + self.sim_marker_yaw -= w * dt + self.sim_marker_x -= (v * math.sin(self.sim_marker_yaw) * dt + w * self.sim_marker_z * dt) + + mock_pose = PoseStamped() + mock_pose.header.stamp = now.to_msg() + mock_pose.header.frame_id = 'camera_optical_link' + mock_pose.pose.position.x = self.sim_marker_x + mock_pose.pose.position.z = self.sim_marker_z + mock_pose.pose.orientation.z = math.sin(self.sim_marker_yaw * 0.5) + mock_pose.pose.orientation.w = math.cos(self.sim_marker_yaw * 0.5) + self.virtual_pose_pub.publish(mock_pose) + + self.last_marker_pose = mock_pose.pose + self.last_marker_time = now + + # Publish cmd_vel + twist = Twist() + twist.linear.x = v + twist.angular.z = w + self.cmd_vel_pub.publish(twist) + + elif self.state == "PARKED": + self.stop_robot() + + def stop_robot(self): + twist = Twist() + self.cmd_vel_pub.publish(twist) + +def main(args=None): + rclpy.init(args=args) + node = ParkingControllerNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py b/src/fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py new file mode 100755 index 0000000..f25bcdb --- /dev/null +++ b/src/fori_serial_bridge/fori_serial_bridge/serial_bridge_node.py @@ -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('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('BBHH', slave_id, 0x03, start_reg, num_regs) + crc = self.calculate_crc(packet) + packet += struct.pack('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() diff --git a/src/fori_serial_bridge/fori_serial_bridge/ui_server_node.py b/src/fori_serial_bridge/fori_serial_bridge/ui_server_node.py new file mode 100755 index 0000000..83ba729 --- /dev/null +++ b/src/fori_serial_bridge/fori_serial_bridge/ui_server_node.py @@ -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() diff --git a/src/fori_serial_bridge/launch/fori_full.launch.py b/src/fori_serial_bridge/launch/fori_full.launch.py new file mode 100644 index 0000000..5c90881 --- /dev/null +++ b/src/fori_serial_bridge/launch/fori_full.launch.py @@ -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 diff --git a/src/fori_serial_bridge/package.xml b/src/fori_serial_bridge/package.xml new file mode 100644 index 0000000..2062d7b --- /dev/null +++ b/src/fori_serial_bridge/package.xml @@ -0,0 +1,25 @@ + + + + fori_serial_bridge + 0.0.1 + Serial Bridge for FORI AGV with Smoothing and Delay Optimization + yoo + Apache License 2.0 + + rclpy + geometry_msgs + sensor_msgs + nav_msgs + nav2_msgs + action_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/fori_serial_bridge/resource/fori_serial_bridge b/src/fori_serial_bridge/resource/fori_serial_bridge new file mode 100644 index 0000000..e69de29 diff --git a/src/fori_serial_bridge/setup.cfg b/src/fori_serial_bridge/setup.cfg new file mode 100644 index 0000000..5547d7b --- /dev/null +++ b/src/fori_serial_bridge/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/fori_serial_bridge +[install] +install_scripts=$base/lib/fori_serial_bridge diff --git a/src/fori_serial_bridge/setup.py b/src/fori_serial_bridge/setup.py new file mode 100644 index 0000000..c3998ce --- /dev/null +++ b/src/fori_serial_bridge/setup.py @@ -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' + ], + }, +) diff --git a/src/fori_serial_bridge/ui/app.js b/src/fori_serial_bridge/ui/app.js new file mode 100644 index 0000000..34c09ba --- /dev/null +++ b/src/fori_serial_bridge/ui/app.js @@ -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  |  Y: ${robotPose.y.toFixed(2)}m  |  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(); +} diff --git a/src/fori_serial_bridge/ui/index.html b/src/fori_serial_bridge/ui/index.html new file mode 100644 index 0000000..d027adc --- /dev/null +++ b/src/fori_serial_bridge/ui/index.html @@ -0,0 +1,194 @@ + + + + + + FORI AGV Control Dashboard + + + + + + + +
+ +
+
+

FORI AGV

+ BLDC & ArUco Parking System +
+
+ Simulation Mode + + Disconnected +
+
+ + +
+ + +
+
+

🗺️ 실시간 2D 맵 시각화

+ 지도를 클릭하여 주차 진입점(Waypoint)을 등록하세요. +
+
+ +
+
+

📍 주차 진입점 (Waypoint) 설정

+
+
+ + +
+
+ + +
+
+ + +
+
+ +
+
+

📍 로봇 실시간 현재 위치 (Pose)

+
+ X: 0.00m  |  Y: 0.00m  |  Yaw: 0° +
+
+
+
+

🔋 FORI 배터리 잔량 (24V 60Ah LiFePO4)

+ --% (--.-V) +
+
+
+
+
+
+ + +
+ + +
+
+

📷 실시간 아루코 인식 카메라

+
+
+
+ 카메라 스트림 대기 중... +
+
+
미인식 (No Marker)
+
+
+ 마커 ID + - +
+
+ Z (직선 거리) + - +
+
+ X (가로 오프셋) + - +
+
+ Y (높이 오프셋) + - +
+
+ Yaw (좌우 회전) + - +
+
+ Roll / Pitch + - +
+
+
+
+
+ + +
+
+

⚙️ 제어 모드 및 텔레메트리

+
+ + +
+ + + +
+ + + + +
+
+ 현재 시스템 상태 + IDLE +
+
+ 로봇 선속도 (Linear) + 0.00 m/s +
+
+ 로봇 각속도 (Angular) + 0.00 rad/s +
+
+ + +
+

🔄 실시간 바퀴 회전수 (Feedback RPM)

+
+
+ Front Left +
+
+
+ 0 RPM +
+
+ Front Right +
+
+
+ 0 RPM +
+
+ Rear Left +
+
+
+ 0 RPM +
+
+ Rear Right +
+
+
+ 0 RPM +
+
+
+
+
+
+
+ + + diff --git a/src/fori_serial_bridge/ui/style.css b/src/fori_serial_bridge/ui/style.css new file mode 100644 index 0000000..24d440c --- /dev/null +++ b/src/fori_serial_bridge/ui/style.css @@ -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); +} + diff --git a/src/livox_ros_driver2/.gitignore b/src/livox_ros_driver2/.gitignore new file mode 100644 index 0000000..d7c3a84 --- /dev/null +++ b/src/livox_ros_driver2/.gitignore @@ -0,0 +1,4 @@ +.vscode +build +package.xml +__pycache__ \ No newline at end of file diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/allocators.h b/src/livox_ros_driver2/3rdparty/rapidjson/allocators.h new file mode 100644 index 0000000..03010d5 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/allocators.h @@ -0,0 +1,308 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_ALLOCATORS_H_ +#define RAPIDJSON_ALLOCATORS_H_ + +#include "rapidjson.h" + +RAPIDJSON_NAMESPACE_BEGIN + +/////////////////////////////////////////////////////////////////////////////// +// Allocator + +/*! \class rapidjson::Allocator + \brief Concept for allocating, resizing and freeing memory block. + + Note that Malloc() and Realloc() are non-static but Free() is static. + + So if an allocator need to support Free(), it needs to put its pointer in + the header of memory block. + +\code +concept Allocator { + static const bool kNeedFree; //!< Whether this allocator needs to call +Free(). + + // Allocate a memory block. + // \param size of the memory block in bytes. + // \returns pointer to the memory block. + void* Malloc(size_t size); + + // Resize a memory block. + // \param originalPtr The pointer to current memory block. Null pointer is +permitted. + // \param originalSize The current size in bytes. (Design issue: since some +allocator may not book-keep this, explicitly pass to it can save memory.) + // \param newSize the new size in bytes. + void* Realloc(void* originalPtr, size_t originalSize, size_t newSize); + + // Free a memory block. + // \param pointer to the memory block. Null pointer is permitted. + static void Free(void *ptr); +}; +\endcode +*/ + +/*! \def RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY + \ingroup RAPIDJSON_CONFIG + \brief User-defined kDefaultChunkCapacity definition. + + User can define this as any \c size that is a power of 2. +*/ + +#ifndef RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY +#define RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY (64 * 1024) +#endif + +/////////////////////////////////////////////////////////////////////////////// +// CrtAllocator + +//! C-runtime library allocator. +/*! This class is just wrapper for standard C library memory routines. + \note implements Allocator concept +*/ +class CrtAllocator { + public: + static const bool kNeedFree = true; + void *Malloc(size_t size) { + if (size) // behavior of malloc(0) is implementation defined. + return std::malloc(size); + else + return NULL; // standardize to returning NULL. + } + void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) { + (void)originalSize; + if (newSize == 0) { + std::free(originalPtr); + return NULL; + } + return std::realloc(originalPtr, newSize); + } + static void Free(void *ptr) { std::free(ptr); } +}; + +/////////////////////////////////////////////////////////////////////////////// +// MemoryPoolAllocator + +//! Default memory allocator used by the parser and DOM. +/*! This allocator allocate memory blocks from pre-allocated memory chunks. + + It does not free memory blocks. And Realloc() only allocate new memory. + + The memory chunks are allocated by BaseAllocator, which is CrtAllocator by + default. + + User may also supply a buffer as the first chunk. + + If the user-buffer is full then additional chunks are allocated by + BaseAllocator. + + The user-buffer is not deallocated by this allocator. + + \tparam BaseAllocator the allocator type for allocating memory chunks. + Default is CrtAllocator. \note implements Allocator concept +*/ +template +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(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(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(chunkHead_) + + RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size - + originalSize) { + size_t increment = static_cast(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(baseAllocator_->Malloc( + RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) { + chunk->capacity = capacity; + chunk->size = 0; + chunk->next = chunkHead_; + chunkHead_ = chunk; + return true; + } else + return false; + } + + static const int kDefaultChunkCapacity = + RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY; //!< Default chunk capacity. + + //! Chunk header for perpending to each chunk. + /*! Chunks are stored as a singly linked list. + */ + struct ChunkHeader { + size_t capacity; //!< Capacity of the chunk in bytes (excluding the header + //!< itself). + size_t size; //!< Current size of allocated memory in bytes. + ChunkHeader *next; //!< Next chunk in the linked list. + }; + + ChunkHeader *chunkHead_; //!< Head of the chunk linked-list. Only the head + //!< chunk serves allocation. + size_t chunk_capacity_; //!< The minimum capacity of chunk when they are + //!< allocated. + void *userBuffer_; //!< User supplied buffer. + BaseAllocator + *baseAllocator_; //!< base allocator for allocating memory chunks. + BaseAllocator *ownBaseAllocator_; //!< base allocator created by this object. +}; + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_ENCODINGS_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/cursorstreamwrapper.h b/src/livox_ros_driver2/3rdparty/rapidjson/cursorstreamwrapper.h new file mode 100644 index 0000000..045ddb8 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/cursorstreamwrapper.h @@ -0,0 +1,81 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_ +#define RAPIDJSON_CURSORSTREAMWRAPPER_H_ + +#include "stream.h" + +#if defined(__GNUC__) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(effc++) +#endif + +#if defined(_MSC_VER) && _MSC_VER <= 1800 +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(4702) // unreachable code +RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +//! Cursor stream wrapper for counting line and column number if error exists. +/*! + \tparam InputStream Any stream that implements Stream Concept +*/ +template > +class CursorStreamWrapper : public GenericStreamWrapper { + public: + typedef typename Encoding::Ch Ch; + + CursorStreamWrapper(InputStream &is) + : GenericStreamWrapper(is), line_(1), col_(0) {} + + // counting line and column number + Ch Take() { + Ch ch = this->is_.Take(); + if (ch == '\n') { + line_++; + col_ = 0; + } else { + col_++; + } + return ch; + } + + //! Get the error line number, if error exists. + size_t GetLine() const { return line_; } + //! Get the error column number, if error exists. + size_t GetColumn() const { return col_; } + + private: + size_t line_; //!< Current Line + size_t col_; //!< Current Column +}; + +#if defined(_MSC_VER) && _MSC_VER <= 1800 +RAPIDJSON_DIAG_POP +#endif + +#if defined(__GNUC__) +RAPIDJSON_DIAG_POP +#endif + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_CURSORSTREAMWRAPPER_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/document.h b/src/livox_ros_driver2/3rdparty/rapidjson/document.h new file mode 100644 index 0000000..27addb7 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/document.h @@ -0,0 +1,3309 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_DOCUMENT_H_ +#define RAPIDJSON_DOCUMENT_H_ + +/*! \file document.h */ + +#include +#include // placement new +#include "encodedstream.h" +#include "internal/meta.h" +#include "internal/strfunc.h" +#include "memorystream.h" +#include "reader.h" + +RAPIDJSON_DIAG_PUSH +#ifdef __clang__ +RAPIDJSON_DIAG_OFF(padded) +RAPIDJSON_DIAG_OFF(switch - enum) +RAPIDJSON_DIAG_OFF(c++ 98 - compat) +#elif defined(_MSC_VER) +RAPIDJSON_DIAG_OFF(4127) // conditional expression is constant +RAPIDJSON_DIAG_OFF( + 4244) // conversion from kXxxFlags to 'uint16_t', possible loss of data +#endif + +#ifdef __GNUC__ +RAPIDJSON_DIAG_OFF(effc++) +#endif // __GNUC__ + +#ifndef RAPIDJSON_NOMEMBERITERATORCLASS +#include // std::random_access_iterator_tag +#endif + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS +#include // std::move +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +// Forward declaration. +template +class GenericValue; + +template +class GenericDocument; + +//! Name-value pair in a JSON object value. +/*! + This class was internal to GenericValue. It used to be a inner struct. + But a compiler (IBM XL C/C++ for AIX) have reported to have problem with + that so it moved as a namespace scope struct. + https://code.google.com/p/rapidjson/issues/detail?id=64 +*/ +template +class GenericMember { + public: + GenericValue + name; //!< name of member (must be a string) + GenericValue value; //!< value of member. + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move constructor in C++11 + GenericMember(GenericMember &&rhs) RAPIDJSON_NOEXCEPT + : name(std::move(rhs.name)), + value(std::move(rhs.value)) {} + + //! Move assignment in C++11 + GenericMember &operator=(GenericMember &&rhs) RAPIDJSON_NOEXCEPT { + return *this = static_cast(rhs); + } +#endif + + //! Assignment with move semantics. + /*! \param rhs Source of the assignment. Its name and value will become a null + * value after assignment. + */ + GenericMember &operator=(GenericMember &rhs) RAPIDJSON_NOEXCEPT { + if (RAPIDJSON_LIKELY(this != &rhs)) { + name = rhs.name; + value = rhs.value; + } + return *this; + } + + // swap() for std::sort() and other potential use in STL. + friend inline void swap(GenericMember &a, + GenericMember &b) RAPIDJSON_NOEXCEPT { + a.name.Swap(b.name); + a.value.Swap(b.value); + } + + private: + //! Copy constructor is not permitted. + GenericMember(const GenericMember &rhs); +}; + +/////////////////////////////////////////////////////////////////////////////// +// GenericMemberIterator + +#ifndef RAPIDJSON_NOMEMBERITERATORCLASS + +//! (Constant) member iterator for a JSON object value +/*! + \tparam Const Is this a constant iterator? + \tparam Encoding Encoding of the value. (Even non-string values need to + have the same encoding in a document) \tparam Allocator Allocator type for + allocating memory of object, array and string. + + This class implements a Random Access Iterator for GenericMember elements + of a GenericValue, see ISO/IEC 14882:2003(E) C++ standard, 24.1 + [lib.iterator.requirements]. + + \note This iterator implementation is mainly intended to avoid implicit + conversions from iterator values to \c NULL, + e.g. from GenericValue::FindMember. + + \note Define \c RAPIDJSON_NOMEMBERITERATORCLASS to fall back to a + pointer-based implementation, if your platform doesn't provide + the C++ header. + + \see GenericMember, GenericValue::MemberIterator, + GenericValue::ConstMemberIterator + */ +template +class GenericMemberIterator { + friend class GenericValue; + template + friend class GenericMemberIterator; + + typedef GenericMember PlainType; + typedef typename internal::MaybeAddConst::Type ValueType; + + public: + //! Iterator type itself + typedef GenericMemberIterator Iterator; + //! Constant iterator type + typedef GenericMemberIterator ConstIterator; + //! Non-constant iterator type + typedef GenericMemberIterator NonConstIterator; + + /** \name std::iterator_traits support */ + //@{ + typedef ValueType value_type; + typedef ValueType *pointer; + typedef ValueType &reference; + typedef std::ptrdiff_t difference_type; + typedef std::random_access_iterator_tag iterator_category; + //@} + + //! Pointer to (const) GenericMember + typedef pointer Pointer; + //! Reference to (const) GenericMember + typedef reference Reference; + //! Signed integer type (e.g. \c ptrdiff_t) + typedef difference_type DifferenceType; + + //! Default constructor (singular value) + /*! Creates an iterator pointing to no element. + \note All operations, except for comparisons, are undefined on such + values. + */ + GenericMemberIterator() : ptr_() {} + + //! Iterator conversions to more const + /*! + \param it (Non-const) iterator to copy from + + Allows the creation of an iterator from another GenericMemberIterator + that is "less const". Especially, creating a non-constant iterator + from a constant iterator are disabled: + \li const -> non-const (not ok) + \li const -> const (ok) + \li non-const -> const (ok) + \li non-const -> non-const (ok) + + \note If the \c Const template parameter is already \c false, this + constructor effectively defines a regular copy-constructor. + Otherwise, the copy constructor is implicitly defined. + */ + GenericMemberIterator(const NonConstIterator &it) : ptr_(it.ptr_) {} + Iterator &operator=(const NonConstIterator &it) { + ptr_ = it.ptr_; + return *this; + } + + //! @name stepping + //@{ + Iterator &operator++() { + ++ptr_; + return *this; + } + Iterator &operator--() { + --ptr_; + return *this; + } + Iterator operator++(int) { + Iterator old(*this); + ++ptr_; + return old; + } + Iterator operator--(int) { + Iterator old(*this); + --ptr_; + return old; + } + //@} + + //! @name increment/decrement + //@{ + Iterator operator+(DifferenceType n) const { return Iterator(ptr_ + n); } + Iterator operator-(DifferenceType n) const { return Iterator(ptr_ - n); } + + Iterator &operator+=(DifferenceType n) { + ptr_ += n; + return *this; + } + Iterator &operator-=(DifferenceType n) { + ptr_ -= n; + return *this; + } + //@} + + //! @name relations + //@{ + bool operator==(ConstIterator that) const { return ptr_ == that.ptr_; } + bool operator!=(ConstIterator that) const { return ptr_ != that.ptr_; } + bool operator<=(ConstIterator that) const { return ptr_ <= that.ptr_; } + bool operator>=(ConstIterator that) const { return ptr_ >= that.ptr_; } + bool operator<(ConstIterator that) const { return ptr_ < that.ptr_; } + bool operator>(ConstIterator that) const { return ptr_ > that.ptr_; } + //@} + + //! @name dereference + //@{ + Reference operator*() const { return *ptr_; } + Pointer operator->() const { return ptr_; } + Reference operator[](DifferenceType n) const { return ptr_[n]; } + //@} + + //! Distance + DifferenceType operator-(ConstIterator that) const { + return ptr_ - that.ptr_; + } + + private: + //! Internal constructor from plain pointer + explicit GenericMemberIterator(Pointer p) : ptr_(p) {} + + Pointer ptr_; //!< raw pointer +}; + +#else // RAPIDJSON_NOMEMBERITERATORCLASS + +// class-based member iterator implementation disabled, use plain pointers + +template +class GenericMemberIterator; + +//! non-const GenericMemberIterator +template +class GenericMemberIterator { + //! use plain pointer as iterator type + typedef GenericMember *Iterator; +}; +//! const GenericMemberIterator +template +class GenericMemberIterator { + //! use plain const pointer as iterator type + typedef const GenericMember *Iterator; +}; + +#endif // RAPIDJSON_NOMEMBERITERATORCLASS + +/////////////////////////////////////////////////////////////////////////////// +// GenericStringRef + +//! Reference to a constant string (not taking a copy) +/*! + \tparam CharType character type of the string + + This helper class is used to automatically infer constant string + references for string literals, especially from \c const \b (!) + character arrays. + + The main use is for creating JSON string values without copying the + source string via an \ref Allocator. This requires that the referenced + string pointers have a sufficient lifetime, which exceeds the lifetime + of the associated GenericValue. + + \b Example + \code + Value v("foo"); // ok, no need to copy & calculate length + const char foo[] = "foo"; + v.SetString(foo); // ok + + const char* bar = foo; + // Value x(bar); // not ok, can't rely on bar's lifetime + Value x(StringRef(bar)); // lifetime explicitly guaranteed by user + Value y(StringRef(bar, 3)); // ok, explicitly pass length + \endcode + + \see StringRef, GenericValue::SetString +*/ +template +struct GenericStringRef { + typedef CharType Ch; //!< character type of the string + +//! Create string reference from \c const character array +#ifndef __clang__ // -Wdocumentation + /*! + This constructor implicitly creates a constant string reference from + a \c const character array. It has better performance than + \ref StringRef(const CharType*) by inferring the string \ref length + from the array length, and also supports strings containing null + characters. + + \tparam N length of the string, automatically inferred + + \param str Constant character array, lifetime assumed to be longer + than the use of the string in e.g. a GenericValue + + \post \ref s == str + + \note Constant complexity. + \note There is a hidden, private overload to disallow references to + non-const character arrays to be created via this constructor. + By this, e.g. function-scope arrays used to be filled via + \c snprintf are excluded from consideration. + In such cases, the referenced string should be \b copied to the + GenericValue instead. + */ +#endif + template + GenericStringRef(const CharType (&str)[N]) RAPIDJSON_NOEXCEPT + : s(str), + length(N - 1) {} + +//! Explicitly create string reference from \c const character pointer +#ifndef __clang__ // -Wdocumentation + /*! + This constructor can be used to \b explicitly create a reference to + a constant string pointer. + + \see StringRef(const CharType*) + + \param str Constant character pointer, lifetime assumed to be longer + than the use of the string in e.g. a GenericValue + + \post \ref s == str + + \note There is a hidden, private overload to disallow references to + non-const character arrays to be created via this constructor. + By this, e.g. function-scope arrays used to be filled via + \c snprintf are excluded from consideration. + In such cases, the referenced string should be \b copied to the + GenericValue instead. + */ +#endif + explicit GenericStringRef(const CharType *str) + : s(str), length(NotNullStrLen(str)) {} + +//! Create constant string reference from pointer and length +#ifndef __clang__ // -Wdocumentation +/*! \param str constant string, lifetime assumed to be longer than the use of + the string in e.g. a GenericValue \param len length of the string, + excluding the trailing NULL terminator + + \post \ref s == str && \ref length == len + \note Constant complexity. + */ +#endif + GenericStringRef(const CharType *str, SizeType len) + : s(RAPIDJSON_LIKELY(str) ? str : emptyString), length(len) { + RAPIDJSON_ASSERT(str != 0 || len == 0u); + } + + GenericStringRef(const GenericStringRef &rhs) + : s(rhs.s), length(rhs.length) {} + + //! implicit conversion to plain CharType pointer + operator const Ch *() const { return s; } + + const Ch *const s; //!< plain CharType pointer + const SizeType length; //!< length of the string (excluding the trailing NULL + //!terminator) + + private: + SizeType NotNullStrLen(const CharType *str) { + RAPIDJSON_ASSERT(str != 0); + return internal::StrLen(str); + } + + /// Empty string - used when passing in a NULL pointer + static const Ch emptyString[]; + + //! Disallow construction from non-const array + template + GenericStringRef(CharType (&str)[N]) /* = delete */; + //! Copy assignment operator not permitted - immutable type + GenericStringRef &operator=(const GenericStringRef &rhs) /* = delete */; +}; + +template +const CharType GenericStringRef::emptyString[] = {CharType()}; + +//! Mark a character pointer as constant string +/*! Mark a plain character pointer as a "string literal". This function + can be used to avoid copying a character string to be referenced as a + value in a JSON GenericValue object, if the string's lifetime is known + to be valid long enough. + \tparam CharType Character type of the string + \param str Constant string, lifetime assumed to be longer than the use of + the string in e.g. a GenericValue \return GenericStringRef string reference + object \relatesalso GenericStringRef + + \see GenericValue::GenericValue(StringRefType), + GenericValue::operator=(StringRefType), + GenericValue::SetString(StringRefType), GenericValue::PushBack(StringRefType, + Allocator&), GenericValue::AddMember +*/ +template +inline GenericStringRef StringRef(const CharType *str) { + return GenericStringRef(str); +} + +//! Mark a character pointer as constant string +/*! Mark a plain character pointer as a "string literal". This function + can be used to avoid copying a character string to be referenced as a + value in a JSON GenericValue object, if the string's lifetime is known + to be valid long enough. + + This version has better performance with supplied length, and also + supports string containing null characters. + + \tparam CharType character type of the string + \param str Constant string, lifetime assumed to be longer than the use of + the string in e.g. a GenericValue \param length The length of source string. + \return GenericStringRef string reference object + \relatesalso GenericStringRef +*/ +template +inline GenericStringRef StringRef(const CharType *str, + size_t length) { + return GenericStringRef(str, SizeType(length)); +} + +#if RAPIDJSON_HAS_STDSTRING +//! Mark a string object as constant string +/*! Mark a string object (e.g. \c std::string) as a "string literal". + This function can be used to avoid copying a string to be referenced as a + value in a JSON GenericValue object, if the string's lifetime is known + to be valid long enough. + + \tparam CharType character type of the string + \param str Constant string, lifetime assumed to be longer than the use of + the string in e.g. a GenericValue \return GenericStringRef string reference + object \relatesalso GenericStringRef \note Requires the definition of the + preprocessor symbol \ref RAPIDJSON_HAS_STDSTRING. +*/ +template +inline GenericStringRef StringRef( + const std::basic_string &str) { + return GenericStringRef(str.data(), SizeType(str.size())); +} +#endif + +/////////////////////////////////////////////////////////////////////////////// +// GenericValue type traits +namespace internal { + +template +struct IsGenericValueImpl : FalseType {}; + +// select candidates according to nested encoding and allocator types +template +struct IsGenericValueImpl::Type, + typename Void::Type> + : IsBaseOf< + GenericValue, + T>::Type {}; + +// helper to match arbitrary GenericValue instantiations, including derived +// classes +template +struct IsGenericValue : IsGenericValueImpl::Type {}; + +} // namespace internal + +/////////////////////////////////////////////////////////////////////////////// +// TypeHelper + +namespace internal { + +template +struct TypeHelper {}; + +template +struct TypeHelper { + static bool Is(const ValueType &v) { return v.IsBool(); } + static bool Get(const ValueType &v) { return v.GetBool(); } + static ValueType &Set(ValueType &v, bool data) { return v.SetBool(data); } + static ValueType &Set(ValueType &v, bool data, + typename ValueType::AllocatorType &) { + return v.SetBool(data); + } +}; + +template +struct TypeHelper { + static bool Is(const ValueType &v) { return v.IsInt(); } + static int Get(const ValueType &v) { return v.GetInt(); } + static ValueType &Set(ValueType &v, int data) { return v.SetInt(data); } + static ValueType &Set(ValueType &v, int data, + typename ValueType::AllocatorType &) { + return v.SetInt(data); + } +}; + +template +struct TypeHelper { + static bool Is(const ValueType &v) { return v.IsUint(); } + static unsigned Get(const ValueType &v) { return v.GetUint(); } + static ValueType &Set(ValueType &v, unsigned data) { return v.SetUint(data); } + static ValueType &Set(ValueType &v, unsigned data, + typename ValueType::AllocatorType &) { + return v.SetUint(data); + } +}; + +#ifdef _MSC_VER +RAPIDJSON_STATIC_ASSERT(sizeof(long) == sizeof(int)); +template +struct TypeHelper { + static bool Is(const ValueType &v) { return v.IsInt(); } + static long Get(const ValueType &v) { return v.GetInt(); } + static ValueType &Set(ValueType &v, long data) { return v.SetInt(data); } + static ValueType &Set(ValueType &v, long data, + typename ValueType::AllocatorType &) { + return v.SetInt(data); + } +}; + +RAPIDJSON_STATIC_ASSERT(sizeof(unsigned long) == sizeof(unsigned)); +template +struct TypeHelper { + static bool Is(const ValueType &v) { return v.IsUint(); } + static unsigned long Get(const ValueType &v) { return v.GetUint(); } + static ValueType &Set(ValueType &v, unsigned long data) { + return v.SetUint(data); + } + static ValueType &Set(ValueType &v, unsigned long data, + typename ValueType::AllocatorType &) { + return v.SetUint(data); + } +}; +#endif + +template +struct TypeHelper { + static bool Is(const ValueType &v) { return v.IsInt64(); } + static int64_t Get(const ValueType &v) { return v.GetInt64(); } + static ValueType &Set(ValueType &v, int64_t data) { return v.SetInt64(data); } + static ValueType &Set(ValueType &v, int64_t data, + typename ValueType::AllocatorType &) { + return v.SetInt64(data); + } +}; + +template +struct TypeHelper { + static bool Is(const ValueType &v) { return v.IsUint64(); } + static uint64_t Get(const ValueType &v) { return v.GetUint64(); } + static ValueType &Set(ValueType &v, uint64_t data) { + return v.SetUint64(data); + } + static ValueType &Set(ValueType &v, uint64_t data, + typename ValueType::AllocatorType &) { + return v.SetUint64(data); + } +}; + +template +struct TypeHelper { + static bool Is(const ValueType &v) { return v.IsDouble(); } + static double Get(const ValueType &v) { return v.GetDouble(); } + static ValueType &Set(ValueType &v, double data) { return v.SetDouble(data); } + static ValueType &Set(ValueType &v, double data, + typename ValueType::AllocatorType &) { + return v.SetDouble(data); + } +}; + +template +struct TypeHelper { + static bool Is(const ValueType &v) { return v.IsFloat(); } + static float Get(const ValueType &v) { return v.GetFloat(); } + static ValueType &Set(ValueType &v, float data) { return v.SetFloat(data); } + static ValueType &Set(ValueType &v, float data, + typename ValueType::AllocatorType &) { + return v.SetFloat(data); + } +}; + +template +struct TypeHelper { + typedef const typename ValueType::Ch *StringType; + static bool Is(const ValueType &v) { return v.IsString(); } + static StringType Get(const ValueType &v) { return v.GetString(); } + static ValueType &Set(ValueType &v, const StringType data) { + return v.SetString(typename ValueType::StringRefType(data)); + } + static ValueType &Set(ValueType &v, const StringType data, + typename ValueType::AllocatorType &a) { + return v.SetString(data, a); + } +}; + +#if RAPIDJSON_HAS_STDSTRING +template +struct TypeHelper> { + typedef std::basic_string StringType; + static bool Is(const ValueType &v) { return v.IsString(); } + static StringType Get(const ValueType &v) { + return StringType(v.GetString(), v.GetStringLength()); + } + static ValueType &Set(ValueType &v, const StringType &data, + typename ValueType::AllocatorType &a) { + return v.SetString(data, a); + } +}; +#endif + +template +struct TypeHelper { + typedef typename ValueType::Array ArrayType; + static bool Is(const ValueType &v) { return v.IsArray(); } + static ArrayType Get(ValueType &v) { return v.GetArray(); } + static ValueType &Set(ValueType &v, ArrayType data) { return v = data; } + static ValueType &Set(ValueType &v, ArrayType data, + typename ValueType::AllocatorType &) { + return v = data; + } +}; + +template +struct TypeHelper { + typedef typename ValueType::ConstArray ArrayType; + static bool Is(const ValueType &v) { return v.IsArray(); } + static ArrayType Get(const ValueType &v) { return v.GetArray(); } +}; + +template +struct TypeHelper { + typedef typename ValueType::Object ObjectType; + static bool Is(const ValueType &v) { return v.IsObject(); } + static ObjectType Get(ValueType &v) { return v.GetObject(); } + static ValueType &Set(ValueType &v, ObjectType data) { return v = data; } + static ValueType &Set(ValueType &v, ObjectType data, + typename ValueType::AllocatorType &) { + return v = data; + } +}; + +template +struct TypeHelper { + typedef typename ValueType::ConstObject ObjectType; + static bool Is(const ValueType &v) { return v.IsObject(); } + static ObjectType Get(const ValueType &v) { return v.GetObject(); } +}; + +} // namespace internal + +// Forward declarations +template +class GenericArray; +template +class GenericObject; + +/////////////////////////////////////////////////////////////////////////////// +// GenericValue + +//! Represents a JSON value. Use Value for UTF8 encoding and default allocator. +/*! + A JSON value can be one of 7 types. This class is a variant type supporting + these types. + + Use the Value if UTF8 and default allocator + + \tparam Encoding Encoding of the value. (Even non-string values need to + have the same encoding in a document) \tparam Allocator Allocator type for + allocating memory of object, array and string. +*/ +template > +class GenericValue { + public: + //! Name-value pair in an object. + typedef GenericMember Member; + typedef Encoding EncodingType; //!< Encoding type from template parameter. + typedef Allocator AllocatorType; //!< Allocator type from template parameter. + typedef typename Encoding::Ch Ch; //!< Character type derived from Encoding. + typedef GenericStringRef + StringRefType; //!< Reference to a constant string + typedef typename GenericMemberIterator::Iterator + MemberIterator; //!< Member iterator for iterating in object. + typedef typename GenericMemberIterator::Iterator + ConstMemberIterator; //!< Constant member iterator for iterating in + //!< object. + typedef GenericValue + *ValueIterator; //!< Value iterator for iterating in array. + typedef const GenericValue + *ConstValueIterator; //!< Constant value iterator for iterating in array. + typedef GenericValue + ValueType; //!< Value type of itself. + typedef GenericArray Array; + typedef GenericArray ConstArray; + typedef GenericObject Object; + typedef GenericObject ConstObject; + + //!@name Constructors and destructor. + //@{ + + //! Default constructor creates a null value. + GenericValue() RAPIDJSON_NOEXCEPT : data_() { data_.f.flags = kNullFlag; } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move constructor in C++11 + GenericValue(GenericValue &&rhs) RAPIDJSON_NOEXCEPT : data_(rhs.data_) { + rhs.data_.f.flags = kNullFlag; // give up contents + } +#endif + + private: + //! Copy constructor is not permitted. + GenericValue(const GenericValue &rhs); + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Moving from a GenericDocument is not permitted. + template + GenericValue(GenericDocument &&rhs); + + //! Move assignment from a GenericDocument is not permitted. + template + GenericValue &operator=( + GenericDocument &&rhs); +#endif + + public: + //! Constructor with JSON value type. + /*! This creates a Value of specified type with default content. + \param type Type of the value. + \note Default content for number is zero. + */ + explicit GenericValue(Type type) RAPIDJSON_NOEXCEPT : data_() { + static const uint16_t defaultFlags[] = { + kNullFlag, kFalseFlag, kTrueFlag, kObjectFlag, + kArrayFlag, kShortStringFlag, kNumberAnyFlag}; + RAPIDJSON_NOEXCEPT_ASSERT(type >= kNullType && type <= kNumberType); + data_.f.flags = defaultFlags[type]; + + // Use ShortString to store empty string. + if (type == kStringType) data_.ss.SetLength(0); + } + + //! Explicit copy constructor (with allocator) + /*! Creates a copy of a Value by using the given Allocator + \tparam SourceAllocator allocator of \c rhs + \param rhs Value to copy from (read-only) + \param allocator Allocator for allocating copied elements and buffers. + Commonly use GenericDocument::GetAllocator(). \param copyConstStrings Force + copying of constant strings (e.g. referencing an in-situ buffer) \see + CopyFrom() + */ + template + GenericValue(const GenericValue &rhs, + Allocator &allocator, bool copyConstStrings = false) { + switch (rhs.GetType()) { + case kObjectType: { + SizeType count = rhs.data_.o.size; + Member *lm = reinterpret_cast( + allocator.Malloc(count * sizeof(Member))); + const typename GenericValue::Member *rm = + rhs.GetMembersPointer(); + for (SizeType i = 0; i < count; i++) { + new (&lm[i].name) + GenericValue(rm[i].name, allocator, copyConstStrings); + new (&lm[i].value) + GenericValue(rm[i].value, allocator, copyConstStrings); + } + data_.f.flags = kObjectFlag; + data_.o.size = data_.o.capacity = count; + SetMembersPointer(lm); + } break; + case kArrayType: { + SizeType count = rhs.data_.a.size; + GenericValue *le = reinterpret_cast( + allocator.Malloc(count * sizeof(GenericValue))); + const GenericValue *re = + rhs.GetElementsPointer(); + for (SizeType i = 0; i < count; i++) + new (&le[i]) GenericValue(re[i], allocator, copyConstStrings); + data_.f.flags = kArrayFlag; + data_.a.size = data_.a.capacity = count; + SetElementsPointer(le); + } break; + case kStringType: + if (rhs.data_.f.flags == kConstStringFlag && !copyConstStrings) { + data_.f.flags = rhs.data_.f.flags; + data_ = *reinterpret_cast(&rhs.data_); + } else + SetStringRaw(StringRef(rhs.GetString(), rhs.GetStringLength()), + allocator); + break; + default: + data_.f.flags = rhs.data_.f.flags; + data_ = *reinterpret_cast(&rhs.data_); + break; + } + } + +//! Constructor for boolean value. +/*! \param b Boolean value + \note This constructor is limited to \em real boolean values and rejects + implicitly converted types like arbitrary pointers. Use an explicit + cast to \c bool, if you want to construct a boolean JSON value in such + cases. + */ +#ifndef RAPIDJSON_DOXYGEN_RUNNING // hide SFINAE from Doxygen + template + explicit GenericValue(T b, RAPIDJSON_ENABLEIF((internal::IsSame))) + RAPIDJSON_NOEXCEPT // See #472 +#else + explicit GenericValue(bool b) RAPIDJSON_NOEXCEPT +#endif + : data_() { + // safe-guard against failing SFINAE + RAPIDJSON_STATIC_ASSERT((internal::IsSame::Value)); + data_.f.flags = b ? kTrueFlag : kFalseFlag; + } + + //! Constructor for int value. + explicit GenericValue(int i) RAPIDJSON_NOEXCEPT : data_() { + data_.n.i64 = i; + data_.f.flags = + (i >= 0) ? (kNumberIntFlag | kUintFlag | kUint64Flag) : kNumberIntFlag; + } + + //! Constructor for unsigned value. + explicit GenericValue(unsigned u) RAPIDJSON_NOEXCEPT : data_() { + data_.n.u64 = u; + data_.f.flags = (u & 0x80000000) + ? kNumberUintFlag + : (kNumberUintFlag | kIntFlag | kInt64Flag); + } + + //! Constructor for int64_t value. + explicit GenericValue(int64_t i64) RAPIDJSON_NOEXCEPT : data_() { + data_.n.i64 = i64; + data_.f.flags = kNumberInt64Flag; + if (i64 >= 0) { + data_.f.flags |= kNumberUint64Flag; + if (!(static_cast(i64) & + RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x00000000))) + data_.f.flags |= kUintFlag; + if (!(static_cast(i64) & + RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x80000000))) + data_.f.flags |= kIntFlag; + } else if (i64 >= static_cast( + RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x80000000))) + data_.f.flags |= kIntFlag; + } + + //! Constructor for uint64_t value. + explicit GenericValue(uint64_t u64) RAPIDJSON_NOEXCEPT : data_() { + data_.n.u64 = u64; + data_.f.flags = kNumberUint64Flag; + if (!(u64 & RAPIDJSON_UINT64_C2(0x80000000, 0x00000000))) + data_.f.flags |= kInt64Flag; + if (!(u64 & RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x00000000))) + data_.f.flags |= kUintFlag; + if (!(u64 & RAPIDJSON_UINT64_C2(0xFFFFFFFF, 0x80000000))) + data_.f.flags |= kIntFlag; + } + + //! Constructor for double value. + explicit GenericValue(double d) RAPIDJSON_NOEXCEPT : data_() { + data_.n.d = d; + data_.f.flags = kNumberDoubleFlag; + } + + //! Constructor for float value. + explicit GenericValue(float f) RAPIDJSON_NOEXCEPT : data_() { + data_.n.d = static_cast(f); + data_.f.flags = kNumberDoubleFlag; + } + + //! Constructor for constant string (i.e. do not make a copy of string) + GenericValue(const Ch *s, SizeType length) RAPIDJSON_NOEXCEPT : data_() { + SetStringRaw(StringRef(s, length)); + } + + //! Constructor for constant string (i.e. do not make a copy of string) + explicit GenericValue(StringRefType s) RAPIDJSON_NOEXCEPT : data_() { + SetStringRaw(s); + } + + //! Constructor for copy-string (i.e. do make a copy of string) + GenericValue(const Ch *s, SizeType length, Allocator &allocator) : data_() { + SetStringRaw(StringRef(s, length), allocator); + } + + //! Constructor for copy-string (i.e. do make a copy of string) + GenericValue(const Ch *s, Allocator &allocator) : data_() { + SetStringRaw(StringRef(s), allocator); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Constructor for copy-string from a string object (i.e. do make a copy of + //! string) + /*! \note Requires the definition of the preprocessor symbol \ref + * RAPIDJSON_HAS_STDSTRING. + */ + GenericValue(const std::basic_string &s, Allocator &allocator) : data_() { + SetStringRaw(StringRef(s), allocator); + } +#endif + + //! Constructor for Array. + /*! + \param a An array obtained by \c GetArray(). + \note \c Array is always pass-by-value. + \note the source array is moved into this value and the sourec array + becomes empty. + */ + GenericValue(Array a) RAPIDJSON_NOEXCEPT : data_(a.value_.data_) { + a.value_.data_ = Data(); + a.value_.data_.f.flags = kArrayFlag; + } + + //! Constructor for Object. + /*! + \param o An object obtained by \c GetObject(). + \note \c Object is always pass-by-value. + \note the source object is moved into this value and the sourec object + becomes empty. + */ + GenericValue(Object o) RAPIDJSON_NOEXCEPT : data_(o.value_.data_) { + o.value_.data_ = Data(); + o.value_.data_.f.flags = kObjectFlag; + } + + //! Destructor. + /*! Need to destruct elements of array, members of object, or copy-string. + */ + ~GenericValue() { + if (Allocator::kNeedFree) { // Shortcut by Allocator's trait + switch (data_.f.flags) { + case kArrayFlag: { + GenericValue *e = GetElementsPointer(); + for (GenericValue *v = e; v != e + data_.a.size; ++v) + v->~GenericValue(); + Allocator::Free(e); + } break; + + case kObjectFlag: + for (MemberIterator m = MemberBegin(); m != MemberEnd(); ++m) + m->~Member(); + Allocator::Free(GetMembersPointer()); + break; + + case kCopyStringFlag: + Allocator::Free(const_cast(GetStringPointer())); + break; + + default: + break; // Do nothing for other types. + } + } + } + + //@} + + //!@name Assignment operators + //@{ + + //! Assignment with move semantics. + /*! \param rhs Source of the assignment. It will become a null value after + * assignment. + */ + GenericValue &operator=(GenericValue &rhs) RAPIDJSON_NOEXCEPT { + if (RAPIDJSON_LIKELY(this != &rhs)) { + this->~GenericValue(); + RawAssign(rhs); + } + return *this; + } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move assignment in C++11 + GenericValue &operator=(GenericValue &&rhs) RAPIDJSON_NOEXCEPT { + return *this = rhs.Move(); + } +#endif + + //! Assignment of constant string reference (no copy) + /*! \param str Constant string reference to be assigned + \note This overload is needed to avoid clashes with the generic primitive + type assignment overload below. \see GenericStringRef, operator=(T) + */ + GenericValue &operator=(StringRefType str) RAPIDJSON_NOEXCEPT { + GenericValue s(str); + return *this = s; + } + + //! Assignment with primitive types. + /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t + \param value The value to be assigned. + + \note The source type \c T explicitly disallows all pointer types, + especially (\c const) \ref Ch*. This helps avoiding implicitly + referencing character strings with insufficient lifetime, use + \ref SetString(const Ch*, Allocator&) (for copying) or + \ref StringRef() (to explicitly mark the pointer as constant) instead. + All other pointer types would implicitly convert to \c bool, + use \ref SetBool() instead. + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::IsPointer), (GenericValue &)) + operator=(T value) { + GenericValue v(value); + return *this = v; + } + + //! Deep-copy assignment from Value + /*! Assigns a \b copy of the Value to the current Value object + \tparam SourceAllocator Allocator type of \c rhs + \param rhs Value to copy from (read-only) + \param allocator Allocator to use for copying + \param copyConstStrings Force copying of constant strings (e.g. + referencing an in-situ buffer) + */ + template + GenericValue &CopyFrom(const GenericValue &rhs, + Allocator &allocator, bool copyConstStrings = false) { + RAPIDJSON_ASSERT(static_cast(this) != + static_cast(&rhs)); + this->~GenericValue(); + new (this) GenericValue(rhs, allocator, copyConstStrings); + return *this; + } + + //! Exchange the contents of this value with those of other. + /*! + \param other Another value. + \note Constant complexity. + */ + GenericValue &Swap(GenericValue &other) RAPIDJSON_NOEXCEPT { + GenericValue temp; + temp.RawAssign(*this); + RawAssign(other); + other.RawAssign(temp); + return *this; + } + + //! free-standing swap function helper + /*! + Helper function to enable support for common swap implementation pattern + based on \c std::swap: \code void swap(MyClass& a, MyClass& b) { using + std::swap; swap(a.value, b.value); + // ... + } + \endcode + \see Swap() + */ + friend inline void swap(GenericValue &a, GenericValue &b) RAPIDJSON_NOEXCEPT { + a.Swap(b); + } + + //! Prepare Value for move semantics + /*! \return *this */ + GenericValue &Move() RAPIDJSON_NOEXCEPT { return *this; } + //@} + + //!@name Equal-to and not-equal-to operators + //@{ + //! Equal-to operator + /*! + \note If an object contains duplicated named member, comparing equality + with any object is always \c false. \note Complexity is quadratic in + Object's member number and linear for the rest (number of all values in the + subtree and total lengths of all strings). + */ + template + bool operator==(const GenericValue &rhs) const { + typedef GenericValue RhsType; + if (GetType() != rhs.GetType()) return false; + + switch (GetType()) { + case kObjectType: // Warning: O(n^2) inner-loop + if (data_.o.size != rhs.data_.o.size) return false; + for (ConstMemberIterator lhsMemberItr = MemberBegin(); + lhsMemberItr != MemberEnd(); ++lhsMemberItr) { + typename RhsType::ConstMemberIterator rhsMemberItr = + rhs.FindMember(lhsMemberItr->name); + if (rhsMemberItr == rhs.MemberEnd() || + lhsMemberItr->value != rhsMemberItr->value) + return false; + } + return true; + + case kArrayType: + if (data_.a.size != rhs.data_.a.size) return false; + for (SizeType i = 0; i < data_.a.size; i++) + if ((*this)[i] != rhs[i]) return false; + return true; + + case kStringType: + return StringEqual(rhs); + + case kNumberType: + if (IsDouble() || rhs.IsDouble()) { + double a = GetDouble(); // May convert from integer to double. + double b = rhs.GetDouble(); // Ditto + return a >= b && a <= b; // Prevent -Wfloat-equal + } else + return data_.n.u64 == rhs.data_.n.u64; + + default: + return true; + } + } + + //! Equal-to operator with const C-string pointer + bool operator==(const Ch *rhs) const { + return *this == GenericValue(StringRef(rhs)); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Equal-to operator with string object + /*! \note Requires the definition of the preprocessor symbol \ref + * RAPIDJSON_HAS_STDSTRING. + */ + bool operator==(const std::basic_string &rhs) const { + return *this == GenericValue(StringRef(rhs)); + } +#endif + + //! Equal-to operator with primitive types + /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t, + * \c double, \c true, \c false + */ + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (bool)) + operator==(const T &rhs) const { + return *this == GenericValue(rhs); + } + + //! Not-equal-to operator + /*! \return !(*this == rhs) + */ + template + bool operator!=(const GenericValue &rhs) const { + return !(*this == rhs); + } + + //! Not-equal-to operator with const C-string pointer + bool operator!=(const Ch *rhs) const { return !(*this == rhs); } + + //! Not-equal-to operator with arbitrary types + /*! \return !(*this == rhs) + */ + template + RAPIDJSON_DISABLEIF_RETURN((internal::IsGenericValue), (bool)) + operator!=(const T &rhs) const { + return !(*this == rhs); + } + + //! Equal-to operator with arbitrary types (symmetric version) + /*! \return (rhs == lhs) + */ + template + friend RAPIDJSON_DISABLEIF_RETURN((internal::IsGenericValue), (bool)) + operator==(const T &lhs, const GenericValue &rhs) { + return rhs == lhs; + } + + //! Not-Equal-to operator with arbitrary types (symmetric version) + /*! \return !(rhs == lhs) + */ + template + friend RAPIDJSON_DISABLEIF_RETURN((internal::IsGenericValue), (bool)) + operator!=(const T &lhs, const GenericValue &rhs) { + return !(rhs == lhs); + } + //@} + + //!@name Type + //@{ + + Type GetType() const { return static_cast(data_.f.flags & kTypeMask); } + bool IsNull() const { return data_.f.flags == kNullFlag; } + bool IsFalse() const { return data_.f.flags == kFalseFlag; } + bool IsTrue() const { return data_.f.flags == kTrueFlag; } + bool IsBool() const { return (data_.f.flags & kBoolFlag) != 0; } + bool IsObject() const { return data_.f.flags == kObjectFlag; } + bool IsArray() const { return data_.f.flags == kArrayFlag; } + bool IsNumber() const { return (data_.f.flags & kNumberFlag) != 0; } + bool IsInt() const { return (data_.f.flags & kIntFlag) != 0; } + bool IsUint() const { return (data_.f.flags & kUintFlag) != 0; } + bool IsInt64() const { return (data_.f.flags & kInt64Flag) != 0; } + bool IsUint64() const { return (data_.f.flags & kUint64Flag) != 0; } + bool IsDouble() const { return (data_.f.flags & kDoubleFlag) != 0; } + bool IsString() const { return (data_.f.flags & kStringFlag) != 0; } + + // Checks whether a number can be losslessly converted to a double. + bool IsLosslessDouble() const { + if (!IsNumber()) return false; + if (IsUint64()) { + uint64_t u = GetUint64(); + volatile double d = static_cast(u); + return (d >= 0.0) && + (d < + static_cast((std::numeric_limits::max)())) && + (u == static_cast(d)); + } + if (IsInt64()) { + int64_t i = GetInt64(); + volatile double d = static_cast(i); + return (d >= + static_cast((std::numeric_limits::min)())) && + (d < static_cast((std::numeric_limits::max)())) && + (i == static_cast(d)); + } + return true; // double, int, uint are always lossless + } + + // Checks whether a number is a float (possible lossy). + bool IsFloat() const { + if ((data_.f.flags & kDoubleFlag) == 0) return false; + double d = GetDouble(); + return d >= -3.4028234e38 && d <= 3.4028234e38; + } + // Checks whether a number can be losslessly converted to a float. + bool IsLosslessFloat() const { + if (!IsNumber()) return false; + double a = GetDouble(); + if (a < static_cast(-(std::numeric_limits::max)()) || + a > static_cast((std::numeric_limits::max)())) + return false; + double b = static_cast(static_cast(a)); + return a >= b && a <= b; // Prevent -Wfloat-equal + } + + //@} + + //!@name Null + //@{ + + GenericValue &SetNull() { + this->~GenericValue(); + new (this) GenericValue(); + return *this; + } + + //@} + + //!@name Bool + //@{ + + bool GetBool() const { + RAPIDJSON_ASSERT(IsBool()); + return data_.f.flags == kTrueFlag; + } + //!< Set boolean value + /*! \post IsBool() == true */ + GenericValue &SetBool(bool b) { + this->~GenericValue(); + new (this) GenericValue(b); + return *this; + } + + //@} + + //!@name Object + //@{ + + //! Set this value as an empty object. + /*! \post IsObject() == true */ + GenericValue &SetObject() { + this->~GenericValue(); + new (this) GenericValue(kObjectType); + return *this; + } + + //! Get the number of members in the object. + SizeType MemberCount() const { + RAPIDJSON_ASSERT(IsObject()); + return data_.o.size; + } + + //! Get the capacity of object. + SizeType MemberCapacity() const { + RAPIDJSON_ASSERT(IsObject()); + return data_.o.capacity; + } + + //! Check whether the object is empty. + bool ObjectEmpty() const { + RAPIDJSON_ASSERT(IsObject()); + return data_.o.size == 0; + } + + //! Get a value from an object associated with the name. + /*! \pre IsObject() == true + \tparam T Either \c Ch or \c const \c Ch (template used for disambiguation + with \ref operator[](SizeType)) \note In version 0.1x, if the member is not + found, this function returns a null value. This makes issue 7. Since 0.2, + if the name is not correct, it will assert. If user is unsure whether a + member exists, user should use HasMember() first. A better approach is to + use FindMember(). \note Linear time complexity. + */ + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::NotExpr< + internal::IsSame::Type, Ch>>), + (GenericValue &)) + operator[](T *name) { + GenericValue n(StringRef(name)); + return (*this)[n]; + } + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::NotExpr< + internal::IsSame::Type, Ch>>), + (const GenericValue &)) + operator[](T *name) const { + return const_cast(*this)[name]; + } + + //! Get a value from an object associated with the name. + /*! \pre IsObject() == true + \tparam SourceAllocator Allocator of the \c name value + + \note Compared to \ref operator[](T*), this version is faster because it + does not need a StrLen(). And it can also handle strings with embedded null + characters. + + \note Linear time complexity. + */ + template + GenericValue &operator[]( + const GenericValue &name) { + MemberIterator member = FindMember(name); + if (member != MemberEnd()) + return member->value; + else { + RAPIDJSON_ASSERT(false); // see above note + + // This will generate -Wexit-time-destructors in clang + // static GenericValue NullValue; + // return NullValue; + + // Use static buffer and placement-new to prevent destruction + static char buffer[sizeof(GenericValue)]; + return *new (buffer) GenericValue(); + } + } + template + const GenericValue &operator[]( + const GenericValue &name) const { + return const_cast(*this)[name]; + } + +#if RAPIDJSON_HAS_STDSTRING + //! Get a value from an object associated with name (string object). + GenericValue &operator[](const std::basic_string &name) { + return (*this)[GenericValue(StringRef(name))]; + } + const GenericValue &operator[](const std::basic_string &name) const { + return (*this)[GenericValue(StringRef(name))]; + } +#endif + + //! Const member iterator + /*! \pre IsObject() == true */ + ConstMemberIterator MemberBegin() const { + RAPIDJSON_ASSERT(IsObject()); + return ConstMemberIterator(GetMembersPointer()); + } + //! Const \em past-the-end member iterator + /*! \pre IsObject() == true */ + ConstMemberIterator MemberEnd() const { + RAPIDJSON_ASSERT(IsObject()); + return ConstMemberIterator(GetMembersPointer() + data_.o.size); + } + //! Member iterator + /*! \pre IsObject() == true */ + MemberIterator MemberBegin() { + RAPIDJSON_ASSERT(IsObject()); + return MemberIterator(GetMembersPointer()); + } + //! \em Past-the-end member iterator + /*! \pre IsObject() == true */ + MemberIterator MemberEnd() { + RAPIDJSON_ASSERT(IsObject()); + return MemberIterator(GetMembersPointer() + data_.o.size); + } + + //! Request the object to have enough capacity to store members. + /*! \param newCapacity The capacity that the object at least need to have. + \param allocator Allocator for reallocating memory. It must be the same + one as used before. Commonly use GenericDocument::GetAllocator(). \return + The value itself for fluent API. \note Linear time complexity. + */ + GenericValue &MemberReserve(SizeType newCapacity, Allocator &allocator) { + RAPIDJSON_ASSERT(IsObject()); + if (newCapacity > data_.o.capacity) { + SetMembersPointer(reinterpret_cast(allocator.Realloc( + GetMembersPointer(), data_.o.capacity * sizeof(Member), + newCapacity * sizeof(Member)))); + data_.o.capacity = newCapacity; + } + return *this; + } + + //! Check whether a member exists in the object. + /*! + \param name Member name to be searched. + \pre IsObject() == true + \return Whether a member with that name exists. + \note It is better to use FindMember() directly if you need the obtain the + value as well. \note Linear time complexity. + */ + bool HasMember(const Ch *name) const { + return FindMember(name) != MemberEnd(); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Check whether a member exists in the object with string object. + /*! + \param name Member name to be searched. + \pre IsObject() == true + \return Whether a member with that name exists. + \note It is better to use FindMember() directly if you need the obtain the + value as well. \note Linear time complexity. + */ + bool HasMember(const std::basic_string &name) const { + return FindMember(name) != MemberEnd(); + } +#endif + + //! Check whether a member exists in the object with GenericValue name. + /*! + This version is faster because it does not need a StrLen(). It can also + handle string with null character. \param name Member name to be searched. + \pre IsObject() == true + \return Whether a member with that name exists. + \note It is better to use FindMember() directly if you need the obtain the + value as well. \note Linear time complexity. + */ + template + bool HasMember(const GenericValue &name) const { + return FindMember(name) != MemberEnd(); + } + + //! Find member by name. + /*! + \param name Member name to be searched. + \pre IsObject() == true + \return Iterator to member, if it exists. + Otherwise returns \ref MemberEnd(). + + \note Earlier versions of Rapidjson returned a \c NULL pointer, in case + the requested member doesn't exist. For consistency with e.g. + \c std::map, this has been changed to MemberEnd() now. + \note Linear time complexity. + */ + MemberIterator FindMember(const Ch *name) { + GenericValue n(StringRef(name)); + return FindMember(n); + } + + ConstMemberIterator FindMember(const Ch *name) const { + return const_cast(*this).FindMember(name); + } + + //! Find member by name. + /*! + This version is faster because it does not need a StrLen(). It can also + handle string with null character. \param name Member name to be searched. + \pre IsObject() == true + \return Iterator to member, if it exists. + Otherwise returns \ref MemberEnd(). + + \note Earlier versions of Rapidjson returned a \c NULL pointer, in case + the requested member doesn't exist. For consistency with e.g. + \c std::map, this has been changed to MemberEnd() now. + \note Linear time complexity. + */ + template + MemberIterator FindMember( + const GenericValue &name) { + RAPIDJSON_ASSERT(IsObject()); + RAPIDJSON_ASSERT(name.IsString()); + MemberIterator member = MemberBegin(); + for (; member != MemberEnd(); ++member) + if (name.StringEqual(member->name)) break; + return member; + } + template + ConstMemberIterator FindMember( + const GenericValue &name) const { + return const_cast(*this).FindMember(name); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Find member by string object name. + /*! + \param name Member name to be searched. + \pre IsObject() == true + \return Iterator to member, if it exists. + Otherwise returns \ref MemberEnd(). + */ + MemberIterator FindMember(const std::basic_string &name) { + return FindMember(GenericValue(StringRef(name))); + } + ConstMemberIterator FindMember(const std::basic_string &name) const { + return FindMember(GenericValue(StringRef(name))); + } +#endif + + //! Add a member (name-value pair) to the object. + /*! \param name A string value as name of member. + \param value Value of any type. + \param allocator Allocator for reallocating memory. It must be the same + one as used before. Commonly use GenericDocument::GetAllocator(). \return + The value itself for fluent API. \note The ownership of \c name and \c + value will be transferred to this object on success. \pre IsObject() && + name.IsString() \post name.IsNull() && value.IsNull() \note Amortized + Constant time complexity. + */ + GenericValue &AddMember(GenericValue &name, GenericValue &value, + Allocator &allocator) { + RAPIDJSON_ASSERT(IsObject()); + RAPIDJSON_ASSERT(name.IsString()); + + ObjectData &o = data_.o; + if (o.size >= o.capacity) + MemberReserve(o.capacity == 0 ? kDefaultObjectCapacity + : (o.capacity + (o.capacity + 1) / 2), + allocator); + Member *members = GetMembersPointer(); + members[o.size].name.RawAssign(name); + members[o.size].value.RawAssign(value); + o.size++; + return *this; + } + + //! Add a constant string value as member (name-value pair) to the object. + /*! \param name A string value as name of member. + \param value constant string reference as value of member. + \param allocator Allocator for reallocating memory. It must be the same + one as used before. Commonly use GenericDocument::GetAllocator(). \return + The value itself for fluent API. \pre IsObject() \note This overload is + needed to avoid clashes with the generic primitive type + AddMember(GenericValue&,T,Allocator&) overload below. \note Amortized + Constant time complexity. + */ + GenericValue &AddMember(GenericValue &name, StringRefType value, + Allocator &allocator) { + GenericValue v(value); + return AddMember(name, v, allocator); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Add a string object as member (name-value pair) to the object. + /*! \param name A string value as name of member. + \param value constant string reference as value of member. + \param allocator Allocator for reallocating memory. It must be the same + one as used before. Commonly use GenericDocument::GetAllocator(). \return + The value itself for fluent API. \pre IsObject() \note This overload is + needed to avoid clashes with the generic primitive type + AddMember(GenericValue&,T,Allocator&) overload below. \note Amortized + Constant time complexity. + */ + GenericValue &AddMember(GenericValue &name, std::basic_string &value, + Allocator &allocator) { + GenericValue v(value, allocator); + return AddMember(name, v, allocator); + } +#endif + + //! Add any primitive value as member (name-value pair) to the object. + /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t + \param name A string value as name of member. + \param value Value of primitive type \c T as value of member + \param allocator Allocator for reallocating memory. Commonly use + GenericDocument::GetAllocator(). \return The value itself for fluent API. + \pre IsObject() + + \note The source type \c T explicitly disallows all pointer types, + especially (\c const) \ref Ch*. This helps avoiding implicitly + referencing character strings with insufficient lifetime, use + \ref AddMember(StringRefType, GenericValue&, Allocator&) or \ref + AddMember(StringRefType, StringRefType, Allocator&). + All other pointer types would implicitly convert to \c bool, + use an explicit cast instead, if needed. + \note Amortized Constant time complexity. + */ + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (GenericValue &)) + AddMember(GenericValue &name, T value, Allocator &allocator) { + GenericValue v(value); + return AddMember(name, v, allocator); + } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericValue &AddMember(GenericValue &&name, GenericValue &&value, + Allocator &allocator) { + return AddMember(name, value, allocator); + } + GenericValue &AddMember(GenericValue &&name, GenericValue &value, + Allocator &allocator) { + return AddMember(name, value, allocator); + } + GenericValue &AddMember(GenericValue &name, GenericValue &&value, + Allocator &allocator) { + return AddMember(name, value, allocator); + } + GenericValue &AddMember(StringRefType name, GenericValue &&value, + Allocator &allocator) { + GenericValue n(name); + return AddMember(n, value, allocator); + } +#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS + + //! Add a member (name-value pair) to the object. + /*! \param name A constant string reference as name of member. + \param value Value of any type. + \param allocator Allocator for reallocating memory. It must be the same + one as used before. Commonly use GenericDocument::GetAllocator(). \return + The value itself for fluent API. \note The ownership of \c value will be + transferred to this object on success. \pre IsObject() \post + value.IsNull() \note Amortized Constant time complexity. + */ + GenericValue &AddMember(StringRefType name, GenericValue &value, + Allocator &allocator) { + GenericValue n(name); + return AddMember(n, value, allocator); + } + + //! Add a constant string value as member (name-value pair) to the object. + /*! \param name A constant string reference as name of member. + \param value constant string reference as value of member. + \param allocator Allocator for reallocating memory. It must be the same + one as used before. Commonly use GenericDocument::GetAllocator(). \return + The value itself for fluent API. \pre IsObject() \note This overload is + needed to avoid clashes with the generic primitive type + AddMember(StringRefType,T,Allocator&) overload below. \note Amortized + Constant time complexity. + */ + GenericValue &AddMember(StringRefType name, StringRefType value, + Allocator &allocator) { + GenericValue v(value); + return AddMember(name, v, allocator); + } + + //! Add any primitive value as member (name-value pair) to the object. + /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t + \param name A constant string reference as name of member. + \param value Value of primitive type \c T as value of member + \param allocator Allocator for reallocating memory. Commonly use + GenericDocument::GetAllocator(). \return The value itself for fluent API. + \pre IsObject() + + \note The source type \c T explicitly disallows all pointer types, + especially (\c const) \ref Ch*. This helps avoiding implicitly + referencing character strings with insufficient lifetime, use + \ref AddMember(StringRefType, GenericValue&, Allocator&) or \ref + AddMember(StringRefType, StringRefType, Allocator&). + All other pointer types would implicitly convert to \c bool, + use an explicit cast instead, if needed. + \note Amortized Constant time complexity. + */ + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (GenericValue &)) + AddMember(StringRefType name, T value, Allocator &allocator) { + GenericValue n(name); + return AddMember(n, value, allocator); + } + + //! Remove all members in the object. + /*! This function do not deallocate memory in the object, i.e. the capacity is + unchanged. \note Linear time complexity. + */ + void RemoveAllMembers() { + RAPIDJSON_ASSERT(IsObject()); + for (MemberIterator m = MemberBegin(); m != MemberEnd(); ++m) m->~Member(); + data_.o.size = 0; + } + + //! Remove a member in object by its name. + /*! \param name Name of member to be removed. + \return Whether the member existed. + \note This function may reorder the object members. Use \ref + EraseMember(ConstMemberIterator) if you need to preserve the + relative order of the remaining members. + \note Linear time complexity. + */ + bool RemoveMember(const Ch *name) { + GenericValue n(StringRef(name)); + return RemoveMember(n); + } + +#if RAPIDJSON_HAS_STDSTRING + bool RemoveMember(const std::basic_string &name) { + return RemoveMember(GenericValue(StringRef(name))); + } +#endif + + template + bool RemoveMember(const GenericValue &name) { + MemberIterator m = FindMember(name); + if (m != MemberEnd()) { + RemoveMember(m); + return true; + } else + return false; + } + + //! Remove a member in object by iterator. + /*! \param m member iterator (obtained by FindMember() or MemberBegin()). + \return the new iterator after removal. + \note This function may reorder the object members. Use \ref + EraseMember(ConstMemberIterator) if you need to preserve the + relative order of the remaining members. + \note Constant time complexity. + */ + MemberIterator RemoveMember(MemberIterator m) { + RAPIDJSON_ASSERT(IsObject()); + RAPIDJSON_ASSERT(data_.o.size > 0); + RAPIDJSON_ASSERT(GetMembersPointer() != 0); + RAPIDJSON_ASSERT(m >= MemberBegin() && m < MemberEnd()); + + MemberIterator last(GetMembersPointer() + (data_.o.size - 1)); + if (data_.o.size > 1 && m != last) + *m = *last; // Move the last one to this place + else + m->~Member(); // Only one left, just destroy + --data_.o.size; + return m; + } + + //! Remove a member from an object by iterator. + /*! \param pos iterator to the member to remove + \pre IsObject() == true && \ref MemberBegin() <= \c pos < \ref MemberEnd() + \return Iterator following the removed element. + If the iterator \c pos refers to the last element, the \ref + MemberEnd() iterator is returned. \note This function preserves the + relative order of the remaining object members. If you do not need this, + use the more efficient \ref RemoveMember(MemberIterator). \note Linear time + complexity. + */ + MemberIterator EraseMember(ConstMemberIterator pos) { + return EraseMember(pos, pos + 1); + } + + //! Remove members in the range [first, last) from an object. + /*! \param first iterator to the first member to remove + \param last iterator following the last member to remove + \pre IsObject() == true && \ref MemberBegin() <= \c first <= \c last <= + \ref MemberEnd() \return Iterator following the last removed element. \note + This function preserves the relative order of the remaining object members. + \note Linear time complexity. + */ + MemberIterator EraseMember(ConstMemberIterator first, + ConstMemberIterator last) { + RAPIDJSON_ASSERT(IsObject()); + RAPIDJSON_ASSERT(data_.o.size > 0); + RAPIDJSON_ASSERT(GetMembersPointer() != 0); + RAPIDJSON_ASSERT(first >= MemberBegin()); + RAPIDJSON_ASSERT(first <= last); + RAPIDJSON_ASSERT(last <= MemberEnd()); + + MemberIterator pos = MemberBegin() + (first - MemberBegin()); + for (MemberIterator itr = pos; itr != last; ++itr) itr->~Member(); + std::memmove(static_cast(&*pos), &*last, + static_cast(MemberEnd() - last) * sizeof(Member)); + data_.o.size -= static_cast(last - first); + return pos; + } + + //! Erase a member in object by its name. + /*! \param name Name of member to be removed. + \return Whether the member existed. + \note Linear time complexity. + */ + bool EraseMember(const Ch *name) { + GenericValue n(StringRef(name)); + return EraseMember(n); + } + +#if RAPIDJSON_HAS_STDSTRING + bool EraseMember(const std::basic_string &name) { + return EraseMember(GenericValue(StringRef(name))); + } +#endif + + template + bool EraseMember(const GenericValue &name) { + MemberIterator m = FindMember(name); + if (m != MemberEnd()) { + EraseMember(m); + return true; + } else + return false; + } + + Object GetObject() { + RAPIDJSON_ASSERT(IsObject()); + return Object(*this); + } + ConstObject GetObject() const { + RAPIDJSON_ASSERT(IsObject()); + return ConstObject(*this); + } + + //@} + + //!@name Array + //@{ + + //! Set this value as an empty array. + /*! \post IsArray == true */ + GenericValue &SetArray() { + this->~GenericValue(); + new (this) GenericValue(kArrayType); + return *this; + } + + //! Get the number of elements in array. + SizeType Size() const { + RAPIDJSON_ASSERT(IsArray()); + return data_.a.size; + } + + //! Get the capacity of array. + SizeType Capacity() const { + RAPIDJSON_ASSERT(IsArray()); + return data_.a.capacity; + } + + //! Check whether the array is empty. + bool Empty() const { + RAPIDJSON_ASSERT(IsArray()); + return data_.a.size == 0; + } + + //! Remove all elements in the array. + /*! This function do not deallocate memory in the array, i.e. the capacity is + unchanged. \note Linear time complexity. + */ + void Clear() { + RAPIDJSON_ASSERT(IsArray()); + GenericValue *e = GetElementsPointer(); + for (GenericValue *v = e; v != e + data_.a.size; ++v) v->~GenericValue(); + data_.a.size = 0; + } + + //! Get an element from array by index. + /*! \pre IsArray() == true + \param index Zero-based index of element. + \see operator[](T*) + */ + GenericValue &operator[](SizeType index) { + RAPIDJSON_ASSERT(IsArray()); + RAPIDJSON_ASSERT(index < data_.a.size); + return GetElementsPointer()[index]; + } + const GenericValue &operator[](SizeType index) const { + return const_cast(*this)[index]; + } + + //! Element iterator + /*! \pre IsArray() == true */ + ValueIterator Begin() { + RAPIDJSON_ASSERT(IsArray()); + return GetElementsPointer(); + } + //! \em Past-the-end element iterator + /*! \pre IsArray() == true */ + ValueIterator End() { + RAPIDJSON_ASSERT(IsArray()); + return GetElementsPointer() + data_.a.size; + } + //! Constant element iterator + /*! \pre IsArray() == true */ + ConstValueIterator Begin() const { + return const_cast(*this).Begin(); + } + //! Constant \em past-the-end element iterator + /*! \pre IsArray() == true */ + ConstValueIterator End() const { + return const_cast(*this).End(); + } + + //! Request the array to have enough capacity to store elements. + /*! \param newCapacity The capacity that the array at least need to have. + \param allocator Allocator for reallocating memory. It must be the same + one as used before. Commonly use GenericDocument::GetAllocator(). \return + The value itself for fluent API. \note Linear time complexity. + */ + GenericValue &Reserve(SizeType newCapacity, Allocator &allocator) { + RAPIDJSON_ASSERT(IsArray()); + if (newCapacity > data_.a.capacity) { + SetElementsPointer(reinterpret_cast(allocator.Realloc( + GetElementsPointer(), data_.a.capacity * sizeof(GenericValue), + newCapacity * sizeof(GenericValue)))); + data_.a.capacity = newCapacity; + } + return *this; + } + + //! Append a GenericValue at the end of the array. + /*! \param value Value to be appended. + \param allocator Allocator for reallocating memory. It must be the same + one as used before. Commonly use GenericDocument::GetAllocator(). \pre + IsArray() == true \post value.IsNull() == true \return The value itself for + fluent API. \note The ownership of \c value will be transferred to this + array on success. \note If the number of elements to be appended is known, + calls Reserve() once first may be more efficient. \note Amortized constant + time complexity. + */ + GenericValue &PushBack(GenericValue &value, Allocator &allocator) { + RAPIDJSON_ASSERT(IsArray()); + if (data_.a.size >= data_.a.capacity) + Reserve(data_.a.capacity == 0 + ? kDefaultArrayCapacity + : (data_.a.capacity + (data_.a.capacity + 1) / 2), + allocator); + GetElementsPointer()[data_.a.size++].RawAssign(value); + return *this; + } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericValue &PushBack(GenericValue &&value, Allocator &allocator) { + return PushBack(value, allocator); + } +#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS + + //! Append a constant string reference at the end of the array. + /*! \param value Constant string reference to be appended. + \param allocator Allocator for reallocating memory. It must be the same + one used previously. Commonly use GenericDocument::GetAllocator(). \pre + IsArray() == true \return The value itself for fluent API. \note If the + number of elements to be appended is known, calls Reserve() once first may + be more efficient. \note Amortized constant time complexity. \see + GenericStringRef + */ + GenericValue &PushBack(StringRefType value, Allocator &allocator) { + return (*this).template PushBack(value, allocator); + } + + //! Append a primitive value at the end of the array. + /*! \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t + \param value Value of primitive type T to be appended. + \param allocator Allocator for reallocating memory. It must be the same + one as used before. Commonly use GenericDocument::GetAllocator(). \pre + IsArray() == true \return The value itself for fluent API. \note If the + number of elements to be appended is known, calls Reserve() once first may + be more efficient. + + \note The source type \c T explicitly disallows all pointer types, + especially (\c const) \ref Ch*. This helps avoiding implicitly + referencing character strings with insufficient lifetime, use + \ref PushBack(GenericValue&, Allocator&) or \ref + PushBack(StringRefType, Allocator&). + All other pointer types would implicitly convert to \c bool, + use an explicit cast instead, if needed. + \note Amortized constant time complexity. + */ + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (GenericValue &)) + PushBack(T value, Allocator &allocator) { + GenericValue v(value); + return PushBack(v, allocator); + } + + //! Remove the last element in the array. + /*! + \note Constant time complexity. + */ + GenericValue &PopBack() { + RAPIDJSON_ASSERT(IsArray()); + RAPIDJSON_ASSERT(!Empty()); + GetElementsPointer()[--data_.a.size].~GenericValue(); + return *this; + } + + //! Remove an element of array by iterator. + /*! + \param pos iterator to the element to remove + \pre IsArray() == true && \ref Begin() <= \c pos < \ref End() + \return Iterator following the removed element. If the iterator pos refers + to the last element, the End() iterator is returned. \note Linear time + complexity. + */ + ValueIterator Erase(ConstValueIterator pos) { return Erase(pos, pos + 1); } + + //! Remove elements in the range [first, last) of the array. + /*! + \param first iterator to the first element to remove + \param last iterator following the last element to remove + \pre IsArray() == true && \ref Begin() <= \c first <= \c last <= \ref + End() \return Iterator following the last removed element. \note Linear + time complexity. + */ + ValueIterator Erase(ConstValueIterator first, ConstValueIterator last) { + RAPIDJSON_ASSERT(IsArray()); + RAPIDJSON_ASSERT(data_.a.size > 0); + RAPIDJSON_ASSERT(GetElementsPointer() != 0); + RAPIDJSON_ASSERT(first >= Begin()); + RAPIDJSON_ASSERT(first <= last); + RAPIDJSON_ASSERT(last <= End()); + ValueIterator pos = Begin() + (first - Begin()); + for (ValueIterator itr = pos; itr != last; ++itr) itr->~GenericValue(); + std::memmove(static_cast(pos), last, + static_cast(End() - last) * sizeof(GenericValue)); + data_.a.size -= static_cast(last - first); + return pos; + } + + Array GetArray() { + RAPIDJSON_ASSERT(IsArray()); + return Array(*this); + } + ConstArray GetArray() const { + RAPIDJSON_ASSERT(IsArray()); + return ConstArray(*this); + } + + //@} + + //!@name Number + //@{ + + int GetInt() const { + RAPIDJSON_ASSERT(data_.f.flags & kIntFlag); + return data_.n.i.i; + } + unsigned GetUint() const { + RAPIDJSON_ASSERT(data_.f.flags & kUintFlag); + return data_.n.u.u; + } + int64_t GetInt64() const { + RAPIDJSON_ASSERT(data_.f.flags & kInt64Flag); + return data_.n.i64; + } + uint64_t GetUint64() const { + RAPIDJSON_ASSERT(data_.f.flags & kUint64Flag); + return data_.n.u64; + } + + //! Get the value as double type. + /*! \note If the value is 64-bit integer type, it may lose precision. Use \c + * IsLosslessDouble() to check whether the converison is lossless. + */ + double GetDouble() const { + RAPIDJSON_ASSERT(IsNumber()); + if ((data_.f.flags & kDoubleFlag) != 0) + return data_.n.d; // exact type, no conversion. + if ((data_.f.flags & kIntFlag) != 0) return data_.n.i.i; // int -> double + if ((data_.f.flags & kUintFlag) != 0) + return data_.n.u.u; // unsigned -> double + if ((data_.f.flags & kInt64Flag) != 0) + return static_cast( + data_.n.i64); // int64_t -> double (may lose precision) + RAPIDJSON_ASSERT((data_.f.flags & kUint64Flag) != 0); + return static_cast( + data_.n.u64); // uint64_t -> double (may lose precision) + } + + //! Get the value as float type. + /*! \note If the value is 64-bit integer type, it may lose precision. Use \c + * IsLosslessFloat() to check whether the converison is lossless. + */ + float GetFloat() const { return static_cast(GetDouble()); } + + GenericValue &SetInt(int i) { + this->~GenericValue(); + new (this) GenericValue(i); + return *this; + } + GenericValue &SetUint(unsigned u) { + this->~GenericValue(); + new (this) GenericValue(u); + return *this; + } + GenericValue &SetInt64(int64_t i64) { + this->~GenericValue(); + new (this) GenericValue(i64); + return *this; + } + GenericValue &SetUint64(uint64_t u64) { + this->~GenericValue(); + new (this) GenericValue(u64); + return *this; + } + GenericValue &SetDouble(double d) { + this->~GenericValue(); + new (this) GenericValue(d); + return *this; + } + GenericValue &SetFloat(float f) { + this->~GenericValue(); + new (this) GenericValue(static_cast(f)); + return *this; + } + + //@} + + //!@name String + //@{ + + const Ch *GetString() const { + RAPIDJSON_ASSERT(IsString()); + return (data_.f.flags & kInlineStrFlag) ? data_.ss.str : GetStringPointer(); + } + + //! Get the length of string. + /*! Since rapidjson permits "\\u0000" in the json string, + * strlen(v.GetString()) may not equal to v.GetStringLength(). + */ + SizeType GetStringLength() const { + RAPIDJSON_ASSERT(IsString()); + return ((data_.f.flags & kInlineStrFlag) ? (data_.ss.GetLength()) + : data_.s.length); + } + + //! Set this value as a string without copying source string. + /*! This version has better performance with supplied length, and also support + string containing null character. \param s source string pointer. \param + length The length of source string, excluding the trailing null terminator. + \return The value itself for fluent API. + \post IsString() == true && GetString() == s && GetStringLength() == + length \see SetString(StringRefType) + */ + GenericValue &SetString(const Ch *s, SizeType length) { + return SetString(StringRef(s, length)); + } + + //! Set this value as a string without copying source string. + /*! \param s source string reference + \return The value itself for fluent API. + \post IsString() == true && GetString() == s && GetStringLength() == + s.length + */ + GenericValue &SetString(StringRefType s) { + this->~GenericValue(); + SetStringRaw(s); + return *this; + } + + //! Set this value as a string by copying from source string. + /*! This version has better performance with supplied length, and also support + string containing null character. \param s source string. \param length The + length of source string, excluding the trailing null terminator. \param + allocator Allocator for allocating copied buffer. Commonly use + GenericDocument::GetAllocator(). \return The value itself for fluent API. + \post IsString() == true && GetString() != s && strcmp(GetString(),s) == 0 + && GetStringLength() == length + */ + GenericValue &SetString(const Ch *s, SizeType length, Allocator &allocator) { + return SetString(StringRef(s, length), allocator); + } + + //! Set this value as a string by copying from source string. + /*! \param s source string. + \param allocator Allocator for allocating copied buffer. Commonly use + GenericDocument::GetAllocator(). \return The value itself for fluent API. + \post IsString() == true && GetString() != s && strcmp(GetString(),s) == 0 + && GetStringLength() == length + */ + GenericValue &SetString(const Ch *s, Allocator &allocator) { + return SetString(StringRef(s), allocator); + } + + //! Set this value as a string by copying from source string. + /*! \param s source string reference + \param allocator Allocator for allocating copied buffer. Commonly use + GenericDocument::GetAllocator(). \return The value itself for fluent API. + \post IsString() == true && GetString() != s.s && strcmp(GetString(),s) == + 0 && GetStringLength() == length + */ + GenericValue &SetString(StringRefType s, Allocator &allocator) { + this->~GenericValue(); + SetStringRaw(s, allocator); + return *this; + } + +#if RAPIDJSON_HAS_STDSTRING + //! Set this value as a string by copying from source string. + /*! \param s source string. + \param allocator Allocator for allocating copied buffer. Commonly use + GenericDocument::GetAllocator(). \return The value itself for fluent API. + \post IsString() == true && GetString() != s.data() && + strcmp(GetString(),s.data() == 0 && GetStringLength() == s.size() \note + Requires the definition of the preprocessor symbol \ref + RAPIDJSON_HAS_STDSTRING. + */ + GenericValue &SetString(const std::basic_string &s, + Allocator &allocator) { + return SetString(StringRef(s), allocator); + } +#endif + + //@} + + //!@name Array + //@{ + + //! Templated version for checking whether this value is type T. + /*! + \tparam T Either \c bool, \c int, \c unsigned, \c int64_t, \c uint64_t, \c + double, \c float, \c const \c char*, \c std::basic_string + */ + template + bool Is() const { + return internal::TypeHelper::Is(*this); + } + + template + T Get() const { + return internal::TypeHelper::Get(*this); + } + + template + T Get() { + return internal::TypeHelper::Get(*this); + } + + template + ValueType &Set(const T &data) { + return internal::TypeHelper::Set(*this, data); + } + + template + ValueType &Set(const T &data, AllocatorType &allocator) { + return internal::TypeHelper::Set(*this, data, allocator); + } + + //@} + + //! Generate events of this value to a Handler. + /*! This function adopts the GoF visitor pattern. + Typical usage is to output this JSON value as JSON text via Writer, which + is a Handler. It can also be used to deep clone this value via + GenericDocument, which is also a Handler. \tparam Handler type of handler. + \param handler An object implementing concept Handler. + */ + template + bool Accept(Handler &handler) const { + switch (GetType()) { + case kNullType: + return handler.Null(); + case kFalseType: + return handler.Bool(false); + case kTrueType: + return handler.Bool(true); + + case kObjectType: + if (RAPIDJSON_UNLIKELY(!handler.StartObject())) return false; + for (ConstMemberIterator m = MemberBegin(); m != MemberEnd(); ++m) { + RAPIDJSON_ASSERT(m->name.IsString()); // User may change the type of + // name by MemberIterator. + if (RAPIDJSON_UNLIKELY( + !handler.Key(m->name.GetString(), m->name.GetStringLength(), + (m->name.data_.f.flags & kCopyFlag) != 0))) + return false; + if (RAPIDJSON_UNLIKELY(!m->value.Accept(handler))) return false; + } + return handler.EndObject(data_.o.size); + + case kArrayType: + if (RAPIDJSON_UNLIKELY(!handler.StartArray())) return false; + for (const GenericValue *v = Begin(); v != End(); ++v) + if (RAPIDJSON_UNLIKELY(!v->Accept(handler))) return false; + return handler.EndArray(data_.a.size); + + case kStringType: + return handler.String(GetString(), GetStringLength(), + (data_.f.flags & kCopyFlag) != 0); + + default: + RAPIDJSON_ASSERT(GetType() == kNumberType); + if (IsDouble()) + return handler.Double(data_.n.d); + else if (IsInt()) + return handler.Int(data_.n.i.i); + else if (IsUint()) + return handler.Uint(data_.n.u.u); + else if (IsInt64()) + return handler.Int64(data_.n.i64); + else + return handler.Uint64(data_.n.u64); + } + } + + private: + template + friend class GenericValue; + template + friend class GenericDocument; + + enum { + kBoolFlag = 0x0008, + kNumberFlag = 0x0010, + kIntFlag = 0x0020, + kUintFlag = 0x0040, + kInt64Flag = 0x0080, + kUint64Flag = 0x0100, + kDoubleFlag = 0x0200, + kStringFlag = 0x0400, + kCopyFlag = 0x0800, + kInlineStrFlag = 0x1000, + + // Initial flags of different types. + kNullFlag = kNullType, + kTrueFlag = kTrueType | kBoolFlag, + kFalseFlag = kFalseType | kBoolFlag, + kNumberIntFlag = kNumberType | kNumberFlag | kIntFlag | kInt64Flag, + kNumberUintFlag = + kNumberType | kNumberFlag | kUintFlag | kUint64Flag | kInt64Flag, + kNumberInt64Flag = kNumberType | kNumberFlag | kInt64Flag, + kNumberUint64Flag = kNumberType | kNumberFlag | kUint64Flag, + kNumberDoubleFlag = kNumberType | kNumberFlag | kDoubleFlag, + kNumberAnyFlag = kNumberType | kNumberFlag | kIntFlag | kInt64Flag | + kUintFlag | kUint64Flag | kDoubleFlag, + kConstStringFlag = kStringType | kStringFlag, + kCopyStringFlag = kStringType | kStringFlag | kCopyFlag, + kShortStringFlag = kStringType | kStringFlag | kCopyFlag | kInlineStrFlag, + kObjectFlag = kObjectType, + kArrayFlag = kArrayType, + + kTypeMask = 0x07 + }; + + static const SizeType kDefaultArrayCapacity = 16; + static const SizeType kDefaultObjectCapacity = 16; + + struct Flag { +#if RAPIDJSON_48BITPOINTER_OPTIMIZATION + char payload[sizeof(SizeType) * 2 + + 6]; // 2 x SizeType + lower 48-bit pointer +#elif RAPIDJSON_64BIT + char payload[sizeof(SizeType) * 2 + sizeof(void *) + 6]; // 6 padding bytes +#else + char payload[sizeof(SizeType) * 2 + sizeof(void *) + + 2]; // 2 padding bytes +#endif + uint16_t flags; + }; + + struct String { + SizeType length; + SizeType hashcode; //!< reserved + const Ch *str; + }; // 12 bytes in 32-bit mode, 16 bytes in 64-bit mode + + // implementation detail: ShortString can represent zero-terminated strings up + // to MaxSize chars (excluding the terminating zero) and store a value to + // determine the length of the contained string in the last character + // str[LenPos] by storing "MaxSize - length" there. If the string to store has + // the maximal length of MaxSize then str[LenPos] will be 0 and therefore act + // as the string terminator as well. For getting the string length back from + // that value just use "MaxSize - str[LenPos]". This allows to store 13-chars + // strings in 32-bit mode, 21-chars strings in 64-bit mode, 13-chars strings + // for RAPIDJSON_48BITPOINTER_OPTIMIZATION=1 inline (for `UTF8`-encoded + // strings). + struct ShortString { + enum { + MaxChars = sizeof(static_cast(0)->payload) / sizeof(Ch), + MaxSize = MaxChars - 1, + LenPos = MaxSize + }; + Ch str[MaxChars]; + + inline static bool Usable(SizeType len) { return (MaxSize >= len); } + inline void SetLength(SizeType len) { + str[LenPos] = static_cast(MaxSize - len); + } + inline SizeType GetLength() const { + return static_cast(MaxSize - str[LenPos]); + } + }; // at most as many bytes as "String" above => 12 bytes in 32-bit mode, 16 + // bytes in 64-bit mode + + // By using proper binary layout, retrieval of different integer types do not + // need conversions. + union Number { +#if RAPIDJSON_ENDIAN == RAPIDJSON_LITTLEENDIAN + struct I { + int i; + char padding[4]; + } i; + struct U { + unsigned u; + char padding2[4]; + } u; +#else + struct I { + char padding[4]; + int i; + } i; + struct U { + char padding2[4]; + unsigned u; + } u; +#endif + int64_t i64; + uint64_t u64; + double d; + }; // 8 bytes + + struct ObjectData { + SizeType size; + SizeType capacity; + Member *members; + }; // 12 bytes in 32-bit mode, 16 bytes in 64-bit mode + + struct ArrayData { + SizeType size; + SizeType capacity; + GenericValue *elements; + }; // 12 bytes in 32-bit mode, 16 bytes in 64-bit mode + + union Data { + String s; + ShortString ss; + Number n; + ObjectData o; + ArrayData a; + Flag f; + }; // 16 bytes in 32-bit mode, 24 bytes in 64-bit mode, 16 bytes in 64-bit + // with RAPIDJSON_48BITPOINTER_OPTIMIZATION + + RAPIDJSON_FORCEINLINE const Ch *GetStringPointer() const { + return RAPIDJSON_GETPOINTER(Ch, data_.s.str); + } + RAPIDJSON_FORCEINLINE const Ch *SetStringPointer(const Ch *str) { + return RAPIDJSON_SETPOINTER(Ch, data_.s.str, str); + } + RAPIDJSON_FORCEINLINE GenericValue *GetElementsPointer() const { + return RAPIDJSON_GETPOINTER(GenericValue, data_.a.elements); + } + RAPIDJSON_FORCEINLINE GenericValue *SetElementsPointer( + GenericValue *elements) { + return RAPIDJSON_SETPOINTER(GenericValue, data_.a.elements, elements); + } + RAPIDJSON_FORCEINLINE Member *GetMembersPointer() const { + return RAPIDJSON_GETPOINTER(Member, data_.o.members); + } + RAPIDJSON_FORCEINLINE Member *SetMembersPointer(Member *members) { + return RAPIDJSON_SETPOINTER(Member, data_.o.members, members); + } + + // Initialize this value as array with initial data, without calling + // destructor. + void SetArrayRaw(GenericValue *values, SizeType count, Allocator &allocator) { + data_.f.flags = kArrayFlag; + if (count) { + GenericValue *e = static_cast( + allocator.Malloc(count * sizeof(GenericValue))); + SetElementsPointer(e); + std::memcpy(static_cast(e), values, count * sizeof(GenericValue)); + } else + SetElementsPointer(0); + data_.a.size = data_.a.capacity = count; + } + + //! Initialize this value as object with initial data, without calling + //! destructor. + void SetObjectRaw(Member *members, SizeType count, Allocator &allocator) { + data_.f.flags = kObjectFlag; + if (count) { + Member *m = + static_cast(allocator.Malloc(count * sizeof(Member))); + SetMembersPointer(m); + std::memcpy(static_cast(m), members, count * sizeof(Member)); + } else + SetMembersPointer(0); + data_.o.size = data_.o.capacity = count; + } + + //! Initialize this value as constant string, without calling destructor. + void SetStringRaw(StringRefType s) RAPIDJSON_NOEXCEPT { + data_.f.flags = kConstStringFlag; + SetStringPointer(s); + data_.s.length = s.length; + } + + //! Initialize this value as copy string with initial data, without calling + //! destructor. + void SetStringRaw(StringRefType s, Allocator &allocator) { + Ch *str = 0; + if (ShortString::Usable(s.length)) { + data_.f.flags = kShortStringFlag; + data_.ss.SetLength(s.length); + str = data_.ss.str; + } else { + data_.f.flags = kCopyStringFlag; + data_.s.length = s.length; + str = static_cast(allocator.Malloc((s.length + 1) * sizeof(Ch))); + SetStringPointer(str); + } + std::memcpy(str, s, s.length * sizeof(Ch)); + str[s.length] = '\0'; + } + + //! Assignment without calling destructor + void RawAssign(GenericValue &rhs) RAPIDJSON_NOEXCEPT { + data_ = rhs.data_; + // data_.f.flags = rhs.data_.f.flags; + rhs.data_.f.flags = kNullFlag; + } + + template + bool StringEqual(const GenericValue &rhs) const { + RAPIDJSON_ASSERT(IsString()); + RAPIDJSON_ASSERT(rhs.IsString()); + + const SizeType len1 = GetStringLength(); + const SizeType len2 = rhs.GetStringLength(); + if (len1 != len2) { + return false; + } + + const Ch *const str1 = GetString(); + const Ch *const str2 = rhs.GetString(); + if (str1 == str2) { + return true; + } // fast path for constant string + + return (std::memcmp(str1, str2, sizeof(Ch) * len1) == 0); + } + + Data data_; +}; + +//! GenericValue with UTF8 encoding +typedef GenericValue> Value; + +/////////////////////////////////////////////////////////////////////////////// +// GenericDocument + +//! A document for parsing JSON text as DOM. +/*! + \note implements Handler concept + \tparam Encoding Encoding for both parsing and string storage. + \tparam Allocator Allocator for allocating memory for the DOM + \tparam StackAllocator Allocator for allocating memory for stack during + parsing. \warning Although GenericDocument inherits from GenericValue, the + API does \b not provide any virtual functions, especially no virtual + destructor. To avoid memory leaks, do not \c delete a GenericDocument object + via a pointer to a GenericValue. +*/ +template , + typename StackAllocator = CrtAllocator> +class GenericDocument : public GenericValue { + public: + typedef typename Encoding::Ch Ch; //!< Character type derived from Encoding. + typedef GenericValue + ValueType; //!< Value type of the document. + typedef Allocator AllocatorType; //!< Allocator type from template parameter. + + //! Constructor + /*! Creates an empty document of specified type. + \param type Mandatory type of object to create. + \param allocator Optional allocator for allocating memory. + \param stackCapacity Optional initial capacity of stack in bytes. + \param stackAllocator Optional allocator for allocating memory for + stack. + */ + explicit GenericDocument(Type type, Allocator *allocator = 0, + size_t stackCapacity = kDefaultStackCapacity, + StackAllocator *stackAllocator = 0) + : GenericValue(type), + allocator_(allocator), + ownAllocator_(0), + stack_(stackAllocator, stackCapacity), + parseResult_() { + if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + } + + //! Constructor + /*! Creates an empty document which type is Null. + \param allocator Optional allocator for allocating memory. + \param stackCapacity Optional initial capacity of stack in bytes. + \param stackAllocator Optional allocator for allocating memory for + stack. + */ + GenericDocument(Allocator *allocator = 0, + size_t stackCapacity = kDefaultStackCapacity, + StackAllocator *stackAllocator = 0) + : allocator_(allocator), + ownAllocator_(0), + stack_(stackAllocator, stackCapacity), + parseResult_() { + if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move constructor in C++11 + GenericDocument(GenericDocument &&rhs) RAPIDJSON_NOEXCEPT + : ValueType(std::forward( + rhs)), // explicit cast to avoid prohibited move from Document + allocator_(rhs.allocator_), + ownAllocator_(rhs.ownAllocator_), + stack_(std::move(rhs.stack_)), + parseResult_(rhs.parseResult_) { + rhs.allocator_ = 0; + rhs.ownAllocator_ = 0; + rhs.parseResult_ = ParseResult(); + } +#endif + + ~GenericDocument() { Destroy(); } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move assignment in C++11 + GenericDocument &operator=(GenericDocument &&rhs) RAPIDJSON_NOEXCEPT { + // The cast to ValueType is necessary here, because otherwise it would + // attempt to call GenericValue's templated assignment operator. + ValueType::operator=(std::forward(rhs)); + + // Calling the destructor here would prematurely call stack_'s destructor + Destroy(); + + allocator_ = rhs.allocator_; + ownAllocator_ = rhs.ownAllocator_; + stack_ = std::move(rhs.stack_); + parseResult_ = rhs.parseResult_; + + rhs.allocator_ = 0; + rhs.ownAllocator_ = 0; + rhs.parseResult_ = ParseResult(); + + return *this; + } +#endif + + //! Exchange the contents of this document with those of another. + /*! + \param rhs Another document. + \note Constant complexity. + \see GenericValue::Swap + */ + GenericDocument &Swap(GenericDocument &rhs) RAPIDJSON_NOEXCEPT { + ValueType::Swap(rhs); + stack_.Swap(rhs.stack_); + internal::Swap(allocator_, rhs.allocator_); + internal::Swap(ownAllocator_, rhs.ownAllocator_); + internal::Swap(parseResult_, rhs.parseResult_); + return *this; + } + + // Allow Swap with ValueType. + // Refer to Effective C++ 3rd Edition/Item 33: Avoid hiding inherited names. + using ValueType::Swap; + + //! free-standing swap function helper + /*! + Helper function to enable support for common swap implementation pattern + based on \c std::swap: \code void swap(MyClass& a, MyClass& b) { using + std::swap; swap(a.doc, b.doc); + // ... + } + \endcode + \see Swap() + */ + friend inline void swap(GenericDocument &a, + GenericDocument &b) RAPIDJSON_NOEXCEPT { + a.Swap(b); + } + + //! Populate this document by a generator which produces SAX events. + /*! \tparam Generator A functor with bool f(Handler) prototype. + \param g Generator functor which sends SAX events to the parameter. + \return The document itself for fluent API. + */ + template + GenericDocument &Populate(Generator &g) { + ClearStackOnExit scope(*this); + if (g(*this)) { + RAPIDJSON_ASSERT(stack_.GetSize() == + sizeof(ValueType)); // Got one and only one root object + ValueType::operator=(*stack_.template Pop( + 1)); // Move value from stack to document + } + return *this; + } + + //!@name Parse from stream + //!@{ + + //! Parse JSON text from an input stream (with Encoding conversion) + /*! \tparam parseFlags Combination of \ref ParseFlag. + \tparam SourceEncoding Encoding of input stream + \tparam InputStream Type of input stream, implementing Stream concept + \param is Input stream to be parsed. + \return The document itself for fluent API. + */ + template + GenericDocument &ParseStream(InputStream &is) { + GenericReader reader( + stack_.HasAllocator() ? &stack_.GetAllocator() : 0); + ClearStackOnExit scope(*this); + parseResult_ = reader.template Parse(is, *this); + if (parseResult_) { + RAPIDJSON_ASSERT(stack_.GetSize() == + sizeof(ValueType)); // Got one and only one root object + ValueType::operator=(*stack_.template Pop( + 1)); // Move value from stack to document + } + return *this; + } + + //! Parse JSON text from an input stream + /*! \tparam parseFlags Combination of \ref ParseFlag. + \tparam InputStream Type of input stream, implementing Stream concept + \param is Input stream to be parsed. + \return The document itself for fluent API. + */ + template + GenericDocument &ParseStream(InputStream &is) { + return ParseStream(is); + } + + //! Parse JSON text from an input stream (with \ref kParseDefaultFlags) + /*! \tparam InputStream Type of input stream, implementing Stream concept + \param is Input stream to be parsed. + \return The document itself for fluent API. + */ + template + GenericDocument &ParseStream(InputStream &is) { + return ParseStream(is); + } + //!@} + + //!@name Parse in-place from mutable string + //!@{ + + //! Parse JSON text from a mutable string + /*! \tparam parseFlags Combination of \ref ParseFlag. + \param str Mutable zero-terminated string to be parsed. + \return The document itself for fluent API. + */ + template + GenericDocument &ParseInsitu(Ch *str) { + GenericInsituStringStream s(str); + return ParseStream(s); + } + + //! Parse JSON text from a mutable string (with \ref kParseDefaultFlags) + /*! \param str Mutable zero-terminated string to be parsed. + \return The document itself for fluent API. + */ + GenericDocument &ParseInsitu(Ch *str) { + return ParseInsitu(str); + } + //!@} + + //!@name Parse from read-only string + //!@{ + + //! Parse JSON text from a read-only string (with Encoding conversion) + /*! \tparam parseFlags Combination of \ref ParseFlag (must not contain \ref + kParseInsituFlag). \tparam SourceEncoding Transcoding from input Encoding + \param str Read-only zero-terminated string to be parsed. + */ + template + GenericDocument &Parse(const typename SourceEncoding::Ch *str) { + RAPIDJSON_ASSERT(!(parseFlags & kParseInsituFlag)); + GenericStringStream s(str); + return ParseStream(s); + } + + //! Parse JSON text from a read-only string + /*! \tparam parseFlags Combination of \ref ParseFlag (must not contain \ref + kParseInsituFlag). \param str Read-only zero-terminated string to be + parsed. + */ + template + GenericDocument &Parse(const Ch *str) { + return Parse(str); + } + + //! Parse JSON text from a read-only string (with \ref kParseDefaultFlags) + /*! \param str Read-only zero-terminated string to be parsed. + */ + GenericDocument &Parse(const Ch *str) { + return Parse(str); + } + + template + GenericDocument &Parse(const typename SourceEncoding::Ch *str, + size_t length) { + RAPIDJSON_ASSERT(!(parseFlags & kParseInsituFlag)); + MemoryStream ms(reinterpret_cast(str), + length * sizeof(typename SourceEncoding::Ch)); + EncodedInputStream is(ms); + ParseStream(is); + return *this; + } + + template + GenericDocument &Parse(const Ch *str, size_t length) { + return Parse(str, length); + } + + GenericDocument &Parse(const Ch *str, size_t length) { + return Parse(str, length); + } + +#if RAPIDJSON_HAS_STDSTRING + template + GenericDocument &Parse( + const std::basic_string &str) { + // c_str() is constant complexity according to standard. Should be faster + // than Parse(const char*, size_t) + return Parse(str.c_str()); + } + + template + GenericDocument &Parse(const std::basic_string &str) { + return Parse(str.c_str()); + } + + GenericDocument &Parse(const std::basic_string &str) { + return Parse(str); + } +#endif // RAPIDJSON_HAS_STDSTRING + + //!@} + + //!@name Handling parse errors + //!@{ + + //! Whether a parse error has occurred in the last parsing. + bool HasParseError() const { return parseResult_.IsError(); } + + //! Get the \ref ParseErrorCode of last parsing. + ParseErrorCode GetParseError() const { return parseResult_.Code(); } + + //! Get the position of last parsing error in input, 0 otherwise. + size_t GetErrorOffset() const { return parseResult_.Offset(); } + +//! Implicit conversion to get the last parse result +#ifndef __clang // -Wdocumentation +/*! \return \ref ParseResult of the last parse operation + + \code + Document doc; + ParseResult ok = doc.Parse(json); + if (!ok) + printf( "JSON parse error: %s (%u)\n", GetParseError_En(ok.Code()), + ok.Offset()); \endcode + */ +#endif + operator ParseResult() const { return parseResult_; } + //!@} + + //! Get the allocator of this document. + Allocator &GetAllocator() { + RAPIDJSON_ASSERT(allocator_); + return *allocator_; + } + + //! Get the capacity of stack in bytes. + size_t GetStackCapacity() const { return stack_.GetCapacity(); } + + private: + // clear stack on any exit from ParseStream, e.g. due to exception + struct ClearStackOnExit { + explicit ClearStackOnExit(GenericDocument &d) : d_(d) {} + ~ClearStackOnExit() { d_.ClearStack(); } + + private: + ClearStackOnExit(const ClearStackOnExit &); + ClearStackOnExit &operator=(const ClearStackOnExit &); + GenericDocument &d_; + }; + + // callers of the following private Handler functions + // template friend class GenericReader; // for + // parsing + template + friend class GenericValue; // for deep copying + + public: + // Implementation of Handler + bool Null() { + new (stack_.template Push()) ValueType(); + return true; + } + bool Bool(bool b) { + new (stack_.template Push()) ValueType(b); + return true; + } + bool Int(int i) { + new (stack_.template Push()) ValueType(i); + return true; + } + bool Uint(unsigned i) { + new (stack_.template Push()) ValueType(i); + return true; + } + bool Int64(int64_t i) { + new (stack_.template Push()) ValueType(i); + return true; + } + bool Uint64(uint64_t i) { + new (stack_.template Push()) ValueType(i); + return true; + } + bool Double(double d) { + new (stack_.template Push()) ValueType(d); + return true; + } + + bool RawNumber(const Ch *str, SizeType length, bool copy) { + if (copy) + new (stack_.template Push()) + ValueType(str, length, GetAllocator()); + else + new (stack_.template Push()) ValueType(str, length); + return true; + } + + bool String(const Ch *str, SizeType length, bool copy) { + if (copy) + new (stack_.template Push()) + ValueType(str, length, GetAllocator()); + else + new (stack_.template Push()) ValueType(str, length); + return true; + } + + bool StartObject() { + new (stack_.template Push()) ValueType(kObjectType); + return true; + } + + bool Key(const Ch *str, SizeType length, bool copy) { + return String(str, length, copy); + } + + bool EndObject(SizeType memberCount) { + typename ValueType::Member *members = + stack_.template Pop(memberCount); + stack_.template Top()->SetObjectRaw(members, memberCount, + GetAllocator()); + return true; + } + + bool StartArray() { + new (stack_.template Push()) ValueType(kArrayType); + return true; + } + + bool EndArray(SizeType elementCount) { + ValueType *elements = stack_.template Pop(elementCount); + stack_.template Top()->SetArrayRaw(elements, elementCount, + GetAllocator()); + return true; + } + + private: + //! Prohibit copying + GenericDocument(const GenericDocument &); + //! Prohibit assignment + GenericDocument &operator=(const GenericDocument &); + + void ClearStack() { + if (Allocator::kNeedFree) + while (stack_.GetSize() > + 0) // Here assumes all elements in stack array are GenericValue + // (Member is actually 2 GenericValue objects) + (stack_.template Pop(1))->~ValueType(); + else + stack_.Clear(); + stack_.ShrinkToFit(); + } + + void Destroy() { RAPIDJSON_DELETE(ownAllocator_); } + + static const size_t kDefaultStackCapacity = 1024; + Allocator *allocator_; + Allocator *ownAllocator_; + internal::Stack stack_; + ParseResult parseResult_; +}; + +//! GenericDocument with UTF8 encoding +typedef GenericDocument> Document; + +//! Helper class for accessing Value of array type. +/*! + Instance of this helper class is obtained by \c GenericValue::GetArray(). + In addition to all APIs for array type, it provides range-based for loop if + \c RAPIDJSON_HAS_CXX11_RANGE_FOR=1. +*/ +template +class GenericArray { + public: + typedef GenericArray ConstArray; + typedef GenericArray Array; + typedef ValueT PlainType; + typedef typename internal::MaybeAddConst::Type ValueType; + typedef ValueType *ValueIterator; // This may be const or non-const iterator + typedef const ValueT *ConstValueIterator; + typedef typename ValueType::AllocatorType AllocatorType; + typedef typename ValueType::StringRefType StringRefType; + + template + friend class GenericValue; + + GenericArray(const GenericArray &rhs) : value_(rhs.value_) {} + GenericArray &operator=(const GenericArray &rhs) { + value_ = rhs.value_; + return *this; + } + ~GenericArray() {} + + SizeType Size() const { return value_.Size(); } + SizeType Capacity() const { return value_.Capacity(); } + bool Empty() const { return value_.Empty(); } + void Clear() const { value_.Clear(); } + ValueType &operator[](SizeType index) const { return value_[index]; } + ValueIterator Begin() const { return value_.Begin(); } + ValueIterator End() const { return value_.End(); } + GenericArray Reserve(SizeType newCapacity, AllocatorType &allocator) const { + value_.Reserve(newCapacity, allocator); + return *this; + } + GenericArray PushBack(ValueType &value, AllocatorType &allocator) const { + value_.PushBack(value, allocator); + return *this; + } +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericArray PushBack(ValueType &&value, AllocatorType &allocator) const { + value_.PushBack(value, allocator); + return *this; + } +#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericArray PushBack(StringRefType value, AllocatorType &allocator) const { + value_.PushBack(value, allocator); + return *this; + } + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (const GenericArray &)) + PushBack(T value, AllocatorType &allocator) const { + value_.PushBack(value, allocator); + return *this; + } + GenericArray PopBack() const { + value_.PopBack(); + return *this; + } + ValueIterator Erase(ConstValueIterator pos) const { + return value_.Erase(pos); + } + ValueIterator Erase(ConstValueIterator first, ConstValueIterator last) const { + return value_.Erase(first, last); + } + +#if RAPIDJSON_HAS_CXX11_RANGE_FOR + ValueIterator begin() const { return value_.Begin(); } + ValueIterator end() const { return value_.End(); } +#endif + + private: + GenericArray(); + GenericArray(ValueType &value) : value_(value) {} + ValueType &value_; +}; + +//! Helper class for accessing Value of object type. +/*! + Instance of this helper class is obtained by \c GenericValue::GetObject(). + In addition to all APIs for array type, it provides range-based for loop if + \c RAPIDJSON_HAS_CXX11_RANGE_FOR=1. +*/ +template +class GenericObject { + public: + typedef GenericObject ConstObject; + typedef GenericObject Object; + typedef ValueT PlainType; + typedef typename internal::MaybeAddConst::Type ValueType; + typedef GenericMemberIterator + MemberIterator; // This may be const or non-const iterator + typedef GenericMemberIterator + ConstMemberIterator; + typedef typename ValueType::AllocatorType AllocatorType; + typedef typename ValueType::StringRefType StringRefType; + typedef typename ValueType::EncodingType EncodingType; + typedef typename ValueType::Ch Ch; + + template + friend class GenericValue; + + GenericObject(const GenericObject &rhs) : value_(rhs.value_) {} + GenericObject &operator=(const GenericObject &rhs) { + value_ = rhs.value_; + return *this; + } + ~GenericObject() {} + + SizeType MemberCount() const { return value_.MemberCount(); } + SizeType MemberCapacity() const { return value_.MemberCapacity(); } + bool ObjectEmpty() const { return value_.ObjectEmpty(); } + template + ValueType &operator[](T *name) const { + return value_[name]; + } + template + ValueType &operator[]( + const GenericValue &name) const { + return value_[name]; + } +#if RAPIDJSON_HAS_STDSTRING + ValueType &operator[](const std::basic_string &name) const { + return value_[name]; + } +#endif + MemberIterator MemberBegin() const { return value_.MemberBegin(); } + MemberIterator MemberEnd() const { return value_.MemberEnd(); } + GenericObject MemberReserve(SizeType newCapacity, + AllocatorType &allocator) const { + value_.MemberReserve(newCapacity, allocator); + return *this; + } + bool HasMember(const Ch *name) const { return value_.HasMember(name); } +#if RAPIDJSON_HAS_STDSTRING + bool HasMember(const std::basic_string &name) const { + return value_.HasMember(name); + } +#endif + template + bool HasMember( + const GenericValue &name) const { + return value_.HasMember(name); + } + MemberIterator FindMember(const Ch *name) const { + return value_.FindMember(name); + } + template + MemberIterator FindMember( + const GenericValue &name) const { + return value_.FindMember(name); + } +#if RAPIDJSON_HAS_STDSTRING + MemberIterator FindMember(const std::basic_string &name) const { + return value_.FindMember(name); + } +#endif + GenericObject AddMember(ValueType &name, ValueType &value, + AllocatorType &allocator) const { + value_.AddMember(name, value, allocator); + return *this; + } + GenericObject AddMember(ValueType &name, StringRefType value, + AllocatorType &allocator) const { + value_.AddMember(name, value, allocator); + return *this; + } +#if RAPIDJSON_HAS_STDSTRING + GenericObject AddMember(ValueType &name, std::basic_string &value, + AllocatorType &allocator) const { + value_.AddMember(name, value, allocator); + return *this; + } +#endif + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (ValueType &)) + AddMember(ValueType &name, T value, AllocatorType &allocator) const { + value_.AddMember(name, value, allocator); + return *this; + } +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericObject AddMember(ValueType &&name, ValueType &&value, + AllocatorType &allocator) const { + value_.AddMember(name, value, allocator); + return *this; + } + GenericObject AddMember(ValueType &&name, ValueType &value, + AllocatorType &allocator) const { + value_.AddMember(name, value, allocator); + return *this; + } + GenericObject AddMember(ValueType &name, ValueType &&value, + AllocatorType &allocator) const { + value_.AddMember(name, value, allocator); + return *this; + } + GenericObject AddMember(StringRefType name, ValueType &&value, + AllocatorType &allocator) const { + value_.AddMember(name, value, allocator); + return *this; + } +#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericObject AddMember(StringRefType name, ValueType &value, + AllocatorType &allocator) const { + value_.AddMember(name, value, allocator); + return *this; + } + GenericObject AddMember(StringRefType name, StringRefType value, + AllocatorType &allocator) const { + value_.AddMember(name, value, allocator); + return *this; + } + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (GenericObject)) + AddMember(StringRefType name, T value, AllocatorType &allocator) const { + value_.AddMember(name, value, allocator); + return *this; + } + void RemoveAllMembers() { value_.RemoveAllMembers(); } + bool RemoveMember(const Ch *name) const { return value_.RemoveMember(name); } +#if RAPIDJSON_HAS_STDSTRING + bool RemoveMember(const std::basic_string &name) const { + return value_.RemoveMember(name); + } +#endif + template + bool RemoveMember( + const GenericValue &name) const { + return value_.RemoveMember(name); + } + MemberIterator RemoveMember(MemberIterator m) const { + return value_.RemoveMember(m); + } + MemberIterator EraseMember(ConstMemberIterator pos) const { + return value_.EraseMember(pos); + } + MemberIterator EraseMember(ConstMemberIterator first, + ConstMemberIterator last) const { + return value_.EraseMember(first, last); + } + bool EraseMember(const Ch *name) const { return value_.EraseMember(name); } +#if RAPIDJSON_HAS_STDSTRING + bool EraseMember(const std::basic_string &name) const { + return EraseMember(ValueType(StringRef(name))); + } +#endif + template + bool EraseMember( + const GenericValue &name) const { + return value_.EraseMember(name); + } + +#if RAPIDJSON_HAS_CXX11_RANGE_FOR + MemberIterator begin() const { return value_.MemberBegin(); } + MemberIterator end() const { return value_.MemberEnd(); } +#endif + + private: + GenericObject(); + GenericObject(ValueType &value) : value_(value) {} + ValueType &value_; +}; + +RAPIDJSON_NAMESPACE_END +RAPIDJSON_DIAG_POP + +#endif // RAPIDJSON_DOCUMENT_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/encodedstream.h b/src/livox_ros_driver2/3rdparty/rapidjson/encodedstream.h new file mode 100644 index 0000000..74ef4ca --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/encodedstream.h @@ -0,0 +1,407 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_ENCODEDSTREAM_H_ +#define RAPIDJSON_ENCODEDSTREAM_H_ + +#include "memorystream.h" +#include "stream.h" + +#ifdef __GNUC__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(effc++) +#endif + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(padded) +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +//! Input byte stream wrapper with a statically bound encoding. +/*! + \tparam Encoding The interpretation of encoding of the stream. Either UTF8, + UTF16LE, UTF16BE, UTF32LE, UTF32BE. \tparam InputByteStream Type of input + byte stream. For example, FileReadStream. +*/ +template +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, MemoryStream> { + public: + typedef UTF8<>::Ch Ch; + + EncodedInputStream(MemoryStream &is) : is_(is) { + if (static_cast(is_.Peek()) == 0xEFu) is_.Take(); + if (static_cast(is_.Peek()) == 0xBBu) is_.Take(); + if (static_cast(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 +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::x, UTF16LE::x, UTF16BE::x, UTF32LE::x, UTF32BE::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 +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(is_->Peek4()); + if (!c) return; + + unsigned bom = + static_cast(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 +class AutoUTFOutputStream { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + + public: + typedef CharType Ch; + + //! Constructor. + /*! + \param os output stream to be wrapped. + \param type UTF encoding type. + \param putBOM Whether to write BOM at the beginning of the stream. + */ + AutoUTFOutputStream(OutputByteStream &os, UTFType type, bool putBOM) + : os_(&os), type_(type) { + RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE); + + // Runtime check whether the size of character type is sufficient. It only + // perform checks with assertion. + if (type_ == kUTF16LE || type_ == kUTF16BE) + RAPIDJSON_ASSERT(sizeof(Ch) >= 2); + if (type_ == kUTF32LE || type_ == kUTF32BE) + RAPIDJSON_ASSERT(sizeof(Ch) >= 4); + + static const PutFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Put)}; + putFunc_ = f[type_]; + + if (putBOM) PutBOM(); + } + + UTFType GetType() const { return type_; } + + void Put(Ch c) { putFunc_(*os_, c); } + void Flush() { os_->Flush(); } + + // Not implemented + Ch Peek() const { + RAPIDJSON_ASSERT(false); + return 0; + } + Ch Take() { + RAPIDJSON_ASSERT(false); + return 0; + } + size_t Tell() const { + RAPIDJSON_ASSERT(false); + return 0; + } + Ch *PutBegin() { + RAPIDJSON_ASSERT(false); + return 0; + } + size_t PutEnd(Ch *) { + RAPIDJSON_ASSERT(false); + return 0; + } + + private: + AutoUTFOutputStream(const AutoUTFOutputStream &); + AutoUTFOutputStream &operator=(const AutoUTFOutputStream &); + + void PutBOM() { + typedef void (*PutBOMFunc)(OutputByteStream &); + static const PutBOMFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(PutBOM)}; + f[type_](*os_); + } + + typedef void (*PutFunc)(OutputByteStream &, Ch); + + OutputByteStream *os_; + UTFType type_; + PutFunc putFunc_; +}; + +#undef RAPIDJSON_ENCODINGS_FUNC + +RAPIDJSON_NAMESPACE_END + +#ifdef __clang__ +RAPIDJSON_DIAG_POP +#endif + +#ifdef __GNUC__ +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_FILESTREAM_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/encodings.h b/src/livox_ros_driver2/3rdparty/rapidjson/encodings.h new file mode 100644 index 0000000..34521cc --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/encodings.h @@ -0,0 +1,816 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_ENCODINGS_H_ +#define RAPIDJSON_ENCODINGS_H_ + +#include "rapidjson.h" + +#if defined(_MSC_VER) && !defined(__clang__) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF( + 4244) // conversion from 'type1' to 'type2', possible loss of data +RAPIDJSON_DIAG_OFF(4702) // unreachable code +#elif defined(__GNUC__) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(effc++) +RAPIDJSON_DIAG_OFF(overflow) +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +/////////////////////////////////////////////////////////////////////////////// +// Encoding + +/*! \class rapidjson::Encoding + \brief Concept for encoding of Unicode characters. + +\code +concept Encoding { + typename Ch; //! Type of character. A "character" is actually a code unit +in unicode's definition. + + enum { supportUnicode = 1 }; // or 0 if not supporting unicode + + //! \brief Encode a Unicode codepoint to an output stream. + //! \param os Output stream. + //! \param codepoint An unicode codepoint, ranging from 0x0 to 0x10FFFF +inclusively. template 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 + 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 + 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 + static CharType TakeBOM(InputByteStream& is); + + //! Take a character from input byte stream. + template + static Ch Take(InputByteStream& is); + + //! Put BOM to output byte stream. + template + static void PutBOM(OutputByteStream& os); + + //! Put a character to output byte stream. + template + 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 +struct UTF8 { + typedef CharType Ch; + + enum { supportUnicode = 1 }; + + template + static void Encode(OutputStream &os, unsigned codepoint) { + if (codepoint <= 0x7F) + os.Put(static_cast(codepoint & 0xFF)); + else if (codepoint <= 0x7FF) { + os.Put(static_cast(0xC0 | ((codepoint >> 6) & 0xFF))); + os.Put(static_cast(0x80 | ((codepoint & 0x3F)))); + } else if (codepoint <= 0xFFFF) { + os.Put(static_cast(0xE0 | ((codepoint >> 12) & 0xFF))); + os.Put(static_cast(0x80 | ((codepoint >> 6) & 0x3F))); + os.Put(static_cast(0x80 | (codepoint & 0x3F))); + } else { + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + os.Put(static_cast(0xF0 | ((codepoint >> 18) & 0xFF))); + os.Put(static_cast(0x80 | ((codepoint >> 12) & 0x3F))); + os.Put(static_cast(0x80 | ((codepoint >> 6) & 0x3F))); + os.Put(static_cast(0x80 | (codepoint & 0x3F))); + } + } + + template + static void EncodeUnsafe(OutputStream &os, unsigned codepoint) { + if (codepoint <= 0x7F) + PutUnsafe(os, static_cast(codepoint & 0xFF)); + else if (codepoint <= 0x7FF) { + PutUnsafe(os, static_cast(0xC0 | ((codepoint >> 6) & 0xFF))); + PutUnsafe(os, static_cast(0x80 | ((codepoint & 0x3F)))); + } else if (codepoint <= 0xFFFF) { + PutUnsafe(os, static_cast(0xE0 | ((codepoint >> 12) & 0xFF))); + PutUnsafe(os, static_cast(0x80 | ((codepoint >> 6) & 0x3F))); + PutUnsafe(os, static_cast(0x80 | (codepoint & 0x3F))); + } else { + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + PutUnsafe(os, static_cast(0xF0 | ((codepoint >> 18) & 0xFF))); + PutUnsafe(os, static_cast(0x80 | ((codepoint >> 12) & 0x3F))); + PutUnsafe(os, static_cast(0x80 | ((codepoint >> 6) & 0x3F))); + PutUnsafe(os, static_cast(0x80 | (codepoint & 0x3F))); + } + } + + template + static bool Decode(InputStream &is, unsigned *codepoint) { +#define RAPIDJSON_COPY() \ + c = is.Take(); \ + *codepoint = (*codepoint << 6) | (static_cast(c) & 0x3Fu) +#define RAPIDJSON_TRANS(mask) \ + result &= ((GetRange(static_cast(c)) & mask) != 0) +#define RAPIDJSON_TAIL() \ + RAPIDJSON_COPY(); \ + RAPIDJSON_TRANS(0x70) + typename InputStream::Ch c = is.Take(); + if (!(c & 0x80)) { + *codepoint = static_cast(c); + return true; + } + + unsigned char type = GetRange(static_cast(c)); + if (type >= 32) { + *codepoint = 0; + } else { + *codepoint = (0xFFu >> type) & static_cast(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 + static bool Validate(InputStream &is, OutputStream &os) { +#define RAPIDJSON_COPY() os.Put(c = is.Take()) +#define RAPIDJSON_TRANS(mask) \ + result &= ((GetRange(static_cast(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(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 + static CharType TakeBOM(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + typename InputByteStream::Ch c = Take(is); + if (static_cast(c) != 0xEFu) return c; + c = is.Take(); + if (static_cast(c) != 0xBBu) return c; + c = is.Take(); + if (static_cast(c) != 0xBFu) return c; + c = is.Take(); + return c; + } + + template + static Ch Take(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + return static_cast(is.Take()); + } + + template + static void PutBOM(OutputByteStream &os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(0xEFu)); + os.Put(static_cast(0xBBu)); + os.Put(static_cast(0xBFu)); + } + + template + static void Put(OutputByteStream &os, Ch c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(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 +struct UTF16 { + typedef CharType Ch; + RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 2); + + enum { supportUnicode = 1 }; + + template + 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(codepoint)); + } else { + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + unsigned v = codepoint - 0x10000; + os.Put(static_cast((v >> 10) | 0xD800)); + os.Put(static_cast((v & 0x3FF) | 0xDC00)); + } + } + + template + 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(codepoint)); + } else { + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + unsigned v = codepoint - 0x10000; + PutUnsafe(os, static_cast((v >> 10) | 0xD800)); + PutUnsafe(os, + static_cast((v & 0x3FF) | 0xDC00)); + } + } + + template + 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(c); + return true; + } else if (c <= 0xDBFF) { + *codepoint = (static_cast(c) & 0x3FF) << 10; + c = is.Take(); + *codepoint |= (static_cast(c) & 0x3FF); + *codepoint += 0x10000; + return c >= 0xDC00 && c <= 0xDFFF; + } + return false; + } + + template + 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(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 +struct UTF16LE : UTF16 { + template + static CharType TakeBOM(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + CharType c = Take(is); + return static_cast(c) == 0xFEFFu ? Take(is) : c; + } + + template + static CharType Take(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + unsigned c = static_cast(is.Take()); + c |= static_cast(static_cast(is.Take())) << 8; + return static_cast(c); + } + + template + static void PutBOM(OutputByteStream &os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(0xFFu)); + os.Put(static_cast(0xFEu)); + } + + template + static void Put(OutputByteStream &os, CharType c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(static_cast(c) & + 0xFFu)); + os.Put(static_cast( + (static_cast(c) >> 8) & 0xFFu)); + } +}; + +//! UTF-16 big endian encoding. +template +struct UTF16BE : UTF16 { + template + static CharType TakeBOM(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + CharType c = Take(is); + return static_cast(c) == 0xFEFFu ? Take(is) : c; + } + + template + static CharType Take(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + unsigned c = static_cast(static_cast(is.Take())) << 8; + c |= static_cast(static_cast(is.Take())); + return static_cast(c); + } + + template + static void PutBOM(OutputByteStream &os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(0xFEu)); + os.Put(static_cast(0xFFu)); + } + + template + static void Put(OutputByteStream &os, CharType c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast( + (static_cast(c) >> 8) & 0xFFu)); + os.Put(static_cast(static_cast(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 +struct UTF32 { + typedef CharType Ch; + RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 4); + + enum { supportUnicode = 1 }; + + template + static void Encode(OutputStream &os, unsigned codepoint) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4); + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + os.Put(codepoint); + } + + template + static void EncodeUnsafe(OutputStream &os, unsigned codepoint) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4); + RAPIDJSON_ASSERT(codepoint <= 0x10FFFF); + PutUnsafe(os, codepoint); + } + + template + 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 + 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 +struct UTF32LE : UTF32 { + template + static CharType TakeBOM(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + CharType c = Take(is); + return static_cast(c) == 0x0000FEFFu ? Take(is) : c; + } + + template + static CharType Take(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + unsigned c = static_cast(is.Take()); + c |= static_cast(static_cast(is.Take())) << 8; + c |= static_cast(static_cast(is.Take())) << 16; + c |= static_cast(static_cast(is.Take())) << 24; + return static_cast(c); + } + + template + static void PutBOM(OutputByteStream &os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(0xFFu)); + os.Put(static_cast(0xFEu)); + os.Put(static_cast(0x00u)); + os.Put(static_cast(0x00u)); + } + + template + static void Put(OutputByteStream &os, CharType c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(c & 0xFFu)); + os.Put(static_cast((c >> 8) & 0xFFu)); + os.Put(static_cast((c >> 16) & 0xFFu)); + os.Put(static_cast((c >> 24) & 0xFFu)); + } +}; + +//! UTF-32 big endian encoding. +template +struct UTF32BE : UTF32 { + template + static CharType TakeBOM(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + CharType c = Take(is); + return static_cast(c) == 0x0000FEFFu ? Take(is) : c; + } + + template + static CharType Take(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + unsigned c = static_cast(static_cast(is.Take())) << 24; + c |= static_cast(static_cast(is.Take())) << 16; + c |= static_cast(static_cast(is.Take())) << 8; + c |= static_cast(static_cast(is.Take())); + return static_cast(c); + } + + template + static void PutBOM(OutputByteStream &os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(0x00u)); + os.Put(static_cast(0x00u)); + os.Put(static_cast(0xFEu)); + os.Put(static_cast(0xFFu)); + } + + template + static void Put(OutputByteStream &os, CharType c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast((c >> 24) & 0xFFu)); + os.Put(static_cast((c >> 16) & 0xFFu)); + os.Put(static_cast((c >> 8) & 0xFFu)); + os.Put(static_cast(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 +struct ASCII { + typedef CharType Ch; + + enum { supportUnicode = 0 }; + + template + static void Encode(OutputStream &os, unsigned codepoint) { + RAPIDJSON_ASSERT(codepoint <= 0x7F); + os.Put(static_cast(codepoint & 0xFF)); + } + + template + static void EncodeUnsafe(OutputStream &os, unsigned codepoint) { + RAPIDJSON_ASSERT(codepoint <= 0x7F); + PutUnsafe(os, static_cast(codepoint & 0xFF)); + } + + template + static bool Decode(InputStream &is, unsigned *codepoint) { + uint8_t c = static_cast(is.Take()); + *codepoint = c; + return c <= 0X7F; + } + + template + static bool Validate(InputStream &is, OutputStream &os) { + uint8_t c = static_cast(is.Take()); + os.Put(static_cast(c)); + return c <= 0x7F; + } + + template + static CharType TakeBOM(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + uint8_t c = static_cast(Take(is)); + return static_cast(c); + } + + template + static Ch Take(InputByteStream &is) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1); + return static_cast(is.Take()); + } + + template + static void PutBOM(OutputByteStream &os) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + (void)os; + } + + template + static void Put(OutputByteStream &os, Ch c) { + RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1); + os.Put(static_cast(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 +struct AutoUTF { + typedef CharType Ch; + + enum { supportUnicode = 1 }; + +#define RAPIDJSON_ENCODINGS_FUNC(x) \ + UTF8::x, UTF16LE::x, UTF16BE::x, UTF32LE::x, UTF32BE::x + + template + 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 + 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 + 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 + 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 +struct Transcoder { + //! Take one Unicode codepoint from source encoding, convert it to target + //! encoding and put it to the output stream. + template + 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 + 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 + static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is, + OutputStream &os) { + return Transcode( + is, os); // Since source/target encoding is different, must transcode. + } +}; + +// Forward declaration. +template +inline void PutUnsafe(Stream &stream, typename Stream::Ch c); + +//! Specialization of Transcoder with same source and target encoding. +template +struct Transcoder { + template + 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 + 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 + static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is, + OutputStream &os) { + return Encoding::Validate(is, os); // source/target encoding are the same + } +}; + +RAPIDJSON_NAMESPACE_END + +#if defined(__GNUC__) || (defined(_MSC_VER) && !defined(__clang__)) +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_ENCODINGS_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/error/en.h b/src/livox_ros_driver2/3rdparty/rapidjson/error/en.h new file mode 100644 index 0000000..a08145d --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/error/en.h @@ -0,0 +1,104 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_ERROR_EN_H_ +#define RAPIDJSON_ERROR_EN_H_ + +#include "error.h" + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(switch - enum) +RAPIDJSON_DIAG_OFF(covered - switch - default) +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +//! Maps error code of parsing into error message. +/*! + \ingroup RAPIDJSON_ERRORS + \param parseErrorCode Error code obtained in parsing. + \return the error message. + \note User can make a copy of this function for localization. + Using switch-case is safer for future modification of error codes. +*/ +inline const RAPIDJSON_ERROR_CHARTYPE* GetParseError_En( + ParseErrorCode parseErrorCode) { + switch (parseErrorCode) { + case kParseErrorNone: + return RAPIDJSON_ERROR_STRING("No error."); + + case kParseErrorDocumentEmpty: + return RAPIDJSON_ERROR_STRING("The document is empty."); + case kParseErrorDocumentRootNotSingular: + return RAPIDJSON_ERROR_STRING( + "The document root must not be followed by other values."); + + case kParseErrorValueInvalid: + return RAPIDJSON_ERROR_STRING("Invalid value."); + + case kParseErrorObjectMissName: + return RAPIDJSON_ERROR_STRING("Missing a name for object member."); + case kParseErrorObjectMissColon: + return RAPIDJSON_ERROR_STRING( + "Missing a colon after a name of object member."); + case kParseErrorObjectMissCommaOrCurlyBracket: + return RAPIDJSON_ERROR_STRING( + "Missing a comma or '}' after an object member."); + + case kParseErrorArrayMissCommaOrSquareBracket: + return RAPIDJSON_ERROR_STRING( + "Missing a comma or ']' after an array element."); + + case kParseErrorStringUnicodeEscapeInvalidHex: + return RAPIDJSON_ERROR_STRING( + "Incorrect hex digit after \\u escape in string."); + case kParseErrorStringUnicodeSurrogateInvalid: + return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid."); + case kParseErrorStringEscapeInvalid: + return RAPIDJSON_ERROR_STRING("Invalid escape character in string."); + case kParseErrorStringMissQuotationMark: + return RAPIDJSON_ERROR_STRING( + "Missing a closing quotation mark in string."); + case kParseErrorStringInvalidEncoding: + return RAPIDJSON_ERROR_STRING("Invalid encoding in string."); + + case kParseErrorNumberTooBig: + return RAPIDJSON_ERROR_STRING("Number too big to be stored in double."); + case kParseErrorNumberMissFraction: + return RAPIDJSON_ERROR_STRING("Miss fraction part in number."); + case kParseErrorNumberMissExponent: + return RAPIDJSON_ERROR_STRING("Miss exponent in number."); + + case kParseErrorTermination: + return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error."); + case kParseErrorUnspecificSyntaxError: + return RAPIDJSON_ERROR_STRING("Unspecific syntax error."); + + default: + return RAPIDJSON_ERROR_STRING("Unknown error."); + } +} + +RAPIDJSON_NAMESPACE_END + +#ifdef __clang__ +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_ERROR_EN_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/error/error.h b/src/livox_ros_driver2/3rdparty/rapidjson/error/error.h new file mode 100644 index 0000000..4814b69 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/error/error.h @@ -0,0 +1,186 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_ERROR_ERROR_H_ +#define RAPIDJSON_ERROR_ERROR_H_ + +#include "../rapidjson.h" + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(padded) +#endif + +/*! \file error.h */ + +/*! \defgroup RAPIDJSON_ERRORS RapidJSON error handling */ + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_ERROR_CHARTYPE + +//! Character type of error messages. +/*! \ingroup RAPIDJSON_ERRORS + The default character type is \c char. + On Windows, user can define this macro as \c TCHAR for supporting both + unicode/non-unicode settings. +*/ +#ifndef RAPIDJSON_ERROR_CHARTYPE +#define RAPIDJSON_ERROR_CHARTYPE char +#endif + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_ERROR_STRING + +//! Macro for converting string literial to \ref RAPIDJSON_ERROR_CHARTYPE[]. +/*! \ingroup RAPIDJSON_ERRORS + By default this conversion macro does nothing. + On Windows, user can define this macro as \c _T(x) for supporting both + unicode/non-unicode settings. +*/ +#ifndef RAPIDJSON_ERROR_STRING +#define RAPIDJSON_ERROR_STRING(x) x +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +/////////////////////////////////////////////////////////////////////////////// +// ParseErrorCode + +//! Error code of parsing. +/*! \ingroup RAPIDJSON_ERRORS + \see GenericReader::Parse, GenericReader::GetParseErrorCode +*/ +enum ParseErrorCode { + kParseErrorNone = 0, //!< No error. + + kParseErrorDocumentEmpty, //!< The document is empty. + kParseErrorDocumentRootNotSingular, //!< The document root must not follow by + //!< other values. + + kParseErrorValueInvalid, //!< Invalid value. + + kParseErrorObjectMissName, //!< Missing a name for object member. + kParseErrorObjectMissColon, //!< Missing a colon after a name of object + //!< member. + kParseErrorObjectMissCommaOrCurlyBracket, //!< Missing a comma or '}' after + //!an + //!< object member. + + kParseErrorArrayMissCommaOrSquareBracket, //!< Missing a comma or ']' after + //!an + //!< array element. + + kParseErrorStringUnicodeEscapeInvalidHex, //!< Incorrect hex digit after \\u + //!< escape in string. + kParseErrorStringUnicodeSurrogateInvalid, //!< The surrogate pair in string + //!is + //!< invalid. + kParseErrorStringEscapeInvalid, //!< Invalid escape character in string. + kParseErrorStringMissQuotationMark, //!< Missing a closing quotation mark in + //!< string. + kParseErrorStringInvalidEncoding, //!< Invalid encoding in string. + + kParseErrorNumberTooBig, //!< Number too big to be stored in double. + kParseErrorNumberMissFraction, //!< Miss fraction part in number. + kParseErrorNumberMissExponent, //!< Miss exponent in number. + + kParseErrorTermination, //!< Parsing was terminated. + kParseErrorUnspecificSyntaxError //!< Unspecific syntax error. +}; + +//! Result of parsing (wraps ParseErrorCode) +/*! + \ingroup RAPIDJSON_ERRORS + \code + Document doc; + ParseResult ok = doc.Parse("[42]"); + if (!ok) { + fprintf(stderr, "JSON parse error: %s (%u)", + GetParseError_En(ok.Code()), ok.Offset()); + exit(EXIT_FAILURE); + } + \endcode + \see GenericReader::Parse, GenericDocument::Parse +*/ +struct ParseResult { + //!! Unspecified boolean type + typedef bool (ParseResult::*BooleanType)() const; + + public: + //! Default constructor, no error. + ParseResult() : code_(kParseErrorNone), offset_(0) {} + //! Constructor to set an error. + ParseResult(ParseErrorCode code, size_t offset) + : code_(code), offset_(offset) {} + + //! Get the error code. + ParseErrorCode Code() const { return code_; } + //! Get the error offset, if \ref IsError(), 0 otherwise. + size_t Offset() const { return offset_; } + + //! Explicit conversion to \c bool, returns \c true, iff !\ref IsError(). + operator BooleanType() const { + return !IsError() ? &ParseResult::IsError : NULL; + } + //! Whether the result is an error. + bool IsError() const { return code_ != kParseErrorNone; } + + bool operator==(const ParseResult &that) const { return code_ == that.code_; } + bool operator==(ParseErrorCode code) const { return code_ == code; } + friend bool operator==(ParseErrorCode code, const ParseResult &err) { + return code == err.code_; + } + + bool operator!=(const ParseResult &that) const { return !(*this == that); } + bool operator!=(ParseErrorCode code) const { return !(*this == code); } + friend bool operator!=(ParseErrorCode code, const ParseResult &err) { + return err != code; + } + + //! Reset error code. + void Clear() { Set(kParseErrorNone); } + //! Update error code and offset. + void Set(ParseErrorCode code, size_t offset = 0) { + code_ = code; + offset_ = offset; + } + + private: + ParseErrorCode code_; + size_t offset_; +}; + +//! Function pointer type of GetParseError(). +/*! \ingroup RAPIDJSON_ERRORS + + This is the prototype for \c GetParseError_X(), where \c X is a locale. + User can dynamically change locale in runtime, e.g.: +\code + GetParseErrorFunc GetParseError = GetParseError_En; // or whatever + const RAPIDJSON_ERROR_CHARTYPE* s = +GetParseError(document.GetParseErrorCode()); \endcode +*/ +typedef const RAPIDJSON_ERROR_CHARTYPE *(*GetParseErrorFunc)(ParseErrorCode); + +RAPIDJSON_NAMESPACE_END + +#ifdef __clang__ +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_ERROR_ERROR_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/filereadstream.h b/src/livox_ros_driver2/3rdparty/rapidjson/filereadstream.h new file mode 100644 index 0000000..250aabb --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/filereadstream.h @@ -0,0 +1,123 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_FILEREADSTREAM_H_ +#define RAPIDJSON_FILEREADSTREAM_H_ + +#include +#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(current_ - buffer_); + } + + // Not implemented + void Put(Ch) { RAPIDJSON_ASSERT(false); } + void Flush() { RAPIDJSON_ASSERT(false); } + Ch *PutBegin() { + RAPIDJSON_ASSERT(false); + return 0; + } + size_t PutEnd(Ch *) { + RAPIDJSON_ASSERT(false); + return 0; + } + + // For encoding detection only. + const Ch *Peek4() const { + return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0; + } + + private: + void Read() { + if (current_ < bufferLast_) + ++current_; + else if (!eof_) { + count_ += readCount_; + readCount_ = std::fread(buffer_, 1, bufferSize_, fp_); + bufferLast_ = buffer_ + readCount_ - 1; + current_ = buffer_; + + if (readCount_ < bufferSize_) { + buffer_[readCount_] = '\0'; + ++bufferLast_; + eof_ = true; + } + } + } + + std::FILE *fp_; + Ch *buffer_; + size_t bufferSize_; + Ch *bufferLast_; + Ch *current_; + size_t readCount_; + size_t count_; //!< Number of characters read + bool eof_; +}; + +RAPIDJSON_NAMESPACE_END + +#ifdef __clang__ +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_FILESTREAM_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/filewritestream.h b/src/livox_ros_driver2/3rdparty/rapidjson/filewritestream.h new file mode 100644 index 0000000..f6defc1 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/filewritestream.h @@ -0,0 +1,128 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_FILEWRITESTREAM_H_ +#define RAPIDJSON_FILEWRITESTREAM_H_ + +#include +#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(bufferEnd_ - current_); + while (n > avail) { + std::memset(current_, c, avail); + current_ += avail; + Flush(); + n -= avail; + avail = static_cast(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(current_ - buffer_), fp_); + if (result < static_cast(current_ - buffer_)) { + // failure deliberately ignored at this time + // added to avoid warn_unused_result build errors + } + current_ = buffer_; + } + } + + // Not implemented + char Peek() const { + RAPIDJSON_ASSERT(false); + return 0; + } + char Take() { + RAPIDJSON_ASSERT(false); + return 0; + } + size_t Tell() const { + RAPIDJSON_ASSERT(false); + return 0; + } + char *PutBegin() { + RAPIDJSON_ASSERT(false); + return 0; + } + size_t PutEnd(char *) { + RAPIDJSON_ASSERT(false); + return 0; + } + + private: + // Prohibit copy constructor & assignment operator. + FileWriteStream(const FileWriteStream &); + FileWriteStream &operator=(const FileWriteStream &); + + std::FILE *fp_; + char *buffer_; + char *bufferEnd_; + char *current_; +}; + +//! Implement specialized version of PutN() with memset() for better +//! performance. +template <> +inline void PutN(FileWriteStream &stream, char c, size_t n) { + stream.PutN(c, n); +} + +RAPIDJSON_NAMESPACE_END + +#ifdef __clang__ +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_FILESTREAM_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/fwd.h b/src/livox_ros_driver2/3rdparty/rapidjson/fwd.h new file mode 100644 index 0000000..cafb48e --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/fwd.h @@ -0,0 +1,170 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_FWD_H_ +#define RAPIDJSON_FWD_H_ + +#include "rapidjson.h" + +RAPIDJSON_NAMESPACE_BEGIN + +// encodings.h + +template +struct UTF8; +template +struct UTF16; +template +struct UTF16BE; +template +struct UTF16LE; +template +struct UTF32; +template +struct UTF32BE; +template +struct UTF32LE; +template +struct ASCII; +template +struct AutoUTF; + +template +struct Transcoder; + +// allocators.h + +class CrtAllocator; + +template +class MemoryPoolAllocator; + +// stream.h + +template +struct GenericStringStream; + +typedef GenericStringStream> StringStream; + +template +struct GenericInsituStringStream; + +typedef GenericInsituStringStream> InsituStringStream; + +// stringbuffer.h + +template +class GenericStringBuffer; + +typedef GenericStringBuffer, CrtAllocator> StringBuffer; + +// filereadstream.h + +class FileReadStream; + +// filewritestream.h + +class FileWriteStream; + +// memorybuffer.h + +template +struct GenericMemoryBuffer; + +typedef GenericMemoryBuffer MemoryBuffer; + +// memorystream.h + +struct MemoryStream; + +// reader.h + +template +struct BaseReaderHandler; + +template +class GenericReader; + +typedef GenericReader, UTF8, CrtAllocator> Reader; + +// writer.h + +template +class Writer; + +// prettywriter.h + +template +class PrettyWriter; + +// document.h + +template +class GenericMember; + +template +class GenericMemberIterator; + +template +struct GenericStringRef; + +template +class GenericValue; + +typedef GenericValue, MemoryPoolAllocator> Value; + +template +class GenericDocument; + +typedef GenericDocument, MemoryPoolAllocator, + CrtAllocator> + Document; + +// pointer.h + +template +class GenericPointer; + +typedef GenericPointer Pointer; + +// schema.h + +template +class IGenericRemoteSchemaDocumentProvider; + +template +class GenericSchemaDocument; + +typedef GenericSchemaDocument SchemaDocument; +typedef IGenericRemoteSchemaDocumentProvider + IRemoteSchemaDocumentProvider; + +template +class GenericSchemaValidator; + +typedef GenericSchemaValidator< + SchemaDocument, BaseReaderHandler, void>, CrtAllocator> + SchemaValidator; + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_RAPIDJSONFWD_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/biginteger.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/biginteger.h new file mode 100644 index 0000000..b60e006 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/biginteger.h @@ -0,0 +1,295 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_BIGINTEGER_H_ +#define RAPIDJSON_BIGINTEGER_H_ + +#include "../rapidjson.h" + +#if defined(_MSC_VER) && !__INTEL_COMPILER && defined(_M_AMD64) +#include // 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(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(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(*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(a) * static_cast(b); + p += k; + *outHigh = static_cast(p >> 64); + return static_cast(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(1) << 32); + uint64_t lo = (x1 << 32) + (x0 & 0xFFFFFFFF); + uint64_t hi = x3 + (x1 >> 32); + + lo += k; + if (lo < k) hi++; + *outHigh = hi; + return lo; +#endif + } + + static const size_t kBitCount = 3328; // 64bit * 54 > 10^1000 + static const size_t kCapacity = kBitCount / sizeof(Type); + static const size_t kTypeBit = sizeof(Type) * 8; + + Type digits_[kCapacity]; + size_t count_; +}; + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_BIGINTEGER_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/clzll.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/clzll.h new file mode 100644 index 0000000..8cb8b8a --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/clzll.h @@ -0,0 +1,77 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_CLZLL_H_ +#define RAPIDJSON_CLZLL_H_ + +#include "../rapidjson.h" + +#if defined(_MSC_VER) +#include +#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(x >> 32))) return 63 - (r + 32); + + // Scan the low 32 bits. + _BitScanReverse(&r, static_cast(x & 0xFFFFFFFF)); +#endif // _WIN64 + + return 63 - r; +#else + uint32_t r; + while (!(x & (static_cast(1) << 63))) { + x <<= 1; + ++r; + } + + return r; +#endif // _MSC_VER +} + +#define RAPIDJSON_CLZLL RAPIDJSON_NAMESPACE::internal::clzll +#endif // (defined(__GNUC__) && __GNUC__ >= 4) || + // RAPIDJSON_HAS_BUILTIN(__builtin_clzll) + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_CLZLL_H_ \ No newline at end of file diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/diyfp.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/diyfp.h new file mode 100644 index 0000000..bed2989 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/diyfp.h @@ -0,0 +1,305 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +// This is a C++ header-only implementation of Grisu2 algorithm from the +// publication: Loitsch, Florian. "Printing floating-point numbers quickly and +// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243. + +#ifndef RAPIDJSON_DIYFP_H_ +#define RAPIDJSON_DIYFP_H_ + +#include +#include "../rapidjson.h" +#include "clzll.h" + +#if defined(_MSC_VER) && defined(_M_AMD64) && !defined(__INTEL_COMPILER) +#include +#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((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(f) * static_cast(rhs.f); + uint64_t h = static_cast(p >> 64); + uint64_t l = static_cast(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(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::infinity(); + } + const uint64_t be = (e == kDpDenormalExponent && (f & kDpHiddenBit) == 0) + ? 0 + : static_cast(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(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(dk); + if (dk - k > 0.0) k++; + + unsigned index = static_cast((k >> 3) + 1); + *K = -(-348 + static_cast( + 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(exp + 348) / 8u; + *outExp = -348 + static_cast(index) * 8; + return GetCachedPowerByIndex(index); +} + +#ifdef __GNUC__ +RAPIDJSON_DIAG_POP +#endif + +#ifdef __clang__ +RAPIDJSON_DIAG_POP +RAPIDJSON_DIAG_OFF(padded) +#endif + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_DIYFP_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/dtoa.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/dtoa.h new file mode 100644 index 0000000..b64a79d --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/dtoa.h @@ -0,0 +1,269 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +// This is a C++ header-only implementation of Grisu2 algorithm from the +// publication: Loitsch, Florian. "Printing floating-point numbers quickly and +// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243. + +#ifndef RAPIDJSON_DTOA_ +#define RAPIDJSON_DTOA_ + +#include "diyfp.h" +#include "ieee754.h" +#include "itoa.h" // GetDigitsLut() + +RAPIDJSON_NAMESPACE_BEGIN +namespace internal { + +#ifdef __GNUC__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(effc++) +RAPIDJSON_DIAG_OFF(array - bounds) // some gcc versions generate wrong warnings +// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59124 +#endif + +inline void GrisuRound(char *buffer, int len, uint64_t delta, uint64_t rest, + uint64_t ten_kappa, uint64_t wp_w) { + while (rest < wp_w && delta - rest >= ten_kappa && + (rest + ten_kappa < wp_w || /// closer + wp_w - rest > rest + ten_kappa - wp_w)) { + buffer[len - 1]--; + rest += ten_kappa; + } +} + +inline int CountDecimalDigit32(uint32_t n) { + // Simple pure C++ implementation was faster than __builtin_clz version in + // this situation. + if (n < 10) return 1; + if (n < 100) return 2; + if (n < 1000) return 3; + if (n < 10000) return 4; + if (n < 100000) return 5; + if (n < 1000000) return 6; + if (n < 10000000) return 7; + if (n < 100000000) return 8; + // Will not reach 10 digits in DigitGen() + // if (n < 1000000000) return 9; + // return 10; + return 9; +} + +inline void DigitGen(const DiyFp &W, const DiyFp &Mp, uint64_t delta, + char *buffer, int *len, int *K) { + static const uint32_t kPow10[] = {1, 10, 100, 1000, + 10000, 100000, 1000000, 10000000, + 100000000, 1000000000}; + const DiyFp one(uint64_t(1) << -Mp.e, Mp.e); + const DiyFp wp_w = Mp - W; + uint32_t p1 = static_cast(Mp.f >> -one.e); + uint64_t p2 = Mp.f & (one.f - 1); + int kappa = CountDecimalDigit32(p1); // kappa in [0, 9] + *len = 0; + + while (kappa > 0) { + uint32_t d = 0; + switch (kappa) { + case 9: + d = p1 / 100000000; + p1 %= 100000000; + break; + case 8: + d = p1 / 10000000; + p1 %= 10000000; + break; + case 7: + d = p1 / 1000000; + p1 %= 1000000; + break; + case 6: + d = p1 / 100000; + p1 %= 100000; + break; + case 5: + d = p1 / 10000; + p1 %= 10000; + break; + case 4: + d = p1 / 1000; + p1 %= 1000; + break; + case 3: + d = p1 / 100; + p1 %= 100; + break; + case 2: + d = p1 / 10; + p1 %= 10; + break; + case 1: + d = p1; + p1 = 0; + break; + default:; + } + if (d || *len) + buffer[(*len)++] = static_cast('0' + static_cast(d)); + kappa--; + uint64_t tmp = (static_cast(p1) << -one.e) + p2; + if (tmp <= delta) { + *K += kappa; + GrisuRound(buffer, *len, delta, tmp, + static_cast(kPow10[kappa]) << -one.e, wp_w.f); + return; + } + } + + // kappa = 0 + for (;;) { + p2 *= 10; + delta *= 10; + char d = static_cast(p2 >> -one.e); + if (d || *len) buffer[(*len)++] = static_cast('0' + d); + p2 &= one.f - 1; + kappa--; + if (p2 < delta) { + *K += kappa; + int index = -kappa; + GrisuRound(buffer, *len, delta, p2, one.f, + wp_w.f * (index < 9 ? kPow10[index] : 0)); + return; + } + } +} + +inline void Grisu2(double value, char *buffer, int *length, int *K) { + const DiyFp v(value); + DiyFp w_m, w_p; + v.NormalizedBoundaries(&w_m, &w_p); + + const DiyFp c_mk = GetCachedPower(w_p.e, K); + const DiyFp W = v.Normalize() * c_mk; + DiyFp Wp = w_p * c_mk; + DiyFp Wm = w_m * c_mk; + Wm.f++; + Wp.f--; + DigitGen(W, Wp, Wp.f - Wm.f, buffer, length, K); +} + +inline char *WriteExponent(int K, char *buffer) { + if (K < 0) { + *buffer++ = '-'; + K = -K; + } + + if (K >= 100) { + *buffer++ = static_cast('0' + static_cast(K / 100)); + K %= 100; + const char *d = GetDigitsLut() + K * 2; + *buffer++ = d[0]; + *buffer++ = d[1]; + } else if (K >= 10) { + const char *d = GetDigitsLut() + K * 2; + *buffer++ = d[0]; + *buffer++ = d[1]; + } else + *buffer++ = static_cast('0' + static_cast(K)); + + return buffer; +} + +inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) { + const int kk = length + k; // 10^(kk-1) <= v < 10^kk + + if (0 <= k && kk <= 21) { + // 1234e7 -> 12340000000 + for (int i = length; i < kk; i++) buffer[i] = '0'; + buffer[kk] = '.'; + buffer[kk + 1] = '0'; + return &buffer[kk + 2]; + } else if (0 < kk && kk <= 21) { + // 1234e-2 -> 12.34 + std::memmove(&buffer[kk + 1], &buffer[kk], + static_cast(length - kk)); + buffer[kk] = '.'; + if (0 > k + maxDecimalPlaces) { + // When maxDecimalPlaces = 2, 1.2345 -> 1.23, 1.102 -> 1.1 + // Remove extra trailing zeros (at least one) after truncation. + for (int i = kk + maxDecimalPlaces; i > kk + 1; i--) + if (buffer[i] != '0') return &buffer[i + 1]; + return &buffer[kk + 2]; // Reserve one zero + } else + return &buffer[length + 1]; + } else if (-6 < kk && kk <= 0) { + // 1234e-6 -> 0.001234 + const int offset = 2 - kk; + std::memmove(&buffer[offset], &buffer[0], static_cast(length)); + buffer[0] = '0'; + buffer[1] = '.'; + for (int i = 2; i < offset; i++) buffer[i] = '0'; + if (length - kk > maxDecimalPlaces) { + // When maxDecimalPlaces = 2, 0.123 -> 0.12, 0.102 -> 0.1 + // Remove extra trailing zeros (at least one) after truncation. + for (int i = maxDecimalPlaces + 1; i > 2; i--) + if (buffer[i] != '0') return &buffer[i + 1]; + return &buffer[3]; // Reserve one zero + } else + return &buffer[length + offset]; + } else if (kk < -maxDecimalPlaces) { + // Truncate to zero + buffer[0] = '0'; + buffer[1] = '.'; + buffer[2] = '0'; + return &buffer[3]; + } else if (length == 1) { + // 1e30 + buffer[1] = 'e'; + return WriteExponent(kk - 1, &buffer[2]); + } else { + // 1234e30 -> 1.234e33 + std::memmove(&buffer[2], &buffer[1], static_cast(length - 1)); + buffer[1] = '.'; + buffer[length + 1] = 'e'; + return WriteExponent(kk - 1, &buffer[0 + length + 2]); + } +} + +inline char *dtoa(double value, char *buffer, int maxDecimalPlaces = 324) { + RAPIDJSON_ASSERT(maxDecimalPlaces >= 1); + Double d(value); + if (d.IsZero()) { + if (d.Sign()) *buffer++ = '-'; // -0.0, Issue #289 + buffer[0] = '0'; + buffer[1] = '.'; + buffer[2] = '0'; + return &buffer[3]; + } else { + if (value < 0) { + *buffer++ = '-'; + value = -value; + } + int length, K; + Grisu2(value, buffer, &length, &K); + return Prettify(buffer, length, K, maxDecimalPlaces); + } +} + +#ifdef __GNUC__ +RAPIDJSON_DIAG_POP +#endif + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_DTOA_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/ieee754.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/ieee754.h new file mode 100644 index 0000000..246c4ac --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/ieee754.h @@ -0,0 +1,100 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_IEEE754_ +#define RAPIDJSON_IEEE754_ + +#include "../rapidjson.h" + +RAPIDJSON_NAMESPACE_BEGIN +namespace internal { + +class Double { + public: + Double() {} + Double(double d) : d_(d) {} + Double(uint64_t u) : u_(u) {} + + double Value() const { return d_; } + uint64_t Uint64Value() const { return u_; } + + double NextPositiveDouble() const { + RAPIDJSON_ASSERT(!Sign()); + return Double(u_ + 1).Value(); + } + + bool Sign() const { return (u_ & kSignMask) != 0; } + uint64_t Significand() const { return u_ & kSignificandMask; } + int Exponent() const { + return static_cast(((u_ & kExponentMask) >> kSignificandSize) - + kExponentBias); + } + + bool IsNan() const { + return (u_ & kExponentMask) == kExponentMask && Significand() != 0; + } + bool IsInf() const { + return (u_ & kExponentMask) == kExponentMask && Significand() == 0; + } + bool IsNanOrInf() const { return (u_ & kExponentMask) == kExponentMask; } + bool IsNormal() const { + return (u_ & kExponentMask) != 0 || Significand() == 0; + } + bool IsZero() const { return (u_ & (kExponentMask | kSignificandMask)) == 0; } + + uint64_t IntegerSignificand() const { + return IsNormal() ? Significand() | kHiddenBit : Significand(); + } + int IntegerExponent() const { + return (IsNormal() ? Exponent() : kDenormalExponent) - kSignificandSize; + } + uint64_t ToBias() const { + return (u_ & kSignMask) ? ~u_ + 1 : u_ | kSignMask; + } + + static int EffectiveSignificandSize(int order) { + if (order >= -1021) + return 53; + else if (order <= -1074) + return 0; + else + return order + 1074; + } + + private: + static const int kSignificandSize = 52; + static const int kExponentBias = 0x3FF; + static const int kDenormalExponent = 1 - kExponentBias; + static const uint64_t kSignMask = RAPIDJSON_UINT64_C2(0x80000000, 0x00000000); + static const uint64_t kExponentMask = + RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000); + static const uint64_t kSignificandMask = + RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF); + static const uint64_t kHiddenBit = + RAPIDJSON_UINT64_C2(0x00100000, 0x00000000); + + union { + double d_; + uint64_t u_; + }; +}; + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_IEEE754_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/itoa.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/itoa.h new file mode 100644 index 0000000..ec1174c --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/itoa.h @@ -0,0 +1,288 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_ITOA_ +#define RAPIDJSON_ITOA_ + +#include "../rapidjson.h" + +RAPIDJSON_NAMESPACE_BEGIN +namespace internal { + +inline const char *GetDigitsLut() { + static const char cDigitsLut[200] = { + '0', '0', '0', '1', '0', '2', '0', '3', '0', '4', '0', '5', '0', '6', '0', + '7', '0', '8', '0', '9', '1', '0', '1', '1', '1', '2', '1', '3', '1', '4', + '1', '5', '1', '6', '1', '7', '1', '8', '1', '9', '2', '0', '2', '1', '2', + '2', '2', '3', '2', '4', '2', '5', '2', '6', '2', '7', '2', '8', '2', '9', + '3', '0', '3', '1', '3', '2', '3', '3', '3', '4', '3', '5', '3', '6', '3', + '7', '3', '8', '3', '9', '4', '0', '4', '1', '4', '2', '4', '3', '4', '4', + '4', '5', '4', '6', '4', '7', '4', '8', '4', '9', '5', '0', '5', '1', '5', + '2', '5', '3', '5', '4', '5', '5', '5', '6', '5', '7', '5', '8', '5', '9', + '6', '0', '6', '1', '6', '2', '6', '3', '6', '4', '6', '5', '6', '6', '6', + '7', '6', '8', '6', '9', '7', '0', '7', '1', '7', '2', '7', '3', '7', '4', + '7', '5', '7', '6', '7', '7', '7', '8', '7', '9', '8', '0', '8', '1', '8', + '2', '8', '3', '8', '4', '8', '5', '8', '6', '8', '7', '8', '8', '8', '9', + '9', '0', '9', '1', '9', '2', '9', '3', '9', '4', '9', '5', '9', '6', '9', + '7', '9', '8', '9', '9'}; + return cDigitsLut; +} + +inline char *u32toa(uint32_t value, char *buffer) { + RAPIDJSON_ASSERT(buffer != 0); + + const char *cDigitsLut = GetDigitsLut(); + + if (value < 10000) { + const uint32_t d1 = (value / 100) << 1; + const uint32_t d2 = (value % 100) << 1; + + if (value >= 1000) *buffer++ = cDigitsLut[d1]; + if (value >= 100) *buffer++ = cDigitsLut[d1 + 1]; + if (value >= 10) *buffer++ = cDigitsLut[d2]; + *buffer++ = cDigitsLut[d2 + 1]; + } else if (value < 100000000) { + // value = bbbbcccc + const uint32_t b = value / 10000; + const uint32_t c = value % 10000; + + const uint32_t d1 = (b / 100) << 1; + const uint32_t d2 = (b % 100) << 1; + + const uint32_t d3 = (c / 100) << 1; + const uint32_t d4 = (c % 100) << 1; + + if (value >= 10000000) *buffer++ = cDigitsLut[d1]; + if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1]; + if (value >= 100000) *buffer++ = cDigitsLut[d2]; + *buffer++ = cDigitsLut[d2 + 1]; + + *buffer++ = cDigitsLut[d3]; + *buffer++ = cDigitsLut[d3 + 1]; + *buffer++ = cDigitsLut[d4]; + *buffer++ = cDigitsLut[d4 + 1]; + } else { + // value = aabbbbcccc in decimal + + const uint32_t a = value / 100000000; // 1 to 42 + value %= 100000000; + + if (a >= 10) { + const unsigned i = a << 1; + *buffer++ = cDigitsLut[i]; + *buffer++ = cDigitsLut[i + 1]; + } else + *buffer++ = static_cast('0' + static_cast(a)); + + const uint32_t b = value / 10000; // 0 to 9999 + const uint32_t c = value % 10000; // 0 to 9999 + + const uint32_t d1 = (b / 100) << 1; + const uint32_t d2 = (b % 100) << 1; + + const uint32_t d3 = (c / 100) << 1; + const uint32_t d4 = (c % 100) << 1; + + *buffer++ = cDigitsLut[d1]; + *buffer++ = cDigitsLut[d1 + 1]; + *buffer++ = cDigitsLut[d2]; + *buffer++ = cDigitsLut[d2 + 1]; + *buffer++ = cDigitsLut[d3]; + *buffer++ = cDigitsLut[d3 + 1]; + *buffer++ = cDigitsLut[d4]; + *buffer++ = cDigitsLut[d4 + 1]; + } + return buffer; +} + +inline char *i32toa(int32_t value, char *buffer) { + RAPIDJSON_ASSERT(buffer != 0); + uint32_t u = static_cast(value); + if (value < 0) { + *buffer++ = '-'; + u = ~u + 1; + } + + return u32toa(u, buffer); +} + +inline char *u64toa(uint64_t value, char *buffer) { + RAPIDJSON_ASSERT(buffer != 0); + const char *cDigitsLut = GetDigitsLut(); + const uint64_t kTen8 = 100000000; + const uint64_t kTen9 = kTen8 * 10; + const uint64_t kTen10 = kTen8 * 100; + const uint64_t kTen11 = kTen8 * 1000; + const uint64_t kTen12 = kTen8 * 10000; + const uint64_t kTen13 = kTen8 * 100000; + const uint64_t kTen14 = kTen8 * 1000000; + const uint64_t kTen15 = kTen8 * 10000000; + const uint64_t kTen16 = kTen8 * kTen8; + + if (value < kTen8) { + uint32_t v = static_cast(value); + if (v < 10000) { + const uint32_t d1 = (v / 100) << 1; + const uint32_t d2 = (v % 100) << 1; + + if (v >= 1000) *buffer++ = cDigitsLut[d1]; + if (v >= 100) *buffer++ = cDigitsLut[d1 + 1]; + if (v >= 10) *buffer++ = cDigitsLut[d2]; + *buffer++ = cDigitsLut[d2 + 1]; + } else { + // value = bbbbcccc + const uint32_t b = v / 10000; + const uint32_t c = v % 10000; + + const uint32_t d1 = (b / 100) << 1; + const uint32_t d2 = (b % 100) << 1; + + const uint32_t d3 = (c / 100) << 1; + const uint32_t d4 = (c % 100) << 1; + + if (value >= 10000000) *buffer++ = cDigitsLut[d1]; + if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1]; + if (value >= 100000) *buffer++ = cDigitsLut[d2]; + *buffer++ = cDigitsLut[d2 + 1]; + + *buffer++ = cDigitsLut[d3]; + *buffer++ = cDigitsLut[d3 + 1]; + *buffer++ = cDigitsLut[d4]; + *buffer++ = cDigitsLut[d4 + 1]; + } + } else if (value < kTen16) { + const uint32_t v0 = static_cast(value / kTen8); + const uint32_t v1 = static_cast(value % kTen8); + + const uint32_t b0 = v0 / 10000; + const uint32_t c0 = v0 % 10000; + + const uint32_t d1 = (b0 / 100) << 1; + const uint32_t d2 = (b0 % 100) << 1; + + const uint32_t d3 = (c0 / 100) << 1; + const uint32_t d4 = (c0 % 100) << 1; + + const uint32_t b1 = v1 / 10000; + const uint32_t c1 = v1 % 10000; + + const uint32_t d5 = (b1 / 100) << 1; + const uint32_t d6 = (b1 % 100) << 1; + + const uint32_t d7 = (c1 / 100) << 1; + const uint32_t d8 = (c1 % 100) << 1; + + if (value >= kTen15) *buffer++ = cDigitsLut[d1]; + if (value >= kTen14) *buffer++ = cDigitsLut[d1 + 1]; + if (value >= kTen13) *buffer++ = cDigitsLut[d2]; + if (value >= kTen12) *buffer++ = cDigitsLut[d2 + 1]; + if (value >= kTen11) *buffer++ = cDigitsLut[d3]; + if (value >= kTen10) *buffer++ = cDigitsLut[d3 + 1]; + if (value >= kTen9) *buffer++ = cDigitsLut[d4]; + + *buffer++ = cDigitsLut[d4 + 1]; + *buffer++ = cDigitsLut[d5]; + *buffer++ = cDigitsLut[d5 + 1]; + *buffer++ = cDigitsLut[d6]; + *buffer++ = cDigitsLut[d6 + 1]; + *buffer++ = cDigitsLut[d7]; + *buffer++ = cDigitsLut[d7 + 1]; + *buffer++ = cDigitsLut[d8]; + *buffer++ = cDigitsLut[d8 + 1]; + } else { + const uint32_t a = static_cast(value / kTen16); // 1 to 1844 + value %= kTen16; + + if (a < 10) + *buffer++ = static_cast('0' + static_cast(a)); + else if (a < 100) { + const uint32_t i = a << 1; + *buffer++ = cDigitsLut[i]; + *buffer++ = cDigitsLut[i + 1]; + } else if (a < 1000) { + *buffer++ = static_cast('0' + static_cast(a / 100)); + + const uint32_t i = (a % 100) << 1; + *buffer++ = cDigitsLut[i]; + *buffer++ = cDigitsLut[i + 1]; + } else { + const uint32_t i = (a / 100) << 1; + const uint32_t j = (a % 100) << 1; + *buffer++ = cDigitsLut[i]; + *buffer++ = cDigitsLut[i + 1]; + *buffer++ = cDigitsLut[j]; + *buffer++ = cDigitsLut[j + 1]; + } + + const uint32_t v0 = static_cast(value / kTen8); + const uint32_t v1 = static_cast(value % kTen8); + + const uint32_t b0 = v0 / 10000; + const uint32_t c0 = v0 % 10000; + + const uint32_t d1 = (b0 / 100) << 1; + const uint32_t d2 = (b0 % 100) << 1; + + const uint32_t d3 = (c0 / 100) << 1; + const uint32_t d4 = (c0 % 100) << 1; + + const uint32_t b1 = v1 / 10000; + const uint32_t c1 = v1 % 10000; + + const uint32_t d5 = (b1 / 100) << 1; + const uint32_t d6 = (b1 % 100) << 1; + + const uint32_t d7 = (c1 / 100) << 1; + const uint32_t d8 = (c1 % 100) << 1; + + *buffer++ = cDigitsLut[d1]; + *buffer++ = cDigitsLut[d1 + 1]; + *buffer++ = cDigitsLut[d2]; + *buffer++ = cDigitsLut[d2 + 1]; + *buffer++ = cDigitsLut[d3]; + *buffer++ = cDigitsLut[d3 + 1]; + *buffer++ = cDigitsLut[d4]; + *buffer++ = cDigitsLut[d4 + 1]; + *buffer++ = cDigitsLut[d5]; + *buffer++ = cDigitsLut[d5 + 1]; + *buffer++ = cDigitsLut[d6]; + *buffer++ = cDigitsLut[d6 + 1]; + *buffer++ = cDigitsLut[d7]; + *buffer++ = cDigitsLut[d7 + 1]; + *buffer++ = cDigitsLut[d8]; + *buffer++ = cDigitsLut[d8 + 1]; + } + + return buffer; +} + +inline char *i64toa(int64_t value, char *buffer) { + RAPIDJSON_ASSERT(buffer != 0); + uint64_t u = static_cast(value); + if (value < 0) { + *buffer++ = '-'; + u = ~u + 1; + } + + return u64toa(u, buffer); +} + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_ITOA_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/meta.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/meta.h new file mode 100644 index 0000000..598f576 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/meta.h @@ -0,0 +1,243 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_INTERNAL_META_H_ +#define RAPIDJSON_INTERNAL_META_H_ + +#include "../rapidjson.h" + +#ifdef __GNUC__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(effc++) +#endif + +#if defined(_MSC_VER) && !defined(__clang__) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(6334) +#endif + +#if RAPIDJSON_HAS_CXX11_TYPETRAITS +#include +#endif + +//@cond RAPIDJSON_INTERNAL +RAPIDJSON_NAMESPACE_BEGIN +namespace internal { + +// Helper to wrap/convert arbitrary types to void, useful for arbitrary type +// matching +template +struct Void { + typedef void Type; +}; + +/////////////////////////////////////////////////////////////////////////////// +// BoolType, TrueType, FalseType +// +template +struct BoolType { + static const bool Value = Cond; + typedef BoolType Type; +}; +typedef BoolType TrueType; +typedef BoolType FalseType; + +/////////////////////////////////////////////////////////////////////////////// +// SelectIf, BoolExpr, NotExpr, AndExpr, OrExpr +// + +template +struct SelectIfImpl { + template + struct Apply { + typedef T1 Type; + }; +}; +template <> +struct SelectIfImpl { + template + struct Apply { + typedef T2 Type; + }; +}; +template +struct SelectIfCond : SelectIfImpl::template Apply {}; +template +struct SelectIf : SelectIfCond {}; + +template +struct AndExprCond : FalseType {}; +template <> +struct AndExprCond : TrueType {}; +template +struct OrExprCond : TrueType {}; +template <> +struct OrExprCond : FalseType {}; + +template +struct BoolExpr : SelectIf::Type {}; +template +struct NotExpr : SelectIf::Type {}; +template +struct AndExpr : AndExprCond::Type {}; +template +struct OrExpr : OrExprCond::Type {}; + +/////////////////////////////////////////////////////////////////////////////// +// AddConst, MaybeAddConst, RemoveConst +template +struct AddConst { + typedef const T Type; +}; +template +struct MaybeAddConst : SelectIfCond {}; +template +struct RemoveConst { + typedef T Type; +}; +template +struct RemoveConst { + typedef T Type; +}; + +/////////////////////////////////////////////////////////////////////////////// +// IsSame, IsConst, IsMoreConst, IsPointer +// +template +struct IsSame : FalseType {}; +template +struct IsSame : TrueType {}; + +template +struct IsConst : FalseType {}; +template +struct IsConst : TrueType {}; + +template +struct IsMoreConst + : AndExpr< + IsSame::Type, typename RemoveConst::Type>, + BoolType::Value >= IsConst::Value>>::Type {}; + +template +struct IsPointer : FalseType {}; +template +struct IsPointer : TrueType {}; + +/////////////////////////////////////////////////////////////////////////////// +// IsBaseOf +// +#if RAPIDJSON_HAS_CXX11_TYPETRAITS + +template +struct IsBaseOf : BoolType<::std::is_base_of::value> {}; + +#else // simplified version adopted from Boost + +template +struct IsBaseOfImpl { + RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0); + RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0); + + typedef char (&Yes)[1]; + typedef char (&No)[2]; + + template + static Yes Check(const D *, T); + static No Check(const B *, int); + + struct Host { + operator const B *() const; + operator const D *(); + }; + + enum { Value = (sizeof(Check(Host(), 0)) == sizeof(Yes)) }; +}; + +template +struct IsBaseOf : OrExpr, BoolExpr>>::Type {}; + +#endif // RAPIDJSON_HAS_CXX11_TYPETRAITS + +////////////////////////////////////////////////////////////////////////// +// EnableIf / DisableIf +// +template +struct EnableIfCond { + typedef T Type; +}; +template +struct EnableIfCond { /* empty */ +}; + +template +struct DisableIfCond { + typedef T Type; +}; +template +struct DisableIfCond { /* empty */ +}; + +template +struct EnableIf : EnableIfCond {}; + +template +struct DisableIf : DisableIfCond {}; + +// SFINAE helpers +struct SfinaeTag {}; +template +struct RemoveSfinaeTag; +template +struct RemoveSfinaeTag { + typedef T Type; +}; + +#define RAPIDJSON_REMOVEFPTR_(type) \ + typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag< \ + ::RAPIDJSON_NAMESPACE::internal::SfinaeTag &(*)type>::Type + +#define RAPIDJSON_ENABLEIF(cond) \ + typename ::RAPIDJSON_NAMESPACE::internal::EnableIf::Type * = NULL + +#define RAPIDJSON_DISABLEIF(cond) \ + typename ::RAPIDJSON_NAMESPACE::internal::DisableIf::Type * = NULL + +#define RAPIDJSON_ENABLEIF_RETURN(cond, returntype) \ + typename ::RAPIDJSON_NAMESPACE::internal::EnableIf< \ + RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type + +#define RAPIDJSON_DISABLEIF_RETURN(cond, returntype) \ + typename ::RAPIDJSON_NAMESPACE::internal::DisableIf< \ + RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type + +} // namespace internal +RAPIDJSON_NAMESPACE_END +//@endcond + +#if defined(_MSC_VER) && !defined(__clang__) +RAPIDJSON_DIAG_POP +#endif + +#ifdef __GNUC__ +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_INTERNAL_META_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/pow10.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/pow10.h new file mode 100644 index 0000000..6f15f74 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/pow10.h @@ -0,0 +1,77 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_POW10_ +#define RAPIDJSON_POW10_ + +#include "../rapidjson.h" + +RAPIDJSON_NAMESPACE_BEGIN +namespace internal { + +//! Computes integer powers of 10 in double (10.0^n). +/*! This function uses lookup table for fast and accurate results. + \param n non-negative exponent. Must <= 308. + \return 10.0^n +*/ +inline double Pow10(int n) { + static const double e[] = { + // 1e-0...1e308: 309 * 8 bytes = 2472 bytes + 1e+0, 1e+1, 1e+2, 1e+3, 1e+4, 1e+5, 1e+6, 1e+7, 1e+8, + 1e+9, 1e+10, 1e+11, 1e+12, 1e+13, 1e+14, 1e+15, 1e+16, 1e+17, + 1e+18, 1e+19, 1e+20, 1e+21, 1e+22, 1e+23, 1e+24, 1e+25, 1e+26, + 1e+27, 1e+28, 1e+29, 1e+30, 1e+31, 1e+32, 1e+33, 1e+34, 1e+35, + 1e+36, 1e+37, 1e+38, 1e+39, 1e+40, 1e+41, 1e+42, 1e+43, 1e+44, + 1e+45, 1e+46, 1e+47, 1e+48, 1e+49, 1e+50, 1e+51, 1e+52, 1e+53, + 1e+54, 1e+55, 1e+56, 1e+57, 1e+58, 1e+59, 1e+60, 1e+61, 1e+62, + 1e+63, 1e+64, 1e+65, 1e+66, 1e+67, 1e+68, 1e+69, 1e+70, 1e+71, + 1e+72, 1e+73, 1e+74, 1e+75, 1e+76, 1e+77, 1e+78, 1e+79, 1e+80, + 1e+81, 1e+82, 1e+83, 1e+84, 1e+85, 1e+86, 1e+87, 1e+88, 1e+89, + 1e+90, 1e+91, 1e+92, 1e+93, 1e+94, 1e+95, 1e+96, 1e+97, 1e+98, + 1e+99, 1e+100, 1e+101, 1e+102, 1e+103, 1e+104, 1e+105, 1e+106, 1e+107, + 1e+108, 1e+109, 1e+110, 1e+111, 1e+112, 1e+113, 1e+114, 1e+115, 1e+116, + 1e+117, 1e+118, 1e+119, 1e+120, 1e+121, 1e+122, 1e+123, 1e+124, 1e+125, + 1e+126, 1e+127, 1e+128, 1e+129, 1e+130, 1e+131, 1e+132, 1e+133, 1e+134, + 1e+135, 1e+136, 1e+137, 1e+138, 1e+139, 1e+140, 1e+141, 1e+142, 1e+143, + 1e+144, 1e+145, 1e+146, 1e+147, 1e+148, 1e+149, 1e+150, 1e+151, 1e+152, + 1e+153, 1e+154, 1e+155, 1e+156, 1e+157, 1e+158, 1e+159, 1e+160, 1e+161, + 1e+162, 1e+163, 1e+164, 1e+165, 1e+166, 1e+167, 1e+168, 1e+169, 1e+170, + 1e+171, 1e+172, 1e+173, 1e+174, 1e+175, 1e+176, 1e+177, 1e+178, 1e+179, + 1e+180, 1e+181, 1e+182, 1e+183, 1e+184, 1e+185, 1e+186, 1e+187, 1e+188, + 1e+189, 1e+190, 1e+191, 1e+192, 1e+193, 1e+194, 1e+195, 1e+196, 1e+197, + 1e+198, 1e+199, 1e+200, 1e+201, 1e+202, 1e+203, 1e+204, 1e+205, 1e+206, + 1e+207, 1e+208, 1e+209, 1e+210, 1e+211, 1e+212, 1e+213, 1e+214, 1e+215, + 1e+216, 1e+217, 1e+218, 1e+219, 1e+220, 1e+221, 1e+222, 1e+223, 1e+224, + 1e+225, 1e+226, 1e+227, 1e+228, 1e+229, 1e+230, 1e+231, 1e+232, 1e+233, + 1e+234, 1e+235, 1e+236, 1e+237, 1e+238, 1e+239, 1e+240, 1e+241, 1e+242, + 1e+243, 1e+244, 1e+245, 1e+246, 1e+247, 1e+248, 1e+249, 1e+250, 1e+251, + 1e+252, 1e+253, 1e+254, 1e+255, 1e+256, 1e+257, 1e+258, 1e+259, 1e+260, + 1e+261, 1e+262, 1e+263, 1e+264, 1e+265, 1e+266, 1e+267, 1e+268, 1e+269, + 1e+270, 1e+271, 1e+272, 1e+273, 1e+274, 1e+275, 1e+276, 1e+277, 1e+278, + 1e+279, 1e+280, 1e+281, 1e+282, 1e+283, 1e+284, 1e+285, 1e+286, 1e+287, + 1e+288, 1e+289, 1e+290, 1e+291, 1e+292, 1e+293, 1e+294, 1e+295, 1e+296, + 1e+297, 1e+298, 1e+299, 1e+300, 1e+301, 1e+302, 1e+303, 1e+304, 1e+305, + 1e+306, 1e+307, 1e+308}; + RAPIDJSON_ASSERT(n >= 0 && n <= 308); + return e[n]; +} + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_POW10_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/regex.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/regex.h new file mode 100644 index 0000000..19f49da --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/regex.h @@ -0,0 +1,753 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_INTERNAL_REGEX_H_ +#define RAPIDJSON_INTERNAL_REGEX_H_ + +#include "../allocators.h" +#include "../stream.h" +#include "stack.h" + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(padded) +RAPIDJSON_DIAG_OFF(switch - enum) +#elif defined(_MSC_VER) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated +#endif + +#ifdef __GNUC__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(effc++) +#endif + +#ifndef RAPIDJSON_REGEX_VERBOSE +#define RAPIDJSON_REGEX_VERBOSE 0 +#endif + +RAPIDJSON_NAMESPACE_BEGIN +namespace internal { + +/////////////////////////////////////////////////////////////////////////////// +// DecodedStream + +template +class DecodedStream { + public: + DecodedStream(SourceStream &ss) : ss_(ss), codepoint_() { Decode(); } + unsigned Peek() { return codepoint_; } + unsigned Take() { + unsigned c = codepoint_; + if (c) // No further decoding when '\0' + Decode(); + return c; + } + + private: + void Decode() { + if (!Encoding::Decode(ss_, &codepoint_)) codepoint_ = 0; + } + + SourceStream &ss_; + unsigned codepoint_; +}; + +/////////////////////////////////////////////////////////////////////////////// +// GenericRegex + +static const SizeType kRegexInvalidState = ~SizeType( + 0); //!< Represents an invalid index in GenericRegex::State::out, out1 +static const SizeType kRegexInvalidRange = ~SizeType(0); + +template +class GenericRegexSearch; + +//! Regular expression engine with subset of ECMAscript grammar. +/*! + Supported regular expression syntax: + - \c ab Concatenation + - \c a|b Alternation + - \c a? Zero or one + - \c a* Zero or more + - \c a+ One or more + - \c a{3} Exactly 3 times + - \c a{3,} At least 3 times + - \c a{3,5} 3 to 5 times + - \c (ab) Grouping + - \c ^a At the beginning + - \c a$ At the end + - \c . Any character + - \c [abc] Character classes + - \c [a-c] Character class range + - \c [a-z0-9_] Character class combination + - \c [^abc] Negated character classes + - \c [^a-c] Negated character class range + - \c [\b] Backspace (U+0008) + - \c \\| \\\\ ... Escape characters + - \c \\f Form feed (U+000C) + - \c \\n Line feed (U+000A) + - \c \\r Carriage return (U+000D) + - \c \\t Tab (U+0009) + - \c \\v Vertical tab (U+000B) + + \note This is a Thompson NFA engine, implemented with reference to + Cox, Russ. "Regular Expression Matching Can Be Simple And Fast (but is + slow in Java, Perl, PHP, Python, Ruby,...).", + https://swtch.com/~rsc/regexp/regexp1.html +*/ +template +class GenericRegex { + public: + typedef Encoding EncodingType; + typedef typename Encoding::Ch Ch; + template + friend class GenericRegexSearch; + + GenericRegex(const Ch *source, Allocator *allocator = 0) + : ownAllocator_(allocator ? 0 : RAPIDJSON_NEW(Allocator)()), + allocator_(allocator ? allocator : ownAllocator_), + states_(allocator_, 256), + ranges_(allocator_, 256), + root_(kRegexInvalidState), + stateCount_(), + rangeCount_(), + anchorBegin_(), + anchorEnd_() { + GenericStringStream ss(source); + DecodedStream, Encoding> ds(ss); + Parse(ds); + } + + ~GenericRegex() { RAPIDJSON_DELETE(ownAllocator_); } + + bool IsValid() const { return root_ != kRegexInvalidState; } + + private: + enum Operator { + kZeroOrOne, + kZeroOrMore, + kOneOrMore, + kConcatenation, + kAlternation, + kLeftParenthesis + }; + + static const unsigned kAnyCharacterClass = 0xFFFFFFFF; //!< For '.' + static const unsigned kRangeCharacterClass = 0xFFFFFFFE; + static const unsigned kRangeNegationFlag = 0x80000000; + + struct Range { + unsigned start; // + unsigned end; + SizeType next; + }; + + struct State { + SizeType out; //!< Equals to kInvalid for matching state + SizeType out1; //!< Equals to non-kInvalid for split + SizeType rangeStart; + unsigned codepoint; + }; + + struct Frag { + Frag(SizeType s, SizeType o, SizeType m) : start(s), out(o), minIndex(m) {} + SizeType start; + SizeType out; //!< link-list of all output states + SizeType minIndex; + }; + + State &GetState(SizeType index) { + RAPIDJSON_ASSERT(index < stateCount_); + return states_.template Bottom()[index]; + } + + const State &GetState(SizeType index) const { + RAPIDJSON_ASSERT(index < stateCount_); + return states_.template Bottom()[index]; + } + + Range &GetRange(SizeType index) { + RAPIDJSON_ASSERT(index < rangeCount_); + return ranges_.template Bottom()[index]; + } + + const Range &GetRange(SizeType index) const { + RAPIDJSON_ASSERT(index < rangeCount_); + return ranges_.template Bottom()[index]; + } + + template + void Parse(DecodedStream &ds) { + Stack operandStack(allocator_, 256); // Frag + Stack operatorStack(allocator_, 256); // Operator + Stack atomCountStack(allocator_, + 256); // unsigned (Atom per parenthesis) + + *atomCountStack.template Push() = 0; + + unsigned codepoint; + while (ds.Peek() != 0) { + switch (codepoint = ds.Take()) { + case '^': + anchorBegin_ = true; + break; + + case '$': + anchorEnd_ = true; + break; + + case '|': + while (!operatorStack.Empty() && + *operatorStack.template Top() < kAlternation) + if (!Eval(operandStack, *operatorStack.template Pop(1))) + return; + *operatorStack.template Push() = kAlternation; + *atomCountStack.template Top() = 0; + break; + + case '(': + *operatorStack.template Push() = kLeftParenthesis; + *atomCountStack.template Push() = 0; + break; + + case ')': + while (!operatorStack.Empty() && + *operatorStack.template Top() != kLeftParenthesis) + if (!Eval(operandStack, *operatorStack.template Pop(1))) + return; + if (operatorStack.Empty()) return; + operatorStack.template Pop(1); + atomCountStack.template Pop(1); + ImplicitConcatenation(atomCountStack, operatorStack); + break; + + case '?': + if (!Eval(operandStack, kZeroOrOne)) return; + break; + + case '*': + if (!Eval(operandStack, kZeroOrMore)) return; + break; + + case '+': + if (!Eval(operandStack, kOneOrMore)) return; + break; + + case '{': { + unsigned n, m; + if (!ParseUnsigned(ds, &n)) return; + + if (ds.Peek() == ',') { + ds.Take(); + if (ds.Peek() == '}') + m = kInfinityQuantifier; + else if (!ParseUnsigned(ds, &m) || m < n) + return; + } else + m = n; + + if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}') return; + ds.Take(); + } break; + + case '.': + PushOperand(operandStack, kAnyCharacterClass); + ImplicitConcatenation(atomCountStack, operatorStack); + break; + + case '[': { + SizeType range; + if (!ParseRange(ds, &range)) return; + SizeType s = NewState(kRegexInvalidState, kRegexInvalidState, + kRangeCharacterClass); + GetState(s).rangeStart = range; + *operandStack.template Push() = Frag(s, s, s); + } + ImplicitConcatenation(atomCountStack, operatorStack); + break; + + case '\\': // Escape character + if (!CharacterEscape(ds, &codepoint)) + return; // Unsupported escape character + // fall through to default + RAPIDJSON_DELIBERATE_FALLTHROUGH; + + default: // Pattern character + PushOperand(operandStack, codepoint); + ImplicitConcatenation(atomCountStack, operatorStack); + } + } + + while (!operatorStack.Empty()) + if (!Eval(operandStack, *operatorStack.template Pop(1))) return; + + // Link the operand to matching state. + if (operandStack.GetSize() == sizeof(Frag)) { + Frag *e = operandStack.template Pop(1); + Patch(e->out, NewState(kRegexInvalidState, kRegexInvalidState, 0)); + root_ = e->start; + +#if RAPIDJSON_REGEX_VERBOSE + printf("root: %d\n", root_); + for (SizeType i = 0; i < stateCount_; i++) { + State &s = GetState(i); + printf("[%2d] out: %2d out1: %2d c: '%c'\n", i, s.out, s.out1, + (char)s.codepoint); + } + printf("\n"); +#endif + } + } + + SizeType NewState(SizeType out, SizeType out1, unsigned codepoint) { + State *s = states_.template Push(); + s->out = out; + s->out1 = out1; + s->codepoint = codepoint; + s->rangeStart = kRegexInvalidRange; + return stateCount_++; + } + + void PushOperand(Stack &operandStack, unsigned codepoint) { + SizeType s = NewState(kRegexInvalidState, kRegexInvalidState, codepoint); + *operandStack.template Push() = Frag(s, s, s); + } + + void ImplicitConcatenation(Stack &atomCountStack, + Stack &operatorStack) { + if (*atomCountStack.template Top()) + *operatorStack.template Push() = kConcatenation; + (*atomCountStack.template Top())++; + } + + SizeType Append(SizeType l1, SizeType l2) { + SizeType old = l1; + while (GetState(l1).out != kRegexInvalidState) l1 = GetState(l1).out; + GetState(l1).out = l2; + return old; + } + + void Patch(SizeType l, SizeType s) { + for (SizeType next; l != kRegexInvalidState; l = next) { + next = GetState(l).out; + GetState(l).out = s; + } + } + + bool Eval(Stack &operandStack, Operator op) { + switch (op) { + case kConcatenation: + RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag) * 2); + { + Frag e2 = *operandStack.template Pop(1); + Frag e1 = *operandStack.template Pop(1); + Patch(e1.out, e2.start); + *operandStack.template Push() = + Frag(e1.start, e2.out, Min(e1.minIndex, e2.minIndex)); + } + return true; + + case kAlternation: + if (operandStack.GetSize() >= sizeof(Frag) * 2) { + Frag e2 = *operandStack.template Pop(1); + Frag e1 = *operandStack.template Pop(1); + SizeType s = NewState(e1.start, e2.start, 0); + *operandStack.template Push() = + Frag(s, Append(e1.out, e2.out), Min(e1.minIndex, e2.minIndex)); + return true; + } + return false; + + case kZeroOrOne: + if (operandStack.GetSize() >= sizeof(Frag)) { + Frag e = *operandStack.template Pop(1); + SizeType s = NewState(kRegexInvalidState, e.start, 0); + *operandStack.template Push() = + Frag(s, Append(e.out, s), e.minIndex); + return true; + } + return false; + + case kZeroOrMore: + if (operandStack.GetSize() >= sizeof(Frag)) { + Frag e = *operandStack.template Pop(1); + SizeType s = NewState(kRegexInvalidState, e.start, 0); + Patch(e.out, s); + *operandStack.template Push() = Frag(s, s, e.minIndex); + return true; + } + return false; + + case kOneOrMore: + if (operandStack.GetSize() >= sizeof(Frag)) { + Frag e = *operandStack.template Pop(1); + SizeType s = NewState(kRegexInvalidState, e.start, 0); + Patch(e.out, s); + *operandStack.template Push() = Frag(e.start, s, e.minIndex); + return true; + } + return false; + + default: + // syntax error (e.g. unclosed kLeftParenthesis) + return false; + } + } + + bool EvalQuantifier(Stack &operandStack, unsigned n, unsigned m) { + RAPIDJSON_ASSERT(n <= m); + RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag)); + + if (n == 0) { + if (m == 0) // a{0} not support + return false; + else if (m == kInfinityQuantifier) + Eval(operandStack, kZeroOrMore); // a{0,} -> a* + else { + Eval(operandStack, kZeroOrOne); // a{0,5} -> a? + for (unsigned i = 0; i < m - 1; i++) + CloneTopOperand(operandStack); // a{0,5} -> a? a? a? a? a? + for (unsigned i = 0; i < m - 1; i++) + Eval(operandStack, kConcatenation); // a{0,5} -> a?a?a?a?a? + } + return true; + } + + for (unsigned i = 0; i < n - 1; i++) // a{3} -> a a a + CloneTopOperand(operandStack); + + if (m == kInfinityQuantifier) + Eval(operandStack, kOneOrMore); // a{3,} -> a a a+ + else if (m > n) { + CloneTopOperand(operandStack); // a{3,5} -> a a a a + Eval(operandStack, kZeroOrOne); // a{3,5} -> a a a a? + for (unsigned i = n; i < m - 1; i++) + CloneTopOperand(operandStack); // a{3,5} -> a a a a? a? + for (unsigned i = n; i < m; i++) + Eval(operandStack, kConcatenation); // a{3,5} -> a a aa?a? + } + + for (unsigned i = 0; i < n - 1; i++) + Eval(operandStack, + kConcatenation); // a{3} -> aaa, a{3,} -> aaa+, a{3.5} -> aaaa?a? + + return true; + } + + static SizeType Min(SizeType a, SizeType b) { return a < b ? a : b; } + + void CloneTopOperand(Stack &operandStack) { + const Frag src = + *operandStack + .template Top(); // Copy constructor to prevent invalidation + SizeType count = + stateCount_ - src.minIndex; // Assumes top operand contains states in + // [src->minIndex, stateCount_) + State *s = states_.template Push(count); + memcpy(s, &GetState(src.minIndex), count * sizeof(State)); + for (SizeType j = 0; j < count; j++) { + if (s[j].out != kRegexInvalidState) s[j].out += count; + if (s[j].out1 != kRegexInvalidState) s[j].out1 += count; + } + *operandStack.template Push() = + Frag(src.start + count, src.out + count, src.minIndex + count); + stateCount_ += count; + } + + template + bool ParseUnsigned(DecodedStream &ds, unsigned *u) { + unsigned r = 0; + if (ds.Peek() < '0' || ds.Peek() > '9') return false; + while (ds.Peek() >= '0' && ds.Peek() <= '9') { + if (r >= 429496729 && ds.Peek() > '5') // 2^32 - 1 = 4294967295 + return false; // overflow + r = r * 10 + (ds.Take() - '0'); + } + *u = r; + return true; + } + + template + bool ParseRange(DecodedStream &ds, SizeType *range) { + bool isBegin = true; + bool negate = false; + int step = 0; + SizeType start = kRegexInvalidRange; + SizeType current = kRegexInvalidRange; + unsigned codepoint; + while ((codepoint = ds.Take()) != 0) { + if (isBegin) { + isBegin = false; + if (codepoint == '^') { + negate = true; + continue; + } + } + + switch (codepoint) { + case ']': + if (start == kRegexInvalidRange) + return false; // Error: nothing inside [] + if (step == 2) { // Add trailing '-' + SizeType r = NewRange('-'); + RAPIDJSON_ASSERT(current != kRegexInvalidRange); + GetRange(current).next = r; + } + if (negate) GetRange(start).start |= kRangeNegationFlag; + *range = start; + return true; + + case '\\': + if (ds.Peek() == 'b') { + ds.Take(); + codepoint = 0x0008; // Escape backspace character + } else if (!CharacterEscape(ds, &codepoint)) + return false; + // fall through to default + RAPIDJSON_DELIBERATE_FALLTHROUGH; + + default: + switch (step) { + case 1: + if (codepoint == '-') { + step++; + break; + } + // fall through to step 0 for other characters + RAPIDJSON_DELIBERATE_FALLTHROUGH; + + case 0: { + SizeType r = NewRange(codepoint); + if (current != kRegexInvalidRange) GetRange(current).next = r; + if (start == kRegexInvalidRange) start = r; + current = r; + } + step = 1; + break; + + default: + RAPIDJSON_ASSERT(step == 2); + GetRange(current).end = codepoint; + step = 0; + } + } + } + return false; + } + + SizeType NewRange(unsigned codepoint) { + Range *r = ranges_.template Push(); + r->start = r->end = codepoint; + r->next = kRegexInvalidRange; + return rangeCount_++; + } + + template + bool CharacterEscape(DecodedStream &ds, + unsigned *escapedCodepoint) { + unsigned codepoint; + switch (codepoint = ds.Take()) { + case '^': + case '$': + case '|': + case '(': + case ')': + case '?': + case '*': + case '+': + case '.': + case '[': + case ']': + case '{': + case '}': + case '\\': + *escapedCodepoint = codepoint; + return true; + case 'f': + *escapedCodepoint = 0x000C; + return true; + case 'n': + *escapedCodepoint = 0x000A; + return true; + case 'r': + *escapedCodepoint = 0x000D; + return true; + case 't': + *escapedCodepoint = 0x0009; + return true; + case 'v': + *escapedCodepoint = 0x000B; + return true; + default: + return false; // Unsupported escape character + } + } + + Allocator *ownAllocator_; + Allocator *allocator_; + Stack states_; + Stack ranges_; + SizeType root_; + SizeType stateCount_; + SizeType rangeCount_; + + static const unsigned kInfinityQuantifier = ~0u; + + // For SearchWithAnchoring() + bool anchorBegin_; + bool anchorEnd_; +}; + +template +class GenericRegexSearch { + public: + typedef typename RegexType::EncodingType Encoding; + typedef typename Encoding::Ch Ch; + + GenericRegexSearch(const RegexType ®ex, Allocator *allocator = 0) + : regex_(regex), + allocator_(allocator), + ownAllocator_(0), + state0_(allocator, 0), + state1_(allocator, 0), + stateSet_() { + RAPIDJSON_ASSERT(regex_.IsValid()); + if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + stateSet_ = static_cast(allocator_->Malloc(GetStateSetSize())); + state0_.template Reserve(regex_.stateCount_); + state1_.template Reserve(regex_.stateCount_); + } + + ~GenericRegexSearch() { + Allocator::Free(stateSet_); + RAPIDJSON_DELETE(ownAllocator_); + } + + template + bool Match(InputStream &is) { + return SearchWithAnchoring(is, true, true); + } + + bool Match(const Ch *s) { + GenericStringStream is(s); + return Match(is); + } + + template + bool Search(InputStream &is) { + return SearchWithAnchoring(is, regex_.anchorBegin_, regex_.anchorEnd_); + } + + bool Search(const Ch *s) { + GenericStringStream is(s); + return Search(is); + } + + private: + typedef typename RegexType::State State; + typedef typename RegexType::Range Range; + + template + bool SearchWithAnchoring(InputStream &is, bool anchorBegin, bool anchorEnd) { + DecodedStream ds(is); + + state0_.Clear(); + Stack *current = &state0_, *next = &state1_; + const size_t stateSetSize = GetStateSetSize(); + std::memset(stateSet_, 0, stateSetSize); + + bool matched = AddState(*current, regex_.root_); + unsigned codepoint; + while (!current->Empty() && (codepoint = ds.Take()) != 0) { + std::memset(stateSet_, 0, stateSetSize); + next->Clear(); + matched = false; + for (const SizeType *s = current->template Bottom(); + s != current->template End(); ++s) { + const State &sr = regex_.GetState(*s); + if (sr.codepoint == codepoint || + sr.codepoint == RegexType::kAnyCharacterClass || + (sr.codepoint == RegexType::kRangeCharacterClass && + MatchRange(sr.rangeStart, codepoint))) { + matched = AddState(*next, sr.out) || matched; + if (!anchorEnd && matched) return true; + } + if (!anchorBegin) AddState(*next, regex_.root_); + } + internal::Swap(current, next); + } + + return matched; + } + + size_t GetStateSetSize() const { return (regex_.stateCount_ + 31) / 32 * 4; } + + // Return whether the added states is a match state + bool AddState(Stack &l, SizeType index) { + RAPIDJSON_ASSERT(index != kRegexInvalidState); + + const State &s = regex_.GetState(index); + if (s.out1 != kRegexInvalidState) { // Split + bool matched = AddState(l, s.out); + return AddState(l, s.out1) || matched; + } else if (!(stateSet_[index >> 5] & (1u << (index & 31)))) { + stateSet_[index >> 5] |= (1u << (index & 31)); + *l.template PushUnsafe() = index; + } + return s.out == + kRegexInvalidState; // by using PushUnsafe() above, we can ensure s + // is not validated due to reallocation. + } + + bool MatchRange(SizeType rangeIndex, unsigned codepoint) const { + bool yes = (regex_.GetRange(rangeIndex).start & + RegexType::kRangeNegationFlag) == 0; + while (rangeIndex != kRegexInvalidRange) { + const Range &r = regex_.GetRange(rangeIndex); + if (codepoint >= (r.start & ~RegexType::kRangeNegationFlag) && + codepoint <= r.end) + return yes; + rangeIndex = r.next; + } + return !yes; + } + + const RegexType ®ex_; + Allocator *allocator_; + Allocator *ownAllocator_; + Stack state0_; + Stack state1_; + uint32_t *stateSet_; +}; + +typedef GenericRegex> Regex; +typedef GenericRegexSearch RegexSearch; + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#ifdef __GNUC__ +RAPIDJSON_DIAG_POP +#endif + +#if defined(__clang__) || defined(_MSC_VER) +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_INTERNAL_REGEX_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/stack.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/stack.h new file mode 100644 index 0000000..bf80503 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/stack.h @@ -0,0 +1,245 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_INTERNAL_STACK_H_ +#define RAPIDJSON_INTERNAL_STACK_H_ + +#include +#include "../allocators.h" +#include "swap.h" + +#if defined(__clang__) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(c++ 98 - compat) +#endif + +RAPIDJSON_NAMESPACE_BEGIN +namespace internal { + +/////////////////////////////////////////////////////////////////////////////// +// Stack + +//! A type-unsafe stack for storing different types of data. +/*! \tparam Allocator Allocator for allocating stack memory. + */ +template +class Stack { + public: + // Optimization note: Do not allocate memory for stack_ in constructor. + // Do it lazily when first Push() -> Expand() -> Resize(). + Stack(Allocator *allocator, size_t stackCapacity) + : allocator_(allocator), + ownAllocator_(0), + stack_(0), + stackTop_(0), + stackEnd_(0), + initialCapacity_(stackCapacity) {} + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + Stack(Stack &&rhs) + : allocator_(rhs.allocator_), + ownAllocator_(rhs.ownAllocator_), + stack_(rhs.stack_), + stackTop_(rhs.stackTop_), + stackEnd_(rhs.stackEnd_), + initialCapacity_(rhs.initialCapacity_) { + rhs.allocator_ = 0; + rhs.ownAllocator_ = 0; + rhs.stack_ = 0; + rhs.stackTop_ = 0; + rhs.stackEnd_ = 0; + rhs.initialCapacity_ = 0; + } +#endif + + ~Stack() { Destroy(); } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + Stack &operator=(Stack &&rhs) { + if (&rhs != this) { + Destroy(); + + allocator_ = rhs.allocator_; + ownAllocator_ = rhs.ownAllocator_; + stack_ = rhs.stack_; + stackTop_ = rhs.stackTop_; + stackEnd_ = rhs.stackEnd_; + initialCapacity_ = rhs.initialCapacity_; + + rhs.allocator_ = 0; + rhs.ownAllocator_ = 0; + rhs.stack_ = 0; + rhs.stackTop_ = 0; + rhs.stackEnd_ = 0; + rhs.initialCapacity_ = 0; + } + return *this; + } +#endif + + void Swap(Stack &rhs) RAPIDJSON_NOEXCEPT { + internal::Swap(allocator_, rhs.allocator_); + internal::Swap(ownAllocator_, rhs.ownAllocator_); + internal::Swap(stack_, rhs.stack_); + internal::Swap(stackTop_, rhs.stackTop_); + internal::Swap(stackEnd_, rhs.stackEnd_); + internal::Swap(initialCapacity_, rhs.initialCapacity_); + } + + void Clear() { stackTop_ = stack_; } + + void ShrinkToFit() { + if (Empty()) { + // If the stack is empty, completely deallocate the memory. + Allocator::Free(stack_); // NOLINT (+clang-analyzer-unix.Malloc) + stack_ = 0; + stackTop_ = 0; + stackEnd_ = 0; + } else + Resize(GetSize()); + } + + // Optimization note: try to minimize the size of this function for force + // inline. Expansion is run very infrequently, so it is moved to another + // (probably non-inline) function. + template + RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) { + // Expand the stack if needed + if (RAPIDJSON_UNLIKELY(static_cast(sizeof(T) * count) > + (stackEnd_ - stackTop_))) + Expand(count); + } + + template + RAPIDJSON_FORCEINLINE T *Push(size_t count = 1) { + Reserve(count); + return PushUnsafe(count); + } + + template + RAPIDJSON_FORCEINLINE T *PushUnsafe(size_t count = 1) { + RAPIDJSON_ASSERT(stackTop_); + RAPIDJSON_ASSERT(static_cast(sizeof(T) * count) <= + (stackEnd_ - stackTop_)); + T *ret = reinterpret_cast(stackTop_); + stackTop_ += sizeof(T) * count; + return ret; + } + + template + T *Pop(size_t count) { + RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T)); + stackTop_ -= count * sizeof(T); + return reinterpret_cast(stackTop_); + } + + template + T *Top() { + RAPIDJSON_ASSERT(GetSize() >= sizeof(T)); + return reinterpret_cast(stackTop_ - sizeof(T)); + } + + template + const T *Top() const { + RAPIDJSON_ASSERT(GetSize() >= sizeof(T)); + return reinterpret_cast(stackTop_ - sizeof(T)); + } + + template + T *End() { + return reinterpret_cast(stackTop_); + } + + template + const T *End() const { + return reinterpret_cast(stackTop_); + } + + template + T *Bottom() { + return reinterpret_cast(stack_); + } + + template + const T *Bottom() const { + return reinterpret_cast(stack_); + } + + bool HasAllocator() const { return allocator_ != 0; } + + Allocator &GetAllocator() { + RAPIDJSON_ASSERT(allocator_); + return *allocator_; + } + + bool Empty() const { return stackTop_ == stack_; } + size_t GetSize() const { return static_cast(stackTop_ - stack_); } + size_t GetCapacity() const { return static_cast(stackEnd_ - stack_); } + + private: + template + void Expand(size_t count) { + // Only expand the capacity if the current stack exists. Otherwise just + // create a stack with initial capacity. + size_t newCapacity; + if (stack_ == 0) { + if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + newCapacity = initialCapacity_; + } else { + newCapacity = GetCapacity(); + newCapacity += (newCapacity + 1) / 2; + } + size_t newSize = GetSize() + sizeof(T) * count; + if (newCapacity < newSize) newCapacity = newSize; + + Resize(newCapacity); + } + + void Resize(size_t newCapacity) { + const size_t size = GetSize(); // Backup the current size + stack_ = static_cast( + allocator_->Realloc(stack_, GetCapacity(), newCapacity)); + stackTop_ = stack_ + size; + stackEnd_ = stack_ + newCapacity; + } + + void Destroy() { + Allocator::Free(stack_); + RAPIDJSON_DELETE(ownAllocator_); // Only delete if it is owned by the stack + } + + // Prohibit copy constructor & assignment operator. + Stack(const Stack &); + Stack &operator=(const Stack &); + + Allocator *allocator_; + Allocator *ownAllocator_; + char *stack_; + char *stackTop_; + char *stackEnd_; + size_t initialCapacity_; +}; + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#if defined(__clang__) +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_STACK_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/strfunc.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/strfunc.h new file mode 100644 index 0000000..9024b2f --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/strfunc.h @@ -0,0 +1,74 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_INTERNAL_STRFUNC_H_ +#define RAPIDJSON_INTERNAL_STRFUNC_H_ + +#include +#include "../stream.h" + +RAPIDJSON_NAMESPACE_BEGIN +namespace internal { + +//! Custom strlen() which works on different character types. +/*! \tparam Ch Character type (e.g. char, wchar_t, short) + \param s Null-terminated input string. + \return Number of characters in the string. + \note This has the same semantics as strlen(), the return value is not + number of Unicode codepoints. +*/ +template +inline SizeType StrLen(const Ch *s) { + RAPIDJSON_ASSERT(s != 0); + const Ch *p = s; + while (*p) ++p; + return SizeType(p - s); +} + +template <> +inline SizeType StrLen(const char *s) { + return SizeType(std::strlen(s)); +} + +template <> +inline SizeType StrLen(const wchar_t *s) { + return SizeType(std::wcslen(s)); +} + +//! Returns number of code points in a encoded string. +template +bool CountStringCodePoint(const typename Encoding::Ch *s, SizeType length, + SizeType *outCount) { + RAPIDJSON_ASSERT(s != 0); + RAPIDJSON_ASSERT(outCount != 0); + GenericStringStream is(s); + const typename Encoding::Ch *end = s + length; + SizeType count = 0; + while (is.src_ < end) { + unsigned codepoint; + if (!Encoding::Decode(is, &codepoint)) return false; + count++; + } + *outCount = count; + return true; +} + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_INTERNAL_STRFUNC_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/strtod.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/strtod.h new file mode 100644 index 0000000..ea7ae43 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/strtod.h @@ -0,0 +1,303 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_STRTOD_ +#define RAPIDJSON_STRTOD_ + +#include +#include +#include "biginteger.h" +#include "diyfp.h" +#include "ieee754.h" +#include "pow10.h" + +RAPIDJSON_NAMESPACE_BEGIN +namespace internal { + +inline double FastPath(double significand, int exp) { + if (exp < -308) + return 0.0; + else if (exp >= 0) + return significand * internal::Pow10(exp); + else + return significand / internal::Pow10(-exp); +} + +inline double StrtodNormalPrecision(double d, int p) { + if (p < -308) { + // Prevent expSum < -308, making Pow10(p) = 0 + d = FastPath(d, -308); + d = FastPath(d, p + 308); + } else + d = FastPath(d, p); + return d; +} + +template +inline T Min3(T a, T b, T c) { + T m = a; + if (m > b) m = b; + if (m > c) m = c; + return m; +} + +inline int CheckWithinHalfULP(double b, const BigInteger &d, int dExp) { + const Double db(b); + const uint64_t bInt = db.IntegerSignificand(); + const int bExp = db.IntegerExponent(); + const int hExp = bExp - 1; + + int dS_Exp2 = 0, dS_Exp5 = 0, bS_Exp2 = 0, bS_Exp5 = 0, hS_Exp2 = 0, + hS_Exp5 = 0; + + // Adjust for decimal exponent + if (dExp >= 0) { + dS_Exp2 += dExp; + dS_Exp5 += dExp; + } else { + bS_Exp2 -= dExp; + bS_Exp5 -= dExp; + hS_Exp2 -= dExp; + hS_Exp5 -= dExp; + } + + // Adjust for binary exponent + if (bExp >= 0) + bS_Exp2 += bExp; + else { + dS_Exp2 -= bExp; + hS_Exp2 -= bExp; + } + + // Adjust for half ulp exponent + if (hExp >= 0) + hS_Exp2 += hExp; + else { + dS_Exp2 -= hExp; + bS_Exp2 -= hExp; + } + + // Remove common power of two factor from all three scaled values + int common_Exp2 = Min3(dS_Exp2, bS_Exp2, hS_Exp2); + dS_Exp2 -= common_Exp2; + bS_Exp2 -= common_Exp2; + hS_Exp2 -= common_Exp2; + + BigInteger dS = d; + dS.MultiplyPow5(static_cast(dS_Exp5)) <<= + static_cast(dS_Exp2); + + BigInteger bS(bInt); + bS.MultiplyPow5(static_cast(bS_Exp5)) <<= + static_cast(bS_Exp2); + + BigInteger hS(1); + hS.MultiplyPow5(static_cast(hS_Exp5)) <<= + static_cast(hS_Exp2); + + BigInteger delta(0); + dS.Difference(bS, &delta); + + return delta.Compare(hS); +} + +inline bool StrtodFast(double d, int p, double *result) { + // Use fast path for string-to-double conversion if possible + // see + // http://www.exploringbinary.com/fast-path-decimal-to-floating-point-conversion/ + if (p > 22 && p < 22 + 16) { + // Fast Path Cases In Disguise + d *= internal::Pow10(p - 22); + p = 22; + } + + if (p >= -22 && p <= 22 && d <= 9007199254740991.0) { // 2^53 - 1 + *result = FastPath(d, p); + return true; + } else + return false; +} + +// Compute an approximation and see if it is within 1/2 ULP +inline bool StrtodDiyFp(const char *decimals, int dLen, int dExp, + double *result) { + uint64_t significand = 0; + int i = 0; // 2^64 - 1 = 18446744073709551615, 1844674407370955161 = + // 0x1999999999999999 + for (; i < dLen; i++) { + if (significand > RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) || + (significand == RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) && + decimals[i] > '5')) + break; + significand = significand * 10u + static_cast(decimals[i] - '0'); + } + + if (i < dLen && decimals[i] >= '5') // Rounding + significand++; + + int remaining = dLen - i; + const int kUlpShift = 3; + const int kUlp = 1 << kUlpShift; + int64_t error = (remaining == 0) ? 0 : kUlp / 2; + + DiyFp v(significand, 0); + v = v.Normalize(); + error <<= -v.e; + + dExp += remaining; + + int actualExp; + DiyFp cachedPower = GetCachedPower10(dExp, &actualExp); + if (actualExp != dExp) { + static const DiyFp kPow10[] = { + DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60), // 10^1 + DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57), // 10^2 + DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54), // 10^3 + DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50), // 10^4 + DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47), // 10^5 + DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44), // 10^6 + DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40) // 10^7 + }; + int adjustment = dExp - actualExp; + RAPIDJSON_ASSERT(adjustment >= 1 && adjustment < 8); + v = v * kPow10[adjustment - 1]; + if (dLen + adjustment > + 19) // has more digits than decimal digits in 64-bit + error += kUlp / 2; + } + + v = v * cachedPower; + + error += kUlp + (error == 0 ? 0 : 1); + + const int oldExp = v.e; + v = v.Normalize(); + error <<= oldExp - v.e; + + const int effectiveSignificandSize = + Double::EffectiveSignificandSize(64 + v.e); + int precisionSize = 64 - effectiveSignificandSize; + if (precisionSize + kUlpShift >= 64) { + int scaleExp = (precisionSize + kUlpShift) - 63; + v.f >>= scaleExp; + v.e += scaleExp; + error = (error >> scaleExp) + 1 + kUlp; + precisionSize -= scaleExp; + } + + DiyFp rounded(v.f >> precisionSize, v.e + precisionSize); + const uint64_t precisionBits = + (v.f & ((uint64_t(1) << precisionSize) - 1)) * kUlp; + const uint64_t halfWay = (uint64_t(1) << (precisionSize - 1)) * kUlp; + if (precisionBits >= halfWay + static_cast(error)) { + rounded.f++; + if (rounded.f & (DiyFp::kDpHiddenBit + << 1)) { // rounding overflows mantissa (issue #340) + rounded.f >>= 1; + rounded.e++; + } + } + + *result = rounded.ToDouble(); + + return halfWay - static_cast(error) >= precisionBits || + precisionBits >= halfWay + static_cast(error); +} + +inline double StrtodBigInteger(double approx, const char *decimals, int dLen, + int dExp) { + RAPIDJSON_ASSERT(dLen >= 0); + const BigInteger dInt(decimals, static_cast(dLen)); + Double a(approx); + int cmp = CheckWithinHalfULP(a.Value(), dInt, dExp); + if (cmp < 0) + return a.Value(); // within half ULP + else if (cmp == 0) { + // Round towards even + if (a.Significand() & 1) + return a.NextPositiveDouble(); + else + return a.Value(); + } else // adjustment + return a.NextPositiveDouble(); +} + +inline double StrtodFullPrecision(double d, int p, const char *decimals, + size_t length, size_t decimalPosition, + int exp) { + RAPIDJSON_ASSERT(d >= 0.0); + RAPIDJSON_ASSERT(length >= 1); + + double result = 0.0; + if (StrtodFast(d, p, &result)) return result; + + RAPIDJSON_ASSERT(length <= INT_MAX); + int dLen = static_cast(length); + + RAPIDJSON_ASSERT(length >= decimalPosition); + RAPIDJSON_ASSERT(length - decimalPosition <= INT_MAX); + int dExpAdjust = static_cast(length - decimalPosition); + + RAPIDJSON_ASSERT(exp >= INT_MIN + dExpAdjust); + int dExp = exp - dExpAdjust; + + // Make sure length+dExp does not overflow + RAPIDJSON_ASSERT(dExp <= INT_MAX - dLen); + + // Trim leading zeros + while (dLen > 0 && *decimals == '0') { + dLen--; + decimals++; + } + + // Trim trailing zeros + while (dLen > 0 && decimals[dLen - 1] == '0') { + dLen--; + dExp++; + } + + if (dLen == 0) { // Buffer only contains zeros. + return 0.0; + } + + // Trim right-most digits + const int kMaxDecimalDigit = 767 + 1; + if (dLen > kMaxDecimalDigit) { + dExp += dLen - kMaxDecimalDigit; + dLen = kMaxDecimalDigit; + } + + // If too small, underflow to zero. + // Any x <= 10^-324 is interpreted as zero. + if (dLen + dExp <= -324) return 0.0; + + // If too large, overflow to infinity. + // Any x >= 10^309 is interpreted as +infinity. + if (dLen + dExp > 309) return std::numeric_limits::infinity(); + + if (StrtodDiyFp(decimals, dLen, dExp, &result)) return result; + + // Use approximation from StrtodDiyFp and make adjustment with BigInteger + // comparison + return StrtodBigInteger(result, decimals, dLen, dExp); +} + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_STRTOD_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/internal/swap.h b/src/livox_ros_driver2/3rdparty/rapidjson/internal/swap.h new file mode 100644 index 0000000..db41b18 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/internal/swap.h @@ -0,0 +1,50 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_INTERNAL_SWAP_H_ +#define RAPIDJSON_INTERNAL_SWAP_H_ + +#include "../rapidjson.h" + +#if defined(__clang__) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(c++ 98 - compat) +#endif + +RAPIDJSON_NAMESPACE_BEGIN +namespace internal { + +//! Custom swap() to avoid dependency on C++ header +/*! \tparam T Type of the arguments to swap, should be instantiated with + primitive C++ types only. \note This has the same semantics as std::swap(). +*/ +template +inline void Swap(T &a, T &b) RAPIDJSON_NOEXCEPT { + T tmp = a; + a = b; + b = tmp; +} + +} // namespace internal +RAPIDJSON_NAMESPACE_END + +#if defined(__clang__) +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_INTERNAL_SWAP_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/istreamwrapper.h b/src/livox_ros_driver2/3rdparty/rapidjson/istreamwrapper.h new file mode 100644 index 0000000..b9663b0 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/istreamwrapper.h @@ -0,0 +1,161 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_ISTREAMWRAPPER_H_ +#define RAPIDJSON_ISTREAMWRAPPER_H_ + +#include +#include +#include "stream.h" + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(padded) +#elif defined(_MSC_VER) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(4351) // new behavior: elements of array 'array' will be + // default initialized +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +//! Wrapper of \c std::basic_istream into RapidJSON's Stream concept. +/*! + The classes can be wrapped including but not limited to: + + - \c std::istringstream + - \c std::stringstream + - \c std::wistringstream + - \c std::wstringstream + - \c std::ifstream + - \c std::fstream + - \c std::wifstream + - \c std::wfstream + + \tparam StreamType Class derived from \c std::basic_istream. +*/ + +template +class BasicIStreamWrapper { + public: + typedef typename StreamType::char_type Ch; + + //! Constructor. + /*! + \param stream stream opened for read. + */ + BasicIStreamWrapper(StreamType &stream) + : stream_(stream), + buffer_(peekBuffer_), + bufferSize_(4), + bufferLast_(0), + current_(buffer_), + readCount_(0), + count_(0), + eof_(false) { + Read(); + } + + //! Constructor. + /*! + \param stream stream opened for read. + \param buffer user-supplied buffer. + \param bufferSize size of buffer in bytes. Must >=4 bytes. + */ + BasicIStreamWrapper(StreamType &stream, char *buffer, size_t bufferSize) + : stream_(stream), + buffer_(buffer), + bufferSize_(bufferSize), + bufferLast_(0), + current_(buffer_), + readCount_(0), + count_(0), + eof_(false) { + RAPIDJSON_ASSERT(bufferSize >= 4); + Read(); + } + + Ch Peek() const { return *current_; } + Ch Take() { + Ch c = *current_; + Read(); + return c; + } + size_t Tell() const { + return count_ + static_cast(current_ - buffer_); + } + + // Not implemented + void Put(Ch) { RAPIDJSON_ASSERT(false); } + void Flush() { RAPIDJSON_ASSERT(false); } + Ch *PutBegin() { + RAPIDJSON_ASSERT(false); + return 0; + } + size_t PutEnd(Ch *) { + RAPIDJSON_ASSERT(false); + return 0; + } + + // For encoding detection only. + const Ch *Peek4() const { + return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0; + } + + private: + BasicIStreamWrapper(); + BasicIStreamWrapper(const BasicIStreamWrapper &); + BasicIStreamWrapper &operator=(const BasicIStreamWrapper &); + + void Read() { + if (current_ < bufferLast_) + ++current_; + else if (!eof_) { + count_ += readCount_; + readCount_ = bufferSize_; + bufferLast_ = buffer_ + readCount_ - 1; + current_ = buffer_; + + if (!stream_.read(buffer_, static_cast(bufferSize_))) { + readCount_ = static_cast(stream_.gcount()); + *(bufferLast_ = buffer_ + readCount_) = '\0'; + eof_ = true; + } + } + } + + StreamType &stream_; + Ch peekBuffer_[4], *buffer_; + size_t bufferSize_; + Ch *bufferLast_; + Ch *current_; + size_t readCount_; + size_t count_; //!< Number of characters read + bool eof_; +}; + +typedef BasicIStreamWrapper IStreamWrapper; +typedef BasicIStreamWrapper WIStreamWrapper; + +#if defined(__clang__) || defined(_MSC_VER) +RAPIDJSON_DIAG_POP +#endif + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_ISTREAMWRAPPER_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/license.txt b/src/livox_ros_driver2/3rdparty/rapidjson/license.txt new file mode 100644 index 0000000..7ccc161 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/license.txt @@ -0,0 +1,57 @@ +Tencent is pleased to support the open source community by making RapidJSON available. + +Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved. + +If you have downloaded a copy of the RapidJSON binary from Tencent, please note that the RapidJSON binary is licensed under the MIT License. +If you have downloaded a copy of the RapidJSON source code from Tencent, please note that RapidJSON source code is licensed under the MIT License, except for the third-party components listed below which are subject to different license terms. Your integration of RapidJSON into your own projects may require compliance with the MIT License, as well as the other licenses applicable to the third-party components included within RapidJSON. To avoid the problematic JSON license in your own projects, it's sufficient to exclude the bin/jsonchecker/ directory, as it's the only code under the JSON license. +A copy of the MIT License is included in this file. + +Other dependencies and licenses: + +Open Source Software Licensed Under the BSD License: +-------------------------------------------------------------------- + +The msinttypes r29 +Copyright (c) 2006-2013 Alexander Chemeris +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. +* Neither the name of copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +Open Source Software Licensed Under the JSON License: +-------------------------------------------------------------------- + +json.org +Copyright (c) 2002 JSON.org +All Rights Reserved. + +JSON_checker +Copyright (c) 2002 JSON.org +All Rights Reserved. + + +Terms of the JSON License: +--------------------------------------------------- + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +The Software shall be used for Good, not Evil. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + +Terms of the MIT License: +-------------------------------------------------------------------- + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/memorybuffer.h b/src/livox_ros_driver2/3rdparty/rapidjson/memorybuffer.h new file mode 100644 index 0000000..a827d6a --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/memorybuffer.h @@ -0,0 +1,78 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_MEMORYBUFFER_H_ +#define RAPIDJSON_MEMORYBUFFER_H_ + +#include "internal/stack.h" +#include "stream.h" + +RAPIDJSON_NAMESPACE_BEGIN + +//! Represents an in-memory output byte stream. +/*! + This class is mainly for being wrapped by EncodedOutputStream or + AutoUTFOutputStream. + + It is similar to FileWriteBuffer but the destination is an in-memory buffer + instead of a file. + + Differences between MemoryBuffer and StringBuffer: + 1. StringBuffer has Encoding but MemoryBuffer is only a byte buffer. + 2. StringBuffer::GetString() returns a null-terminated string. + MemoryBuffer::GetBuffer() returns a buffer without terminator. + + \tparam Allocator type for allocating memory buffer. + \note implements Stream concept +*/ +template +struct GenericMemoryBuffer { + typedef char Ch; // byte + + GenericMemoryBuffer(Allocator *allocator = 0, + size_t capacity = kDefaultCapacity) + : stack_(allocator, capacity) {} + + void Put(Ch c) { *stack_.template Push() = c; } + void Flush() {} + + void Clear() { stack_.Clear(); } + void ShrinkToFit() { stack_.ShrinkToFit(); } + Ch *Push(size_t count) { return stack_.template Push(count); } + void Pop(size_t count) { stack_.template Pop(count); } + + const Ch *GetBuffer() const { return stack_.template Bottom(); } + + size_t GetSize() const { return stack_.GetSize(); } + + static const size_t kDefaultCapacity = 256; + mutable internal::Stack stack_; +}; + +typedef GenericMemoryBuffer<> MemoryBuffer; + +//! Implement specialized version of PutN() with memset() for better +//! performance. +template <> +inline void PutN(MemoryBuffer &memoryBuffer, char c, size_t n) { + std::memset(memoryBuffer.stack_.Push(n), c, n * sizeof(c)); +} + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_MEMORYBUFFER_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/memorystream.h b/src/livox_ros_driver2/3rdparty/rapidjson/memorystream.h new file mode 100644 index 0000000..f542ece --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/memorystream.h @@ -0,0 +1,84 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_MEMORYSTREAM_H_ +#define RAPIDJSON_MEMORYSTREAM_H_ + +#include "stream.h" + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(unreachable - code) +RAPIDJSON_DIAG_OFF(missing - noreturn) +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +//! Represents an in-memory input byte stream. +/*! + This class is mainly for being wrapped by EncodedInputStream or + AutoUTFInputStream. + + It is similar to FileReadBuffer but the source is an in-memory buffer + instead of a file. + + Differences between MemoryStream and StringStream: + 1. StringStream has encoding but MemoryStream is a byte stream. + 2. MemoryStream needs size of the source buffer and the buffer don't need to + be null terminated. StringStream assume null-terminated string as source. + 3. MemoryStream supports Peek4() for encoding detection. StringStream is + specified with an encoding so it should not have Peek4(). \note implements + Stream concept +*/ +struct MemoryStream { + typedef char Ch; // byte + + MemoryStream(const Ch *src, size_t size) + : src_(src), begin_(src), end_(src + size), size_(size) {} + + Ch Peek() const { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_; } + Ch Take() { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_++; } + size_t Tell() const { return static_cast(src_ - begin_); } + + Ch *PutBegin() { + RAPIDJSON_ASSERT(false); + return 0; + } + void Put(Ch) { RAPIDJSON_ASSERT(false); } + void Flush() { RAPIDJSON_ASSERT(false); } + size_t PutEnd(Ch *) { + RAPIDJSON_ASSERT(false); + return 0; + } + + // For encoding detection only. + const Ch *Peek4() const { return Tell() + 4 <= size_ ? src_ : 0; } + + const Ch *src_; //!< Current read position. + const Ch *begin_; //!< Original head of the string. + const Ch *end_; //!< End of stream. + size_t size_; //!< Size of the stream. +}; + +RAPIDJSON_NAMESPACE_END + +#ifdef __clang__ +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_MEMORYBUFFER_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/msinttypes/inttypes.h b/src/livox_ros_driver2/3rdparty/rapidjson/msinttypes/inttypes.h new file mode 100644 index 0000000..bc32dad --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/msinttypes/inttypes.h @@ -0,0 +1,316 @@ +// ISO C9x compliant inttypes.h for Microsoft Visual Studio +// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124 +// +// Copyright (c) 2006-2013 Alexander Chemeris +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// 3. Neither the name of the product nor the names of its contributors may +// be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED +// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO +// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////////// + +// The above software in this distribution may have been modified by +// THL A29 Limited ("Tencent Modifications"). +// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited. + +#ifndef _MSC_VER // [ +#error "Use this header only with Microsoft Visual C++ compilers!" +#endif // _MSC_VER ] + +#ifndef _MSC_INTTYPES_H_ // [ +#define _MSC_INTTYPES_H_ + +#if _MSC_VER > 1000 +#pragma once +#endif + +#include "stdint.h" + +// miloyip: VC supports inttypes.h since VC2013 +#if _MSC_VER >= 1800 +#include +#else + +// 7.8 Format conversion of integer types + +typedef struct { + intmax_t quot; + intmax_t rem; +} imaxdiv_t; + +// 7.8.1 Macros for format specifiers + +#if !defined(__cplusplus) || \ + defined(__STDC_FORMAT_MACROS) // [ See footnote 185 at page 198 + +// The fprintf macros for signed integers are: +#define PRId8 "d" +#define PRIi8 "i" +#define PRIdLEAST8 "d" +#define PRIiLEAST8 "i" +#define PRIdFAST8 "d" +#define PRIiFAST8 "i" + +#define PRId16 "hd" +#define PRIi16 "hi" +#define PRIdLEAST16 "hd" +#define PRIiLEAST16 "hi" +#define PRIdFAST16 "hd" +#define PRIiFAST16 "hi" + +#define PRId32 "I32d" +#define PRIi32 "I32i" +#define PRIdLEAST32 "I32d" +#define PRIiLEAST32 "I32i" +#define PRIdFAST32 "I32d" +#define PRIiFAST32 "I32i" + +#define PRId64 "I64d" +#define PRIi64 "I64i" +#define PRIdLEAST64 "I64d" +#define PRIiLEAST64 "I64i" +#define PRIdFAST64 "I64d" +#define PRIiFAST64 "I64i" + +#define PRIdMAX "I64d" +#define PRIiMAX "I64i" + +#define PRIdPTR "Id" +#define PRIiPTR "Ii" + +// The fprintf macros for unsigned integers are: +#define PRIo8 "o" +#define PRIu8 "u" +#define PRIx8 "x" +#define PRIX8 "X" +#define PRIoLEAST8 "o" +#define PRIuLEAST8 "u" +#define PRIxLEAST8 "x" +#define PRIXLEAST8 "X" +#define PRIoFAST8 "o" +#define PRIuFAST8 "u" +#define PRIxFAST8 "x" +#define PRIXFAST8 "X" + +#define PRIo16 "ho" +#define PRIu16 "hu" +#define PRIx16 "hx" +#define PRIX16 "hX" +#define PRIoLEAST16 "ho" +#define PRIuLEAST16 "hu" +#define PRIxLEAST16 "hx" +#define PRIXLEAST16 "hX" +#define PRIoFAST16 "ho" +#define PRIuFAST16 "hu" +#define PRIxFAST16 "hx" +#define PRIXFAST16 "hX" + +#define PRIo32 "I32o" +#define PRIu32 "I32u" +#define PRIx32 "I32x" +#define PRIX32 "I32X" +#define PRIoLEAST32 "I32o" +#define PRIuLEAST32 "I32u" +#define PRIxLEAST32 "I32x" +#define PRIXLEAST32 "I32X" +#define PRIoFAST32 "I32o" +#define PRIuFAST32 "I32u" +#define PRIxFAST32 "I32x" +#define PRIXFAST32 "I32X" + +#define PRIo64 "I64o" +#define PRIu64 "I64u" +#define PRIx64 "I64x" +#define PRIX64 "I64X" +#define PRIoLEAST64 "I64o" +#define PRIuLEAST64 "I64u" +#define PRIxLEAST64 "I64x" +#define PRIXLEAST64 "I64X" +#define PRIoFAST64 "I64o" +#define PRIuFAST64 "I64u" +#define PRIxFAST64 "I64x" +#define PRIXFAST64 "I64X" + +#define PRIoMAX "I64o" +#define PRIuMAX "I64u" +#define PRIxMAX "I64x" +#define PRIXMAX "I64X" + +#define PRIoPTR "Io" +#define PRIuPTR "Iu" +#define PRIxPTR "Ix" +#define PRIXPTR "IX" + +// The fscanf macros for signed integers are: +#define SCNd8 "d" +#define SCNi8 "i" +#define SCNdLEAST8 "d" +#define SCNiLEAST8 "i" +#define SCNdFAST8 "d" +#define SCNiFAST8 "i" + +#define SCNd16 "hd" +#define SCNi16 "hi" +#define SCNdLEAST16 "hd" +#define SCNiLEAST16 "hi" +#define SCNdFAST16 "hd" +#define SCNiFAST16 "hi" + +#define SCNd32 "ld" +#define SCNi32 "li" +#define SCNdLEAST32 "ld" +#define SCNiLEAST32 "li" +#define SCNdFAST32 "ld" +#define SCNiFAST32 "li" + +#define SCNd64 "I64d" +#define SCNi64 "I64i" +#define SCNdLEAST64 "I64d" +#define SCNiLEAST64 "I64i" +#define SCNdFAST64 "I64d" +#define SCNiFAST64 "I64i" + +#define SCNdMAX "I64d" +#define SCNiMAX "I64i" + +#ifdef _WIN64 // [ +#define SCNdPTR "I64d" +#define SCNiPTR "I64i" +#else // _WIN64 ][ +#define SCNdPTR "ld" +#define SCNiPTR "li" +#endif // _WIN64 ] + +// The fscanf macros for unsigned integers are: +#define SCNo8 "o" +#define SCNu8 "u" +#define SCNx8 "x" +#define SCNX8 "X" +#define SCNoLEAST8 "o" +#define SCNuLEAST8 "u" +#define SCNxLEAST8 "x" +#define SCNXLEAST8 "X" +#define SCNoFAST8 "o" +#define SCNuFAST8 "u" +#define SCNxFAST8 "x" +#define SCNXFAST8 "X" + +#define SCNo16 "ho" +#define SCNu16 "hu" +#define SCNx16 "hx" +#define SCNX16 "hX" +#define SCNoLEAST16 "ho" +#define SCNuLEAST16 "hu" +#define SCNxLEAST16 "hx" +#define SCNXLEAST16 "hX" +#define SCNoFAST16 "ho" +#define SCNuFAST16 "hu" +#define SCNxFAST16 "hx" +#define SCNXFAST16 "hX" + +#define SCNo32 "lo" +#define SCNu32 "lu" +#define SCNx32 "lx" +#define SCNX32 "lX" +#define SCNoLEAST32 "lo" +#define SCNuLEAST32 "lu" +#define SCNxLEAST32 "lx" +#define SCNXLEAST32 "lX" +#define SCNoFAST32 "lo" +#define SCNuFAST32 "lu" +#define SCNxFAST32 "lx" +#define SCNXFAST32 "lX" + +#define SCNo64 "I64o" +#define SCNu64 "I64u" +#define SCNx64 "I64x" +#define SCNX64 "I64X" +#define SCNoLEAST64 "I64o" +#define SCNuLEAST64 "I64u" +#define SCNxLEAST64 "I64x" +#define SCNXLEAST64 "I64X" +#define SCNoFAST64 "I64o" +#define SCNuFAST64 "I64u" +#define SCNxFAST64 "I64x" +#define SCNXFAST64 "I64X" + +#define SCNoMAX "I64o" +#define SCNuMAX "I64u" +#define SCNxMAX "I64x" +#define SCNXMAX "I64X" + +#ifdef _WIN64 // [ +#define SCNoPTR "I64o" +#define SCNuPTR "I64u" +#define SCNxPTR "I64x" +#define SCNXPTR "I64X" +#else // _WIN64 ][ +#define SCNoPTR "lo" +#define SCNuPTR "lu" +#define SCNxPTR "lx" +#define SCNXPTR "lX" +#endif // _WIN64 ] + +#endif // __STDC_FORMAT_MACROS ] + +// 7.8.2 Functions for greatest-width integer types + +// 7.8.2.1 The imaxabs function +#define imaxabs _abs64 + +// 7.8.2.2 The imaxdiv function + +// This is modified version of div() function from Microsoft's div.c found +// in %MSVC.NET%\crt\src\div.c +#ifdef STATIC_IMAXDIV // [ +static +#else // STATIC_IMAXDIV ][ +_inline +#endif // STATIC_IMAXDIV ] + imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) { + imaxdiv_t result; + + result.quot = numer / denom; + result.rem = numer % denom; + + if (numer < 0 && result.rem > 0) { + // did division wrong; must fix up + ++result.quot; + result.rem -= denom; + } + + return result; +} + +// 7.8.2.3 The strtoimax and strtoumax functions +#define strtoimax _strtoi64 +#define strtoumax _strtoui64 + +// 7.8.2.4 The wcstoimax and wcstoumax functions +#define wcstoimax _wcstoi64 +#define wcstoumax _wcstoui64 + +#endif // _MSC_VER >= 1800 + +#endif // _MSC_INTTYPES_H_ ] diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/msinttypes/stdint.h b/src/livox_ros_driver2/3rdparty/rapidjson/msinttypes/stdint.h new file mode 100644 index 0000000..8fd0655 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/msinttypes/stdint.h @@ -0,0 +1,301 @@ +// ISO C9x compliant stdint.h for Microsoft Visual Studio +// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124 +// +// Copyright (c) 2006-2013 Alexander Chemeris +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// 3. Neither the name of the product nor the names of its contributors may +// be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED +// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO +// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; +// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, +// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR +// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////////// + +// The above software in this distribution may have been modified by +// THL A29 Limited ("Tencent Modifications"). +// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited. + +#ifndef _MSC_VER // [ +#error "Use this header only with Microsoft Visual C++ compilers!" +#endif // _MSC_VER ] + +#ifndef _MSC_STDINT_H_ // [ +#define _MSC_STDINT_H_ + +#if _MSC_VER > 1000 +#pragma once +#endif + +// miloyip: Originally Visual Studio 2010 uses its own stdint.h. However it +// generates warning with INT64_C(), so change to use this file for vs2010. +#if _MSC_VER >= 1600 // [ +#include + +#if !defined(__cplusplus) || \ + defined(__STDC_CONSTANT_MACROS) // [ See footnote 224 at page 260 + +#undef INT8_C +#undef INT16_C +#undef INT32_C +#undef INT64_C +#undef UINT8_C +#undef UINT16_C +#undef UINT32_C +#undef UINT64_C + +// 7.18.4.1 Macros for minimum-width integer constants + +#define INT8_C(val) val##i8 +#define INT16_C(val) val##i16 +#define INT32_C(val) val##i32 +#define INT64_C(val) val##i64 + +#define UINT8_C(val) val##ui8 +#define UINT16_C(val) val##ui16 +#define UINT32_C(val) val##ui32 +#define UINT64_C(val) val##ui64 + +// 7.18.4.2 Macros for greatest-width integer constants +// These #ifndef's are needed to prevent collisions with . +// Check out Issue 9 for the details. +#ifndef INTMAX_C // [ +#define INTMAX_C INT64_C +#endif // INTMAX_C ] +#ifndef UINTMAX_C // [ +#define UINTMAX_C UINT64_C +#endif // UINTMAX_C ] + +#endif // __STDC_CONSTANT_MACROS ] + +#else // ] _MSC_VER >= 1700 [ + +#include + +// For Visual Studio 6 in C++ mode and for many Visual Studio versions when +// compiling for ARM we have to wrap include with 'extern "C++" {}' +// or compiler would give many errors like this: +// error C2733: second C linkage of overloaded function 'wmemchr' not allowed +#if defined(__cplusplus) && !defined(_M_ARM) +extern "C" { +#endif +#include +#if defined(__cplusplus) && !defined(_M_ARM) +} +#endif + +// Define _W64 macros to mark types changing their size, like intptr_t. +#ifndef _W64 +#if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300 +#define _W64 __w64 +#else +#define _W64 +#endif +#endif + +// 7.18.1 Integer types + +// 7.18.1.1 Exact-width integer types + +// Visual Studio 6 and Embedded Visual C++ 4 doesn't +// realize that, e.g. char has the same size as __int8 +// so we give up on __intX for them. +#if (_MSC_VER < 1300) +typedef signed char int8_t; +typedef signed short int16_t; +typedef signed int int32_t; +typedef unsigned char uint8_t; +typedef unsigned short uint16_t; +typedef unsigned int uint32_t; +#else +typedef signed __int8 int8_t; +typedef signed __int16 int16_t; +typedef signed __int32 int32_t; +typedef unsigned __int8 uint8_t; +typedef unsigned __int16 uint16_t; +typedef unsigned __int32 uint32_t; +#endif +typedef signed __int64 int64_t; +typedef unsigned __int64 uint64_t; + +// 7.18.1.2 Minimum-width integer types +typedef int8_t int_least8_t; +typedef int16_t int_least16_t; +typedef int32_t int_least32_t; +typedef int64_t int_least64_t; +typedef uint8_t uint_least8_t; +typedef uint16_t uint_least16_t; +typedef uint32_t uint_least32_t; +typedef uint64_t uint_least64_t; + +// 7.18.1.3 Fastest minimum-width integer types +typedef int8_t int_fast8_t; +typedef int16_t int_fast16_t; +typedef int32_t int_fast32_t; +typedef int64_t int_fast64_t; +typedef uint8_t uint_fast8_t; +typedef uint16_t uint_fast16_t; +typedef uint32_t uint_fast32_t; +typedef uint64_t uint_fast64_t; + +// 7.18.1.4 Integer types capable of holding object pointers +#ifdef _WIN64 // [ +typedef signed __int64 intptr_t; +typedef unsigned __int64 uintptr_t; +#else // _WIN64 ][ +typedef _W64 signed int intptr_t; +typedef _W64 unsigned int uintptr_t; +#endif // _WIN64 ] + +// 7.18.1.5 Greatest-width integer types +typedef int64_t intmax_t; +typedef uint64_t uintmax_t; + +// 7.18.2 Limits of specified-width integer types + +#if !defined(__cplusplus) || \ + defined(__STDC_LIMIT_MACROS) // [ See footnote 220 at page 257 and +// footnote 221 at page 259 + +// 7.18.2.1 Limits of exact-width integer types +#define INT8_MIN ((int8_t)_I8_MIN) +#define INT8_MAX _I8_MAX +#define INT16_MIN ((int16_t)_I16_MIN) +#define INT16_MAX _I16_MAX +#define INT32_MIN ((int32_t)_I32_MIN) +#define INT32_MAX _I32_MAX +#define INT64_MIN ((int64_t)_I64_MIN) +#define INT64_MAX _I64_MAX +#define UINT8_MAX _UI8_MAX +#define UINT16_MAX _UI16_MAX +#define UINT32_MAX _UI32_MAX +#define UINT64_MAX _UI64_MAX + +// 7.18.2.2 Limits of minimum-width integer types +#define INT_LEAST8_MIN INT8_MIN +#define INT_LEAST8_MAX INT8_MAX +#define INT_LEAST16_MIN INT16_MIN +#define INT_LEAST16_MAX INT16_MAX +#define INT_LEAST32_MIN INT32_MIN +#define INT_LEAST32_MAX INT32_MAX +#define INT_LEAST64_MIN INT64_MIN +#define INT_LEAST64_MAX INT64_MAX +#define UINT_LEAST8_MAX UINT8_MAX +#define UINT_LEAST16_MAX UINT16_MAX +#define UINT_LEAST32_MAX UINT32_MAX +#define UINT_LEAST64_MAX UINT64_MAX + +// 7.18.2.3 Limits of fastest minimum-width integer types +#define INT_FAST8_MIN INT8_MIN +#define INT_FAST8_MAX INT8_MAX +#define INT_FAST16_MIN INT16_MIN +#define INT_FAST16_MAX INT16_MAX +#define INT_FAST32_MIN INT32_MIN +#define INT_FAST32_MAX INT32_MAX +#define INT_FAST64_MIN INT64_MIN +#define INT_FAST64_MAX INT64_MAX +#define UINT_FAST8_MAX UINT8_MAX +#define UINT_FAST16_MAX UINT16_MAX +#define UINT_FAST32_MAX UINT32_MAX +#define UINT_FAST64_MAX UINT64_MAX + +// 7.18.2.4 Limits of integer types capable of holding object pointers +#ifdef _WIN64 // [ +#define INTPTR_MIN INT64_MIN +#define INTPTR_MAX INT64_MAX +#define UINTPTR_MAX UINT64_MAX +#else // _WIN64 ][ +#define INTPTR_MIN INT32_MIN +#define INTPTR_MAX INT32_MAX +#define UINTPTR_MAX UINT32_MAX +#endif // _WIN64 ] + +// 7.18.2.5 Limits of greatest-width integer types +#define INTMAX_MIN INT64_MIN +#define INTMAX_MAX INT64_MAX +#define UINTMAX_MAX UINT64_MAX + +// 7.18.3 Limits of other integer types + +#ifdef _WIN64 // [ +#define PTRDIFF_MIN _I64_MIN +#define PTRDIFF_MAX _I64_MAX +#else // _WIN64 ][ +#define PTRDIFF_MIN _I32_MIN +#define PTRDIFF_MAX _I32_MAX +#endif // _WIN64 ] + +#define SIG_ATOMIC_MIN INT_MIN +#define SIG_ATOMIC_MAX INT_MAX + +#ifndef SIZE_MAX // [ +#ifdef _WIN64 // [ +#define SIZE_MAX _UI64_MAX +#else // _WIN64 ][ +#define SIZE_MAX _UI32_MAX +#endif // _WIN64 ] +#endif // SIZE_MAX ] + +// WCHAR_MIN and WCHAR_MAX are also defined in +#ifndef WCHAR_MIN // [ +#define WCHAR_MIN 0 +#endif // WCHAR_MIN ] +#ifndef WCHAR_MAX // [ +#define WCHAR_MAX _UI16_MAX +#endif // WCHAR_MAX ] + +#define WINT_MIN 0 +#define WINT_MAX _UI16_MAX + +#endif // __STDC_LIMIT_MACROS ] + +// 7.18.4 Limits of other integer types + +#if !defined(__cplusplus) || \ + defined(__STDC_CONSTANT_MACROS) // [ See footnote 224 at page 260 + +// 7.18.4.1 Macros for minimum-width integer constants + +#define INT8_C(val) val##i8 +#define INT16_C(val) val##i16 +#define INT32_C(val) val##i32 +#define INT64_C(val) val##i64 + +#define UINT8_C(val) val##ui8 +#define UINT16_C(val) val##ui16 +#define UINT32_C(val) val##ui32 +#define UINT64_C(val) val##ui64 + +// 7.18.4.2 Macros for greatest-width integer constants +// These #ifndef's are needed to prevent collisions with . +// Check out Issue 9 for the details. +#ifndef INTMAX_C // [ +#define INTMAX_C INT64_C +#endif // INTMAX_C ] +#ifndef UINTMAX_C // [ +#define UINTMAX_C UINT64_C +#endif // UINTMAX_C ] + +#endif // __STDC_CONSTANT_MACROS ] + +#endif // _MSC_VER >= 1600 ] + +#endif // _MSC_STDINT_H_ ] diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/ostreamwrapper.h b/src/livox_ros_driver2/3rdparty/rapidjson/ostreamwrapper.h new file mode 100644 index 0000000..56c50a7 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/ostreamwrapper.h @@ -0,0 +1,96 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_OSTREAMWRAPPER_H_ +#define RAPIDJSON_OSTREAMWRAPPER_H_ + +#include +#include "stream.h" + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(padded) +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +//! Wrapper of \c std::basic_ostream into RapidJSON's Stream concept. +/*! + The classes can be wrapped including but not limited to: + + - \c std::ostringstream + - \c std::stringstream + - \c std::wpstringstream + - \c std::wstringstream + - \c std::ifstream + - \c std::fstream + - \c std::wofstream + - \c std::wfstream + + \tparam StreamType Class derived from \c std::basic_ostream. +*/ + +template +class BasicOStreamWrapper { + public: + typedef typename StreamType::char_type Ch; + BasicOStreamWrapper(StreamType &stream) : stream_(stream) {} + + void Put(Ch c) { stream_.put(c); } + + void Flush() { stream_.flush(); } + + // Not implemented + char Peek() const { + RAPIDJSON_ASSERT(false); + return 0; + } + char Take() { + RAPIDJSON_ASSERT(false); + return 0; + } + size_t Tell() const { + RAPIDJSON_ASSERT(false); + return 0; + } + char *PutBegin() { + RAPIDJSON_ASSERT(false); + return 0; + } + size_t PutEnd(char *) { + RAPIDJSON_ASSERT(false); + return 0; + } + + private: + BasicOStreamWrapper(const BasicOStreamWrapper &); + BasicOStreamWrapper &operator=(const BasicOStreamWrapper &); + + StreamType &stream_; +}; + +typedef BasicOStreamWrapper OStreamWrapper; +typedef BasicOStreamWrapper WOStreamWrapper; + +#ifdef __clang__ +RAPIDJSON_DIAG_POP +#endif + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_OSTREAMWRAPPER_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/pointer.h b/src/livox_ros_driver2/3rdparty/rapidjson/pointer.h new file mode 100644 index 0000000..218dd6c --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/pointer.h @@ -0,0 +1,1712 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_POINTER_H_ +#define RAPIDJSON_POINTER_H_ + +#include "document.h" +#include "internal/itoa.h" + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(switch - enum) +#elif defined(_MSC_VER) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +static const SizeType kPointerInvalidIndex = + ~SizeType(0); //!< Represents an invalid index in GenericPointer::Token + +//! Error code of parsing. +/*! \ingroup RAPIDJSON_ERRORS + \see GenericPointer::GenericPointer, GenericPointer::GetParseErrorCode +*/ +enum PointerParseErrorCode { + kPointerParseErrorNone = 0, //!< The parse is successful + + kPointerParseErrorTokenMustBeginWithSolidus, //!< A token must begin with a + //!< '/' + kPointerParseErrorInvalidEscape, //!< Invalid escape + kPointerParseErrorInvalidPercentEncoding, //!< Invalid percent encoding in + //!URI + //!< fragment + kPointerParseErrorCharacterMustPercentEncode //!< A character must percent + //!< encoded in URI fragment +}; + +/////////////////////////////////////////////////////////////////////////////// +// GenericPointer + +//! Represents a JSON Pointer. Use Pointer for UTF8 encoding and default +//! allocator. +/*! + This class implements RFC 6901 "JavaScript Object Notation (JSON) Pointer" + (https://tools.ietf.org/html/rfc6901). + + A JSON pointer is for identifying a specific value in a JSON document + (GenericDocument). It can simplify coding of DOM tree manipulation, because + it can access multiple-level depth of DOM tree with single API call. + + After it parses a string representation (e.g. "/foo/0" or URI fragment + representation (e.g. "#/foo/0") into its internal representation (tokens), + it can be used to resolve a specific value in multiple documents, or + sub-tree of documents. + + Contrary to GenericValue, Pointer can be copy constructed and copy assigned. + Apart from assignment, a Pointer cannot be modified after construction. + + Although Pointer is very convenient, please aware that constructing Pointer + involves parsing and dynamic memory allocation. A special constructor with + user- supplied tokens eliminates these. + + GenericPointer depends on GenericDocument and GenericValue. + + \tparam ValueType The value type of the DOM tree. E.g. GenericValue > + \tparam Allocator The allocator type for allocating memory for internal + representation. + + \note GenericPointer uses same encoding of ValueType. + However, Allocator of GenericPointer is independent of Allocator of Value. +*/ +template +class GenericPointer { + public: + typedef typename ValueType::EncodingType + EncodingType; //!< Encoding type from Value + typedef typename ValueType::Ch Ch; //!< Character type from Value + + //! A token is the basic units of internal representation. + /*! + A JSON pointer string representation "/foo/123" is parsed to two tokens: + "foo" and 123. 123 will be represented in both numeric form and string + form. They are resolved according to the actual value type (object or + array). + + For token that are not numbers, or the numeric value is out of bound + (greater than limits of SizeType), they are only treated as string form + (i.e. the token's index will be equal to kPointerInvalidIndex). + + This struct is public so that user can create a Pointer without parsing + and allocation, using a special constructor. + */ + struct Token { + const Ch + *name; //!< Name of the token. It has null character at the end but + //!< it can contain null character. + SizeType length; //!< Length of the name. + SizeType index; //!< A valid array index, if it is not equal to + //!< kPointerInvalidIndex. + }; + + //!@name Constructors and destructor. + //@{ + + //! Default constructor. + GenericPointer(Allocator *allocator = 0) + : allocator_(allocator), + ownAllocator_(), + nameBuffer_(), + tokens_(), + tokenCount_(), + parseErrorOffset_(), + parseErrorCode_(kPointerParseErrorNone) {} + + //! Constructor that parses a string or URI fragment representation. + /*! + \param source A null-terminated, string or URI fragment representation of + JSON pointer. \param allocator User supplied allocator for this pointer. If + no allocator is provided, it creates a self-owned one. + */ + explicit GenericPointer(const Ch *source, Allocator *allocator = 0) + : allocator_(allocator), + ownAllocator_(), + nameBuffer_(), + tokens_(), + tokenCount_(), + parseErrorOffset_(), + parseErrorCode_(kPointerParseErrorNone) { + Parse(source, internal::StrLen(source)); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Constructor that parses a string or URI fragment representation. + /*! + \param source A string or URI fragment representation of JSON pointer. + \param allocator User supplied allocator for this pointer. If no allocator + is provided, it creates a self-owned one. \note Requires the definition of + the preprocessor symbol \ref RAPIDJSON_HAS_STDSTRING. + */ + explicit GenericPointer(const std::basic_string &source, + Allocator *allocator = 0) + : allocator_(allocator), + ownAllocator_(), + nameBuffer_(), + tokens_(), + tokenCount_(), + parseErrorOffset_(), + parseErrorCode_(kPointerParseErrorNone) { + Parse(source.c_str(), source.size()); + } +#endif + + //! Constructor that parses a string or URI fragment representation, with + //! length of the source string. + /*! + \param source A string or URI fragment representation of JSON pointer. + \param length Length of source. + \param allocator User supplied allocator for this pointer. If no allocator + is provided, it creates a self-owned one. \note Slightly faster than the + overload without length. + */ + GenericPointer(const Ch *source, size_t length, Allocator *allocator = 0) + : allocator_(allocator), + ownAllocator_(), + nameBuffer_(), + tokens_(), + tokenCount_(), + parseErrorOffset_(), + parseErrorCode_(kPointerParseErrorNone) { + Parse(source, length); + } + + //! Constructor with user-supplied tokens. + /*! + This constructor let user supplies const array of tokens. + This prevents the parsing process and eliminates allocation. + This is preferred for memory constrained environments. + + \param tokens An constant array of tokens representing the JSON pointer. + \param tokenCount Number of tokens. + + \b Example + \code + #define NAME(s) { s, sizeof(s) / sizeof(s[0]) - 1, kPointerInvalidIndex } + #define INDEX(i) { #i, sizeof(#i) - 1, i } + + static const Pointer::Token kTokens[] = { NAME("foo"), INDEX(123) }; + static const Pointer p(kTokens, sizeof(kTokens) / sizeof(kTokens[0])); + // Equivalent to static const Pointer p("/foo/123"); + + #undef NAME + #undef INDEX + \endcode + */ + GenericPointer(const Token *tokens, size_t tokenCount) + : allocator_(), + ownAllocator_(), + nameBuffer_(), + tokens_(const_cast(tokens)), + tokenCount_(tokenCount), + parseErrorOffset_(), + parseErrorCode_(kPointerParseErrorNone) {} + + //! Copy constructor. + GenericPointer(const GenericPointer &rhs) + : allocator_(rhs.allocator_), + ownAllocator_(), + nameBuffer_(), + tokens_(), + tokenCount_(), + parseErrorOffset_(), + parseErrorCode_(kPointerParseErrorNone) { + *this = rhs; + } + + //! Copy constructor. + GenericPointer(const GenericPointer &rhs, Allocator *allocator) + : allocator_(allocator), + ownAllocator_(), + nameBuffer_(), + tokens_(), + tokenCount_(), + parseErrorOffset_(), + parseErrorCode_(kPointerParseErrorNone) { + *this = rhs; + } + + //! Destructor. + ~GenericPointer() { + if (nameBuffer_) // If user-supplied tokens constructor is used, + // nameBuffer_ + // is nullptr and tokens_ are not deallocated. + Allocator::Free(tokens_); + RAPIDJSON_DELETE(ownAllocator_); + } + + //! Assignment operator. + GenericPointer &operator=(const GenericPointer &rhs) { + if (this != &rhs) { + // Do not delete ownAllcator + if (nameBuffer_) Allocator::Free(tokens_); + + tokenCount_ = rhs.tokenCount_; + parseErrorOffset_ = rhs.parseErrorOffset_; + parseErrorCode_ = rhs.parseErrorCode_; + + if (rhs.nameBuffer_) + CopyFromRaw(rhs); // Normally parsed tokens. + else { + tokens_ = rhs.tokens_; // User supplied const tokens. + nameBuffer_ = 0; + } + } + return *this; + } + + //! Swap the content of this pointer with an other. + /*! + \param other The pointer to swap with. + \note Constant complexity. + */ + GenericPointer &Swap(GenericPointer &other) RAPIDJSON_NOEXCEPT { + internal::Swap(allocator_, other.allocator_); + internal::Swap(ownAllocator_, other.ownAllocator_); + internal::Swap(nameBuffer_, other.nameBuffer_); + internal::Swap(tokens_, other.tokens_); + internal::Swap(tokenCount_, other.tokenCount_); + internal::Swap(parseErrorOffset_, other.parseErrorOffset_); + internal::Swap(parseErrorCode_, other.parseErrorCode_); + return *this; + } + + //! free-standing swap function helper + /*! + Helper function to enable support for common swap implementation pattern + based on \c std::swap: \code void swap(MyClass& a, MyClass& b) { using + std::swap; swap(a.pointer, b.pointer); + // ... + } + \endcode + \see Swap() + */ + friend inline void swap(GenericPointer &a, + GenericPointer &b) RAPIDJSON_NOEXCEPT { + a.Swap(b); + } + + //@} + + //!@name Append token + //@{ + + //! Append a token and return a new Pointer + /*! + \param token Token to be appended. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + GenericPointer Append(const Token &token, Allocator *allocator = 0) const { + GenericPointer r; + r.allocator_ = allocator; + Ch *p = r.CopyFromRaw(*this, 1, token.length + 1); + std::memcpy(p, token.name, (token.length + 1) * sizeof(Ch)); + r.tokens_[tokenCount_].name = p; + r.tokens_[tokenCount_].length = token.length; + r.tokens_[tokenCount_].index = token.index; + return r; + } + + //! Append a name token with length, and return a new Pointer + /*! + \param name Name to be appended. + \param length Length of name. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + GenericPointer Append(const Ch *name, SizeType length, + Allocator *allocator = 0) const { + Token token = {name, length, kPointerInvalidIndex}; + return Append(token, allocator); + } + + //! Append a name token without length, and return a new Pointer + /*! + \param name Name (const Ch*) to be appended. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::NotExpr< + internal::IsSame::Type, Ch>>), + (GenericPointer)) + Append(T *name, Allocator *allocator = 0) const { + return Append(name, internal::StrLen(name), allocator); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Append a name token, and return a new Pointer + /*! + \param name Name to be appended. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + GenericPointer Append(const std::basic_string &name, + Allocator *allocator = 0) const { + return Append(name.c_str(), static_cast(name.size()), allocator); + } +#endif + + //! Append a index token, and return a new Pointer + /*! + \param index Index to be appended. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + GenericPointer Append(SizeType index, Allocator *allocator = 0) const { + char buffer[21]; + char *end = sizeof(SizeType) == 4 ? internal::u32toa(index, buffer) + : internal::u64toa(index, buffer); + SizeType length = static_cast(end - buffer); + buffer[length] = '\0'; + + if (sizeof(Ch) == 1) { + Token token = {reinterpret_cast(buffer), length, index}; + return Append(token, allocator); + } else { + Ch name[21]; + for (size_t i = 0; i <= length; i++) name[i] = static_cast(buffer[i]); + Token token = {name, length, index}; + return Append(token, allocator); + } + } + + //! Append a token by value, and return a new Pointer + /*! + \param token token to be appended. + \param allocator Allocator for the newly return Pointer. + \return A new Pointer with appended token. + */ + GenericPointer Append(const ValueType &token, + Allocator *allocator = 0) const { + if (token.IsString()) + return Append(token.GetString(), token.GetStringLength(), allocator); + else { + RAPIDJSON_ASSERT(token.IsUint64()); + RAPIDJSON_ASSERT(token.GetUint64() <= SizeType(~0)); + return Append(static_cast(token.GetUint64()), allocator); + } + } + + //!@name Handling Parse Error + //@{ + + //! Check whether this is a valid pointer. + bool IsValid() const { return parseErrorCode_ == kPointerParseErrorNone; } + + //! Get the parsing error offset in code unit. + size_t GetParseErrorOffset() const { return parseErrorOffset_; } + + //! Get the parsing error code. + PointerParseErrorCode GetParseErrorCode() const { return parseErrorCode_; } + + //@} + + //! Get the allocator of this pointer. + Allocator &GetAllocator() { return *allocator_; } + + //!@name Tokens + //@{ + + //! Get the token array (const version only). + const Token *GetTokens() const { return tokens_; } + + //! Get the number of tokens. + size_t GetTokenCount() const { return tokenCount_; } + + //@} + + //!@name Equality/inequality operators + //@{ + + //! Equality operator. + /*! + \note When any pointers are invalid, always returns false. + */ + bool operator==(const GenericPointer &rhs) const { + if (!IsValid() || !rhs.IsValid() || tokenCount_ != rhs.tokenCount_) + return false; + + for (size_t i = 0; i < tokenCount_; i++) { + if (tokens_[i].index != rhs.tokens_[i].index || + tokens_[i].length != rhs.tokens_[i].length || + (tokens_[i].length != 0 && + std::memcmp(tokens_[i].name, rhs.tokens_[i].name, + sizeof(Ch) * tokens_[i].length) != 0)) { + return false; + } + } + + return true; + } + + //! Inequality operator. + /*! + \note When any pointers are invalid, always returns true. + */ + bool operator!=(const GenericPointer &rhs) const { return !(*this == rhs); } + + //! Less than operator. + /*! + \note Invalid pointers are always greater than valid ones. + */ + bool operator<(const GenericPointer &rhs) const { + if (!IsValid()) return false; + if (!rhs.IsValid()) return true; + + if (tokenCount_ != rhs.tokenCount_) return tokenCount_ < rhs.tokenCount_; + + for (size_t i = 0; i < tokenCount_; i++) { + if (tokens_[i].index != rhs.tokens_[i].index) + return tokens_[i].index < rhs.tokens_[i].index; + + if (tokens_[i].length != rhs.tokens_[i].length) + return tokens_[i].length < rhs.tokens_[i].length; + + if (int cmp = std::memcmp(tokens_[i].name, rhs.tokens_[i].name, + sizeof(Ch) * tokens_[i].length)) + return cmp < 0; + } + + return false; + } + + //@} + + //!@name Stringify + //@{ + + //! Stringify the pointer into string representation. + /*! + \tparam OutputStream Type of output stream. + \param os The output stream. + */ + template + bool Stringify(OutputStream &os) const { + return Stringify(os); + } + + //! Stringify the pointer into URI fragment representation. + /*! + \tparam OutputStream Type of output stream. + \param os The output stream. + */ + template + bool StringifyUriFragment(OutputStream &os) const { + return Stringify(os); + } + + //@} + + //!@name Create value + //@{ + + //! Create a value in a subtree. + /*! + If the value is not exist, it creates all parent values and a JSON Null + value. So it always succeed and return the newly created or existing value. + + Remind that it may change types of parents according to tokens, so it + potentially removes previously stored values. For example, if a document + was an array, and "/foo" is used to create a value, then the document + will be changed to an object, and all existing array elements are lost. + + \param root Root value of a DOM subtree to be resolved. It can be any + value other than document root. \param allocator Allocator for creating the + values if the specified value or its parents are not exist. \param + alreadyExist If non-null, it stores whether the resolved value is already + exist. \return The resolved newly created (a JSON Null value), or already + exists value. + */ + ValueType &Create(ValueType &root, + typename ValueType::AllocatorType &allocator, + bool *alreadyExist = 0) const { + RAPIDJSON_ASSERT(IsValid()); + ValueType *v = &root; + bool exist = true; + for (const Token *t = tokens_; t != tokens_ + tokenCount_; ++t) { + if (v->IsArray() && t->name[0] == '-' && t->length == 1) { + v->PushBack(ValueType().Move(), allocator); + v = &((*v)[v->Size() - 1]); + exist = false; + } else { + if (t->index == kPointerInvalidIndex) { // must be object name + if (!v->IsObject()) v->SetObject(); // Change to Object + } else { // object name or array index + if (!v->IsArray() && !v->IsObject()) + v->SetArray(); // Change to Array + } + + if (v->IsArray()) { + if (t->index >= v->Size()) { + v->Reserve(t->index + 1, allocator); + while (t->index >= v->Size()) + v->PushBack(ValueType().Move(), allocator); + exist = false; + } + v = &((*v)[t->index]); + } else { + typename ValueType::MemberIterator m = + v->FindMember(GenericValue( + GenericStringRef(t->name, t->length))); + if (m == v->MemberEnd()) { + v->AddMember(ValueType(t->name, t->length, allocator).Move(), + ValueType().Move(), allocator); + m = v->MemberEnd(); + v = &(--m)->value; // Assumes AddMember() appends at the end + exist = false; + } else + v = &m->value; + } + } + } + + if (alreadyExist) *alreadyExist = exist; + + return *v; + } + + //! Creates a value in a document. + /*! + \param document A document to be resolved. + \param alreadyExist If non-null, it stores whether the resolved value is + already exist. \return The resolved newly created, or already exists value. + */ + template + ValueType &Create( + GenericDocument &document, + bool *alreadyExist = 0) const { + return Create(document, document.GetAllocator(), alreadyExist); + } + + //@} + + //!@name Query value + //@{ + + //! Query a value in a subtree. + /*! + \param root Root value of a DOM sub-tree to be resolved. It can be any + value other than document root. \param unresolvedTokenIndex If the pointer + cannot resolve a token in the pointer, this parameter can obtain the index + of unresolved token. \return Pointer to the value if it can be resolved. + Otherwise null. + + \note + There are only 3 situations when a value cannot be resolved: + 1. A value in the path is not an array nor object. + 2. An object value does not contain the token. + 3. A token is out of range of an array value. + + Use unresolvedTokenIndex to retrieve the token index. + */ + ValueType *Get(ValueType &root, size_t *unresolvedTokenIndex = 0) const { + RAPIDJSON_ASSERT(IsValid()); + ValueType *v = &root; + for (const Token *t = tokens_; t != tokens_ + tokenCount_; ++t) { + switch (v->GetType()) { + case kObjectType: { + typename ValueType::MemberIterator m = + v->FindMember(GenericValue( + GenericStringRef(t->name, t->length))); + if (m == v->MemberEnd()) break; + v = &m->value; + } + continue; + case kArrayType: + if (t->index == kPointerInvalidIndex || t->index >= v->Size()) break; + v = &((*v)[t->index]); + continue; + default: + break; + } + + // Error: unresolved token + if (unresolvedTokenIndex) + *unresolvedTokenIndex = static_cast(t - tokens_); + return 0; + } + return v; + } + + //! Query a const value in a const subtree. + /*! + \param root Root value of a DOM sub-tree to be resolved. It can be any + value other than document root. \return Pointer to the value if it can be + resolved. Otherwise null. + */ + const ValueType *Get(const ValueType &root, + size_t *unresolvedTokenIndex = 0) const { + return Get(const_cast(root), unresolvedTokenIndex); + } + + //@} + + //!@name Query a value with default + //@{ + + //! Query a value in a subtree with default value. + /*! + Similar to Get(), but if the specified value do not exists, it creates all + parents and clone the default value. So that this function always succeed. + + \param root Root value of a DOM sub-tree to be resolved. It can be any + value other than document root. \param defaultValue Default value to be + cloned if the value was not exists. \param allocator Allocator for creating + the values if the specified value or its parents are not exist. \see + Create() + */ + ValueType &GetWithDefault( + ValueType &root, const ValueType &defaultValue, + typename ValueType::AllocatorType &allocator) const { + bool alreadyExist; + ValueType &v = Create(root, allocator, &alreadyExist); + return alreadyExist ? v : v.CopyFrom(defaultValue, allocator); + } + + //! Query a value in a subtree with default null-terminated string. + ValueType &GetWithDefault( + ValueType &root, const Ch *defaultValue, + typename ValueType::AllocatorType &allocator) const { + bool alreadyExist; + ValueType &v = Create(root, allocator, &alreadyExist); + return alreadyExist ? v : v.SetString(defaultValue, allocator); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Query a value in a subtree with default std::basic_string. + ValueType &GetWithDefault( + ValueType &root, const std::basic_string &defaultValue, + typename ValueType::AllocatorType &allocator) const { + bool alreadyExist; + ValueType &v = Create(root, allocator, &alreadyExist); + return alreadyExist ? v : v.SetString(defaultValue, allocator); + } +#endif + + //! Query a value in a subtree with default primitive value. + /*! + \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t, + \c bool + */ + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (ValueType &)) + GetWithDefault(ValueType &root, T defaultValue, + typename ValueType::AllocatorType &allocator) const { + return GetWithDefault(root, ValueType(defaultValue).Move(), allocator); + } + + //! Query a value in a document with default value. + template + ValueType &GetWithDefault( + GenericDocument &document, + const ValueType &defaultValue) const { + return GetWithDefault(document, defaultValue, document.GetAllocator()); + } + + //! Query a value in a document with default null-terminated string. + template + ValueType &GetWithDefault( + GenericDocument &document, + const Ch *defaultValue) const { + return GetWithDefault(document, defaultValue, document.GetAllocator()); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Query a value in a document with default std::basic_string. + template + ValueType &GetWithDefault( + GenericDocument &document, + const std::basic_string &defaultValue) const { + return GetWithDefault(document, defaultValue, document.GetAllocator()); + } +#endif + + //! Query a value in a document with default primitive value. + /*! + \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t, + \c bool + */ + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (ValueType &)) + GetWithDefault( + GenericDocument &document, + T defaultValue) const { + return GetWithDefault(document, defaultValue, document.GetAllocator()); + } + + //@} + + //!@name Set a value + //@{ + + //! Set a value in a subtree, with move semantics. + /*! + It creates all parents if they are not exist or types are different to the + tokens. So this function always succeeds but potentially remove existing + values. + + \param root Root value of a DOM sub-tree to be resolved. It can be any + value other than document root. \param value Value to be set. \param + allocator Allocator for creating the values if the specified value or its + parents are not exist. \see Create() + */ + ValueType &Set(ValueType &root, ValueType &value, + typename ValueType::AllocatorType &allocator) const { + return Create(root, allocator) = value; + } + + //! Set a value in a subtree, with copy semantics. + ValueType &Set(ValueType &root, const ValueType &value, + typename ValueType::AllocatorType &allocator) const { + return Create(root, allocator).CopyFrom(value, allocator); + } + + //! Set a null-terminated string in a subtree. + ValueType &Set(ValueType &root, const Ch *value, + typename ValueType::AllocatorType &allocator) const { + return Create(root, allocator) = ValueType(value, allocator).Move(); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Set a std::basic_string in a subtree. + ValueType &Set(ValueType &root, const std::basic_string &value, + typename ValueType::AllocatorType &allocator) const { + return Create(root, allocator) = ValueType(value, allocator).Move(); + } +#endif + + //! Set a primitive value in a subtree. + /*! + \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t, + \c bool + */ + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (ValueType &)) + Set(ValueType &root, T value, + typename ValueType::AllocatorType &allocator) const { + return Create(root, allocator) = ValueType(value).Move(); + } + + //! Set a value in a document, with move semantics. + template + ValueType &Set( + GenericDocument &document, + ValueType &value) const { + return Create(document) = value; + } + + //! Set a value in a document, with copy semantics. + template + ValueType &Set( + GenericDocument &document, + const ValueType &value) const { + return Create(document).CopyFrom(value, document.GetAllocator()); + } + + //! Set a null-terminated string in a document. + template + ValueType &Set( + GenericDocument &document, + const Ch *value) const { + return Create(document) = ValueType(value, document.GetAllocator()).Move(); + } + +#if RAPIDJSON_HAS_STDSTRING + //! Sets a std::basic_string in a document. + template + ValueType &Set( + GenericDocument &document, + const std::basic_string &value) const { + return Create(document) = ValueType(value, document.GetAllocator()).Move(); + } +#endif + + //! Set a primitive value in a document. + /*! + \tparam T Either \ref Type, \c int, \c unsigned, \c int64_t, \c uint64_t, \c + bool + */ + template + RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (ValueType &)) + Set(GenericDocument &document, + T value) const { + return Create(document) = value; + } + + //@} + + //!@name Swap a value + //@{ + + //! Swap a value with a value in a subtree. + /*! + It creates all parents if they are not exist or types are different to the + tokens. So this function always succeeds but potentially remove existing + values. + + \param root Root value of a DOM sub-tree to be resolved. It can be any + value other than document root. \param value Value to be swapped. \param + allocator Allocator for creating the values if the specified value or its + parents are not exist. \see Create() + */ + ValueType &Swap(ValueType &root, ValueType &value, + typename ValueType::AllocatorType &allocator) const { + return Create(root, allocator).Swap(value); + } + + //! Swap a value with a value in a document. + template + ValueType &Swap( + GenericDocument &document, + ValueType &value) const { + return Create(document).Swap(value); + } + + //@} + + //! Erase a value in a subtree. + /*! + \param root Root value of a DOM sub-tree to be resolved. It can be any + value other than document root. \return Whether the resolved value is found + and erased. + + \note Erasing with an empty pointer \c Pointer(""), i.e. the root, always + fail and return false. + */ + bool Erase(ValueType &root) const { + RAPIDJSON_ASSERT(IsValid()); + if (tokenCount_ == 0) // Cannot erase the root + return false; + + ValueType *v = &root; + const Token *last = tokens_ + (tokenCount_ - 1); + for (const Token *t = tokens_; t != last; ++t) { + switch (v->GetType()) { + case kObjectType: { + typename ValueType::MemberIterator m = + v->FindMember(GenericValue( + GenericStringRef(t->name, t->length))); + if (m == v->MemberEnd()) return false; + v = &m->value; + } break; + case kArrayType: + if (t->index == kPointerInvalidIndex || t->index >= v->Size()) + return false; + v = &((*v)[t->index]); + break; + default: + return false; + } + } + + switch (v->GetType()) { + case kObjectType: + return v->EraseMember(GenericStringRef(last->name, last->length)); + case kArrayType: + if (last->index == kPointerInvalidIndex || last->index >= v->Size()) + return false; + v->Erase(v->Begin() + last->index); + return true; + default: + return false; + } + } + + private: + //! Clone the content from rhs to this. + /*! + \param rhs Source pointer. + \param extraToken Extra tokens to be allocated. + \param extraNameBufferSize Extra name buffer size (in number of Ch) to be + allocated. \return Start of non-occupied name buffer, for storing extra + names. + */ + Ch *CopyFromRaw(const GenericPointer &rhs, size_t extraToken = 0, + size_t extraNameBufferSize = 0) { + if (!allocator_) // allocator is independently owned. + ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + + size_t nameBufferSize = rhs.tokenCount_; // null terminators for tokens + for (Token *t = rhs.tokens_; t != rhs.tokens_ + rhs.tokenCount_; ++t) + nameBufferSize += t->length; + + tokenCount_ = rhs.tokenCount_ + extraToken; + tokens_ = static_cast(allocator_->Malloc( + tokenCount_ * sizeof(Token) + + (nameBufferSize + extraNameBufferSize) * sizeof(Ch))); + nameBuffer_ = reinterpret_cast(tokens_ + tokenCount_); + if (rhs.tokenCount_ > 0) { + std::memcpy(tokens_, rhs.tokens_, rhs.tokenCount_ * sizeof(Token)); + } + if (nameBufferSize > 0) { + std::memcpy(nameBuffer_, rhs.nameBuffer_, nameBufferSize * sizeof(Ch)); + } + + // Adjust pointers to name buffer + std::ptrdiff_t diff = nameBuffer_ - rhs.nameBuffer_; + for (Token *t = tokens_; t != tokens_ + rhs.tokenCount_; ++t) + t->name += diff; + + return nameBuffer_ + nameBufferSize; + } + + //! Check whether a character should be percent-encoded. + /*! + According to RFC 3986 2.3 Unreserved Characters. + \param c The character (code unit) to be tested. + */ + bool NeedPercentEncode(Ch c) const { + return !((c >= '0' && c <= '9') || (c >= 'A' && c <= 'Z') || + (c >= 'a' && c <= 'z') || c == '-' || c == '.' || c == '_' || + c == '~'); + } + +//! Parse a JSON String or its URI fragment representation into tokens. +#ifndef __clang__ // -Wdocumentation + /*! + \param source Either a JSON Pointer string, or its URI fragment + representation. Not need to be null terminated. \param length + Length of the + source string. \note Source cannot be JSON String + Representation of JSON + Pointer, e.g. In "/\u0000", \u0000 will not be unescaped. + */ +#endif + void Parse(const Ch *source, size_t length) { + RAPIDJSON_ASSERT(source != NULL); + RAPIDJSON_ASSERT(nameBuffer_ == 0); + RAPIDJSON_ASSERT(tokens_ == 0); + + // Create own allocator if user did not supply. + if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + + // Count number of '/' as tokenCount + tokenCount_ = 0; + for (const Ch *s = source; s != source + length; s++) + if (*s == '/') tokenCount_++; + + Token *token = tokens_ = static_cast( + allocator_->Malloc(tokenCount_ * sizeof(Token) + length * sizeof(Ch))); + Ch *name = nameBuffer_ = reinterpret_cast(tokens_ + tokenCount_); + size_t i = 0; + + // Detect if it is a URI fragment + bool uriFragment = false; + if (source[i] == '#') { + uriFragment = true; + i++; + } + + if (i != length && source[i] != '/') { + parseErrorCode_ = kPointerParseErrorTokenMustBeginWithSolidus; + goto error; + } + + while (i < length) { + RAPIDJSON_ASSERT(source[i] == '/'); + i++; // consumes '/' + + token->name = name; + bool isNumber = true; + + while (i < length && source[i] != '/') { + Ch c = source[i]; + if (uriFragment) { + // Decoding percent-encoding for URI fragment + if (c == '%') { + PercentDecodeStream is(&source[i], source + length); + GenericInsituStringStream os(name); + Ch *begin = os.PutBegin(); + if (!Transcoder, EncodingType>().Validate(is, os) || + !is.IsValid()) { + parseErrorCode_ = kPointerParseErrorInvalidPercentEncoding; + goto error; + } + size_t len = os.PutEnd(begin); + i += is.Tell() - 1; + if (len == 1) + c = *name; + else { + name += len; + isNumber = false; + i++; + continue; + } + } else if (NeedPercentEncode(c)) { + parseErrorCode_ = kPointerParseErrorCharacterMustPercentEncode; + goto error; + } + } + + i++; + + // Escaping "~0" -> '~', "~1" -> '/' + if (c == '~') { + if (i < length) { + c = source[i]; + if (c == '0') + c = '~'; + else if (c == '1') + c = '/'; + else { + parseErrorCode_ = kPointerParseErrorInvalidEscape; + goto error; + } + i++; + } else { + parseErrorCode_ = kPointerParseErrorInvalidEscape; + goto error; + } + } + + // First check for index: all of characters are digit + if (c < '0' || c > '9') isNumber = false; + + *name++ = c; + } + token->length = static_cast(name - token->name); + if (token->length == 0) isNumber = false; + *name++ = '\0'; // Null terminator + + // Second check for index: more than one digit cannot have leading zero + if (isNumber && token->length > 1 && token->name[0] == '0') + isNumber = false; + + // String to SizeType conversion + SizeType n = 0; + if (isNumber) { + for (size_t j = 0; j < token->length; j++) { + SizeType m = n * 10 + static_cast(token->name[j] - '0'); + if (m < n) { // overflow detection + isNumber = false; + break; + } + n = m; + } + } + + token->index = isNumber ? n : kPointerInvalidIndex; + token++; + } + + RAPIDJSON_ASSERT(name <= + nameBuffer_ + length); // Should not overflow buffer + parseErrorCode_ = kPointerParseErrorNone; + return; + + error: + Allocator::Free(tokens_); + nameBuffer_ = 0; + tokens_ = 0; + tokenCount_ = 0; + parseErrorOffset_ = i; + return; + } + + //! Stringify to string or URI fragment representation. + /*! + \tparam uriFragment True for stringifying to URI fragment representation. + False for string representation. \tparam OutputStream type of output + stream. \param os The output stream. + */ + template + bool Stringify(OutputStream &os) const { + RAPIDJSON_ASSERT(IsValid()); + + if (uriFragment) os.Put('#'); + + for (Token *t = tokens_; t != tokens_ + tokenCount_; ++t) { + os.Put('/'); + for (size_t j = 0; j < t->length; j++) { + Ch c = t->name[j]; + if (c == '~') { + os.Put('~'); + os.Put('0'); + } else if (c == '/') { + os.Put('~'); + os.Put('1'); + } else if (uriFragment && NeedPercentEncode(c)) { + // Transcode to UTF8 sequence + GenericStringStream source( + &t->name[j]); + PercentEncodeStream target(os); + if (!Transcoder>().Validate(source, target)) + return false; + j += source.Tell() - 1; + } else + os.Put(c); + } + } + return true; + } + + //! A helper stream for decoding a percent-encoded sequence into code unit. + /*! + This stream decodes %XY triplet into code unit (0-255). + If it encounters invalid characters, it sets output code unit as 0 and + mark invalid, and to be checked by IsValid(). + */ + class PercentDecodeStream { + public: + typedef typename ValueType::Ch Ch; + + //! Constructor + /*! + \param source Start of the stream + \param end Past-the-end of the stream. + */ + PercentDecodeStream(const Ch *source, const Ch *end) + : src_(source), head_(source), end_(end), valid_(true) {} + + Ch Take() { + if (*src_ != '%' || src_ + 3 > end_) { // %XY triplet + valid_ = false; + return 0; + } + src_++; + Ch c = 0; + for (int j = 0; j < 2; j++) { + c = static_cast(c << 4); + Ch h = *src_; + if (h >= '0' && h <= '9') + c = static_cast(c + h - '0'); + else if (h >= 'A' && h <= 'F') + c = static_cast(c + h - 'A' + 10); + else if (h >= 'a' && h <= 'f') + c = static_cast(c + h - 'a' + 10); + else { + valid_ = false; + return 0; + } + src_++; + } + return c; + } + + size_t Tell() const { return static_cast(src_ - head_); } + bool IsValid() const { return valid_; } + + private: + const Ch *src_; //!< Current read position. + const Ch *head_; //!< Original head of the string. + const Ch *end_; //!< Past-the-end position. + bool valid_; //!< Whether the parsing is valid. + }; + + //! A helper stream to encode character (UTF-8 code unit) into percent-encoded + //! sequence. + template + class PercentEncodeStream { + public: + PercentEncodeStream(OutputStream &os) : os_(os) {} + void Put(char c) { // UTF-8 must be byte + unsigned char u = static_cast(c); + static const char hexDigits[16] = {'0', '1', '2', '3', '4', '5', + '6', '7', '8', '9', 'A', 'B', + 'C', 'D', 'E', 'F'}; + os_.Put('%'); + os_.Put(static_cast(hexDigits[u >> 4])); + os_.Put(static_cast(hexDigits[u & 15])); + } + + private: + OutputStream &os_; + }; + + Allocator *allocator_; //!< The current allocator. It is either user-supplied + //!< or equal to ownAllocator_. + Allocator *ownAllocator_; //!< Allocator owned by this Pointer. + Ch *nameBuffer_; //!< A buffer containing all names in tokens. + Token *tokens_; //!< A list of tokens. + size_t tokenCount_; //!< Number of tokens in tokens_. + size_t parseErrorOffset_; //!< Offset in code unit when parsing fail. + PointerParseErrorCode parseErrorCode_; //!< Parsing error code. +}; + +//! GenericPointer for Value (UTF-8, default allocator). +typedef GenericPointer Pointer; + +//!@name Helper functions for GenericPointer +//@{ + +////////////////////////////////////////////////////////////////////////////// + +template +typename T::ValueType &CreateValueByPointer( + T &root, const GenericPointer &pointer, + typename T::AllocatorType &a) { + return pointer.Create(root, a); +} + +template +typename T::ValueType &CreateValueByPointer(T &root, + const CharType (&source)[N], + typename T::AllocatorType &a) { + return GenericPointer(source, N - 1).Create(root, a); +} + +// No allocator parameter + +template +typename DocumentType::ValueType &CreateValueByPointer( + DocumentType &document, + const GenericPointer &pointer) { + return pointer.Create(document); +} + +template +typename DocumentType::ValueType &CreateValueByPointer( + DocumentType &document, const CharType (&source)[N]) { + return GenericPointer(source, N - 1) + .Create(document); +} + +////////////////////////////////////////////////////////////////////////////// + +template +typename T::ValueType *GetValueByPointer( + T &root, const GenericPointer &pointer, + size_t *unresolvedTokenIndex = 0) { + return pointer.Get(root, unresolvedTokenIndex); +} + +template +const typename T::ValueType *GetValueByPointer( + const T &root, const GenericPointer &pointer, + size_t *unresolvedTokenIndex = 0) { + return pointer.Get(root, unresolvedTokenIndex); +} + +template +typename T::ValueType *GetValueByPointer(T &root, const CharType (&source)[N], + size_t *unresolvedTokenIndex = 0) { + return GenericPointer(source, N - 1) + .Get(root, unresolvedTokenIndex); +} + +template +const typename T::ValueType *GetValueByPointer( + const T &root, const CharType (&source)[N], + size_t *unresolvedTokenIndex = 0) { + return GenericPointer(source, N - 1) + .Get(root, unresolvedTokenIndex); +} + +////////////////////////////////////////////////////////////////////////////// + +template +typename T::ValueType &GetValueByPointerWithDefault( + T &root, const GenericPointer &pointer, + const typename T::ValueType &defaultValue, typename T::AllocatorType &a) { + return pointer.GetWithDefault(root, defaultValue, a); +} + +template +typename T::ValueType &GetValueByPointerWithDefault( + T &root, const GenericPointer &pointer, + const typename T::Ch *defaultValue, typename T::AllocatorType &a) { + return pointer.GetWithDefault(root, defaultValue, a); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename T::ValueType &GetValueByPointerWithDefault( + T &root, const GenericPointer &pointer, + const std::basic_string &defaultValue, + typename T::AllocatorType &a) { + return pointer.GetWithDefault(root, defaultValue, a); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (typename T::ValueType &)) +GetValueByPointerWithDefault( + T &root, const GenericPointer &pointer, + T2 defaultValue, typename T::AllocatorType &a) { + return pointer.GetWithDefault(root, defaultValue, a); +} + +template +typename T::ValueType &GetValueByPointerWithDefault( + T &root, const CharType (&source)[N], + const typename T::ValueType &defaultValue, typename T::AllocatorType &a) { + return GenericPointer(source, N - 1) + .GetWithDefault(root, defaultValue, a); +} + +template +typename T::ValueType &GetValueByPointerWithDefault( + T &root, const CharType (&source)[N], const typename T::Ch *defaultValue, + typename T::AllocatorType &a) { + return GenericPointer(source, N - 1) + .GetWithDefault(root, defaultValue, a); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename T::ValueType &GetValueByPointerWithDefault( + T &root, const CharType (&source)[N], + const std::basic_string &defaultValue, + typename T::AllocatorType &a) { + return GenericPointer(source, N - 1) + .GetWithDefault(root, defaultValue, a); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (typename T::ValueType &)) +GetValueByPointerWithDefault(T &root, const CharType (&source)[N], + T2 defaultValue, typename T::AllocatorType &a) { + return GenericPointer(source, N - 1) + .GetWithDefault(root, defaultValue, a); +} + +// No allocator parameter + +template +typename DocumentType::ValueType &GetValueByPointerWithDefault( + DocumentType &document, + const GenericPointer &pointer, + const typename DocumentType::ValueType &defaultValue) { + return pointer.GetWithDefault(document, defaultValue); +} + +template +typename DocumentType::ValueType &GetValueByPointerWithDefault( + DocumentType &document, + const GenericPointer &pointer, + const typename DocumentType::Ch *defaultValue) { + return pointer.GetWithDefault(document, defaultValue); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename DocumentType::ValueType &GetValueByPointerWithDefault( + DocumentType &document, + const GenericPointer &pointer, + const std::basic_string &defaultValue) { + return pointer.GetWithDefault(document, defaultValue); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (typename DocumentType::ValueType &)) +GetValueByPointerWithDefault( + DocumentType &document, + const GenericPointer &pointer, + T2 defaultValue) { + return pointer.GetWithDefault(document, defaultValue); +} + +template +typename DocumentType::ValueType &GetValueByPointerWithDefault( + DocumentType &document, const CharType (&source)[N], + const typename DocumentType::ValueType &defaultValue) { + return GenericPointer(source, N - 1) + .GetWithDefault(document, defaultValue); +} + +template +typename DocumentType::ValueType &GetValueByPointerWithDefault( + DocumentType &document, const CharType (&source)[N], + const typename DocumentType::Ch *defaultValue) { + return GenericPointer(source, N - 1) + .GetWithDefault(document, defaultValue); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename DocumentType::ValueType &GetValueByPointerWithDefault( + DocumentType &document, const CharType (&source)[N], + const std::basic_string &defaultValue) { + return GenericPointer(source, N - 1) + .GetWithDefault(document, defaultValue); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (typename DocumentType::ValueType &)) +GetValueByPointerWithDefault(DocumentType &document, + const CharType (&source)[N], T2 defaultValue) { + return GenericPointer(source, N - 1) + .GetWithDefault(document, defaultValue); +} + +////////////////////////////////////////////////////////////////////////////// + +template +typename T::ValueType &SetValueByPointer( + T &root, const GenericPointer &pointer, + typename T::ValueType &value, typename T::AllocatorType &a) { + return pointer.Set(root, value, a); +} + +template +typename T::ValueType &SetValueByPointer( + T &root, const GenericPointer &pointer, + const typename T::ValueType &value, typename T::AllocatorType &a) { + return pointer.Set(root, value, a); +} + +template +typename T::ValueType &SetValueByPointer( + T &root, const GenericPointer &pointer, + const typename T::Ch *value, typename T::AllocatorType &a) { + return pointer.Set(root, value, a); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename T::ValueType &SetValueByPointer( + T &root, const GenericPointer &pointer, + const std::basic_string &value, + typename T::AllocatorType &a) { + return pointer.Set(root, value, a); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (typename T::ValueType &)) +SetValueByPointer(T &root, const GenericPointer &pointer, + T2 value, typename T::AllocatorType &a) { + return pointer.Set(root, value, a); +} + +template +typename T::ValueType &SetValueByPointer(T &root, const CharType (&source)[N], + typename T::ValueType &value, + typename T::AllocatorType &a) { + return GenericPointer(source, N - 1) + .Set(root, value, a); +} + +template +typename T::ValueType &SetValueByPointer(T &root, const CharType (&source)[N], + const typename T::ValueType &value, + typename T::AllocatorType &a) { + return GenericPointer(source, N - 1) + .Set(root, value, a); +} + +template +typename T::ValueType &SetValueByPointer(T &root, const CharType (&source)[N], + const typename T::Ch *value, + typename T::AllocatorType &a) { + return GenericPointer(source, N - 1) + .Set(root, value, a); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename T::ValueType &SetValueByPointer( + T &root, const CharType (&source)[N], + const std::basic_string &value, + typename T::AllocatorType &a) { + return GenericPointer(source, N - 1) + .Set(root, value, a); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (typename T::ValueType &)) +SetValueByPointer(T &root, const CharType (&source)[N], T2 value, + typename T::AllocatorType &a) { + return GenericPointer(source, N - 1) + .Set(root, value, a); +} + +// No allocator parameter + +template +typename DocumentType::ValueType &SetValueByPointer( + DocumentType &document, + const GenericPointer &pointer, + typename DocumentType::ValueType &value) { + return pointer.Set(document, value); +} + +template +typename DocumentType::ValueType &SetValueByPointer( + DocumentType &document, + const GenericPointer &pointer, + const typename DocumentType::ValueType &value) { + return pointer.Set(document, value); +} + +template +typename DocumentType::ValueType &SetValueByPointer( + DocumentType &document, + const GenericPointer &pointer, + const typename DocumentType::Ch *value) { + return pointer.Set(document, value); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename DocumentType::ValueType &SetValueByPointer( + DocumentType &document, + const GenericPointer &pointer, + const std::basic_string &value) { + return pointer.Set(document, value); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (typename DocumentType::ValueType &)) +SetValueByPointer( + DocumentType &document, + const GenericPointer &pointer, T2 value) { + return pointer.Set(document, value); +} + +template +typename DocumentType::ValueType &SetValueByPointer( + DocumentType &document, const CharType (&source)[N], + typename DocumentType::ValueType &value) { + return GenericPointer(source, N - 1) + .Set(document, value); +} + +template +typename DocumentType::ValueType &SetValueByPointer( + DocumentType &document, const CharType (&source)[N], + const typename DocumentType::ValueType &value) { + return GenericPointer(source, N - 1) + .Set(document, value); +} + +template +typename DocumentType::ValueType &SetValueByPointer( + DocumentType &document, const CharType (&source)[N], + const typename DocumentType::Ch *value) { + return GenericPointer(source, N - 1) + .Set(document, value); +} + +#if RAPIDJSON_HAS_STDSTRING +template +typename DocumentType::ValueType &SetValueByPointer( + DocumentType &document, const CharType (&source)[N], + const std::basic_string &value) { + return GenericPointer(source, N - 1) + .Set(document, value); +} +#endif + +template +RAPIDJSON_DISABLEIF_RETURN( + (internal::OrExpr, internal::IsGenericValue>), + (typename DocumentType::ValueType &)) +SetValueByPointer(DocumentType &document, const CharType (&source)[N], + T2 value) { + return GenericPointer(source, N - 1) + .Set(document, value); +} + +////////////////////////////////////////////////////////////////////////////// + +template +typename T::ValueType &SwapValueByPointer( + T &root, const GenericPointer &pointer, + typename T::ValueType &value, typename T::AllocatorType &a) { + return pointer.Swap(root, value, a); +} + +template +typename T::ValueType &SwapValueByPointer(T &root, const CharType (&source)[N], + typename T::ValueType &value, + typename T::AllocatorType &a) { + return GenericPointer(source, N - 1) + .Swap(root, value, a); +} + +template +typename DocumentType::ValueType &SwapValueByPointer( + DocumentType &document, + const GenericPointer &pointer, + typename DocumentType::ValueType &value) { + return pointer.Swap(document, value); +} + +template +typename DocumentType::ValueType &SwapValueByPointer( + DocumentType &document, const CharType (&source)[N], + typename DocumentType::ValueType &value) { + return GenericPointer(source, N - 1) + .Swap(document, value); +} + +////////////////////////////////////////////////////////////////////////////// + +template +bool EraseValueByPointer(T &root, + const GenericPointer &pointer) { + return pointer.Erase(root); +} + +template +bool EraseValueByPointer(T &root, const CharType (&source)[N]) { + return GenericPointer(source, N - 1).Erase(root); +} + +//@} + +RAPIDJSON_NAMESPACE_END + +#if defined(__clang__) || defined(_MSC_VER) +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_POINTER_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/prettywriter.h b/src/livox_ros_driver2/3rdparty/rapidjson/prettywriter.h new file mode 100644 index 0000000..f24bd0f --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/prettywriter.h @@ -0,0 +1,333 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_PRETTYWRITER_H_ +#define RAPIDJSON_PRETTYWRITER_H_ + +#include "writer.h" + +#ifdef __GNUC__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(effc++) +#endif + +#if defined(__clang__) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(c++ 98 - compat) +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +//! Combination of PrettyWriter format flags. +/*! \see PrettyWriter::SetFormatOptions + */ +enum PrettyFormatOptions { + kFormatDefault = 0, //!< Default pretty formatting. + kFormatSingleLineArray = 1 //!< Format arrays on a single line. +}; + +//! Writer with indentation and spacing. +/*! + \tparam OutputStream Type of output os. + \tparam SourceEncoding Encoding of source string. + \tparam TargetEncoding Encoding of output stream. + \tparam StackAllocator Type of allocator for allocating memory of stack. +*/ +template , + typename TargetEncoding = UTF8<>, + typename StackAllocator = CrtAllocator, + unsigned writeFlags = kWriteDefaultFlags> +class PrettyWriter : public Writer { + public: + typedef Writer + Base; + typedef typename Base::Ch Ch; + + //! Constructor + /*! \param os Output stream. + \param allocator User supplied allocator. If it is null, it will create a + private one. \param levelDepth Initial capacity of stack. + */ + explicit PrettyWriter(OutputStream &os, StackAllocator *allocator = 0, + size_t levelDepth = Base::kDefaultLevelDepth) + : Base(os, allocator, levelDepth), + indentChar_(' '), + indentCharCount_(4), + formatOptions_(kFormatDefault) {} + + explicit PrettyWriter(StackAllocator *allocator = 0, + size_t levelDepth = Base::kDefaultLevelDepth) + : Base(allocator, levelDepth), indentChar_(' '), indentCharCount_(4) {} + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + PrettyWriter(PrettyWriter &&rhs) + : Base(std::forward(rhs)), + indentChar_(rhs.indentChar_), + indentCharCount_(rhs.indentCharCount_), + formatOptions_(rhs.formatOptions_) {} +#endif + + //! Set custom indentation. + /*! \param indentChar Character for indentation. Must be whitespace + character (' ', '\\t', '\\n', '\\r'). \param indentCharCount Number of + indent characters for each indentation level. \note The default indentation + is 4 spaces. + */ + PrettyWriter &SetIndent(Ch indentChar, unsigned indentCharCount) { + RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' || + indentChar == '\n' || indentChar == '\r'); + indentChar_ = indentChar; + indentCharCount_ = indentCharCount; + return *this; + } + + //! Set pretty writer formatting options. + /*! \param options Formatting options. + */ + PrettyWriter &SetFormatOptions(PrettyFormatOptions options) { + formatOptions_ = options; + return *this; + } + + /*! @name Implementation of Handler + \see Handler + */ + //@{ + + bool Null() { + PrettyPrefix(kNullType); + return Base::EndValue(Base::WriteNull()); + } + bool Bool(bool b) { + PrettyPrefix(b ? kTrueType : kFalseType); + return Base::EndValue(Base::WriteBool(b)); + } + bool Int(int i) { + PrettyPrefix(kNumberType); + return Base::EndValue(Base::WriteInt(i)); + } + bool Uint(unsigned u) { + PrettyPrefix(kNumberType); + return Base::EndValue(Base::WriteUint(u)); + } + bool Int64(int64_t i64) { + PrettyPrefix(kNumberType); + return Base::EndValue(Base::WriteInt64(i64)); + } + bool Uint64(uint64_t u64) { + PrettyPrefix(kNumberType); + return Base::EndValue(Base::WriteUint64(u64)); + } + bool Double(double d) { + PrettyPrefix(kNumberType); + return Base::EndValue(Base::WriteDouble(d)); + } + + bool RawNumber(const Ch *str, SizeType length, bool copy = false) { + RAPIDJSON_ASSERT(str != 0); + (void)copy; + PrettyPrefix(kNumberType); + return Base::EndValue(Base::WriteString(str, length)); + } + + bool String(const Ch *str, SizeType length, bool copy = false) { + RAPIDJSON_ASSERT(str != 0); + (void)copy; + PrettyPrefix(kStringType); + return Base::EndValue(Base::WriteString(str, length)); + } + +#if RAPIDJSON_HAS_STDSTRING + bool String(const std::basic_string &str) { + return String(str.data(), SizeType(str.size())); + } +#endif + + bool StartObject() { + PrettyPrefix(kObjectType); + new (Base::level_stack_.template Push()) + typename Base::Level(false); + return Base::WriteStartObject(); + } + + bool Key(const Ch *str, SizeType length, bool copy = false) { + return String(str, length, copy); + } + +#if RAPIDJSON_HAS_STDSTRING + bool Key(const std::basic_string &str) { + return Key(str.data(), SizeType(str.size())); + } +#endif + + bool EndObject(SizeType memberCount = 0) { + (void)memberCount; + RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >= + sizeof(typename Base::Level)); // not inside an Object + RAPIDJSON_ASSERT(!Base::level_stack_.template Top() + ->inArray); // currently inside an Array, not Object + RAPIDJSON_ASSERT( + 0 == + Base::level_stack_.template Top()->valueCount % + 2); // Object has a Key without a Value + + bool empty = + Base::level_stack_.template Pop(1)->valueCount == + 0; + + if (!empty) { + Base::os_->Put('\n'); + WriteIndent(); + } + bool ret = Base::EndValue(Base::WriteEndObject()); + (void)ret; + RAPIDJSON_ASSERT(ret == true); + if (Base::level_stack_.Empty()) // end of json text + Base::Flush(); + return true; + } + + bool StartArray() { + PrettyPrefix(kArrayType); + new (Base::level_stack_.template Push()) + typename Base::Level(true); + return Base::WriteStartArray(); + } + + bool EndArray(SizeType memberCount = 0) { + (void)memberCount; + RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >= + sizeof(typename Base::Level)); + RAPIDJSON_ASSERT( + Base::level_stack_.template Top()->inArray); + bool empty = + Base::level_stack_.template Pop(1)->valueCount == + 0; + + if (!empty && !(formatOptions_ & kFormatSingleLineArray)) { + Base::os_->Put('\n'); + WriteIndent(); + } + bool ret = Base::EndValue(Base::WriteEndArray()); + (void)ret; + RAPIDJSON_ASSERT(ret == true); + if (Base::level_stack_.Empty()) // end of json text + Base::Flush(); + return true; + } + + //@} + + /*! @name Convenience extensions */ + //@{ + + //! Simpler but slower overload. + bool String(const Ch *str) { return String(str, internal::StrLen(str)); } + bool Key(const Ch *str) { return Key(str, internal::StrLen(str)); } + + //@} + + //! Write a raw JSON value. + /*! + For user to write a stringified JSON as a value. + + \param json A well-formed JSON value. It should not contain null character + within [0, length - 1] range. \param length Length of the json. \param type + Type of the root of json. \note When using PrettyWriter::RawValue(), the + result json may not be indented correctly. + */ + bool RawValue(const Ch *json, size_t length, Type type) { + RAPIDJSON_ASSERT(json != 0); + PrettyPrefix(type); + return Base::EndValue(Base::WriteRawValue(json, length)); + } + + protected: + void PrettyPrefix(Type type) { + (void)type; + if (Base::level_stack_.GetSize() != 0) { // this value is not at root + typename Base::Level *level = + Base::level_stack_.template Top(); + + if (level->inArray) { + if (level->valueCount > 0) { + Base::os_->Put( + ','); // add comma if it is not the first element in array + if (formatOptions_ & kFormatSingleLineArray) Base::os_->Put(' '); + } + + if (!(formatOptions_ & kFormatSingleLineArray)) { + Base::os_->Put('\n'); + WriteIndent(); + } + } else { // in object + if (level->valueCount > 0) { + if (level->valueCount % 2 == 0) { + Base::os_->Put(','); + Base::os_->Put('\n'); + } else { + Base::os_->Put(':'); + Base::os_->Put(' '); + } + } else + Base::os_->Put('\n'); + + if (level->valueCount % 2 == 0) WriteIndent(); + } + if (!level->inArray && level->valueCount % 2 == 0) + RAPIDJSON_ASSERT(type == kStringType); // if it's in object, then even + // number should be a name + level->valueCount++; + } else { + RAPIDJSON_ASSERT( + !Base::hasRoot_); // Should only has one and only one root. + Base::hasRoot_ = true; + } + } + + void WriteIndent() { + size_t count = + (Base::level_stack_.GetSize() / sizeof(typename Base::Level)) * + indentCharCount_; + PutN(*Base::os_, static_cast(indentChar_), + count); + } + + Ch indentChar_; + unsigned indentCharCount_; + PrettyFormatOptions formatOptions_; + + private: + // Prohibit copy constructor & assignment operator. + PrettyWriter(const PrettyWriter &); + PrettyWriter &operator=(const PrettyWriter &); +}; + +RAPIDJSON_NAMESPACE_END + +#if defined(__clang__) +RAPIDJSON_DIAG_POP +#endif + +#ifdef __GNUC__ +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_RAPIDJSON_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/rapidjson.h b/src/livox_ros_driver2/3rdparty/rapidjson/rapidjson.h new file mode 100644 index 0000000..7af85e8 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/rapidjson.h @@ -0,0 +1,719 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_RAPIDJSON_H_ +#define RAPIDJSON_RAPIDJSON_H_ + +/*!\file rapidjson.h + \brief common definitions and configuration + + \see RAPIDJSON_CONFIG + */ + +/*! \defgroup RAPIDJSON_CONFIG RapidJSON configuration + \brief Configuration macros for library features + + Some RapidJSON features are configurable to adapt the library to a wide + variety of platforms, environments and usage scenarios. Most of the + features can be configured in terms of overridden or predefined + preprocessor macros at compile-time. + + Some additional customization is available in the \ref RAPIDJSON_ERRORS + APIs. + + \note These macros should be given on the compiler command-line + (where applicable) to avoid inconsistent values when compiling + different translation units of a single application. + */ + +#include // malloc(), realloc(), free(), size_t +#include // memset(), memcpy(), memmove(), memcmp() + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_VERSION_STRING +// +// ALWAYS synchronize the following 3 macros with corresponding variables in +// /CMakeLists.txt. +// + +//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN +// token stringification +#define RAPIDJSON_STRINGIFY(x) RAPIDJSON_DO_STRINGIFY(x) +#define RAPIDJSON_DO_STRINGIFY(x) #x + +// token concatenation +#define RAPIDJSON_JOIN(X, Y) RAPIDJSON_DO_JOIN(X, Y) +#define RAPIDJSON_DO_JOIN(X, Y) RAPIDJSON_DO_JOIN2(X, Y) +#define RAPIDJSON_DO_JOIN2(X, Y) X##Y +//!@endcond + +/*! \def RAPIDJSON_MAJOR_VERSION + \ingroup RAPIDJSON_CONFIG + \brief Major version of RapidJSON in integer. +*/ +/*! \def RAPIDJSON_MINOR_VERSION + \ingroup RAPIDJSON_CONFIG + \brief Minor version of RapidJSON in integer. +*/ +/*! \def RAPIDJSON_PATCH_VERSION + \ingroup RAPIDJSON_CONFIG + \brief Patch version of RapidJSON in integer. +*/ +/*! \def RAPIDJSON_VERSION_STRING + \ingroup RAPIDJSON_CONFIG + \brief Version of RapidJSON in ".." string format. +*/ +#define RAPIDJSON_MAJOR_VERSION 1 +#define RAPIDJSON_MINOR_VERSION 1 +#define RAPIDJSON_PATCH_VERSION 0 +#define RAPIDJSON_VERSION_STRING \ + RAPIDJSON_STRINGIFY( \ + RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION) + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_NAMESPACE_(BEGIN|END) +/*! \def RAPIDJSON_NAMESPACE + \ingroup RAPIDJSON_CONFIG + \brief provide custom rapidjson namespace + + In order to avoid symbol clashes and/or "One Definition Rule" errors + between multiple inclusions of (different versions of) RapidJSON in + a single binary, users can customize the name of the main RapidJSON + namespace. + + In case of a single nesting level, defining \c RAPIDJSON_NAMESPACE + to a custom name (e.g. \c MyRapidJSON) is sufficient. If multiple + levels are needed, both \ref RAPIDJSON_NAMESPACE_BEGIN and \ref + RAPIDJSON_NAMESPACE_END need to be defined as well: + + \code + // in some .cpp file + #define RAPIDJSON_NAMESPACE my::rapidjson + #define RAPIDJSON_NAMESPACE_BEGIN namespace my { namespace rapidjson { + #define RAPIDJSON_NAMESPACE_END } } + #include "rapidjson/..." + \endcode + + \see rapidjson + */ +/*! \def RAPIDJSON_NAMESPACE_BEGIN + \ingroup RAPIDJSON_CONFIG + \brief provide custom rapidjson namespace (opening expression) + \see RAPIDJSON_NAMESPACE +*/ +/*! \def RAPIDJSON_NAMESPACE_END + \ingroup RAPIDJSON_CONFIG + \brief provide custom rapidjson namespace (closing expression) + \see RAPIDJSON_NAMESPACE +*/ +#ifndef RAPIDJSON_NAMESPACE +#define RAPIDJSON_NAMESPACE rapidjson +#endif +#ifndef RAPIDJSON_NAMESPACE_BEGIN +#define RAPIDJSON_NAMESPACE_BEGIN namespace RAPIDJSON_NAMESPACE { +#endif +#ifndef RAPIDJSON_NAMESPACE_END +#define RAPIDJSON_NAMESPACE_END } +#endif + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_HAS_STDSTRING + +#ifndef RAPIDJSON_HAS_STDSTRING +#ifdef RAPIDJSON_DOXYGEN_RUNNING +#define RAPIDJSON_HAS_STDSTRING 1 // force generation of documentation +#else +#define RAPIDJSON_HAS_STDSTRING 0 // no std::string support by default +#endif +/*! \def RAPIDJSON_HAS_STDSTRING + \ingroup RAPIDJSON_CONFIG + \brief Enable RapidJSON support for \c std::string + + By defining this preprocessor symbol to \c 1, several convenience functions + for using \ref rapidjson::GenericValue with \c std::string are enabled, + especially for construction and comparison. + + \hideinitializer +*/ +#endif // !defined(RAPIDJSON_HAS_STDSTRING) + +#if RAPIDJSON_HAS_STDSTRING +#include +#endif // RAPIDJSON_HAS_STDSTRING + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_NO_INT64DEFINE + +/*! \def RAPIDJSON_NO_INT64DEFINE + \ingroup RAPIDJSON_CONFIG + \brief Use external 64-bit integer types. + + RapidJSON requires the 64-bit integer types \c int64_t and \c uint64_t + types to be available at global scope. + + If users have their own definition, define RAPIDJSON_NO_INT64DEFINE to + prevent RapidJSON from defining its own types. +*/ +#ifndef RAPIDJSON_NO_INT64DEFINE +//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN +#if defined(_MSC_VER) && (_MSC_VER < 1800) // Visual Studio 2013 +#include "msinttypes/inttypes.h" +#include "msinttypes/stdint.h" +#else +// Other compilers should have this. +#include +#include +#endif +//!@endcond +#ifdef RAPIDJSON_DOXYGEN_RUNNING +#define RAPIDJSON_NO_INT64DEFINE +#endif +#endif // RAPIDJSON_NO_INT64TYPEDEF + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_FORCEINLINE + +#ifndef RAPIDJSON_FORCEINLINE +//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN +#if defined(_MSC_VER) && defined(NDEBUG) +#define RAPIDJSON_FORCEINLINE __forceinline +#elif defined(__GNUC__) && __GNUC__ >= 4 && defined(NDEBUG) +#define RAPIDJSON_FORCEINLINE __attribute__((always_inline)) +#else +#define RAPIDJSON_FORCEINLINE +#endif +//!@endcond +#endif // RAPIDJSON_FORCEINLINE + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_ENDIAN +#define RAPIDJSON_LITTLEENDIAN 0 //!< Little endian machine +#define RAPIDJSON_BIGENDIAN 1 //!< Big endian machine + +//! Endianness of the machine. +/*! + \def RAPIDJSON_ENDIAN + \ingroup RAPIDJSON_CONFIG + + GCC 4.6 provided macro for detecting endianness of the target machine. But + other compilers may not have this. User can define RAPIDJSON_ENDIAN to either + \ref RAPIDJSON_LITTLEENDIAN or \ref RAPIDJSON_BIGENDIAN. + + Default detection implemented with reference to + \li + https://gcc.gnu.org/onlinedocs/gcc-4.6.0/cpp/Common-Predefined-Macros.html + \li http://www.boost.org/doc/libs/1_42_0/boost/detail/endian.hpp +*/ +#ifndef RAPIDJSON_ENDIAN +// Detect with GCC 4.6's macro +#ifdef __BYTE_ORDER__ +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ +#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN +#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ +#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN +#else +# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN. +#endif // __BYTE_ORDER__ +// Detect with GLIBC's endian.h +#elif defined(__GLIBC__) +#include +#if (__BYTE_ORDER == __LITTLE_ENDIAN) +#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN +#elif (__BYTE_ORDER == __BIG_ENDIAN) +#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN +#else +# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN. +#endif // __GLIBC__ +// Detect with _LITTLE_ENDIAN and _BIG_ENDIAN macro +#elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN) +#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN +#elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN) +#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN +// Detect with architecture macros +#elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || \ + defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || \ + defined(__hppa) || defined(_MIPSEB) || defined(_POWER) || \ + defined(__s390__) +#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN +#elif defined(__i386__) || defined(__alpha__) || defined(__ia64) || \ + defined(__ia64__) || defined(_M_IX86) || defined(_M_IA64) || \ + defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) || \ + defined(_M_AMD64) || defined(__x86_64) || defined(__x86_64__) || \ + defined(_M_X64) || defined(__bfin__) +#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN +#elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64)) +#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN +#elif defined(RAPIDJSON_DOXYGEN_RUNNING) +#define RAPIDJSON_ENDIAN +#else +# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN. +#endif +#endif // RAPIDJSON_ENDIAN + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_64BIT + +//! Whether using 64-bit architecture +#ifndef RAPIDJSON_64BIT +#if defined(__LP64__) || (defined(__x86_64__) && defined(__ILP32__)) || \ + defined(_WIN64) || defined(__EMSCRIPTEN__) +#define RAPIDJSON_64BIT 1 +#else +#define RAPIDJSON_64BIT 0 +#endif +#endif // RAPIDJSON_64BIT + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_ALIGN + +//! Data alignment of the machine. +/*! \ingroup RAPIDJSON_CONFIG + \param x pointer to align + + Some machines require strict data alignment. The default is 8 bytes. + User can customize by defining the RAPIDJSON_ALIGN function macro. +*/ +#ifndef RAPIDJSON_ALIGN +#define RAPIDJSON_ALIGN(x) \ + (((x) + static_cast(7u)) & ~static_cast(7u)) +#endif + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_UINT64_C2 + +//! Construct a 64-bit literal by a pair of 32-bit integer. +/*! + 64-bit literal with or without ULL suffix is prone to compiler warnings. + UINT64_C() is C macro which cause compilation problems. + Use this macro to define 64-bit constants by a pair of 32-bit integer. +*/ +#ifndef RAPIDJSON_UINT64_C2 +#define RAPIDJSON_UINT64_C2(high32, low32) \ + ((static_cast(high32) << 32) | static_cast(low32)) +#endif + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_48BITPOINTER_OPTIMIZATION + +//! Use only lower 48-bit address for some pointers. +/*! + \ingroup RAPIDJSON_CONFIG + + This optimization uses the fact that current X86-64 architecture only + implement lower 48-bit virtual address. The higher 16-bit can be used for + storing other data. \c GenericValue uses this optimization to reduce its size + form 24 bytes to 16 bytes in 64-bit architecture. +*/ +#ifndef RAPIDJSON_48BITPOINTER_OPTIMIZATION +#if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || \ + defined(__x86_64) || defined(_M_X64) || defined(_M_AMD64) +#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 1 +#else +#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 0 +#endif +#endif // RAPIDJSON_48BITPOINTER_OPTIMIZATION + +#if RAPIDJSON_48BITPOINTER_OPTIMIZATION == 1 +#if RAPIDJSON_64BIT != 1 +#error RAPIDJSON_48BITPOINTER_OPTIMIZATION can only be set to 1 when RAPIDJSON_64BIT=1 +#endif +#define RAPIDJSON_SETPOINTER(type, p, x) \ + (p = reinterpret_cast( \ + (reinterpret_cast(p) & \ + static_cast(RAPIDJSON_UINT64_C2(0xFFFF0000, 0x00000000))) | \ + reinterpret_cast(reinterpret_cast(x)))) +#define RAPIDJSON_GETPOINTER(type, p) \ + (reinterpret_cast( \ + reinterpret_cast(p) & \ + static_cast(RAPIDJSON_UINT64_C2(0x0000FFFF, 0xFFFFFFFF)))) +#else +#define RAPIDJSON_SETPOINTER(type, p, x) (p = (x)) +#define RAPIDJSON_GETPOINTER(type, p) (p) +#endif + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_SSE2/RAPIDJSON_SSE42/RAPIDJSON_NEON/RAPIDJSON_SIMD + +/*! \def RAPIDJSON_SIMD + \ingroup RAPIDJSON_CONFIG + \brief Enable SSE2/SSE4.2/Neon optimization. + + RapidJSON supports optimized implementations for some parsing operations + based on the SSE2, SSE4.2 or NEon SIMD extensions on modern Intel + or ARM compatible processors. + + To enable these optimizations, three different symbols can be defined; + \code + // Enable SSE2 optimization. + #define RAPIDJSON_SSE2 + + // Enable SSE4.2 optimization. + #define RAPIDJSON_SSE42 + \endcode + + // Enable ARM Neon optimization. + #define RAPIDJSON_NEON + \endcode + + \c RAPIDJSON_SSE42 takes precedence over SSE2, if both are defined. + + If any of these symbols is defined, RapidJSON defines the macro + \c RAPIDJSON_SIMD to indicate the availability of the optimized code. +*/ +#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) || \ + defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING) +#define RAPIDJSON_SIMD +#endif + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_NO_SIZETYPEDEFINE + +#ifndef RAPIDJSON_NO_SIZETYPEDEFINE +/*! \def RAPIDJSON_NO_SIZETYPEDEFINE + \ingroup RAPIDJSON_CONFIG + \brief User-provided \c SizeType definition. + + In order to avoid using 32-bit size types for indexing strings and arrays, + define this preprocessor symbol and provide the type rapidjson::SizeType + before including RapidJSON: + \code + #define RAPIDJSON_NO_SIZETYPEDEFINE + namespace rapidjson { typedef ::std::size_t SizeType; } + #include "rapidjson/..." + \endcode + + \see rapidjson::SizeType +*/ +#ifdef RAPIDJSON_DOXYGEN_RUNNING +#define RAPIDJSON_NO_SIZETYPEDEFINE +#endif +RAPIDJSON_NAMESPACE_BEGIN +//! Size type (for string lengths, array sizes, etc.) +/*! RapidJSON uses 32-bit array/string indices even on 64-bit platforms, + instead of using \c size_t. Users may override the SizeType by defining + \ref RAPIDJSON_NO_SIZETYPEDEFINE. +*/ +typedef unsigned SizeType; +RAPIDJSON_NAMESPACE_END +#endif + +// always import std::size_t to rapidjson namespace +RAPIDJSON_NAMESPACE_BEGIN +using std::size_t; +RAPIDJSON_NAMESPACE_END + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_ASSERT + +//! Assertion. +/*! \ingroup RAPIDJSON_CONFIG + By default, rapidjson uses C \c assert() for internal assertions. + User can override it by defining RAPIDJSON_ASSERT(x) macro. + + \note Parsing errors are handled and can be customized by the + \ref RAPIDJSON_ERRORS APIs. +*/ +#ifndef RAPIDJSON_ASSERT +#include +#define RAPIDJSON_ASSERT(x) assert(x) +#endif // RAPIDJSON_ASSERT + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_STATIC_ASSERT + +// Prefer C++11 static_assert, if available +#ifndef RAPIDJSON_STATIC_ASSERT +#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800) +#define RAPIDJSON_STATIC_ASSERT(x) static_assert(x, RAPIDJSON_STRINGIFY(x)) +#endif // C++11 +#endif // RAPIDJSON_STATIC_ASSERT + +// Adopt C++03 implementation from boost +#ifndef RAPIDJSON_STATIC_ASSERT +#ifndef __clang__ +//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN +#endif +RAPIDJSON_NAMESPACE_BEGIN +template +struct STATIC_ASSERTION_FAILURE; +template <> +struct STATIC_ASSERTION_FAILURE { + enum { value = 1 }; +}; +template +struct StaticAssertTest {}; +RAPIDJSON_NAMESPACE_END + +#if defined(__GNUC__) || defined(__clang__) +#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE __attribute__((unused)) +#else +#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE +#endif +#ifndef __clang__ +//!@endcond +#endif + +/*! \def RAPIDJSON_STATIC_ASSERT + \brief (Internal) macro to check for conditions at compile-time + \param x compile-time condition + \hideinitializer + */ +#define RAPIDJSON_STATIC_ASSERT(x) \ + typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest)> \ + RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__) \ + RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE +#endif // RAPIDJSON_STATIC_ASSERT + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_LIKELY, RAPIDJSON_UNLIKELY + +//! Compiler branching hint for expression with high probability to be true. +/*! + \ingroup RAPIDJSON_CONFIG + \param x Boolean expression likely to be true. +*/ +#ifndef RAPIDJSON_LIKELY +#if defined(__GNUC__) || defined(__clang__) +#define RAPIDJSON_LIKELY(x) __builtin_expect(!!(x), 1) +#else +#define RAPIDJSON_LIKELY(x) (x) +#endif +#endif + +//! Compiler branching hint for expression with low probability to be true. +/*! + \ingroup RAPIDJSON_CONFIG + \param x Boolean expression unlikely to be true. +*/ +#ifndef RAPIDJSON_UNLIKELY +#if defined(__GNUC__) || defined(__clang__) +#define RAPIDJSON_UNLIKELY(x) __builtin_expect(!!(x), 0) +#else +#define RAPIDJSON_UNLIKELY(x) (x) +#endif +#endif + +/////////////////////////////////////////////////////////////////////////////// +// Helpers + +//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN + +#define RAPIDJSON_MULTILINEMACRO_BEGIN do { +#define RAPIDJSON_MULTILINEMACRO_END \ + } \ + while ((void)0, 0) + +// adopted from Boost +#define RAPIDJSON_VERSION_CODE(x, y, z) (((x)*100000) + ((y)*100) + (z)) + +#if defined(__has_builtin) +#define RAPIDJSON_HAS_BUILTIN(x) __has_builtin(x) +#else +#define RAPIDJSON_HAS_BUILTIN(x) 0 +#endif + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_DIAG_PUSH/POP, RAPIDJSON_DIAG_OFF + +#if defined(__GNUC__) +#define RAPIDJSON_GNUC \ + RAPIDJSON_VERSION_CODE(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__) +#endif + +#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \ + RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 2, 0)) + +#define RAPIDJSON_PRAGMA(x) _Pragma(RAPIDJSON_STRINGIFY(x)) +#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(GCC diagnostic x) +#define RAPIDJSON_DIAG_OFF(x) \ + RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W, x))) + +// push/pop support in Clang and GCC>=4.6 +#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \ + RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) +#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push) +#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop) +#else // GCC >= 4.2, < 4.6 +#define RAPIDJSON_DIAG_PUSH /* ignored */ +#define RAPIDJSON_DIAG_POP /* ignored */ +#endif + +#elif defined(_MSC_VER) + +// pragma (MSVC specific) +#define RAPIDJSON_PRAGMA(x) __pragma(x) +#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(warning(x)) + +#define RAPIDJSON_DIAG_OFF(x) RAPIDJSON_DIAG_PRAGMA(disable : x) +#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push) +#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop) + +#else + +#define RAPIDJSON_DIAG_OFF(x) /* ignored */ +#define RAPIDJSON_DIAG_PUSH /* ignored */ +#define RAPIDJSON_DIAG_POP /* ignored */ + +#endif // RAPIDJSON_DIAG_* + +/////////////////////////////////////////////////////////////////////////////// +// C++11 features + +#ifndef RAPIDJSON_HAS_CXX11_RVALUE_REFS +#if defined(__clang__) +#if __has_feature(cxx_rvalue_references) && \ + (defined(_MSC_VER) || defined(_LIBCPP_VERSION) || \ + defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306) +#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1 +#else +#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0 +#endif +#elif (defined(RAPIDJSON_GNUC) && \ + (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 3, 0)) && \ + defined(__GXX_EXPERIMENTAL_CXX0X__)) || \ + (defined(_MSC_VER) && _MSC_VER >= 1600) || \ + (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \ + defined(__GXX_EXPERIMENTAL_CXX0X__)) + +#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1 +#else +#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0 +#endif +#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS + +#ifndef RAPIDJSON_HAS_CXX11_NOEXCEPT +#if defined(__clang__) +#define RAPIDJSON_HAS_CXX11_NOEXCEPT __has_feature(cxx_noexcept) +#elif (defined(RAPIDJSON_GNUC) && \ + (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \ + defined(__GXX_EXPERIMENTAL_CXX0X__)) || \ + (defined(_MSC_VER) && _MSC_VER >= 1900) || \ + (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \ + defined(__GXX_EXPERIMENTAL_CXX0X__)) +#define RAPIDJSON_HAS_CXX11_NOEXCEPT 1 +#else +#define RAPIDJSON_HAS_CXX11_NOEXCEPT 0 +#endif +#endif +#if RAPIDJSON_HAS_CXX11_NOEXCEPT +#define RAPIDJSON_NOEXCEPT noexcept +#else +#define RAPIDJSON_NOEXCEPT /* noexcept */ +#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT + +// no automatic detection, yet +#ifndef RAPIDJSON_HAS_CXX11_TYPETRAITS +#if (defined(_MSC_VER) && _MSC_VER >= 1700) +#define RAPIDJSON_HAS_CXX11_TYPETRAITS 1 +#else +#define RAPIDJSON_HAS_CXX11_TYPETRAITS 0 +#endif +#endif + +#ifndef RAPIDJSON_HAS_CXX11_RANGE_FOR +#if defined(__clang__) +#define RAPIDJSON_HAS_CXX11_RANGE_FOR __has_feature(cxx_range_for) +#elif (defined(RAPIDJSON_GNUC) && \ + (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \ + defined(__GXX_EXPERIMENTAL_CXX0X__)) || \ + (defined(_MSC_VER) && _MSC_VER >= 1700) || \ + (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \ + defined(__GXX_EXPERIMENTAL_CXX0X__)) +#define RAPIDJSON_HAS_CXX11_RANGE_FOR 1 +#else +#define RAPIDJSON_HAS_CXX11_RANGE_FOR 0 +#endif +#endif // RAPIDJSON_HAS_CXX11_RANGE_FOR + +/////////////////////////////////////////////////////////////////////////////// +// C++17 features + +#if defined(__has_cpp_attribute) +#if __has_cpp_attribute(fallthrough) +#define RAPIDJSON_DELIBERATE_FALLTHROUGH [[fallthrough]] +#else +#define RAPIDJSON_DELIBERATE_FALLTHROUGH +#endif +#else +#define RAPIDJSON_DELIBERATE_FALLTHROUGH +#endif + +//!@endcond + +//! Assertion (in non-throwing contexts). +/*! \ingroup RAPIDJSON_CONFIG + Some functions provide a \c noexcept guarantee, if the compiler supports it. + In these cases, the \ref RAPIDJSON_ASSERT macro cannot be overridden to + throw an exception. This macro adds a separate customization point for + such cases. + + Defaults to C \c assert() (as \ref RAPIDJSON_ASSERT), if \c noexcept is + supported, and to \ref RAPIDJSON_ASSERT otherwise. +*/ + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_NOEXCEPT_ASSERT + +#ifndef RAPIDJSON_NOEXCEPT_ASSERT +#ifdef RAPIDJSON_ASSERT_THROWS +#if RAPIDJSON_HAS_CXX11_NOEXCEPT +#define RAPIDJSON_NOEXCEPT_ASSERT(x) +#else +#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x) +#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT +#else +#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x) +#endif // RAPIDJSON_ASSERT_THROWS +#endif // RAPIDJSON_NOEXCEPT_ASSERT + +/////////////////////////////////////////////////////////////////////////////// +// new/delete + +#ifndef RAPIDJSON_NEW +///! customization point for global \c new +#define RAPIDJSON_NEW(TypeName) new TypeName +#endif +#ifndef RAPIDJSON_DELETE +///! customization point for global \c delete +#define RAPIDJSON_DELETE(x) delete x +#endif + +/////////////////////////////////////////////////////////////////////////////// +// Type + +/*! \namespace rapidjson + \brief main RapidJSON namespace + \see RAPIDJSON_NAMESPACE +*/ +RAPIDJSON_NAMESPACE_BEGIN + +//! Type of JSON value +enum Type { + kNullType = 0, //!< null + kFalseType = 1, //!< false + kTrueType = 2, //!< true + kObjectType = 3, //!< object + kArrayType = 4, //!< array + kStringType = 5, //!< string + kNumberType = 6 //!< number +}; + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_RAPIDJSON_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/reader.h b/src/livox_ros_driver2/3rdparty/rapidjson/reader.h new file mode 100644 index 0000000..3a7203a --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/reader.h @@ -0,0 +1,2458 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_READER_H_ +#define RAPIDJSON_READER_H_ + +/*! \file reader.h */ + +#include +#include "allocators.h" +#include "encodedstream.h" +#include "internal/clzll.h" +#include "internal/meta.h" +#include "internal/stack.h" +#include "internal/strtod.h" +#include "stream.h" + +#if defined(RAPIDJSON_SIMD) && defined(_MSC_VER) +#include +#pragma intrinsic(_BitScanForward) +#endif +#ifdef RAPIDJSON_SSE42 +#include +#elif defined(RAPIDJSON_SSE2) +#include +#elif defined(RAPIDJSON_NEON) +#include +#endif + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(old - style - cast) +RAPIDJSON_DIAG_OFF(padded) +RAPIDJSON_DIAG_OFF(switch - enum) +#elif defined(_MSC_VER) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(4127) // conditional expression is constant +RAPIDJSON_DIAG_OFF(4702) // unreachable code +#endif + +#ifdef __GNUC__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(effc++) +#endif + +//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN +#define RAPIDJSON_NOTHING /* deliberately empty */ +#ifndef RAPIDJSON_PARSE_ERROR_EARLY_RETURN +#define RAPIDJSON_PARSE_ERROR_EARLY_RETURN(value) \ + RAPIDJSON_MULTILINEMACRO_BEGIN \ + if (RAPIDJSON_UNLIKELY(HasParseError())) { \ + return value; \ + } \ + RAPIDJSON_MULTILINEMACRO_END +#endif +#define RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID \ + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(RAPIDJSON_NOTHING) +//!@endcond + +/*! \def RAPIDJSON_PARSE_ERROR_NORETURN + \ingroup RAPIDJSON_ERRORS + \brief Macro to indicate a parse error. + \param parseErrorCode \ref rapidjson::ParseErrorCode of the error + \param offset position of the error in JSON input (\c size_t) + + This macros can be used as a customization point for the internal + error handling mechanism of RapidJSON. + + A common usage model is to throw an exception instead of requiring the + caller to explicitly check the \ref rapidjson::GenericReader::Parse's + return value: + + \code + #define RAPIDJSON_PARSE_ERROR_NORETURN(parseErrorCode,offset) \ + throw ParseException(parseErrorCode, #parseErrorCode, offset) + + #include // std::runtime_error + #include "rapidjson/error/error.h" // rapidjson::ParseResult + + struct ParseException : std::runtime_error, rapidjson::ParseResult { + ParseException(rapidjson::ParseErrorCode code, const char* msg, size_t + offset) : std::runtime_error(msg), ParseResult(code, offset) {} + }; + + #include "rapidjson/reader.h" + \endcode + + \see RAPIDJSON_PARSE_ERROR, rapidjson::GenericReader::Parse + */ +#ifndef RAPIDJSON_PARSE_ERROR_NORETURN +#define RAPIDJSON_PARSE_ERROR_NORETURN(parseErrorCode, offset) \ + RAPIDJSON_MULTILINEMACRO_BEGIN \ + RAPIDJSON_ASSERT(!HasParseError()); /* Error can only be assigned once */ \ + SetParseError(parseErrorCode, offset); \ + RAPIDJSON_MULTILINEMACRO_END +#endif + +/*! \def RAPIDJSON_PARSE_ERROR + \ingroup RAPIDJSON_ERRORS + \brief (Internal) macro to indicate and handle a parse error. + \param parseErrorCode \ref rapidjson::ParseErrorCode of the error + \param offset position of the error in JSON input (\c size_t) + + Invokes RAPIDJSON_PARSE_ERROR_NORETURN and stops the parsing. + + \see RAPIDJSON_PARSE_ERROR_NORETURN + \hideinitializer + */ +#ifndef RAPIDJSON_PARSE_ERROR +#define RAPIDJSON_PARSE_ERROR(parseErrorCode, offset) \ + RAPIDJSON_MULTILINEMACRO_BEGIN \ + RAPIDJSON_PARSE_ERROR_NORETURN(parseErrorCode, offset); \ + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; \ + RAPIDJSON_MULTILINEMACRO_END +#endif + +#include "error/error.h" // ParseErrorCode, ParseResult + +RAPIDJSON_NAMESPACE_BEGIN + +/////////////////////////////////////////////////////////////////////////////// +// ParseFlag + +/*! \def RAPIDJSON_PARSE_DEFAULT_FLAGS + \ingroup RAPIDJSON_CONFIG + \brief User-defined kParseDefaultFlags definition. + + User can define this as any \c ParseFlag combinations. +*/ +#ifndef RAPIDJSON_PARSE_DEFAULT_FLAGS +#define RAPIDJSON_PARSE_DEFAULT_FLAGS kParseNoFlags +#endif + +//! Combination of parseFlags +/*! \see Reader::Parse, Document::Parse, Document::ParseInsitu, + * Document::ParseStream + */ +enum ParseFlag { + kParseNoFlags = 0, //!< No flags are set. + kParseInsituFlag = 1, //!< In-situ(destructive) parsing. + kParseValidateEncodingFlag = 2, //!< Validate encoding of JSON strings. + kParseIterativeFlag = 4, //!< Iterative(constant complexity in terms of + //!< function call stack size) parsing. + kParseStopWhenDoneFlag = + 8, //!< After parsing a complete JSON root from stream, stop further + //!< processing the rest of stream. When this flag is used, parser will + //!< not generate kParseErrorDocumentRootNotSingular error. + kParseFullPrecisionFlag = + 16, //!< Parse number in full precision (but slower). + kParseCommentsFlag = + 32, //!< Allow one-line (//) and multi-line (/**/) comments. + kParseNumbersAsStringsFlag = + 64, //!< Parse all numbers (ints/doubles) as strings. + kParseTrailingCommasFlag = + 128, //!< Allow trailing commas at the end of objects and arrays. + kParseNanAndInfFlag = 256, //!< Allow parsing NaN, Inf, Infinity, -Inf and + //!-Infinity as doubles. + kParseDefaultFlags = + RAPIDJSON_PARSE_DEFAULT_FLAGS //!< Default parse flags. Can be customized + //!< by defining + //!< RAPIDJSON_PARSE_DEFAULT_FLAGS +}; + +/////////////////////////////////////////////////////////////////////////////// +// Handler + +/*! \class rapidjson::Handler + \brief Concept for receiving events from GenericReader upon parsing. + The functions return true if no error occurs. If they return false, + the event publisher should terminate the process. +\code +concept Handler { + typename Ch; + + bool Null(); + bool Bool(bool b); + bool Int(int i); + bool Uint(unsigned i); + bool Int64(int64_t i); + bool Uint64(uint64_t i); + bool Double(double d); + /// enabled via kParseNumbersAsStringsFlag, string is not null-terminated +(use length) bool RawNumber(const Ch* str, SizeType length, bool copy); bool +String(const Ch* str, SizeType length, bool copy); bool StartObject(); bool +Key(const Ch* str, SizeType length, bool copy); bool EndObject(SizeType +memberCount); bool StartArray(); bool EndArray(SizeType elementCount); +}; +\endcode +*/ +/////////////////////////////////////////////////////////////////////////////// +// BaseReaderHandler + +//! Default implementation of Handler. +/*! This can be used as base class of any reader handler. + \note implements Handler concept +*/ +template , typename Derived = void> +struct BaseReaderHandler { + typedef typename Encoding::Ch Ch; + + typedef + typename internal::SelectIf, + BaseReaderHandler, Derived>::Type Override; + + bool Default() { return true; } + bool Null() { return static_cast(*this).Default(); } + bool Bool(bool) { return static_cast(*this).Default(); } + bool Int(int) { return static_cast(*this).Default(); } + bool Uint(unsigned) { return static_cast(*this).Default(); } + bool Int64(int64_t) { return static_cast(*this).Default(); } + bool Uint64(uint64_t) { return static_cast(*this).Default(); } + bool Double(double) { return static_cast(*this).Default(); } + /// enabled via kParseNumbersAsStringsFlag, string is not null-terminated (use + /// length) + bool RawNumber(const Ch *str, SizeType len, bool copy) { + return static_cast(*this).String(str, len, copy); + } + bool String(const Ch *, SizeType, bool) { + return static_cast(*this).Default(); + } + bool StartObject() { return static_cast(*this).Default(); } + bool Key(const Ch *str, SizeType len, bool copy) { + return static_cast(*this).String(str, len, copy); + } + bool EndObject(SizeType) { return static_cast(*this).Default(); } + bool StartArray() { return static_cast(*this).Default(); } + bool EndArray(SizeType) { return static_cast(*this).Default(); } +}; + +/////////////////////////////////////////////////////////////////////////////// +// StreamLocalCopy + +namespace internal { + +template ::copyOptimization> +class StreamLocalCopy; + +//! Do copy optimization. +template +class StreamLocalCopy { + public: + StreamLocalCopy(Stream &original) : s(original), original_(original) {} + ~StreamLocalCopy() { original_ = s; } + + Stream s; + + private: + StreamLocalCopy &operator=(const StreamLocalCopy &) /* = delete */; + + Stream &original_; +}; + +//! Keep reference. +template +class StreamLocalCopy { + public: + StreamLocalCopy(Stream &original) : s(original) {} + + Stream &s; + + private: + StreamLocalCopy &operator=(const StreamLocalCopy &) /* = delete */; +}; + +} // namespace internal + +/////////////////////////////////////////////////////////////////////////////// +// SkipWhitespace + +//! Skip the JSON white spaces in a stream. +/*! \param is A input stream for skipping white spaces. + \note This function has SSE2/SSE4.2 specialization. +*/ +template +void SkipWhitespace(InputStream &is) { + internal::StreamLocalCopy copy(is); + InputStream &s(copy.s); + + typename InputStream::Ch c; + while ((c = s.Peek()) == ' ' || c == '\n' || c == '\r' || c == '\t') s.Take(); +} + +inline const char *SkipWhitespace(const char *p, const char *end) { + while (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')) ++p; + return p; +} + +#ifdef RAPIDJSON_SSE42 +//! Skip whitespace with SSE 4.2 pcmpistrm instruction, testing 16 8-byte +//! characters at once. +inline const char *SkipWhitespace_SIMD(const char *p) { + // Fast return for single non-whitespace + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + + // 16-byte align to the next boundary + const char *nextAligned = reinterpret_cast( + (reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + + // The rest of string using SIMD + static const char whitespace[16] = " \n\r\t"; + const __m128i w = + _mm_loadu_si128(reinterpret_cast(&whitespace[0])); + + for (;; p += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(p)); + const int r = _mm_cmpistri(w, s, _SIDD_UBYTE_OPS | _SIDD_CMP_EQUAL_ANY | + _SIDD_LEAST_SIGNIFICANT | + _SIDD_NEGATIVE_POLARITY); + if (r != 16) // some of characters is non-whitespace + return p + r; + } +} + +inline const char *SkipWhitespace_SIMD(const char *p, const char *end) { + // Fast return for single non-whitespace + if (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')) + ++p; + else + return p; + + // The middle of string using SIMD + static const char whitespace[16] = " \n\r\t"; + const __m128i w = + _mm_loadu_si128(reinterpret_cast(&whitespace[0])); + + for (; p <= end - 16; p += 16) { + const __m128i s = _mm_loadu_si128(reinterpret_cast(p)); + const int r = _mm_cmpistri(w, s, _SIDD_UBYTE_OPS | _SIDD_CMP_EQUAL_ANY | + _SIDD_LEAST_SIGNIFICANT | + _SIDD_NEGATIVE_POLARITY); + if (r != 16) // some of characters is non-whitespace + return p + r; + } + + return SkipWhitespace(p, end); +} + +#elif defined(RAPIDJSON_SSE2) + +//! Skip whitespace with SSE2 instructions, testing 16 8-byte characters at +//! once. +inline const char *SkipWhitespace_SIMD(const char *p) { + // Fast return for single non-whitespace + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + + // 16-byte align to the next boundary + const char *nextAligned = reinterpret_cast( + (reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + +// The rest of string +#define C16(c) \ + { c, c, c, c, c, c, c, c, c, c, c, c, c, c, c, c } + static const char whitespaces[4][16] = {C16(' '), C16('\n'), C16('\r'), + C16('\t')}; +#undef C16 + + const __m128i w0 = + _mm_loadu_si128(reinterpret_cast(&whitespaces[0][0])); + const __m128i w1 = + _mm_loadu_si128(reinterpret_cast(&whitespaces[1][0])); + const __m128i w2 = + _mm_loadu_si128(reinterpret_cast(&whitespaces[2][0])); + const __m128i w3 = + _mm_loadu_si128(reinterpret_cast(&whitespaces[3][0])); + + for (;; p += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(p)); + __m128i x = _mm_cmpeq_epi8(s, w0); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w1)); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w2)); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w3)); + unsigned short r = static_cast(~_mm_movemask_epi8(x)); + if (r != 0) { // some of characters may be non-whitespace +#ifdef _MSC_VER // Find the index of first non-whitespace + unsigned long offset; + _BitScanForward(&offset, r); + return p + offset; +#else + return p + __builtin_ffs(r) - 1; +#endif + } + } +} + +inline const char *SkipWhitespace_SIMD(const char *p, const char *end) { + // Fast return for single non-whitespace + if (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')) + ++p; + else + return p; + +// The rest of string +#define C16(c) \ + { c, c, c, c, c, c, c, c, c, c, c, c, c, c, c, c } + static const char whitespaces[4][16] = {C16(' '), C16('\n'), C16('\r'), + C16('\t')}; +#undef C16 + + const __m128i w0 = + _mm_loadu_si128(reinterpret_cast(&whitespaces[0][0])); + const __m128i w1 = + _mm_loadu_si128(reinterpret_cast(&whitespaces[1][0])); + const __m128i w2 = + _mm_loadu_si128(reinterpret_cast(&whitespaces[2][0])); + const __m128i w3 = + _mm_loadu_si128(reinterpret_cast(&whitespaces[3][0])); + + for (; p <= end - 16; p += 16) { + const __m128i s = _mm_loadu_si128(reinterpret_cast(p)); + __m128i x = _mm_cmpeq_epi8(s, w0); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w1)); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w2)); + x = _mm_or_si128(x, _mm_cmpeq_epi8(s, w3)); + unsigned short r = static_cast(~_mm_movemask_epi8(x)); + if (r != 0) { // some of characters may be non-whitespace +#ifdef _MSC_VER // Find the index of first non-whitespace + unsigned long offset; + _BitScanForward(&offset, r); + return p + offset; +#else + return p + __builtin_ffs(r) - 1; +#endif + } + } + + return SkipWhitespace(p, end); +} + +#elif defined(RAPIDJSON_NEON) + +//! Skip whitespace with ARM Neon instructions, testing 16 8-byte characters at +//! once. +inline const char *SkipWhitespace_SIMD(const char *p) { + // Fast return for single non-whitespace + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + + // 16-byte align to the next boundary + const char *nextAligned = reinterpret_cast( + (reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t') + ++p; + else + return p; + + const uint8x16_t w0 = vmovq_n_u8(' '); + const uint8x16_t w1 = vmovq_n_u8('\n'); + const uint8x16_t w2 = vmovq_n_u8('\r'); + const uint8x16_t w3 = vmovq_n_u8('\t'); + + for (;; p += 16) { + const uint8x16_t s = vld1q_u8(reinterpret_cast(p)); + uint8x16_t x = vceqq_u8(s, w0); + x = vorrq_u8(x, vceqq_u8(s, w1)); + x = vorrq_u8(x, vceqq_u8(s, w2)); + x = vorrq_u8(x, vceqq_u8(s, w3)); + + x = vmvnq_u8(x); // Negate + x = vrev64q_u8(x); // Rev in 64 + uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0); // extract + uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1); // extract + + if (low == 0) { + if (high != 0) { + uint32_t lz = RAPIDJSON_CLZLL(high); + return p + 8 + (lz >> 3); + } + } else { + uint32_t lz = RAPIDJSON_CLZLL(low); + return p + (lz >> 3); + } + } +} + +inline const char *SkipWhitespace_SIMD(const char *p, const char *end) { + // Fast return for single non-whitespace + if (p != end && (*p == ' ' || *p == '\n' || *p == '\r' || *p == '\t')) + ++p; + else + return p; + + const uint8x16_t w0 = vmovq_n_u8(' '); + const uint8x16_t w1 = vmovq_n_u8('\n'); + const uint8x16_t w2 = vmovq_n_u8('\r'); + const uint8x16_t w3 = vmovq_n_u8('\t'); + + for (; p <= end - 16; p += 16) { + const uint8x16_t s = vld1q_u8(reinterpret_cast(p)); + uint8x16_t x = vceqq_u8(s, w0); + x = vorrq_u8(x, vceqq_u8(s, w1)); + x = vorrq_u8(x, vceqq_u8(s, w2)); + x = vorrq_u8(x, vceqq_u8(s, w3)); + + x = vmvnq_u8(x); // Negate + x = vrev64q_u8(x); // Rev in 64 + uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0); // extract + uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1); // extract + + if (low == 0) { + if (high != 0) { + uint32_t lz = RAPIDJSON_CLZLL(high); + return p + 8 + (lz >> 3); + } + } else { + uint32_t lz = RAPIDJSON_CLZLL(low); + return p + (lz >> 3); + } + } + + return SkipWhitespace(p, end); +} + +#endif // RAPIDJSON_NEON + +#ifdef RAPIDJSON_SIMD +//! Template function specialization for InsituStringStream +template <> +inline void SkipWhitespace(InsituStringStream &is) { + is.src_ = const_cast(SkipWhitespace_SIMD(is.src_)); +} + +//! Template function specialization for StringStream +template <> +inline void SkipWhitespace(StringStream &is) { + is.src_ = SkipWhitespace_SIMD(is.src_); +} + +template <> +inline void SkipWhitespace(EncodedInputStream, MemoryStream> &is) { + is.is_.src_ = SkipWhitespace_SIMD(is.is_.src_, is.is_.end_); +} +#endif // RAPIDJSON_SIMD + +/////////////////////////////////////////////////////////////////////////////// +// GenericReader + +//! SAX-style JSON parser. Use \ref Reader for UTF8 encoding and default +//! allocator. +/*! GenericReader parses JSON text from a stream, and send events synchronously + to an object implementing Handler concept. + + It needs to allocate a stack for storing a single decoded string during + non-destructive parsing. + + For in-situ parsing, the decoded string is directly written to the source + text string, no temporary buffer is required. + + A GenericReader object can be reused for parsing multiple JSON text. + + \tparam SourceEncoding Encoding of the input stream. + \tparam TargetEncoding Encoding of the parse output. + \tparam StackAllocator Allocator type for stack. +*/ +template +class GenericReader { + public: + typedef typename SourceEncoding::Ch Ch; //!< SourceEncoding character type + + //! Constructor. + /*! \param stackAllocator Optional allocator for allocating stack memory. + (Only use for non-destructive parsing) \param stackCapacity stack capacity + in bytes for storing a single decoded string. (Only use for + non-destructive parsing) + */ + GenericReader(StackAllocator *stackAllocator = 0, + size_t stackCapacity = kDefaultStackCapacity) + : stack_(stackAllocator, stackCapacity), + parseResult_(), + state_(IterativeParsingStartState) {} + + //! Parse JSON text. + /*! \tparam parseFlags Combination of \ref ParseFlag. + \tparam InputStream Type of input stream, implementing Stream concept. + \tparam Handler Type of handler, implementing Handler concept. + \param is Input stream to be parsed. + \param handler The handler to receive events. + \return Whether the parsing is successful. + */ + template + ParseResult Parse(InputStream &is, Handler &handler) { + if (parseFlags & kParseIterativeFlag) + return IterativeParse(is, handler); + + parseResult_.Clear(); + + ClearStackOnExit scope(*this); + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + + if (RAPIDJSON_UNLIKELY(is.Peek() == '\0')) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorDocumentEmpty, is.Tell()); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + } else { + ParseValue(is, handler); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + + if (!(parseFlags & kParseStopWhenDoneFlag)) { + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + + if (RAPIDJSON_UNLIKELY(is.Peek() != '\0')) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorDocumentRootNotSingular, + is.Tell()); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + } + } + } + + return parseResult_; + } + + //! Parse JSON text (with \ref kParseDefaultFlags) + /*! \tparam InputStream Type of input stream, implementing Stream concept + \tparam Handler Type of handler, implementing Handler concept. + \param is Input stream to be parsed. + \param handler The handler to receive events. + \return Whether the parsing is successful. + */ + template + ParseResult Parse(InputStream &is, Handler &handler) { + return Parse(is, handler); + } + + //! Initialize JSON text token-by-token parsing + /*! + */ + void IterativeParseInit() { + parseResult_.Clear(); + state_ = IterativeParsingStartState; + } + + //! Parse one token from JSON text + /*! \tparam InputStream Type of input stream, implementing Stream concept + \tparam Handler Type of handler, implementing Handler concept. + \param is Input stream to be parsed. + \param handler The handler to receive events. + \return Whether the parsing is successful. + */ + template + bool IterativeParseNext(InputStream &is, Handler &handler) { + while (RAPIDJSON_LIKELY(is.Peek() != '\0')) { + SkipWhitespaceAndComments(is); + + Token t = Tokenize(is.Peek()); + IterativeParsingState n = Predict(state_, t); + IterativeParsingState d = Transit(state_, t, n, is, handler); + + // If we've finished or hit an error... + if (RAPIDJSON_UNLIKELY(IsIterativeParsingCompleteState(d))) { + // Report errors. + if (d == IterativeParsingErrorState) { + HandleError(state_, is); + return false; + } + + // Transition to the finish state. + RAPIDJSON_ASSERT(d == IterativeParsingFinishState); + state_ = d; + + // If StopWhenDone is not set... + if (!(parseFlags & kParseStopWhenDoneFlag)) { + // ... and extra non-whitespace data is found... + SkipWhitespaceAndComments(is); + if (is.Peek() != '\0') { + // ... this is considered an error. + HandleError(state_, is); + return false; + } + } + + // Success! We are done! + return true; + } + + // Transition to the new state. + state_ = d; + + // If we parsed anything other than a delimiter, we invoked the handler, + // so we can return true now. + if (!IsIterativeParsingDelimiterState(n)) return true; + } + + // We reached the end of file. + stack_.Clear(); + + if (state_ != IterativeParsingFinishState) { + HandleError(state_, is); + return false; + } + + return true; + } + + //! Check if token-by-token parsing JSON text is complete + /*! \return Whether the JSON has been fully decoded. + */ + RAPIDJSON_FORCEINLINE bool IterativeParseComplete() const { + return IsIterativeParsingCompleteState(state_); + } + + //! Whether a parse error has occurred in the last parsing. + bool HasParseError() const { return parseResult_.IsError(); } + + //! Get the \ref ParseErrorCode of last parsing. + ParseErrorCode GetParseErrorCode() const { return parseResult_.Code(); } + + //! Get the position of last parsing error in input, 0 otherwise. + size_t GetErrorOffset() const { return parseResult_.Offset(); } + + protected: + void SetParseError(ParseErrorCode code, size_t offset) { + parseResult_.Set(code, offset); + } + + private: + // Prohibit copy constructor & assignment operator. + GenericReader(const GenericReader &); + GenericReader &operator=(const GenericReader &); + + void ClearStack() { stack_.Clear(); } + + // clear stack on any exit from ParseStream, e.g. due to exception + struct ClearStackOnExit { + explicit ClearStackOnExit(GenericReader &r) : r_(r) {} + ~ClearStackOnExit() { r_.ClearStack(); } + + private: + GenericReader &r_; + ClearStackOnExit(const ClearStackOnExit &); + ClearStackOnExit &operator=(const ClearStackOnExit &); + }; + + template + void SkipWhitespaceAndComments(InputStream &is) { + SkipWhitespace(is); + + if (parseFlags & kParseCommentsFlag) { + while (RAPIDJSON_UNLIKELY(Consume(is, '/'))) { + if (Consume(is, '*')) { + while (true) { + if (RAPIDJSON_UNLIKELY(is.Peek() == '\0')) + RAPIDJSON_PARSE_ERROR(kParseErrorUnspecificSyntaxError, + is.Tell()); + else if (Consume(is, '*')) { + if (Consume(is, '/')) break; + } else + is.Take(); + } + } else if (RAPIDJSON_LIKELY(Consume(is, '/'))) + while (is.Peek() != '\0' && is.Take() != '\n') { + } + else + RAPIDJSON_PARSE_ERROR(kParseErrorUnspecificSyntaxError, is.Tell()); + + SkipWhitespace(is); + } + } + } + + // Parse object: { string : value, ... } + template + void ParseObject(InputStream &is, Handler &handler) { + RAPIDJSON_ASSERT(is.Peek() == '{'); + is.Take(); // Skip '{' + + if (RAPIDJSON_UNLIKELY(!handler.StartObject())) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + if (Consume(is, '}')) { + if (RAPIDJSON_UNLIKELY(!handler.EndObject(0))) // empty object + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + return; + } + + for (SizeType memberCount = 0;;) { + if (RAPIDJSON_UNLIKELY(is.Peek() != '"')) + RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissName, is.Tell()); + + ParseString(is, handler, true); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + if (RAPIDJSON_UNLIKELY(!Consume(is, ':'))) + RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissColon, is.Tell()); + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + ParseValue(is, handler); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + ++memberCount; + + switch (is.Peek()) { + case ',': + is.Take(); + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + break; + case '}': + is.Take(); + if (RAPIDJSON_UNLIKELY(!handler.EndObject(memberCount))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + return; + default: + RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissCommaOrCurlyBracket, + is.Tell()); + break; // This useless break is only for making warning and coverage + // happy + } + + if (parseFlags & kParseTrailingCommasFlag) { + if (is.Peek() == '}') { + if (RAPIDJSON_UNLIKELY(!handler.EndObject(memberCount))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + is.Take(); + return; + } + } + } + } + + // Parse array: [ value, ... ] + template + void ParseArray(InputStream &is, Handler &handler) { + RAPIDJSON_ASSERT(is.Peek() == '['); + is.Take(); // Skip '[' + + if (RAPIDJSON_UNLIKELY(!handler.StartArray())) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + if (Consume(is, ']')) { + if (RAPIDJSON_UNLIKELY(!handler.EndArray(0))) // empty array + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + return; + } + + for (SizeType elementCount = 0;;) { + ParseValue(is, handler); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + ++elementCount; + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + + if (Consume(is, ',')) { + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + } else if (Consume(is, ']')) { + if (RAPIDJSON_UNLIKELY(!handler.EndArray(elementCount))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + return; + } else + RAPIDJSON_PARSE_ERROR(kParseErrorArrayMissCommaOrSquareBracket, + is.Tell()); + + if (parseFlags & kParseTrailingCommasFlag) { + if (is.Peek() == ']') { + if (RAPIDJSON_UNLIKELY(!handler.EndArray(elementCount))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + is.Take(); + return; + } + } + } + } + + template + void ParseNull(InputStream &is, Handler &handler) { + RAPIDJSON_ASSERT(is.Peek() == 'n'); + is.Take(); + + if (RAPIDJSON_LIKELY(Consume(is, 'u') && Consume(is, 'l') && + Consume(is, 'l'))) { + if (RAPIDJSON_UNLIKELY(!handler.Null())) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + } else + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell()); + } + + template + void ParseTrue(InputStream &is, Handler &handler) { + RAPIDJSON_ASSERT(is.Peek() == 't'); + is.Take(); + + if (RAPIDJSON_LIKELY(Consume(is, 'r') && Consume(is, 'u') && + Consume(is, 'e'))) { + if (RAPIDJSON_UNLIKELY(!handler.Bool(true))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + } else + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell()); + } + + template + void ParseFalse(InputStream &is, Handler &handler) { + RAPIDJSON_ASSERT(is.Peek() == 'f'); + is.Take(); + + if (RAPIDJSON_LIKELY(Consume(is, 'a') && Consume(is, 'l') && + Consume(is, 's') && Consume(is, 'e'))) { + if (RAPIDJSON_UNLIKELY(!handler.Bool(false))) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, is.Tell()); + } else + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell()); + } + + template + RAPIDJSON_FORCEINLINE static bool Consume(InputStream &is, + typename InputStream::Ch expect) { + if (RAPIDJSON_LIKELY(is.Peek() == expect)) { + is.Take(); + return true; + } else + return false; + } + + // Helper function to parse four hexadecimal digits in \uXXXX in + // ParseString(). + template + unsigned ParseHex4(InputStream &is, size_t escapeOffset) { + unsigned codepoint = 0; + for (int i = 0; i < 4; i++) { + Ch c = is.Peek(); + codepoint <<= 4; + codepoint += static_cast(c); + if (c >= '0' && c <= '9') + codepoint -= '0'; + else if (c >= 'A' && c <= 'F') + codepoint -= 'A' - 10; + else if (c >= 'a' && c <= 'f') + codepoint -= 'a' - 10; + else { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorStringUnicodeEscapeInvalidHex, + escapeOffset); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(0); + } + is.Take(); + } + return codepoint; + } + + template + class StackStream { + public: + typedef CharType Ch; + + StackStream(internal::Stack &stack) + : stack_(stack), length_(0) {} + RAPIDJSON_FORCEINLINE void Put(Ch c) { + *stack_.template Push() = c; + ++length_; + } + + RAPIDJSON_FORCEINLINE void *Push(SizeType count) { + length_ += count; + return stack_.template Push(count); + } + + size_t Length() const { return length_; } + + Ch *Pop() { return stack_.template Pop(length_); } + + private: + StackStream(const StackStream &); + StackStream &operator=(const StackStream &); + + internal::Stack &stack_; + SizeType length_; + }; + + // Parse string and generate String event. Different code paths for + // kParseInsituFlag. + template + void ParseString(InputStream &is, Handler &handler, bool isKey = false) { + internal::StreamLocalCopy copy(is); + InputStream &s(copy.s); + + RAPIDJSON_ASSERT(s.Peek() == '\"'); + s.Take(); // Skip '\"' + + bool success = false; + if (parseFlags & kParseInsituFlag) { + typename InputStream::Ch *head = s.PutBegin(); + ParseStringToStream(s, s); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + size_t length = s.PutEnd(head) - 1; + RAPIDJSON_ASSERT(length <= 0xFFFFFFFF); + const typename TargetEncoding::Ch *const str = + reinterpret_cast(head); + success = (isKey ? handler.Key(str, SizeType(length), false) + : handler.String(str, SizeType(length), false)); + } else { + StackStream stackStream(stack_); + ParseStringToStream( + s, stackStream); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + SizeType length = static_cast(stackStream.Length()) - 1; + const typename TargetEncoding::Ch *const str = stackStream.Pop(); + success = (isKey ? handler.Key(str, length, true) + : handler.String(str, length, true)); + } + if (RAPIDJSON_UNLIKELY(!success)) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, s.Tell()); + } + + // Parse string to an output is + // This function handles the prefix/suffix double quotes, escaping, and + // optional encoding validation. + template + RAPIDJSON_FORCEINLINE void ParseStringToStream(InputStream &is, + OutputStream &os) { +//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN +#define Z16 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 + static const char escape[256] = { + Z16, Z16, 0, 0, '\"', 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, '/', Z16, Z16, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, '\\', 0, 0, 0, 0, 0, '\b', + 0, 0, 0, '\f', 0, 0, 0, 0, 0, 0, 0, '\n', 0, + 0, 0, '\r', 0, '\t', 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16}; +#undef Z16 + //!@endcond + + for (;;) { + // Scan and copy string before "\\\"" or < 0x20. This is an optional + // optimzation. + if (!(parseFlags & kParseValidateEncodingFlag)) + ScanCopyUnescapedString(is, os); + + Ch c = is.Peek(); + if (RAPIDJSON_UNLIKELY(c == '\\')) { // Escape + size_t escapeOffset = is.Tell(); // For invalid escaping, report the + // initial '\\' as error offset + is.Take(); + Ch e = is.Peek(); + if ((sizeof(Ch) == 1 || unsigned(e) < 256) && + RAPIDJSON_LIKELY(escape[static_cast(e)])) { + is.Take(); + os.Put(static_cast( + escape[static_cast(e)])); + } else if (RAPIDJSON_LIKELY(e == 'u')) { // Unicode + is.Take(); + unsigned codepoint = ParseHex4(is, escapeOffset); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + if (RAPIDJSON_UNLIKELY(codepoint >= 0xD800 && codepoint <= 0xDBFF)) { + // Handle UTF-16 surrogate pair + if (RAPIDJSON_UNLIKELY(!Consume(is, '\\') || !Consume(is, 'u'))) + RAPIDJSON_PARSE_ERROR(kParseErrorStringUnicodeSurrogateInvalid, + escapeOffset); + unsigned codepoint2 = ParseHex4(is, escapeOffset); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN_VOID; + if (RAPIDJSON_UNLIKELY(codepoint2 < 0xDC00 || codepoint2 > 0xDFFF)) + RAPIDJSON_PARSE_ERROR(kParseErrorStringUnicodeSurrogateInvalid, + escapeOffset); + codepoint = (((codepoint - 0xD800) << 10) | (codepoint2 - 0xDC00)) + + 0x10000; + } + TEncoding::Encode(os, codepoint); + } else + RAPIDJSON_PARSE_ERROR(kParseErrorStringEscapeInvalid, escapeOffset); + } else if (RAPIDJSON_UNLIKELY(c == '"')) { // Closing double quote + is.Take(); + os.Put('\0'); // null-terminate the string + return; + } else if (RAPIDJSON_UNLIKELY(static_cast(c) < + 0x20)) { // RFC 4627: unescaped = %x20-21 / + // %x23-5B / %x5D-10FFFF + if (c == '\0') + RAPIDJSON_PARSE_ERROR(kParseErrorStringMissQuotationMark, is.Tell()); + else + RAPIDJSON_PARSE_ERROR(kParseErrorStringInvalidEncoding, is.Tell()); + } else { + size_t offset = is.Tell(); + if (RAPIDJSON_UNLIKELY( + (parseFlags & kParseValidateEncodingFlag + ? !Transcoder::Validate(is, os) + : !Transcoder::Transcode(is, os)))) + RAPIDJSON_PARSE_ERROR(kParseErrorStringInvalidEncoding, offset); + } + } + } + + template + static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString(InputStream &, + OutputStream &) { + // Do nothing for generic version + } + +#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) + // StringStream -> StackStream + static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString( + StringStream &is, StackStream &os) { + const char *p = is.src_; + + // Scan one by one until alignment (unaligned load may cross page boundary + // and cause crash) + const char *nextAligned = reinterpret_cast( + (reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || + RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = p; + return; + } else + os.Put(*p++); + + // The rest of string using SIMD + static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"', + '\"', '\"', '\"', '\"', '\"', '\"', + '\"', '\"', '\"', '\"'}; + static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\', + '\\', '\\', '\\', '\\', '\\', '\\', + '\\', '\\', '\\', '\\'}; + static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, + 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, + 0x1F, 0x1F, 0x1F, 0x1F}; + const __m128i dq = + _mm_loadu_si128(reinterpret_cast(&dquote[0])); + const __m128i bs = + _mm_loadu_si128(reinterpret_cast(&bslash[0])); + const __m128i sp = + _mm_loadu_si128(reinterpret_cast(&space[0])); + + for (;; p += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(p)); + const __m128i t1 = _mm_cmpeq_epi8(s, dq); + const __m128i t2 = _mm_cmpeq_epi8(s, bs); + const __m128i t3 = _mm_cmpeq_epi8( + _mm_max_epu8(s, sp), sp); // s < 0x20 <=> max(s, 0x1F) == 0x1F + const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3); + unsigned short r = static_cast(_mm_movemask_epi8(x)); + if (RAPIDJSON_UNLIKELY(r != 0)) { // some of characters is escaped + SizeType length; +#ifdef _MSC_VER // Find the index of first escaped + unsigned long offset; + _BitScanForward(&offset, r); + length = offset; +#else + length = static_cast(__builtin_ffs(r) - 1); +#endif + if (length != 0) { + char *q = reinterpret_cast(os.Push(length)); + for (size_t i = 0; i < length; i++) q[i] = p[i]; + + p += length; + } + break; + } + _mm_storeu_si128(reinterpret_cast<__m128i *>(os.Push(16)), s); + } + + is.src_ = p; + } + + // InsituStringStream -> InsituStringStream + static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString( + InsituStringStream &is, InsituStringStream &os) { + RAPIDJSON_ASSERT(&is == &os); + (void)os; + + if (is.src_ == is.dst_) { + SkipUnescapedString(is); + return; + } + + char *p = is.src_; + char *q = is.dst_; + + // Scan one by one until alignment (unaligned load may cross page boundary + // and cause crash) + const char *nextAligned = reinterpret_cast( + (reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || + RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = p; + is.dst_ = q; + return; + } else + *q++ = *p++; + + // The rest of string using SIMD + static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"', + '\"', '\"', '\"', '\"', '\"', '\"', + '\"', '\"', '\"', '\"'}; + static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\', + '\\', '\\', '\\', '\\', '\\', '\\', + '\\', '\\', '\\', '\\'}; + static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, + 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, + 0x1F, 0x1F, 0x1F, 0x1F}; + const __m128i dq = + _mm_loadu_si128(reinterpret_cast(&dquote[0])); + const __m128i bs = + _mm_loadu_si128(reinterpret_cast(&bslash[0])); + const __m128i sp = + _mm_loadu_si128(reinterpret_cast(&space[0])); + + for (;; p += 16, q += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(p)); + const __m128i t1 = _mm_cmpeq_epi8(s, dq); + const __m128i t2 = _mm_cmpeq_epi8(s, bs); + const __m128i t3 = _mm_cmpeq_epi8( + _mm_max_epu8(s, sp), sp); // s < 0x20 <=> max(s, 0x1F) == 0x1F + const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3); + unsigned short r = static_cast(_mm_movemask_epi8(x)); + if (RAPIDJSON_UNLIKELY(r != 0)) { // some of characters is escaped + size_t length; +#ifdef _MSC_VER // Find the index of first escaped + unsigned long offset; + _BitScanForward(&offset, r); + length = offset; +#else + length = static_cast(__builtin_ffs(r) - 1); +#endif + for (const char *pend = p + length; p != pend;) *q++ = *p++; + break; + } + _mm_storeu_si128(reinterpret_cast<__m128i *>(q), s); + } + + is.src_ = p; + is.dst_ = q; + } + + // When read/write pointers are the same for insitu stream, just skip + // unescaped characters + static RAPIDJSON_FORCEINLINE void SkipUnescapedString( + InsituStringStream &is) { + RAPIDJSON_ASSERT(is.src_ == is.dst_); + char *p = is.src_; + + // Scan one by one until alignment (unaligned load may cross page boundary + // and cause crash) + const char *nextAligned = reinterpret_cast( + (reinterpret_cast(p) + 15) & static_cast(~15)); + for (; p != nextAligned; p++) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || + RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = is.dst_ = p; + return; + } + + // The rest of string using SIMD + static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"', + '\"', '\"', '\"', '\"', '\"', '\"', + '\"', '\"', '\"', '\"'}; + static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\', + '\\', '\\', '\\', '\\', '\\', '\\', + '\\', '\\', '\\', '\\'}; + static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, + 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, + 0x1F, 0x1F, 0x1F, 0x1F}; + const __m128i dq = + _mm_loadu_si128(reinterpret_cast(&dquote[0])); + const __m128i bs = + _mm_loadu_si128(reinterpret_cast(&bslash[0])); + const __m128i sp = + _mm_loadu_si128(reinterpret_cast(&space[0])); + + for (;; p += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(p)); + const __m128i t1 = _mm_cmpeq_epi8(s, dq); + const __m128i t2 = _mm_cmpeq_epi8(s, bs); + const __m128i t3 = _mm_cmpeq_epi8( + _mm_max_epu8(s, sp), sp); // s < 0x20 <=> max(s, 0x1F) == 0x1F + const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3); + unsigned short r = static_cast(_mm_movemask_epi8(x)); + if (RAPIDJSON_UNLIKELY(r != 0)) { // some of characters is escaped + size_t length; +#ifdef _MSC_VER // Find the index of first escaped + unsigned long offset; + _BitScanForward(&offset, r); + length = offset; +#else + length = static_cast(__builtin_ffs(r) - 1); +#endif + p += length; + break; + } + } + + is.src_ = is.dst_ = p; + } +#elif defined(RAPIDJSON_NEON) + // StringStream -> StackStream + static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString( + StringStream &is, StackStream &os) { + const char *p = is.src_; + + // Scan one by one until alignment (unaligned load may cross page boundary + // and cause crash) + const char *nextAligned = reinterpret_cast( + (reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || + RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = p; + return; + } else + os.Put(*p++); + + // The rest of string using SIMD + const uint8x16_t s0 = vmovq_n_u8('"'); + const uint8x16_t s1 = vmovq_n_u8('\\'); + const uint8x16_t s2 = vmovq_n_u8('\b'); + const uint8x16_t s3 = vmovq_n_u8(32); + + for (;; p += 16) { + const uint8x16_t s = vld1q_u8(reinterpret_cast(p)); + uint8x16_t x = vceqq_u8(s, s0); + x = vorrq_u8(x, vceqq_u8(s, s1)); + x = vorrq_u8(x, vceqq_u8(s, s2)); + x = vorrq_u8(x, vcltq_u8(s, s3)); + + x = vrev64q_u8(x); // Rev in 64 + uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0); // extract + uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1); // extract + + SizeType length = 0; + bool escaped = false; + if (low == 0) { + if (high != 0) { + uint32_t lz = RAPIDJSON_CLZLL(high); + length = 8 + (lz >> 3); + escaped = true; + } + } else { + uint32_t lz = RAPIDJSON_CLZLL(low); + length = lz >> 3; + escaped = true; + } + if (RAPIDJSON_UNLIKELY(escaped)) { // some of characters is escaped + if (length != 0) { + char *q = reinterpret_cast(os.Push(length)); + for (size_t i = 0; i < length; i++) q[i] = p[i]; + + p += length; + } + break; + } + vst1q_u8(reinterpret_cast(os.Push(16)), s); + } + + is.src_ = p; + } + + // InsituStringStream -> InsituStringStream + static RAPIDJSON_FORCEINLINE void ScanCopyUnescapedString( + InsituStringStream &is, InsituStringStream &os) { + RAPIDJSON_ASSERT(&is == &os); + (void)os; + + if (is.src_ == is.dst_) { + SkipUnescapedString(is); + return; + } + + char *p = is.src_; + char *q = is.dst_; + + // Scan one by one until alignment (unaligned load may cross page boundary + // and cause crash) + const char *nextAligned = reinterpret_cast( + (reinterpret_cast(p) + 15) & static_cast(~15)); + while (p != nextAligned) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || + RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = p; + is.dst_ = q; + return; + } else + *q++ = *p++; + + // The rest of string using SIMD + const uint8x16_t s0 = vmovq_n_u8('"'); + const uint8x16_t s1 = vmovq_n_u8('\\'); + const uint8x16_t s2 = vmovq_n_u8('\b'); + const uint8x16_t s3 = vmovq_n_u8(32); + + for (;; p += 16, q += 16) { + const uint8x16_t s = vld1q_u8(reinterpret_cast(p)); + uint8x16_t x = vceqq_u8(s, s0); + x = vorrq_u8(x, vceqq_u8(s, s1)); + x = vorrq_u8(x, vceqq_u8(s, s2)); + x = vorrq_u8(x, vcltq_u8(s, s3)); + + x = vrev64q_u8(x); // Rev in 64 + uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0); // extract + uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1); // extract + + SizeType length = 0; + bool escaped = false; + if (low == 0) { + if (high != 0) { + uint32_t lz = RAPIDJSON_CLZLL(high); + length = 8 + (lz >> 3); + escaped = true; + } + } else { + uint32_t lz = RAPIDJSON_CLZLL(low); + length = lz >> 3; + escaped = true; + } + if (RAPIDJSON_UNLIKELY(escaped)) { // some of characters is escaped + for (const char *pend = p + length; p != pend;) { + *q++ = *p++; + } + break; + } + vst1q_u8(reinterpret_cast(q), s); + } + + is.src_ = p; + is.dst_ = q; + } + + // When read/write pointers are the same for insitu stream, just skip + // unescaped characters + static RAPIDJSON_FORCEINLINE void SkipUnescapedString( + InsituStringStream &is) { + RAPIDJSON_ASSERT(is.src_ == is.dst_); + char *p = is.src_; + + // Scan one by one until alignment (unaligned load may cross page boundary + // and cause crash) + const char *nextAligned = reinterpret_cast( + (reinterpret_cast(p) + 15) & static_cast(~15)); + for (; p != nextAligned; p++) + if (RAPIDJSON_UNLIKELY(*p == '\"') || RAPIDJSON_UNLIKELY(*p == '\\') || + RAPIDJSON_UNLIKELY(static_cast(*p) < 0x20)) { + is.src_ = is.dst_ = p; + return; + } + + // The rest of string using SIMD + const uint8x16_t s0 = vmovq_n_u8('"'); + const uint8x16_t s1 = vmovq_n_u8('\\'); + const uint8x16_t s2 = vmovq_n_u8('\b'); + const uint8x16_t s3 = vmovq_n_u8(32); + + for (;; p += 16) { + const uint8x16_t s = vld1q_u8(reinterpret_cast(p)); + uint8x16_t x = vceqq_u8(s, s0); + x = vorrq_u8(x, vceqq_u8(s, s1)); + x = vorrq_u8(x, vceqq_u8(s, s2)); + x = vorrq_u8(x, vcltq_u8(s, s3)); + + x = vrev64q_u8(x); // Rev in 64 + uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0); // extract + uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1); // extract + + if (low == 0) { + if (high != 0) { + uint32_t lz = RAPIDJSON_CLZLL(high); + p += 8 + (lz >> 3); + break; + } + } else { + uint32_t lz = RAPIDJSON_CLZLL(low); + p += lz >> 3; + break; + } + } + + is.src_ = is.dst_ = p; + } +#endif // RAPIDJSON_NEON + + template + class NumberStream; + + template + class NumberStream { + public: + typedef typename InputStream::Ch Ch; + + NumberStream(GenericReader &reader, InputStream &s) : is(s) { + (void)reader; + } + + RAPIDJSON_FORCEINLINE Ch Peek() const { return is.Peek(); } + RAPIDJSON_FORCEINLINE Ch TakePush() { return is.Take(); } + RAPIDJSON_FORCEINLINE Ch Take() { return is.Take(); } + RAPIDJSON_FORCEINLINE void Push(char) {} + + size_t Tell() { return is.Tell(); } + size_t Length() { return 0; } + const char *Pop() { return 0; } + + protected: + NumberStream &operator=(const NumberStream &); + + InputStream &is; + }; + + template + class NumberStream + : public NumberStream { + typedef NumberStream Base; + + public: + NumberStream(GenericReader &reader, InputStream &is) + : Base(reader, is), stackStream(reader.stack_) {} + + RAPIDJSON_FORCEINLINE Ch TakePush() { + stackStream.Put(static_cast(Base::is.Peek())); + return Base::is.Take(); + } + + RAPIDJSON_FORCEINLINE void Push(char c) { stackStream.Put(c); } + + size_t Length() { return stackStream.Length(); } + + const char *Pop() { + stackStream.Put('\0'); + return stackStream.Pop(); + } + + private: + StackStream stackStream; + }; + + template + class NumberStream + : public NumberStream { + typedef NumberStream Base; + + public: + NumberStream(GenericReader &reader, InputStream &is) : Base(reader, is) {} + + RAPIDJSON_FORCEINLINE Ch Take() { return Base::TakePush(); } + }; + + template + void ParseNumber(InputStream &is, Handler &handler) { + internal::StreamLocalCopy copy(is); + NumberStream + s(*this, copy.s); + + size_t startOffset = s.Tell(); + double d = 0.0; + bool useNanOrInf = false; + + // Parse minus + bool minus = Consume(s, '-'); + + // Parse int: zero / ( digit1-9 *DIGIT ) + unsigned i = 0; + uint64_t i64 = 0; + bool use64bit = false; + int significandDigit = 0; + if (RAPIDJSON_UNLIKELY(s.Peek() == '0')) { + i = 0; + s.TakePush(); + } else if (RAPIDJSON_LIKELY(s.Peek() >= '1' && s.Peek() <= '9')) { + i = static_cast(s.TakePush() - '0'); + + if (minus) + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (RAPIDJSON_UNLIKELY(i >= 214748364)) { // 2^31 = 2147483648 + if (RAPIDJSON_LIKELY(i != 214748364 || s.Peek() > '8')) { + i64 = i; + use64bit = true; + break; + } + } + i = i * 10 + static_cast(s.TakePush() - '0'); + significandDigit++; + } + else + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (RAPIDJSON_UNLIKELY(i >= 429496729)) { // 2^32 - 1 = 4294967295 + if (RAPIDJSON_LIKELY(i != 429496729 || s.Peek() > '5')) { + i64 = i; + use64bit = true; + break; + } + } + i = i * 10 + static_cast(s.TakePush() - '0'); + significandDigit++; + } + } + // Parse NaN or Infinity here + else if ((parseFlags & kParseNanAndInfFlag) && + RAPIDJSON_LIKELY((s.Peek() == 'I' || s.Peek() == 'N'))) { + if (Consume(s, 'N')) { + if (Consume(s, 'a') && Consume(s, 'N')) { + d = std::numeric_limits::quiet_NaN(); + useNanOrInf = true; + } + } else if (RAPIDJSON_LIKELY(Consume(s, 'I'))) { + if (Consume(s, 'n') && Consume(s, 'f')) { + d = (minus ? -std::numeric_limits::infinity() + : std::numeric_limits::infinity()); + useNanOrInf = true; + + if (RAPIDJSON_UNLIKELY(s.Peek() == 'i' && + !(Consume(s, 'i') && Consume(s, 'n') && + Consume(s, 'i') && Consume(s, 't') && + Consume(s, 'y')))) { + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, s.Tell()); + } + } + } + + if (RAPIDJSON_UNLIKELY(!useNanOrInf)) { + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, s.Tell()); + } + } else + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, s.Tell()); + + // Parse 64bit int + bool useDouble = false; + if (use64bit) { + if (minus) + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (RAPIDJSON_UNLIKELY( + i64 >= + RAPIDJSON_UINT64_C2( + 0x0CCCCCCC, 0xCCCCCCCC))) // 2^63 = 9223372036854775808 + if (RAPIDJSON_LIKELY( + i64 != RAPIDJSON_UINT64_C2(0x0CCCCCCC, 0xCCCCCCCC) || + s.Peek() > '8')) { + d = static_cast(i64); + useDouble = true; + break; + } + i64 = i64 * 10 + static_cast(s.TakePush() - '0'); + significandDigit++; + } + else + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (RAPIDJSON_UNLIKELY( + i64 >= RAPIDJSON_UINT64_C2( + 0x19999999, + 0x99999999))) // 2^64 - 1 = 18446744073709551615 + if (RAPIDJSON_LIKELY( + i64 != RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) || + s.Peek() > '5')) { + d = static_cast(i64); + useDouble = true; + break; + } + i64 = i64 * 10 + static_cast(s.TakePush() - '0'); + significandDigit++; + } + } + + // Force double for big integer + if (useDouble) { + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + d = d * 10 + (s.TakePush() - '0'); + } + } + + // Parse frac = decimal-point 1*DIGIT + int expFrac = 0; + size_t decimalPosition; + if (Consume(s, '.')) { + decimalPosition = s.Length(); + + if (RAPIDJSON_UNLIKELY(!(s.Peek() >= '0' && s.Peek() <= '9'))) + RAPIDJSON_PARSE_ERROR(kParseErrorNumberMissFraction, s.Tell()); + + if (!useDouble) { +#if RAPIDJSON_64BIT + // Use i64 to store significand in 64-bit architecture + if (!use64bit) i64 = i; + + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (i64 > RAPIDJSON_UINT64_C2(0x1FFFFF, + 0xFFFFFFFF)) // 2^53 - 1 for fast path + break; + else { + i64 = i64 * 10 + static_cast(s.TakePush() - '0'); + --expFrac; + if (i64 != 0) significandDigit++; + } + } + + d = static_cast(i64); +#else + // Use double to store significand in 32-bit architecture + d = static_cast(use64bit ? i64 : i); +#endif + useDouble = true; + } + + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + if (significandDigit < 17) { + d = d * 10.0 + (s.TakePush() - '0'); + --expFrac; + if (RAPIDJSON_LIKELY(d > 0.0)) significandDigit++; + } else + s.TakePush(); + } + } else + decimalPosition = s.Length(); // decimal position at the end of integer. + + // Parse exp = e [ minus / plus ] 1*DIGIT + int exp = 0; + if (Consume(s, 'e') || Consume(s, 'E')) { + if (!useDouble) { + d = static_cast(use64bit ? i64 : i); + useDouble = true; + } + + bool expMinus = false; + if (Consume(s, '+')) + ; + else if (Consume(s, '-')) + expMinus = true; + + if (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + exp = static_cast(s.Take() - '0'); + if (expMinus) { + // (exp + expFrac) must not underflow int => we're detecting when -exp + // gets dangerously close to INT_MIN (a pessimistic next digit 9 would + // push it into underflow territory): + // + // -(exp * 10 + 9) + expFrac >= INT_MIN + // <=> exp <= (expFrac - INT_MIN - 9) / 10 + RAPIDJSON_ASSERT(expFrac <= 0); + int maxExp = (expFrac + 2147483639) / 10; + + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + exp = exp * 10 + static_cast(s.Take() - '0'); + if (RAPIDJSON_UNLIKELY(exp > maxExp)) { + while (RAPIDJSON_UNLIKELY( + s.Peek() >= '0' && + s.Peek() <= '9')) // Consume the rest of exponent + s.Take(); + } + } + } else { // positive exp + int maxExp = 308 - expFrac; + while (RAPIDJSON_LIKELY(s.Peek() >= '0' && s.Peek() <= '9')) { + exp = exp * 10 + static_cast(s.Take() - '0'); + if (RAPIDJSON_UNLIKELY(exp > maxExp)) + RAPIDJSON_PARSE_ERROR(kParseErrorNumberTooBig, startOffset); + } + } + } else + RAPIDJSON_PARSE_ERROR(kParseErrorNumberMissExponent, s.Tell()); + + if (expMinus) exp = -exp; + } + + // Finish parsing, call event according to the type of number. + bool cont = true; + + if (parseFlags & kParseNumbersAsStringsFlag) { + if (parseFlags & kParseInsituFlag) { + s.Pop(); // Pop stack no matter if it will be used or not. + typename InputStream::Ch *head = is.PutBegin(); + const size_t length = s.Tell() - startOffset; + RAPIDJSON_ASSERT(length <= 0xFFFFFFFF); + // unable to insert the \0 character here, it will erase the comma after + // this number + const typename TargetEncoding::Ch *const str = + reinterpret_cast(head); + cont = handler.RawNumber(str, SizeType(length), false); + } else { + SizeType numCharsToCopy = static_cast(s.Length()); + StringStream srcStream(s.Pop()); + StackStream dstStream(stack_); + while (numCharsToCopy--) { + Transcoder, TargetEncoding>::Transcode(srcStream, dstStream); + } + dstStream.Put('\0'); + const typename TargetEncoding::Ch *str = dstStream.Pop(); + const SizeType length = static_cast(dstStream.Length()) - 1; + cont = handler.RawNumber(str, SizeType(length), true); + } + } else { + size_t length = s.Length(); + const char *decimal = + s.Pop(); // Pop stack no matter if it will be used or not. + + if (useDouble) { + int p = exp + expFrac; + if (parseFlags & kParseFullPrecisionFlag) + d = internal::StrtodFullPrecision(d, p, decimal, length, + decimalPosition, exp); + else + d = internal::StrtodNormalPrecision(d, p); + + // Use > max, instead of == inf, to fix bogus warning -Wfloat-equal + if (d > (std::numeric_limits::max)()) { + // Overflow + // TODO: internal::StrtodX should report overflow (or underflow) + RAPIDJSON_PARSE_ERROR(kParseErrorNumberTooBig, startOffset); + } + + cont = handler.Double(minus ? -d : d); + } else if (useNanOrInf) { + cont = handler.Double(d); + } else { + if (use64bit) { + if (minus) + cont = handler.Int64(static_cast(~i64 + 1)); + else + cont = handler.Uint64(i64); + } else { + if (minus) + cont = handler.Int(static_cast(~i + 1)); + else + cont = handler.Uint(i); + } + } + } + if (RAPIDJSON_UNLIKELY(!cont)) + RAPIDJSON_PARSE_ERROR(kParseErrorTermination, startOffset); + } + + // Parse any JSON value + template + void ParseValue(InputStream &is, Handler &handler) { + switch (is.Peek()) { + case 'n': + ParseNull(is, handler); + break; + case 't': + ParseTrue(is, handler); + break; + case 'f': + ParseFalse(is, handler); + break; + case '"': + ParseString(is, handler); + break; + case '{': + ParseObject(is, handler); + break; + case '[': + ParseArray(is, handler); + break; + default: + ParseNumber(is, handler); + break; + } + } + + // Iterative Parsing + + // States + enum IterativeParsingState { + IterativeParsingFinishState = 0, // sink states at top + IterativeParsingErrorState, // sink states at top + IterativeParsingStartState, + + // Object states + IterativeParsingObjectInitialState, + IterativeParsingMemberKeyState, + IterativeParsingMemberValueState, + IterativeParsingObjectFinishState, + + // Array states + IterativeParsingArrayInitialState, + IterativeParsingElementState, + IterativeParsingArrayFinishState, + + // Single value state + IterativeParsingValueState, + + // Delimiter states (at bottom) + IterativeParsingElementDelimiterState, + IterativeParsingMemberDelimiterState, + IterativeParsingKeyValueDelimiterState, + + cIterativeParsingStateCount + }; + + // Tokens + enum Token { + LeftBracketToken = 0, + RightBracketToken, + + LeftCurlyBracketToken, + RightCurlyBracketToken, + + CommaToken, + ColonToken, + + StringToken, + FalseToken, + TrueToken, + NullToken, + NumberToken, + + kTokenCount + }; + + RAPIDJSON_FORCEINLINE Token Tokenize(Ch c) const { +//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN +#define N NumberToken +#define N16 N, N, N, N, N, N, N, N, N, N, N, N, N, N, N, N + // Maps from ASCII to Token + static const unsigned char tokenMap[256] = { + N16, // 00~0F + N16, // 10~1F + N, N, + StringToken, N, + N, N, + N, N, + N, N, + N, N, + CommaToken, N, + N, N, // 20~2F + N, N, + N, N, + N, N, + N, N, + N, N, + ColonToken, N, + N, N, + N, N, // 30~3F + N16, // 40~4F + N, N, + N, N, + N, N, + N, N, + N, N, + N, LeftBracketToken, + N, RightBracketToken, + N, N, // 50~5F + N, N, + N, N, + N, N, + FalseToken, N, + N, N, + N, N, + N, N, + NullToken, N, // 60~6F + N, N, + N, N, + TrueToken, N, + N, N, + N, N, + N, LeftCurlyBracketToken, + N, RightCurlyBracketToken, + N, N, // 70~7F + N16, N16, + N16, N16, + N16, N16, + N16, N16 // 80~FF + }; +#undef N +#undef N16 + //!@endcond + + if (sizeof(Ch) == 1 || static_cast(c) < 256) + return static_cast(tokenMap[static_cast(c)]); + else + return NumberToken; + } + + RAPIDJSON_FORCEINLINE IterativeParsingState + Predict(IterativeParsingState state, Token token) const { + // current state x one lookahead token -> new state + static const char G[cIterativeParsingStateCount][kTokenCount] = { + // Finish(sink state) + {IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState}, + // Error(sink state) + {IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState}, + // Start + { + IterativeParsingArrayInitialState, // Left bracket + IterativeParsingErrorState, // Right bracket + IterativeParsingObjectInitialState, // Left curly bracket + IterativeParsingErrorState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingValueState, // String + IterativeParsingValueState, // False + IterativeParsingValueState, // True + IterativeParsingValueState, // Null + IterativeParsingValueState // Number + }, + // ObjectInitial + { + IterativeParsingErrorState, // Left bracket + IterativeParsingErrorState, // Right bracket + IterativeParsingErrorState, // Left curly bracket + IterativeParsingObjectFinishState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingMemberKeyState, // String + IterativeParsingErrorState, // False + IterativeParsingErrorState, // True + IterativeParsingErrorState, // Null + IterativeParsingErrorState // Number + }, + // MemberKey + { + IterativeParsingErrorState, // Left bracket + IterativeParsingErrorState, // Right bracket + IterativeParsingErrorState, // Left curly bracket + IterativeParsingErrorState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingKeyValueDelimiterState, // Colon + IterativeParsingErrorState, // String + IterativeParsingErrorState, // False + IterativeParsingErrorState, // True + IterativeParsingErrorState, // Null + IterativeParsingErrorState // Number + }, + // MemberValue + { + IterativeParsingErrorState, // Left bracket + IterativeParsingErrorState, // Right bracket + IterativeParsingErrorState, // Left curly bracket + IterativeParsingObjectFinishState, // Right curly bracket + IterativeParsingMemberDelimiterState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingErrorState, // String + IterativeParsingErrorState, // False + IterativeParsingErrorState, // True + IterativeParsingErrorState, // Null + IterativeParsingErrorState // Number + }, + // ObjectFinish(sink state) + {IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState}, + // ArrayInitial + { + IterativeParsingArrayInitialState, // Left bracket(push Element + // state) + IterativeParsingArrayFinishState, // Right bracket + IterativeParsingObjectInitialState, // Left curly bracket(push + // Element state) + IterativeParsingErrorState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingElementState, // String + IterativeParsingElementState, // False + IterativeParsingElementState, // True + IterativeParsingElementState, // Null + IterativeParsingElementState // Number + }, + // Element + { + IterativeParsingErrorState, // Left bracket + IterativeParsingArrayFinishState, // Right bracket + IterativeParsingErrorState, // Left curly bracket + IterativeParsingErrorState, // Right curly bracket + IterativeParsingElementDelimiterState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingErrorState, // String + IterativeParsingErrorState, // False + IterativeParsingErrorState, // True + IterativeParsingErrorState, // Null + IterativeParsingErrorState // Number + }, + // ArrayFinish(sink state) + {IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState}, + // Single Value (sink state) + {IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState, IterativeParsingErrorState, + IterativeParsingErrorState}, + // ElementDelimiter + { + IterativeParsingArrayInitialState, // Left bracket(push Element + // state) + IterativeParsingArrayFinishState, // Right bracket + IterativeParsingObjectInitialState, // Left curly bracket(push + // Element state) + IterativeParsingErrorState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingElementState, // String + IterativeParsingElementState, // False + IterativeParsingElementState, // True + IterativeParsingElementState, // Null + IterativeParsingElementState // Number + }, + // MemberDelimiter + { + IterativeParsingErrorState, // Left bracket + IterativeParsingErrorState, // Right bracket + IterativeParsingErrorState, // Left curly bracket + IterativeParsingObjectFinishState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingMemberKeyState, // String + IterativeParsingErrorState, // False + IterativeParsingErrorState, // True + IterativeParsingErrorState, // Null + IterativeParsingErrorState // Number + }, + // KeyValueDelimiter + { + IterativeParsingArrayInitialState, // Left bracket(push MemberValue + // state) + IterativeParsingErrorState, // Right bracket + IterativeParsingObjectInitialState, // Left curly bracket(push + // MemberValue state) + IterativeParsingErrorState, // Right curly bracket + IterativeParsingErrorState, // Comma + IterativeParsingErrorState, // Colon + IterativeParsingMemberValueState, // String + IterativeParsingMemberValueState, // False + IterativeParsingMemberValueState, // True + IterativeParsingMemberValueState, // Null + IterativeParsingMemberValueState // Number + }, + }; // End of G + + return static_cast(G[state][token]); + } + + // Make an advance in the token stream and state based on the candidate + // destination state which was returned by Transit(). May return a new state + // on state pop. + template + RAPIDJSON_FORCEINLINE IterativeParsingState Transit(IterativeParsingState src, + Token token, + IterativeParsingState dst, + InputStream &is, + Handler &handler) { + (void)token; + + switch (dst) { + case IterativeParsingErrorState: + return dst; + + case IterativeParsingObjectInitialState: + case IterativeParsingArrayInitialState: { + // Push the state(Element or MemeberValue) if we are nested in another + // array or value of member. In this way we can get the correct state on + // ObjectFinish or ArrayFinish by frame pop. + IterativeParsingState n = src; + if (src == IterativeParsingArrayInitialState || + src == IterativeParsingElementDelimiterState) + n = IterativeParsingElementState; + else if (src == IterativeParsingKeyValueDelimiterState) + n = IterativeParsingMemberValueState; + // Push current state. + *stack_.template Push(1) = n; + // Initialize and push the member/element count. + *stack_.template Push(1) = 0; + // Call handler + bool hr = (dst == IterativeParsingObjectInitialState) + ? handler.StartObject() + : handler.StartArray(); + // On handler short circuits the parsing. + if (!hr) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorTermination, is.Tell()); + return IterativeParsingErrorState; + } else { + is.Take(); + return dst; + } + } + + case IterativeParsingMemberKeyState: + ParseString(is, handler, true); + if (HasParseError()) + return IterativeParsingErrorState; + else + return dst; + + case IterativeParsingKeyValueDelimiterState: + RAPIDJSON_ASSERT(token == ColonToken); + is.Take(); + return dst; + + case IterativeParsingMemberValueState: + // Must be non-compound value. Or it would be ObjectInitial or + // ArrayInitial state. + ParseValue(is, handler); + if (HasParseError()) { + return IterativeParsingErrorState; + } + return dst; + + case IterativeParsingElementState: + // Must be non-compound value. Or it would be ObjectInitial or + // ArrayInitial state. + ParseValue(is, handler); + if (HasParseError()) { + return IterativeParsingErrorState; + } + return dst; + + case IterativeParsingMemberDelimiterState: + case IterativeParsingElementDelimiterState: + is.Take(); + // Update member/element count. + *stack_.template Top() = *stack_.template Top() + 1; + return dst; + + case IterativeParsingObjectFinishState: { + // Transit from delimiter is only allowed when trailing commas are + // enabled + if (!(parseFlags & kParseTrailingCommasFlag) && + src == IterativeParsingMemberDelimiterState) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorObjectMissName, is.Tell()); + return IterativeParsingErrorState; + } + // Get member count. + SizeType c = *stack_.template Pop(1); + // If the object is not empty, count the last member. + if (src == IterativeParsingMemberValueState) ++c; + // Restore the state. + IterativeParsingState n = static_cast( + *stack_.template Pop(1)); + // Transit to Finish state if this is the topmost scope. + if (n == IterativeParsingStartState) n = IterativeParsingFinishState; + // Call handler + bool hr = handler.EndObject(c); + // On handler short circuits the parsing. + if (!hr) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorTermination, is.Tell()); + return IterativeParsingErrorState; + } else { + is.Take(); + return n; + } + } + + case IterativeParsingArrayFinishState: { + // Transit from delimiter is only allowed when trailing commas are + // enabled + if (!(parseFlags & kParseTrailingCommasFlag) && + src == IterativeParsingElementDelimiterState) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorValueInvalid, is.Tell()); + return IterativeParsingErrorState; + } + // Get element count. + SizeType c = *stack_.template Pop(1); + // If the array is not empty, count the last element. + if (src == IterativeParsingElementState) ++c; + // Restore the state. + IterativeParsingState n = static_cast( + *stack_.template Pop(1)); + // Transit to Finish state if this is the topmost scope. + if (n == IterativeParsingStartState) n = IterativeParsingFinishState; + // Call handler + bool hr = handler.EndArray(c); + // On handler short circuits the parsing. + if (!hr) { + RAPIDJSON_PARSE_ERROR_NORETURN(kParseErrorTermination, is.Tell()); + return IterativeParsingErrorState; + } else { + is.Take(); + return n; + } + } + + default: + // This branch is for IterativeParsingValueState actually. + // Use `default:` rather than + // `case IterativeParsingValueState:` is for code coverage. + + // The IterativeParsingStartState is not enumerated in this switch-case. + // It is impossible for that case. And it can be caught by following + // assertion. + + // The IterativeParsingFinishState is not enumerated in this switch-case + // either. It is a "derivative" state which cannot triggered from + // Predict() directly. Therefore it cannot happen here. And it can be + // caught by following assertion. + RAPIDJSON_ASSERT(dst == IterativeParsingValueState); + + // Must be non-compound value. Or it would be ObjectInitial or + // ArrayInitial state. + ParseValue(is, handler); + if (HasParseError()) { + return IterativeParsingErrorState; + } + return IterativeParsingFinishState; + } + } + + template + void HandleError(IterativeParsingState src, InputStream &is) { + if (HasParseError()) { + // Error flag has been set. + return; + } + + switch (src) { + case IterativeParsingStartState: + RAPIDJSON_PARSE_ERROR(kParseErrorDocumentEmpty, is.Tell()); + return; + case IterativeParsingFinishState: + RAPIDJSON_PARSE_ERROR(kParseErrorDocumentRootNotSingular, is.Tell()); + return; + case IterativeParsingObjectInitialState: + case IterativeParsingMemberDelimiterState: + RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissName, is.Tell()); + return; + case IterativeParsingMemberKeyState: + RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissColon, is.Tell()); + return; + case IterativeParsingMemberValueState: + RAPIDJSON_PARSE_ERROR(kParseErrorObjectMissCommaOrCurlyBracket, + is.Tell()); + return; + case IterativeParsingKeyValueDelimiterState: + case IterativeParsingArrayInitialState: + case IterativeParsingElementDelimiterState: + RAPIDJSON_PARSE_ERROR(kParseErrorValueInvalid, is.Tell()); + return; + default: + RAPIDJSON_ASSERT(src == IterativeParsingElementState); + RAPIDJSON_PARSE_ERROR(kParseErrorArrayMissCommaOrSquareBracket, + is.Tell()); + return; + } + } + + RAPIDJSON_FORCEINLINE bool IsIterativeParsingDelimiterState( + IterativeParsingState s) const { + return s >= IterativeParsingElementDelimiterState; + } + + RAPIDJSON_FORCEINLINE bool IsIterativeParsingCompleteState( + IterativeParsingState s) const { + return s <= IterativeParsingErrorState; + } + + template + ParseResult IterativeParse(InputStream &is, Handler &handler) { + parseResult_.Clear(); + ClearStackOnExit scope(*this); + IterativeParsingState state = IterativeParsingStartState; + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + while (is.Peek() != '\0') { + Token t = Tokenize(is.Peek()); + IterativeParsingState n = Predict(state, t); + IterativeParsingState d = Transit(state, t, n, is, handler); + + if (d == IterativeParsingErrorState) { + HandleError(state, is); + break; + } + + state = d; + + // Do not further consume streams if a root JSON has been parsed. + if ((parseFlags & kParseStopWhenDoneFlag) && + state == IterativeParsingFinishState) + break; + + SkipWhitespaceAndComments(is); + RAPIDJSON_PARSE_ERROR_EARLY_RETURN(parseResult_); + } + + // Handle the end of file. + if (state != IterativeParsingFinishState) HandleError(state, is); + + return parseResult_; + } + + static const size_t kDefaultStackCapacity = + 256; //!< Default stack capacity in bytes for storing a single decoded + //!< string. + internal::Stack + stack_; //!< A stack for storing decoded string temporarily during + //!< non-destructive parsing. + ParseResult parseResult_; + IterativeParsingState state_; +}; // class GenericReader + +//! Reader with UTF8 encoding and default allocator. +typedef GenericReader, UTF8<>> Reader; + +RAPIDJSON_NAMESPACE_END + +#if defined(__clang__) || defined(_MSC_VER) +RAPIDJSON_DIAG_POP +#endif + +#ifdef __GNUC__ +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_READER_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/schema.h b/src/livox_ros_driver2/3rdparty/rapidjson/schema.h new file mode 100644 index 0000000..b9dc8c8 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/schema.h @@ -0,0 +1,2743 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available-> +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip-> All +// rights reserved-> +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License-> You may obtain a copy of the License +// at +// +// http://opensource->org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied-> See the +// License for the specific language governing permissions and limitations under +// the License-> + +#ifndef RAPIDJSON_SCHEMA_H_ +#define RAPIDJSON_SCHEMA_H_ + +#include // abs, floor +#include "document.h" +#include "pointer.h" +#include "stringbuffer.h" + +#if !defined(RAPIDJSON_SCHEMA_USE_INTERNALREGEX) +#define RAPIDJSON_SCHEMA_USE_INTERNALREGEX 1 +#else +#define RAPIDJSON_SCHEMA_USE_INTERNALREGEX 0 +#endif + +#if !RAPIDJSON_SCHEMA_USE_INTERNALREGEX && \ + defined(RAPIDJSON_SCHEMA_USE_STDREGEX) && \ + (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800)) +#define RAPIDJSON_SCHEMA_USE_STDREGEX 1 +#else +#define RAPIDJSON_SCHEMA_USE_STDREGEX 0 +#endif + +#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX +#include "internal/regex.h" +#elif RAPIDJSON_SCHEMA_USE_STDREGEX +#include +#endif + +#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX || RAPIDJSON_SCHEMA_USE_STDREGEX +#define RAPIDJSON_SCHEMA_HAS_REGEX 1 +#else +#define RAPIDJSON_SCHEMA_HAS_REGEX 0 +#endif + +#ifndef RAPIDJSON_SCHEMA_VERBOSE +#define RAPIDJSON_SCHEMA_VERBOSE 0 +#endif + +#if RAPIDJSON_SCHEMA_VERBOSE +#include "stringbuffer.h" +#endif + +RAPIDJSON_DIAG_PUSH + +#if defined(__GNUC__) +RAPIDJSON_DIAG_OFF(effc++) +#endif + +#ifdef __clang__ +RAPIDJSON_DIAG_OFF(weak - vtables) +RAPIDJSON_DIAG_OFF(exit - time - destructors) +RAPIDJSON_DIAG_OFF(c++ 98 - compat - pedantic) +RAPIDJSON_DIAG_OFF(variadic - macros) +#elif defined(_MSC_VER) +RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +/////////////////////////////////////////////////////////////////////////////// +// Verbose Utilities + +#if RAPIDJSON_SCHEMA_VERBOSE + +namespace internal { + +inline void PrintInvalidKeyword(const char *keyword) { + printf("Fail keyword: %s\n", keyword); +} + +inline void PrintInvalidKeyword(const wchar_t *keyword) { + wprintf(L"Fail keyword: %ls\n", keyword); +} + +inline void PrintInvalidDocument(const char *document) { + printf("Fail document: %s\n\n", document); +} + +inline void PrintInvalidDocument(const wchar_t *document) { + wprintf(L"Fail document: %ls\n\n", document); +} + +inline void PrintValidatorPointers(unsigned depth, const char *s, + const char *d) { + printf("S: %*s%s\nD: %*s%s\n\n", depth * 4, " ", s, depth * 4, " ", d); +} + +inline void PrintValidatorPointers(unsigned depth, const wchar_t *s, + const wchar_t *d) { + wprintf(L"S: %*ls%ls\nD: %*ls%ls\n\n", depth * 4, L" ", s, depth * 4, L" ", + d); +} + +} // namespace internal + +#endif // RAPIDJSON_SCHEMA_VERBOSE + +/////////////////////////////////////////////////////////////////////////////// +// RAPIDJSON_INVALID_KEYWORD_RETURN + +#if RAPIDJSON_SCHEMA_VERBOSE +#define RAPIDJSON_INVALID_KEYWORD_VERBOSE(keyword) \ + internal::PrintInvalidKeyword(keyword) +#else +#define RAPIDJSON_INVALID_KEYWORD_VERBOSE(keyword) +#endif + +#define RAPIDJSON_INVALID_KEYWORD_RETURN(keyword) \ + RAPIDJSON_MULTILINEMACRO_BEGIN \ + context.invalidKeyword = keyword.GetString(); \ + RAPIDJSON_INVALID_KEYWORD_VERBOSE(keyword.GetString()); \ + return false; \ + RAPIDJSON_MULTILINEMACRO_END + +/////////////////////////////////////////////////////////////////////////////// +// Forward declarations + +template +class GenericSchemaDocument; + +namespace internal { + +template +class Schema; + +/////////////////////////////////////////////////////////////////////////////// +// ISchemaValidator + +class ISchemaValidator { + public: + virtual ~ISchemaValidator() {} + virtual bool IsValid() const = 0; +}; + +/////////////////////////////////////////////////////////////////////////////// +// ISchemaStateFactory + +template +class ISchemaStateFactory { + public: + virtual ~ISchemaStateFactory() {} + virtual ISchemaValidator *CreateSchemaValidator(const SchemaType &) = 0; + virtual void DestroySchemaValidator(ISchemaValidator *validator) = 0; + virtual void *CreateHasher() = 0; + virtual uint64_t GetHashCode(void *hasher) = 0; + virtual void DestroryHasher(void *hasher) = 0; + virtual void *MallocState(size_t size) = 0; + virtual void FreeState(void *p) = 0; +}; + +/////////////////////////////////////////////////////////////////////////////// +// IValidationErrorHandler + +template +class IValidationErrorHandler { + public: + typedef typename SchemaType::Ch Ch; + typedef typename SchemaType::SValue SValue; + + virtual ~IValidationErrorHandler() {} + + virtual void NotMultipleOf(int64_t actual, const SValue &expected) = 0; + virtual void NotMultipleOf(uint64_t actual, const SValue &expected) = 0; + virtual void NotMultipleOf(double actual, const SValue &expected) = 0; + virtual void AboveMaximum(int64_t actual, const SValue &expected, + bool exclusive) = 0; + virtual void AboveMaximum(uint64_t actual, const SValue &expected, + bool exclusive) = 0; + virtual void AboveMaximum(double actual, const SValue &expected, + bool exclusive) = 0; + virtual void BelowMinimum(int64_t actual, const SValue &expected, + bool exclusive) = 0; + virtual void BelowMinimum(uint64_t actual, const SValue &expected, + bool exclusive) = 0; + virtual void BelowMinimum(double actual, const SValue &expected, + bool exclusive) = 0; + + virtual void TooLong(const Ch *str, SizeType length, SizeType expected) = 0; + virtual void TooShort(const Ch *str, SizeType length, SizeType expected) = 0; + virtual void DoesNotMatch(const Ch *str, SizeType length) = 0; + + virtual void DisallowedItem(SizeType index) = 0; + virtual void TooFewItems(SizeType actualCount, SizeType expectedCount) = 0; + virtual void TooManyItems(SizeType actualCount, SizeType expectedCount) = 0; + virtual void DuplicateItems(SizeType index1, SizeType index2) = 0; + + virtual void TooManyProperties(SizeType actualCount, + SizeType expectedCount) = 0; + virtual void TooFewProperties(SizeType actualCount, + SizeType expectedCount) = 0; + virtual void StartMissingProperties() = 0; + virtual void AddMissingProperty(const SValue &name) = 0; + virtual bool EndMissingProperties() = 0; + virtual void PropertyViolations(ISchemaValidator **subvalidators, + SizeType count) = 0; + virtual void DisallowedProperty(const Ch *name, SizeType length) = 0; + + virtual void StartDependencyErrors() = 0; + virtual void StartMissingDependentProperties() = 0; + virtual void AddMissingDependentProperty(const SValue &targetName) = 0; + virtual void EndMissingDependentProperties(const SValue &sourceName) = 0; + virtual void AddDependencySchemaError(const SValue &souceName, + ISchemaValidator *subvalidator) = 0; + virtual bool EndDependencyErrors() = 0; + + virtual void DisallowedValue() = 0; + virtual void StartDisallowedType() = 0; + virtual void AddExpectedType( + const typename SchemaType::ValueType &expectedType) = 0; + virtual void EndDisallowedType( + const typename SchemaType::ValueType &actualType) = 0; + virtual void NotAllOf(ISchemaValidator **subvalidators, SizeType count) = 0; + virtual void NoneOf(ISchemaValidator **subvalidators, SizeType count) = 0; + virtual void NotOneOf(ISchemaValidator **subvalidators, SizeType count) = 0; + virtual void Disallowed() = 0; +}; + +/////////////////////////////////////////////////////////////////////////////// +// Hasher + +// For comparison of compound value +template +class Hasher { + public: + typedef typename Encoding::Ch Ch; + + Hasher(Allocator *allocator = 0, size_t stackCapacity = kDefaultSize) + : stack_(allocator, stackCapacity) {} + + bool Null() { return WriteType(kNullType); } + bool Bool(bool b) { return WriteType(b ? kTrueType : kFalseType); } + bool Int(int i) { + Number n; + n.u.i = i; + n.d = static_cast(i); + return WriteNumber(n); + } + bool Uint(unsigned u) { + Number n; + n.u.u = u; + n.d = static_cast(u); + return WriteNumber(n); + } + bool Int64(int64_t i) { + Number n; + n.u.i = i; + n.d = static_cast(i); + return WriteNumber(n); + } + bool Uint64(uint64_t u) { + Number n; + n.u.u = u; + n.d = static_cast(u); + return WriteNumber(n); + } + bool Double(double d) { + Number n; + if (d < 0) + n.u.i = static_cast(d); + else + n.u.u = static_cast(d); + n.d = d; + return WriteNumber(n); + } + + bool RawNumber(const Ch *str, SizeType len, bool) { + WriteBuffer(kNumberType, str, len * sizeof(Ch)); + return true; + } + + bool String(const Ch *str, SizeType len, bool) { + WriteBuffer(kStringType, str, len * sizeof(Ch)); + return true; + } + + bool StartObject() { return true; } + bool Key(const Ch *str, SizeType len, bool copy) { + return String(str, len, copy); + } + bool EndObject(SizeType memberCount) { + uint64_t h = Hash(0, kObjectType); + uint64_t *kv = stack_.template Pop(memberCount * 2); + for (SizeType i = 0; i < memberCount; i++) + h ^= Hash(kv[i * 2], + kv[i * 2 + 1]); // Use xor to achieve member order insensitive + *stack_.template Push() = h; + return true; + } + + bool StartArray() { return true; } + bool EndArray(SizeType elementCount) { + uint64_t h = Hash(0, kArrayType); + uint64_t *e = stack_.template Pop(elementCount); + for (SizeType i = 0; i < elementCount; i++) + h = Hash(h, e[i]); // Use hash to achieve element order sensitive + *stack_.template Push() = h; + return true; + } + + bool IsValid() const { return stack_.GetSize() == sizeof(uint64_t); } + + uint64_t GetHashCode() const { + RAPIDJSON_ASSERT(IsValid()); + return *stack_.template Top(); + } + + private: + static const size_t kDefaultSize = 256; + struct Number { + union U { + uint64_t u; + int64_t i; + } u; + double d; + }; + + bool WriteType(Type type) { return WriteBuffer(type, 0, 0); } + + bool WriteNumber(const Number &n) { + return WriteBuffer(kNumberType, &n, sizeof(n)); + } + + bool WriteBuffer(Type type, const void *data, size_t len) { + // FNV-1a from http://isthe.com/chongo/tech/comp/fnv/ + uint64_t h = Hash(RAPIDJSON_UINT64_C2(0x84222325, 0xcbf29ce4), type); + const unsigned char *d = static_cast(data); + for (size_t i = 0; i < len; i++) h = Hash(h, d[i]); + *stack_.template Push() = h; + return true; + } + + static uint64_t Hash(uint64_t h, uint64_t d) { + static const uint64_t kPrime = RAPIDJSON_UINT64_C2(0x00000100, 0x000001b3); + h ^= d; + h *= kPrime; + return h; + } + + Stack stack_; +}; + +/////////////////////////////////////////////////////////////////////////////// +// SchemaValidationContext + +template +struct SchemaValidationContext { + typedef Schema SchemaType; + typedef ISchemaStateFactory SchemaValidatorFactoryType; + typedef IValidationErrorHandler ErrorHandlerType; + typedef typename SchemaType::ValueType ValueType; + typedef typename ValueType::Ch Ch; + + enum PatternValidatorType { + kPatternValidatorOnly, + kPatternValidatorWithProperty, + kPatternValidatorWithAdditionalProperty + }; + + SchemaValidationContext(SchemaValidatorFactoryType &f, ErrorHandlerType &eh, + const SchemaType *s) + : factory(f), + error_handler(eh), + schema(s), + valueSchema(), + invalidKeyword(), + hasher(), + arrayElementHashCodes(), + validators(), + validatorCount(), + patternPropertiesValidators(), + patternPropertiesValidatorCount(), + patternPropertiesSchemas(), + patternPropertiesSchemaCount(), + valuePatternValidatorType(kPatternValidatorOnly), + propertyExist(), + inArray(false), + valueUniqueness(false), + arrayUniqueness(false) {} + + ~SchemaValidationContext() { + if (hasher) factory.DestroryHasher(hasher); + if (validators) { + for (SizeType i = 0; i < validatorCount; i++) + factory.DestroySchemaValidator(validators[i]); + factory.FreeState(validators); + } + if (patternPropertiesValidators) { + for (SizeType i = 0; i < patternPropertiesValidatorCount; i++) + factory.DestroySchemaValidator(patternPropertiesValidators[i]); + factory.FreeState(patternPropertiesValidators); + } + if (patternPropertiesSchemas) factory.FreeState(patternPropertiesSchemas); + if (propertyExist) factory.FreeState(propertyExist); + } + + SchemaValidatorFactoryType &factory; + ErrorHandlerType &error_handler; + const SchemaType *schema; + const SchemaType *valueSchema; + const Ch *invalidKeyword; + void *hasher; // Only validator access + void *arrayElementHashCodes; // Only validator access this + ISchemaValidator **validators; + SizeType validatorCount; + ISchemaValidator **patternPropertiesValidators; + SizeType patternPropertiesValidatorCount; + const SchemaType **patternPropertiesSchemas; + SizeType patternPropertiesSchemaCount; + PatternValidatorType valuePatternValidatorType; + PatternValidatorType objectPatternValidatorType; + SizeType arrayElementIndex; + bool *propertyExist; + bool inArray; + bool valueUniqueness; + bool arrayUniqueness; +}; + +/////////////////////////////////////////////////////////////////////////////// +// Schema + +template +class Schema { + public: + typedef typename SchemaDocumentType::ValueType ValueType; + typedef typename SchemaDocumentType::AllocatorType AllocatorType; + typedef typename SchemaDocumentType::PointerType PointerType; + typedef typename ValueType::EncodingType EncodingType; + typedef typename EncodingType::Ch Ch; + typedef SchemaValidationContext Context; + typedef Schema SchemaType; + typedef GenericValue SValue; + typedef IValidationErrorHandler ErrorHandler; + friend class GenericSchemaDocument; + + Schema(SchemaDocumentType *schemaDocument, const PointerType &p, + const ValueType &value, const ValueType &document, + AllocatorType *allocator) + : allocator_(allocator), + uri_(schemaDocument->GetURI(), *allocator), + pointer_(p, allocator), + typeless_(schemaDocument->GetTypeless()), + enum_(), + enumCount_(), + not_(), + type_((1 << kTotalSchemaType) - 1), // typeless + validatorCount_(), + notValidatorIndex_(), + properties_(), + additionalPropertiesSchema_(), + patternProperties_(), + patternPropertyCount_(), + propertyCount_(), + minProperties_(), + maxProperties_(SizeType(~0)), + additionalProperties_(true), + hasDependencies_(), + hasRequired_(), + hasSchemaDependencies_(), + additionalItemsSchema_(), + itemsList_(), + itemsTuple_(), + itemsTupleCount_(), + minItems_(), + maxItems_(SizeType(~0)), + additionalItems_(true), + uniqueItems_(false), + pattern_(), + minLength_(0), + maxLength_(~SizeType(0)), + exclusiveMinimum_(false), + exclusiveMaximum_(false), + defaultValueLength_(0) { + typedef typename SchemaDocumentType::ValueType ValueType; + typedef typename ValueType::ConstValueIterator ConstValueIterator; + typedef typename ValueType::ConstMemberIterator ConstMemberIterator; + + if (!value.IsObject()) return; + + if (const ValueType *v = GetMember(value, GetTypeString())) { + type_ = 0; + if (v->IsString()) + AddType(*v); + else if (v->IsArray()) + for (ConstValueIterator itr = v->Begin(); itr != v->End(); ++itr) + AddType(*itr); + } + + if (const ValueType *v = GetMember(value, GetEnumString())) + if (v->IsArray() && v->Size() > 0) { + enum_ = static_cast( + allocator_->Malloc(sizeof(uint64_t) * v->Size())); + for (ConstValueIterator itr = v->Begin(); itr != v->End(); ++itr) { + typedef Hasher> EnumHasherType; + char buffer[256u + 24]; + MemoryPoolAllocator<> hasherAllocator(buffer, sizeof(buffer)); + EnumHasherType h(&hasherAllocator, 256); + itr->Accept(h); + enum_[enumCount_++] = h.GetHashCode(); + } + } + + if (schemaDocument) { + AssignIfExist(allOf_, *schemaDocument, p, value, GetAllOfString(), + document); + AssignIfExist(anyOf_, *schemaDocument, p, value, GetAnyOfString(), + document); + AssignIfExist(oneOf_, *schemaDocument, p, value, GetOneOfString(), + document); + } + + if (const ValueType *v = GetMember(value, GetNotString())) { + schemaDocument->CreateSchema(¬_, p.Append(GetNotString(), allocator_), + *v, document); + notValidatorIndex_ = validatorCount_; + validatorCount_++; + } + + // Object + + const ValueType *properties = GetMember(value, GetPropertiesString()); + const ValueType *required = GetMember(value, GetRequiredString()); + const ValueType *dependencies = GetMember(value, GetDependenciesString()); + { + // Gather properties from properties/required/dependencies + SValue allProperties(kArrayType); + + if (properties && properties->IsObject()) + for (ConstMemberIterator itr = properties->MemberBegin(); + itr != properties->MemberEnd(); ++itr) + AddUniqueElement(allProperties, itr->name); + + if (required && required->IsArray()) + for (ConstValueIterator itr = required->Begin(); itr != required->End(); + ++itr) + if (itr->IsString()) AddUniqueElement(allProperties, *itr); + + if (dependencies && dependencies->IsObject()) + for (ConstMemberIterator itr = dependencies->MemberBegin(); + itr != dependencies->MemberEnd(); ++itr) { + AddUniqueElement(allProperties, itr->name); + if (itr->value.IsArray()) + for (ConstValueIterator i = itr->value.Begin(); + i != itr->value.End(); ++i) + if (i->IsString()) AddUniqueElement(allProperties, *i); + } + + if (allProperties.Size() > 0) { + propertyCount_ = allProperties.Size(); + properties_ = static_cast( + allocator_->Malloc(sizeof(Property) * propertyCount_)); + for (SizeType i = 0; i < propertyCount_; i++) { + new (&properties_[i]) Property(); + properties_[i].name = allProperties[i]; + properties_[i].schema = typeless_; + } + } + } + + if (properties && properties->IsObject()) { + PointerType q = p.Append(GetPropertiesString(), allocator_); + for (ConstMemberIterator itr = properties->MemberBegin(); + itr != properties->MemberEnd(); ++itr) { + SizeType index; + if (FindPropertyIndex(itr->name, &index)) + schemaDocument->CreateSchema(&properties_[index].schema, + q.Append(itr->name, allocator_), + itr->value, document); + } + } + + if (const ValueType *v = GetMember(value, GetPatternPropertiesString())) { + PointerType q = p.Append(GetPatternPropertiesString(), allocator_); + patternProperties_ = static_cast( + allocator_->Malloc(sizeof(PatternProperty) * v->MemberCount())); + patternPropertyCount_ = 0; + + for (ConstMemberIterator itr = v->MemberBegin(); itr != v->MemberEnd(); + ++itr) { + new (&patternProperties_[patternPropertyCount_]) PatternProperty(); + patternProperties_[patternPropertyCount_].pattern = + CreatePattern(itr->name); + schemaDocument->CreateSchema( + &patternProperties_[patternPropertyCount_].schema, + q.Append(itr->name, allocator_), itr->value, document); + patternPropertyCount_++; + } + } + + if (required && required->IsArray()) + for (ConstValueIterator itr = required->Begin(); itr != required->End(); + ++itr) + if (itr->IsString()) { + SizeType index; + if (FindPropertyIndex(*itr, &index)) { + properties_[index].required = true; + hasRequired_ = true; + } + } + + if (dependencies && dependencies->IsObject()) { + PointerType q = p.Append(GetDependenciesString(), allocator_); + hasDependencies_ = true; + for (ConstMemberIterator itr = dependencies->MemberBegin(); + itr != dependencies->MemberEnd(); ++itr) { + SizeType sourceIndex; + if (FindPropertyIndex(itr->name, &sourceIndex)) { + if (itr->value.IsArray()) { + properties_[sourceIndex].dependencies = static_cast( + allocator_->Malloc(sizeof(bool) * propertyCount_)); + std::memset(properties_[sourceIndex].dependencies, 0, + sizeof(bool) * propertyCount_); + for (ConstValueIterator targetItr = itr->value.Begin(); + targetItr != itr->value.End(); ++targetItr) { + SizeType targetIndex; + if (FindPropertyIndex(*targetItr, &targetIndex)) + properties_[sourceIndex].dependencies[targetIndex] = true; + } + } else if (itr->value.IsObject()) { + hasSchemaDependencies_ = true; + schemaDocument->CreateSchema( + &properties_[sourceIndex].dependenciesSchema, + q.Append(itr->name, allocator_), itr->value, document); + properties_[sourceIndex].dependenciesValidatorIndex = + validatorCount_; + validatorCount_++; + } + } + } + } + + if (const ValueType *v = + GetMember(value, GetAdditionalPropertiesString())) { + if (v->IsBool()) + additionalProperties_ = v->GetBool(); + else if (v->IsObject()) + schemaDocument->CreateSchema( + &additionalPropertiesSchema_, + p.Append(GetAdditionalPropertiesString(), allocator_), *v, + document); + } + + AssignIfExist(minProperties_, value, GetMinPropertiesString()); + AssignIfExist(maxProperties_, value, GetMaxPropertiesString()); + + // Array + if (const ValueType *v = GetMember(value, GetItemsString())) { + PointerType q = p.Append(GetItemsString(), allocator_); + if (v->IsObject()) // List validation + schemaDocument->CreateSchema(&itemsList_, q, *v, document); + else if (v->IsArray()) { // Tuple validation + itemsTuple_ = static_cast( + allocator_->Malloc(sizeof(const Schema *) * v->Size())); + SizeType index = 0; + for (ConstValueIterator itr = v->Begin(); itr != v->End(); + ++itr, index++) + schemaDocument->CreateSchema(&itemsTuple_[itemsTupleCount_++], + q.Append(index, allocator_), *itr, + document); + } + } + + AssignIfExist(minItems_, value, GetMinItemsString()); + AssignIfExist(maxItems_, value, GetMaxItemsString()); + + if (const ValueType *v = GetMember(value, GetAdditionalItemsString())) { + if (v->IsBool()) + additionalItems_ = v->GetBool(); + else if (v->IsObject()) + schemaDocument->CreateSchema( + &additionalItemsSchema_, + p.Append(GetAdditionalItemsString(), allocator_), *v, document); + } + + AssignIfExist(uniqueItems_, value, GetUniqueItemsString()); + + // String + AssignIfExist(minLength_, value, GetMinLengthString()); + AssignIfExist(maxLength_, value, GetMaxLengthString()); + + if (const ValueType *v = GetMember(value, GetPatternString())) + pattern_ = CreatePattern(*v); + + // Number + if (const ValueType *v = GetMember(value, GetMinimumString())) + if (v->IsNumber()) minimum_.CopyFrom(*v, *allocator_); + + if (const ValueType *v = GetMember(value, GetMaximumString())) + if (v->IsNumber()) maximum_.CopyFrom(*v, *allocator_); + + AssignIfExist(exclusiveMinimum_, value, GetExclusiveMinimumString()); + AssignIfExist(exclusiveMaximum_, value, GetExclusiveMaximumString()); + + if (const ValueType *v = GetMember(value, GetMultipleOfString())) + if (v->IsNumber() && v->GetDouble() > 0.0) + multipleOf_.CopyFrom(*v, *allocator_); + + // Default + if (const ValueType *v = GetMember(value, GetDefaultValueString())) + if (v->IsString()) defaultValueLength_ = v->GetStringLength(); + } + + ~Schema() { + AllocatorType::Free(enum_); + if (properties_) { + for (SizeType i = 0; i < propertyCount_; i++) properties_[i].~Property(); + AllocatorType::Free(properties_); + } + if (patternProperties_) { + for (SizeType i = 0; i < patternPropertyCount_; i++) + patternProperties_[i].~PatternProperty(); + AllocatorType::Free(patternProperties_); + } + AllocatorType::Free(itemsTuple_); +#if RAPIDJSON_SCHEMA_HAS_REGEX + if (pattern_) { + pattern_->~RegexType(); + AllocatorType::Free(pattern_); + } +#endif + } + + const SValue &GetURI() const { return uri_; } + + const PointerType &GetPointer() const { return pointer_; } + + bool BeginValue(Context &context) const { + if (context.inArray) { + if (uniqueItems_) context.valueUniqueness = true; + + if (itemsList_) + context.valueSchema = itemsList_; + else if (itemsTuple_) { + if (context.arrayElementIndex < itemsTupleCount_) + context.valueSchema = itemsTuple_[context.arrayElementIndex]; + else if (additionalItemsSchema_) + context.valueSchema = additionalItemsSchema_; + else if (additionalItems_) + context.valueSchema = typeless_; + else { + context.error_handler.DisallowedItem(context.arrayElementIndex); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetItemsString()); + } + } else + context.valueSchema = typeless_; + + context.arrayElementIndex++; + } + return true; + } + + RAPIDJSON_FORCEINLINE bool EndValue(Context &context) const { + if (context.patternPropertiesValidatorCount > 0) { + bool otherValid = false; + SizeType count = context.patternPropertiesValidatorCount; + if (context.objectPatternValidatorType != Context::kPatternValidatorOnly) + otherValid = context.patternPropertiesValidators[--count]->IsValid(); + + bool patternValid = true; + for (SizeType i = 0; i < count; i++) + if (!context.patternPropertiesValidators[i]->IsValid()) { + patternValid = false; + break; + } + + if (context.objectPatternValidatorType == + Context::kPatternValidatorOnly) { + if (!patternValid) { + context.error_handler.PropertyViolations( + context.patternPropertiesValidators, count); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternPropertiesString()); + } + } else if (context.objectPatternValidatorType == + Context::kPatternValidatorWithProperty) { + if (!patternValid || !otherValid) { + context.error_handler.PropertyViolations( + context.patternPropertiesValidators, count + 1); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternPropertiesString()); + } + } else if (!patternValid && + !otherValid) { // kPatternValidatorWithAdditionalProperty) + context.error_handler.PropertyViolations( + context.patternPropertiesValidators, count + 1); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternPropertiesString()); + } + } + + if (enum_) { + const uint64_t h = context.factory.GetHashCode(context.hasher); + for (SizeType i = 0; i < enumCount_; i++) + if (enum_[i] == h) goto foundEnum; + context.error_handler.DisallowedValue(); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetEnumString()); + foundEnum:; + } + + if (allOf_.schemas) + for (SizeType i = allOf_.begin; i < allOf_.begin + allOf_.count; i++) + if (!context.validators[i]->IsValid()) { + context.error_handler.NotAllOf(&context.validators[allOf_.begin], + allOf_.count); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetAllOfString()); + } + + if (anyOf_.schemas) { + for (SizeType i = anyOf_.begin; i < anyOf_.begin + anyOf_.count; i++) + if (context.validators[i]->IsValid()) goto foundAny; + context.error_handler.NoneOf(&context.validators[anyOf_.begin], + anyOf_.count); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetAnyOfString()); + foundAny:; + } + + if (oneOf_.schemas) { + bool oneValid = false; + for (SizeType i = oneOf_.begin; i < oneOf_.begin + oneOf_.count; i++) + if (context.validators[i]->IsValid()) { + if (oneValid) { + context.error_handler.NotOneOf(&context.validators[oneOf_.begin], + oneOf_.count); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetOneOfString()); + } else + oneValid = true; + } + if (!oneValid) { + context.error_handler.NotOneOf(&context.validators[oneOf_.begin], + oneOf_.count); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetOneOfString()); + } + } + + if (not_ && context.validators[notValidatorIndex_]->IsValid()) { + context.error_handler.Disallowed(); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetNotString()); + } + + return true; + } + + bool Null(Context &context) const { + if (!(type_ & (1 << kNullSchemaType))) { + DisallowedType(context, GetNullString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + return CreateParallelValidator(context); + } + + bool Bool(Context &context, bool) const { + if (!(type_ & (1 << kBooleanSchemaType))) { + DisallowedType(context, GetBooleanString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + return CreateParallelValidator(context); + } + + bool Int(Context &context, int i) const { + if (!CheckInt(context, i)) return false; + return CreateParallelValidator(context); + } + + bool Uint(Context &context, unsigned u) const { + if (!CheckUint(context, u)) return false; + return CreateParallelValidator(context); + } + + bool Int64(Context &context, int64_t i) const { + if (!CheckInt(context, i)) return false; + return CreateParallelValidator(context); + } + + bool Uint64(Context &context, uint64_t u) const { + if (!CheckUint(context, u)) return false; + return CreateParallelValidator(context); + } + + bool Double(Context &context, double d) const { + if (!(type_ & (1 << kNumberSchemaType))) { + DisallowedType(context, GetNumberString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + if (!minimum_.IsNull() && !CheckDoubleMinimum(context, d)) return false; + + if (!maximum_.IsNull() && !CheckDoubleMaximum(context, d)) return false; + + if (!multipleOf_.IsNull() && !CheckDoubleMultipleOf(context, d)) + return false; + + return CreateParallelValidator(context); + } + + bool String(Context &context, const Ch *str, SizeType length, bool) const { + if (!(type_ & (1 << kStringSchemaType))) { + DisallowedType(context, GetStringString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + if (minLength_ != 0 || maxLength_ != SizeType(~0)) { + SizeType count; + if (internal::CountStringCodePoint(str, length, &count)) { + if (count < minLength_) { + context.error_handler.TooShort(str, length, minLength_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinLengthString()); + } + if (count > maxLength_) { + context.error_handler.TooLong(str, length, maxLength_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaxLengthString()); + } + } + } + + if (pattern_ && !IsPatternMatch(pattern_, str, length)) { + context.error_handler.DoesNotMatch(str, length); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetPatternString()); + } + + return CreateParallelValidator(context); + } + + bool StartObject(Context &context) const { + if (!(type_ & (1 << kObjectSchemaType))) { + DisallowedType(context, GetObjectString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + if (hasDependencies_ || hasRequired_) { + context.propertyExist = static_cast( + context.factory.MallocState(sizeof(bool) * propertyCount_)); + std::memset(context.propertyExist, 0, sizeof(bool) * propertyCount_); + } + + if (patternProperties_) { // pre-allocate schema array + SizeType count = + patternPropertyCount_ + 1; // extra for valuePatternValidatorType + context.patternPropertiesSchemas = static_cast( + context.factory.MallocState(sizeof(const SchemaType *) * count)); + context.patternPropertiesSchemaCount = 0; + std::memset(context.patternPropertiesSchemas, 0, + sizeof(SchemaType *) * count); + } + + return CreateParallelValidator(context); + } + + bool Key(Context &context, const Ch *str, SizeType len, bool) const { + if (patternProperties_) { + context.patternPropertiesSchemaCount = 0; + for (SizeType i = 0; i < patternPropertyCount_; i++) + if (patternProperties_[i].pattern && + IsPatternMatch(patternProperties_[i].pattern, str, len)) { + context.patternPropertiesSchemas + [context.patternPropertiesSchemaCount++] = + patternProperties_[i].schema; + context.valueSchema = typeless_; + } + } + + SizeType index = 0; + if (FindPropertyIndex(ValueType(str, len).Move(), &index)) { + if (context.patternPropertiesSchemaCount > 0) { + context + .patternPropertiesSchemas[context.patternPropertiesSchemaCount++] = + properties_[index].schema; + context.valueSchema = typeless_; + context.valuePatternValidatorType = + Context::kPatternValidatorWithProperty; + } else + context.valueSchema = properties_[index].schema; + + if (context.propertyExist) context.propertyExist[index] = true; + + return true; + } + + if (additionalPropertiesSchema_) { + if (additionalPropertiesSchema_ && + context.patternPropertiesSchemaCount > 0) { + context + .patternPropertiesSchemas[context.patternPropertiesSchemaCount++] = + additionalPropertiesSchema_; + context.valueSchema = typeless_; + context.valuePatternValidatorType = + Context::kPatternValidatorWithAdditionalProperty; + } else + context.valueSchema = additionalPropertiesSchema_; + return true; + } else if (additionalProperties_) { + context.valueSchema = typeless_; + return true; + } + + if (context.patternPropertiesSchemaCount == + 0) { // patternProperties are not additional properties + context.error_handler.DisallowedProperty(str, len); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetAdditionalPropertiesString()); + } + + return true; + } + + bool EndObject(Context &context, SizeType memberCount) const { + if (hasRequired_) { + context.error_handler.StartMissingProperties(); + for (SizeType index = 0; index < propertyCount_; index++) + if (properties_[index].required && !context.propertyExist[index]) + if (properties_[index].schema->defaultValueLength_ == 0) + context.error_handler.AddMissingProperty(properties_[index].name); + if (context.error_handler.EndMissingProperties()) + RAPIDJSON_INVALID_KEYWORD_RETURN(GetRequiredString()); + } + + if (memberCount < minProperties_) { + context.error_handler.TooFewProperties(memberCount, minProperties_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinPropertiesString()); + } + + if (memberCount > maxProperties_) { + context.error_handler.TooManyProperties(memberCount, maxProperties_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaxPropertiesString()); + } + + if (hasDependencies_) { + context.error_handler.StartDependencyErrors(); + for (SizeType sourceIndex = 0; sourceIndex < propertyCount_; + sourceIndex++) { + const Property &source = properties_[sourceIndex]; + if (context.propertyExist[sourceIndex]) { + if (source.dependencies) { + context.error_handler.StartMissingDependentProperties(); + for (SizeType targetIndex = 0; targetIndex < propertyCount_; + targetIndex++) + if (source.dependencies[targetIndex] && + !context.propertyExist[targetIndex]) + context.error_handler.AddMissingDependentProperty( + properties_[targetIndex].name); + context.error_handler.EndMissingDependentProperties(source.name); + } else if (source.dependenciesSchema) { + ISchemaValidator *dependenciesValidator = + context.validators[source.dependenciesValidatorIndex]; + if (!dependenciesValidator->IsValid()) + context.error_handler.AddDependencySchemaError( + source.name, dependenciesValidator); + } + } + } + if (context.error_handler.EndDependencyErrors()) + RAPIDJSON_INVALID_KEYWORD_RETURN(GetDependenciesString()); + } + + return true; + } + + bool StartArray(Context &context) const { + if (!(type_ & (1 << kArraySchemaType))) { + DisallowedType(context, GetArrayString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + context.arrayElementIndex = 0; + context.inArray = true; + + return CreateParallelValidator(context); + } + + bool EndArray(Context &context, SizeType elementCount) const { + context.inArray = false; + + if (elementCount < minItems_) { + context.error_handler.TooFewItems(elementCount, minItems_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinItemsString()); + } + + if (elementCount > maxItems_) { + context.error_handler.TooManyItems(elementCount, maxItems_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaxItemsString()); + } + + return true; + } + +// Generate functions for string literal according to Ch +#define RAPIDJSON_STRING_(name, ...) \ + static const ValueType &Get##name##String() { \ + static const Ch s[] = {__VA_ARGS__, '\0'}; \ + static const ValueType v( \ + s, static_cast(sizeof(s) / sizeof(Ch) - 1)); \ + return v; \ + } + + RAPIDJSON_STRING_(Null, 'n', 'u', 'l', 'l') + RAPIDJSON_STRING_(Boolean, 'b', 'o', 'o', 'l', 'e', 'a', 'n') + RAPIDJSON_STRING_(Object, 'o', 'b', 'j', 'e', 'c', 't') + RAPIDJSON_STRING_(Array, 'a', 'r', 'r', 'a', 'y') + RAPIDJSON_STRING_(String, 's', 't', 'r', 'i', 'n', 'g') + RAPIDJSON_STRING_(Number, 'n', 'u', 'm', 'b', 'e', 'r') + RAPIDJSON_STRING_(Integer, 'i', 'n', 't', 'e', 'g', 'e', 'r') + RAPIDJSON_STRING_(Type, 't', 'y', 'p', 'e') + RAPIDJSON_STRING_(Enum, 'e', 'n', 'u', 'm') + RAPIDJSON_STRING_(AllOf, 'a', 'l', 'l', 'O', 'f') + RAPIDJSON_STRING_(AnyOf, 'a', 'n', 'y', 'O', 'f') + RAPIDJSON_STRING_(OneOf, 'o', 'n', 'e', 'O', 'f') + RAPIDJSON_STRING_(Not, 'n', 'o', 't') + RAPIDJSON_STRING_(Properties, 'p', 'r', 'o', 'p', 'e', 'r', 't', 'i', 'e', + 's') + RAPIDJSON_STRING_(Required, 'r', 'e', 'q', 'u', 'i', 'r', 'e', 'd') + RAPIDJSON_STRING_(Dependencies, 'd', 'e', 'p', 'e', 'n', 'd', 'e', 'n', 'c', + 'i', 'e', 's') + RAPIDJSON_STRING_(PatternProperties, 'p', 'a', 't', 't', 'e', 'r', 'n', 'P', + 'r', 'o', 'p', 'e', 'r', 't', 'i', 'e', 's') + RAPIDJSON_STRING_(AdditionalProperties, 'a', 'd', 'd', 'i', 't', 'i', 'o', + 'n', 'a', 'l', 'P', 'r', 'o', 'p', 'e', 'r', 't', 'i', 'e', + 's') + RAPIDJSON_STRING_(MinProperties, 'm', 'i', 'n', 'P', 'r', 'o', 'p', 'e', 'r', + 't', 'i', 'e', 's') + RAPIDJSON_STRING_(MaxProperties, 'm', 'a', 'x', 'P', 'r', 'o', 'p', 'e', 'r', + 't', 'i', 'e', 's') + RAPIDJSON_STRING_(Items, 'i', 't', 'e', 'm', 's') + RAPIDJSON_STRING_(MinItems, 'm', 'i', 'n', 'I', 't', 'e', 'm', 's') + RAPIDJSON_STRING_(MaxItems, 'm', 'a', 'x', 'I', 't', 'e', 'm', 's') + RAPIDJSON_STRING_(AdditionalItems, 'a', 'd', 'd', 'i', 't', 'i', 'o', 'n', + 'a', 'l', 'I', 't', 'e', 'm', 's') + RAPIDJSON_STRING_(UniqueItems, 'u', 'n', 'i', 'q', 'u', 'e', 'I', 't', 'e', + 'm', 's') + RAPIDJSON_STRING_(MinLength, 'm', 'i', 'n', 'L', 'e', 'n', 'g', 't', 'h') + RAPIDJSON_STRING_(MaxLength, 'm', 'a', 'x', 'L', 'e', 'n', 'g', 't', 'h') + RAPIDJSON_STRING_(Pattern, 'p', 'a', 't', 't', 'e', 'r', 'n') + RAPIDJSON_STRING_(Minimum, 'm', 'i', 'n', 'i', 'm', 'u', 'm') + RAPIDJSON_STRING_(Maximum, 'm', 'a', 'x', 'i', 'm', 'u', 'm') + RAPIDJSON_STRING_(ExclusiveMinimum, 'e', 'x', 'c', 'l', 'u', 's', 'i', 'v', + 'e', 'M', 'i', 'n', 'i', 'm', 'u', 'm') + RAPIDJSON_STRING_(ExclusiveMaximum, 'e', 'x', 'c', 'l', 'u', 's', 'i', 'v', + 'e', 'M', 'a', 'x', 'i', 'm', 'u', 'm') + RAPIDJSON_STRING_(MultipleOf, 'm', 'u', 'l', 't', 'i', 'p', 'l', 'e', 'O', + 'f') + RAPIDJSON_STRING_(DefaultValue, 'd', 'e', 'f', 'a', 'u', 'l', 't') + +#undef RAPIDJSON_STRING_ + + private: + enum SchemaValueType { + kNullSchemaType, + kBooleanSchemaType, + kObjectSchemaType, + kArraySchemaType, + kStringSchemaType, + kNumberSchemaType, + kIntegerSchemaType, + kTotalSchemaType + }; + +#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX + typedef internal::GenericRegex RegexType; +#elif RAPIDJSON_SCHEMA_USE_STDREGEX + typedef std::basic_regex RegexType; +#else + typedef char RegexType; +#endif + + struct SchemaArray { + SchemaArray() : schemas(), count() {} + ~SchemaArray() { AllocatorType::Free(schemas); } + const SchemaType **schemas; + SizeType begin; // begin index of context.validators + SizeType count; + }; + + template + void AddUniqueElement(V1 &a, const V2 &v) { + for (typename V1::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) + if (*itr == v) return; + V1 c(v, *allocator_); + a.PushBack(c, *allocator_); + } + + static const ValueType *GetMember(const ValueType &value, + const ValueType &name) { + typename ValueType::ConstMemberIterator itr = value.FindMember(name); + return itr != value.MemberEnd() ? &(itr->value) : 0; + } + + static void AssignIfExist(bool &out, const ValueType &value, + const ValueType &name) { + if (const ValueType *v = GetMember(value, name)) + if (v->IsBool()) out = v->GetBool(); + } + + static void AssignIfExist(SizeType &out, const ValueType &value, + const ValueType &name) { + if (const ValueType *v = GetMember(value, name)) + if (v->IsUint64() && v->GetUint64() <= SizeType(~0)) + out = static_cast(v->GetUint64()); + } + + void AssignIfExist(SchemaArray &out, SchemaDocumentType &schemaDocument, + const PointerType &p, const ValueType &value, + const ValueType &name, const ValueType &document) { + if (const ValueType *v = GetMember(value, name)) { + if (v->IsArray() && v->Size() > 0) { + PointerType q = p.Append(name, allocator_); + out.count = v->Size(); + out.schemas = static_cast( + allocator_->Malloc(out.count * sizeof(const Schema *))); + memset(out.schemas, 0, sizeof(Schema *) * out.count); + for (SizeType i = 0; i < out.count; i++) + schemaDocument.CreateSchema(&out.schemas[i], q.Append(i, allocator_), + (*v)[i], document); + out.begin = validatorCount_; + validatorCount_ += out.count; + } + } + } + +#if RAPIDJSON_SCHEMA_USE_INTERNALREGEX + template + RegexType *CreatePattern(const ValueType &value) { + if (value.IsString()) { + RegexType *r = new (allocator_->Malloc(sizeof(RegexType))) + RegexType(value.GetString(), allocator_); + if (!r->IsValid()) { + r->~RegexType(); + AllocatorType::Free(r); + r = 0; + } + return r; + } + return 0; + } + + static bool IsPatternMatch(const RegexType *pattern, const Ch *str, + SizeType) { + GenericRegexSearch rs(*pattern); + return rs.Search(str); + } +#elif RAPIDJSON_SCHEMA_USE_STDREGEX + template + RegexType *CreatePattern(const ValueType &value) { + if (value.IsString()) { + RegexType *r = + static_cast(allocator_->Malloc(sizeof(RegexType))); + try { + return new (r) + RegexType(value.GetString(), std::size_t(value.GetStringLength()), + std::regex_constants::ECMAScript); + } catch (const std::regex_error &) { + AllocatorType::Free(r); + } + } + return 0; + } + + static bool IsPatternMatch(const RegexType *pattern, const Ch *str, + SizeType length) { + std::match_results r; + return std::regex_search(str, str + length, r, *pattern); + } +#else + template + RegexType *CreatePattern(const ValueType &) { + return 0; + } + + static bool IsPatternMatch(const RegexType *, const Ch *, SizeType) { + return true; + } +#endif // RAPIDJSON_SCHEMA_USE_STDREGEX + + void AddType(const ValueType &type) { + if (type == GetNullString()) + type_ |= 1 << kNullSchemaType; + else if (type == GetBooleanString()) + type_ |= 1 << kBooleanSchemaType; + else if (type == GetObjectString()) + type_ |= 1 << kObjectSchemaType; + else if (type == GetArrayString()) + type_ |= 1 << kArraySchemaType; + else if (type == GetStringString()) + type_ |= 1 << kStringSchemaType; + else if (type == GetIntegerString()) + type_ |= 1 << kIntegerSchemaType; + else if (type == GetNumberString()) + type_ |= (1 << kNumberSchemaType) | (1 << kIntegerSchemaType); + } + + bool CreateParallelValidator(Context &context) const { + if (enum_ || context.arrayUniqueness) + context.hasher = context.factory.CreateHasher(); + + if (validatorCount_) { + RAPIDJSON_ASSERT(context.validators == 0); + context.validators = + static_cast(context.factory.MallocState( + sizeof(ISchemaValidator *) * validatorCount_)); + context.validatorCount = validatorCount_; + + if (allOf_.schemas) CreateSchemaValidators(context, allOf_); + + if (anyOf_.schemas) CreateSchemaValidators(context, anyOf_); + + if (oneOf_.schemas) CreateSchemaValidators(context, oneOf_); + + if (not_) + context.validators[notValidatorIndex_] = + context.factory.CreateSchemaValidator(*not_); + + if (hasSchemaDependencies_) { + for (SizeType i = 0; i < propertyCount_; i++) + if (properties_[i].dependenciesSchema) + context.validators[properties_[i].dependenciesValidatorIndex] = + context.factory.CreateSchemaValidator( + *properties_[i].dependenciesSchema); + } + } + + return true; + } + + void CreateSchemaValidators(Context &context, + const SchemaArray &schemas) const { + for (SizeType i = 0; i < schemas.count; i++) + context.validators[schemas.begin + i] = + context.factory.CreateSchemaValidator(*schemas.schemas[i]); + } + + // O(n) + bool FindPropertyIndex(const ValueType &name, SizeType *outIndex) const { + SizeType len = name.GetStringLength(); + const Ch *str = name.GetString(); + for (SizeType index = 0; index < propertyCount_; index++) + if (properties_[index].name.GetStringLength() == len && + (std::memcmp(properties_[index].name.GetString(), str, + sizeof(Ch) * len) == 0)) { + *outIndex = index; + return true; + } + return false; + } + + bool CheckInt(Context &context, int64_t i) const { + if (!(type_ & ((1 << kIntegerSchemaType) | (1 << kNumberSchemaType)))) { + DisallowedType(context, GetIntegerString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + if (!minimum_.IsNull()) { + if (minimum_.IsInt64()) { + if (exclusiveMinimum_ ? i <= minimum_.GetInt64() + : i < minimum_.GetInt64()) { + context.error_handler.BelowMinimum(i, minimum_, exclusiveMinimum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinimumString()); + } + } else if (minimum_.IsUint64()) { + context.error_handler.BelowMinimum(i, minimum_, exclusiveMinimum_); + RAPIDJSON_INVALID_KEYWORD_RETURN( + GetMinimumString()); // i <= max(int64_t) < minimum.GetUint64() + } else if (!CheckDoubleMinimum(context, static_cast(i))) + return false; + } + + if (!maximum_.IsNull()) { + if (maximum_.IsInt64()) { + if (exclusiveMaximum_ ? i >= maximum_.GetInt64() + : i > maximum_.GetInt64()) { + context.error_handler.AboveMaximum(i, maximum_, exclusiveMaximum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaximumString()); + } + } else if (maximum_.IsUint64()) { + } + /* do nothing */ // i <= max(int64_t) < maximum_.GetUint64() + else if (!CheckDoubleMaximum(context, static_cast(i))) + return false; + } + + if (!multipleOf_.IsNull()) { + if (multipleOf_.IsUint64()) { + if (static_cast(i >= 0 ? i : -i) % multipleOf_.GetUint64() != + 0) { + context.error_handler.NotMultipleOf(i, multipleOf_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMultipleOfString()); + } + } else if (!CheckDoubleMultipleOf(context, static_cast(i))) + return false; + } + + return true; + } + + bool CheckUint(Context &context, uint64_t i) const { + if (!(type_ & ((1 << kIntegerSchemaType) | (1 << kNumberSchemaType)))) { + DisallowedType(context, GetIntegerString()); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetTypeString()); + } + + if (!minimum_.IsNull()) { + if (minimum_.IsUint64()) { + if (exclusiveMinimum_ ? i <= minimum_.GetUint64() + : i < minimum_.GetUint64()) { + context.error_handler.BelowMinimum(i, minimum_, exclusiveMinimum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinimumString()); + } + } else if (minimum_.IsInt64()) + /* do nothing */; // i >= 0 > minimum.Getint64() + else if (!CheckDoubleMinimum(context, static_cast(i))) + return false; + } + + if (!maximum_.IsNull()) { + if (maximum_.IsUint64()) { + if (exclusiveMaximum_ ? i >= maximum_.GetUint64() + : i > maximum_.GetUint64()) { + context.error_handler.AboveMaximum(i, maximum_, exclusiveMaximum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaximumString()); + } + } else if (maximum_.IsInt64()) { + context.error_handler.AboveMaximum(i, maximum_, exclusiveMaximum_); + RAPIDJSON_INVALID_KEYWORD_RETURN( + GetMaximumString()); // i >= 0 > maximum_ + } else if (!CheckDoubleMaximum(context, static_cast(i))) + return false; + } + + if (!multipleOf_.IsNull()) { + if (multipleOf_.IsUint64()) { + if (i % multipleOf_.GetUint64() != 0) { + context.error_handler.NotMultipleOf(i, multipleOf_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMultipleOfString()); + } + } else if (!CheckDoubleMultipleOf(context, static_cast(i))) + return false; + } + + return true; + } + + bool CheckDoubleMinimum(Context &context, double d) const { + if (exclusiveMinimum_ ? d <= minimum_.GetDouble() + : d < minimum_.GetDouble()) { + context.error_handler.BelowMinimum(d, minimum_, exclusiveMinimum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMinimumString()); + } + return true; + } + + bool CheckDoubleMaximum(Context &context, double d) const { + if (exclusiveMaximum_ ? d >= maximum_.GetDouble() + : d > maximum_.GetDouble()) { + context.error_handler.AboveMaximum(d, maximum_, exclusiveMaximum_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMaximumString()); + } + return true; + } + + bool CheckDoubleMultipleOf(Context &context, double d) const { + double a = std::abs(d), b = std::abs(multipleOf_.GetDouble()); + double q = std::floor(a / b); + double r = a - q * b; + if (r > 0.0) { + context.error_handler.NotMultipleOf(d, multipleOf_); + RAPIDJSON_INVALID_KEYWORD_RETURN(GetMultipleOfString()); + } + return true; + } + + void DisallowedType(Context &context, const ValueType &actualType) const { + ErrorHandler &eh = context.error_handler; + eh.StartDisallowedType(); + + if (type_ & (1 << kNullSchemaType)) eh.AddExpectedType(GetNullString()); + if (type_ & (1 << kBooleanSchemaType)) + eh.AddExpectedType(GetBooleanString()); + if (type_ & (1 << kObjectSchemaType)) eh.AddExpectedType(GetObjectString()); + if (type_ & (1 << kArraySchemaType)) eh.AddExpectedType(GetArrayString()); + if (type_ & (1 << kStringSchemaType)) eh.AddExpectedType(GetStringString()); + + if (type_ & (1 << kNumberSchemaType)) + eh.AddExpectedType(GetNumberString()); + else if (type_ & (1 << kIntegerSchemaType)) + eh.AddExpectedType(GetIntegerString()); + + eh.EndDisallowedType(actualType); + } + + struct Property { + Property() + : schema(), + dependenciesSchema(), + dependenciesValidatorIndex(), + dependencies(), + required(false) {} + ~Property() { AllocatorType::Free(dependencies); } + SValue name; + const SchemaType *schema; + const SchemaType *dependenciesSchema; + SizeType dependenciesValidatorIndex; + bool *dependencies; + bool required; + }; + + struct PatternProperty { + PatternProperty() : schema(), pattern() {} + ~PatternProperty() { + if (pattern) { + pattern->~RegexType(); + AllocatorType::Free(pattern); + } + } + const SchemaType *schema; + RegexType *pattern; + }; + + AllocatorType *allocator_; + SValue uri_; + PointerType pointer_; + const SchemaType *typeless_; + uint64_t *enum_; + SizeType enumCount_; + SchemaArray allOf_; + SchemaArray anyOf_; + SchemaArray oneOf_; + const SchemaType *not_; + unsigned type_; // bitmask of kSchemaType + SizeType validatorCount_; + SizeType notValidatorIndex_; + + Property *properties_; + const SchemaType *additionalPropertiesSchema_; + PatternProperty *patternProperties_; + SizeType patternPropertyCount_; + SizeType propertyCount_; + SizeType minProperties_; + SizeType maxProperties_; + bool additionalProperties_; + bool hasDependencies_; + bool hasRequired_; + bool hasSchemaDependencies_; + + const SchemaType *additionalItemsSchema_; + const SchemaType *itemsList_; + const SchemaType **itemsTuple_; + SizeType itemsTupleCount_; + SizeType minItems_; + SizeType maxItems_; + bool additionalItems_; + bool uniqueItems_; + + RegexType *pattern_; + SizeType minLength_; + SizeType maxLength_; + + SValue minimum_; + SValue maximum_; + SValue multipleOf_; + bool exclusiveMinimum_; + bool exclusiveMaximum_; + + SizeType defaultValueLength_; +}; + +template +struct TokenHelper { + RAPIDJSON_FORCEINLINE static void AppendIndexToken(Stack &documentStack, + SizeType index) { + *documentStack.template Push() = '/'; + char buffer[21]; + size_t length = + static_cast((sizeof(SizeType) == 4 ? u32toa(index, buffer) + : u64toa(index, buffer)) - + buffer); + for (size_t i = 0; i < length; i++) + *documentStack.template Push() = static_cast(buffer[i]); + } +}; + +// Partial specialized version for char to prevent buffer copying. +template +struct TokenHelper { + RAPIDJSON_FORCEINLINE static void AppendIndexToken(Stack &documentStack, + SizeType index) { + if (sizeof(SizeType) == 4) { + char *buffer = documentStack.template Push(1 + 10); // '/' + uint + *buffer++ = '/'; + const char *end = internal::u32toa(index, buffer); + documentStack.template Pop( + static_cast(10 - (end - buffer))); + } else { + char *buffer = documentStack.template Push(1 + 20); // '/' + uint64 + *buffer++ = '/'; + const char *end = internal::u64toa(index, buffer); + documentStack.template Pop( + static_cast(20 - (end - buffer))); + } + } +}; + +} // namespace internal + +/////////////////////////////////////////////////////////////////////////////// +// IGenericRemoteSchemaDocumentProvider + +template +class IGenericRemoteSchemaDocumentProvider { + public: + typedef typename SchemaDocumentType::Ch Ch; + + virtual ~IGenericRemoteSchemaDocumentProvider() {} + virtual const SchemaDocumentType *GetRemoteDocument(const Ch *uri, + SizeType length) = 0; +}; + +/////////////////////////////////////////////////////////////////////////////// +// GenericSchemaDocument + +//! JSON schema document. +/*! + A JSON schema document is a compiled version of a JSON schema. + It is basically a tree of internal::Schema. + + \note This is an immutable class (i.e. its instance cannot be modified after + construction). \tparam ValueT Type of JSON value (e.g. \c Value ), which also + determine the encoding. \tparam Allocator Allocator type for allocating + memory of this document. +*/ +template +class GenericSchemaDocument { + public: + typedef ValueT ValueType; + typedef IGenericRemoteSchemaDocumentProvider + IRemoteSchemaDocumentProviderType; + typedef Allocator AllocatorType; + typedef typename ValueType::EncodingType EncodingType; + typedef typename EncodingType::Ch Ch; + typedef internal::Schema SchemaType; + typedef GenericPointer PointerType; + typedef GenericValue URIType; + friend class internal::Schema; + template + friend class GenericSchemaValidator; + + //! Constructor. + /*! + Compile a JSON document into schema document. + + \param document A JSON document as source. + \param uri The base URI of this schema document for purposes of violation + reporting. \param uriLength Length of \c name, in code points. \param + remoteProvider An optional remote schema document provider for resolving + remote reference. Can be null. \param allocator An optional allocator + instance for allocating memory. Can be null. + */ + explicit GenericSchemaDocument( + const ValueType &document, const Ch *uri = 0, SizeType uriLength = 0, + IRemoteSchemaDocumentProviderType *remoteProvider = 0, + Allocator *allocator = 0) + : remoteProvider_(remoteProvider), + allocator_(allocator), + ownAllocator_(), + root_(), + typeless_(), + schemaMap_(allocator, kInitialSchemaMapSize), + schemaRef_(allocator, kInitialSchemaRefSize) { + if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)(); + + Ch noUri[1] = {0}; + uri_.SetString(uri ? uri : noUri, uriLength, *allocator_); + + typeless_ = + static_cast(allocator_->Malloc(sizeof(SchemaType))); + new (typeless_) + SchemaType(this, PointerType(), ValueType(kObjectType).Move(), + ValueType(kObjectType).Move(), allocator_); + + // Generate root schema, it will call CreateSchema() to create sub-schemas, + // And call AddRefSchema() if there are $ref. + CreateSchemaRecursive(&root_, PointerType(), document, document); + + // Resolve $ref + while (!schemaRef_.Empty()) { + SchemaRefEntry *refEntry = schemaRef_.template Pop(1); + if (const SchemaType *s = GetSchema(refEntry->target)) { + if (refEntry->schema) *refEntry->schema = s; + + // Create entry in map if not exist + if (!GetSchema(refEntry->source)) { + new (schemaMap_.template Push()) SchemaEntry( + refEntry->source, const_cast(s), false, allocator_); + } + } else if (refEntry->schema) + *refEntry->schema = typeless_; + + refEntry->~SchemaRefEntry(); + } + + RAPIDJSON_ASSERT(root_ != 0); + + schemaRef_.ShrinkToFit(); // Deallocate all memory for ref + } + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + //! Move constructor in C++11 + GenericSchemaDocument(GenericSchemaDocument &&rhs) RAPIDJSON_NOEXCEPT + : remoteProvider_(rhs.remoteProvider_), + allocator_(rhs.allocator_), + ownAllocator_(rhs.ownAllocator_), + root_(rhs.root_), + typeless_(rhs.typeless_), + schemaMap_(std::move(rhs.schemaMap_)), + schemaRef_(std::move(rhs.schemaRef_)), + uri_(std::move(rhs.uri_)) { + rhs.remoteProvider_ = 0; + rhs.allocator_ = 0; + rhs.ownAllocator_ = 0; + rhs.typeless_ = 0; + } +#endif + + //! Destructor + ~GenericSchemaDocument() { + while (!schemaMap_.Empty()) + schemaMap_.template Pop(1)->~SchemaEntry(); + + if (typeless_) { + typeless_->~SchemaType(); + Allocator::Free(typeless_); + } + + RAPIDJSON_DELETE(ownAllocator_); + } + + const URIType &GetURI() const { return uri_; } + + //! Get the root schema. + const SchemaType &GetRoot() const { return *root_; } + + private: + //! Prohibit copying + GenericSchemaDocument(const GenericSchemaDocument &); + //! Prohibit assignment + GenericSchemaDocument &operator=(const GenericSchemaDocument &); + + struct SchemaRefEntry { + SchemaRefEntry(const PointerType &s, const PointerType &t, + const SchemaType **outSchema, Allocator *allocator) + : source(s, allocator), target(t, allocator), schema(outSchema) {} + PointerType source; + PointerType target; + const SchemaType **schema; + }; + + struct SchemaEntry { + SchemaEntry(const PointerType &p, SchemaType *s, bool o, + Allocator *allocator) + : pointer(p, allocator), schema(s), owned(o) {} + ~SchemaEntry() { + if (owned) { + schema->~SchemaType(); + Allocator::Free(schema); + } + } + PointerType pointer; + SchemaType *schema; + bool owned; + }; + + void CreateSchemaRecursive(const SchemaType **schema, + const PointerType &pointer, const ValueType &v, + const ValueType &document) { + if (schema) *schema = typeless_; + + if (v.GetType() == kObjectType) { + const SchemaType *s = GetSchema(pointer); + if (!s) CreateSchema(schema, pointer, v, document); + + for (typename ValueType::ConstMemberIterator itr = v.MemberBegin(); + itr != v.MemberEnd(); ++itr) + CreateSchemaRecursive(0, pointer.Append(itr->name, allocator_), + itr->value, document); + } else if (v.GetType() == kArrayType) + for (SizeType i = 0; i < v.Size(); i++) + CreateSchemaRecursive(0, pointer.Append(i, allocator_), v[i], document); + } + + void CreateSchema(const SchemaType **schema, const PointerType &pointer, + const ValueType &v, const ValueType &document) { + RAPIDJSON_ASSERT(pointer.IsValid()); + if (v.IsObject()) { + if (!HandleRefSchema(pointer, schema, v, document)) { + SchemaType *s = new (allocator_->Malloc(sizeof(SchemaType))) + SchemaType(this, pointer, v, document, allocator_); + new (schemaMap_.template Push()) + SchemaEntry(pointer, s, true, allocator_); + if (schema) *schema = s; + } + } + } + + bool HandleRefSchema(const PointerType &source, const SchemaType **schema, + const ValueType &v, const ValueType &document) { + static const Ch kRefString[] = {'$', 'r', 'e', 'f', '\0'}; + static const ValueType kRefValue(kRefString, 4); + + typename ValueType::ConstMemberIterator itr = v.FindMember(kRefValue); + if (itr == v.MemberEnd()) return false; + + if (itr->value.IsString()) { + SizeType len = itr->value.GetStringLength(); + if (len > 0) { + const Ch *s = itr->value.GetString(); + SizeType i = 0; + while (i < len && s[i] != '#') // Find the first # + i++; + + if (i > 0) { // Remote reference, resolve immediately + if (remoteProvider_) { + if (const GenericSchemaDocument *remoteDocument = + remoteProvider_->GetRemoteDocument(s, i)) { + PointerType pointer(&s[i], len - i, allocator_); + if (pointer.IsValid()) { + if (const SchemaType *sc = remoteDocument->GetSchema(pointer)) { + if (schema) *schema = sc; + new (schemaMap_.template Push()) SchemaEntry( + source, const_cast(sc), false, allocator_); + return true; + } + } + } + } + } else if (s[i] == '#') { // Local reference, defer resolution + PointerType pointer(&s[i], len - i, allocator_); + if (pointer.IsValid()) { + if (const ValueType *nv = pointer.Get(document)) + if (HandleRefSchema(source, schema, *nv, document)) return true; + + new (schemaRef_.template Push()) + SchemaRefEntry(source, pointer, schema, allocator_); + return true; + } + } + } + } + return false; + } + + const SchemaType *GetSchema(const PointerType &pointer) const { + for (const SchemaEntry *target = schemaMap_.template Bottom(); + target != schemaMap_.template End(); ++target) + if (pointer == target->pointer) return target->schema; + return 0; + } + + PointerType GetPointer(const SchemaType *schema) const { + for (const SchemaEntry *target = schemaMap_.template Bottom(); + target != schemaMap_.template End(); ++target) + if (schema == target->schema) return target->pointer; + return PointerType(); + } + + const SchemaType *GetTypeless() const { return typeless_; } + + static const size_t kInitialSchemaMapSize = 64; + static const size_t kInitialSchemaRefSize = 64; + + IRemoteSchemaDocumentProviderType *remoteProvider_; + Allocator *allocator_; + Allocator *ownAllocator_; + const SchemaType *root_; //!< Root schema. + SchemaType *typeless_; + internal::Stack schemaMap_; // Stores created Pointer -> Schemas + internal::Stack + schemaRef_; // Stores Pointer from $ref and schema which holds the $ref + URIType uri_; +}; + +//! GenericSchemaDocument using Value type. +typedef GenericSchemaDocument SchemaDocument; +//! IGenericRemoteSchemaDocumentProvider using SchemaDocument. +typedef IGenericRemoteSchemaDocumentProvider + IRemoteSchemaDocumentProvider; + +/////////////////////////////////////////////////////////////////////////////// +// GenericSchemaValidator + +//! JSON Schema Validator. +/*! + A SAX style JSON schema validator. + It uses a \c GenericSchemaDocument to validate SAX events. + It delegates the incoming SAX events to an output handler. + The default output handler does nothing. + It can be reused multiple times by calling \c Reset(). + + \tparam SchemaDocumentType Type of schema document. + \tparam OutputHandler Type of output handler. Default handler does nothing. + \tparam StateAllocator Allocator for storing the internal validation states. +*/ +template , + typename StateAllocator = CrtAllocator> +class GenericSchemaValidator : public internal::ISchemaStateFactory< + typename SchemaDocumentType::SchemaType>, + public internal::ISchemaValidator, + public internal::IValidationErrorHandler< + typename SchemaDocumentType::SchemaType> { + public: + typedef typename SchemaDocumentType::SchemaType SchemaType; + typedef typename SchemaDocumentType::PointerType PointerType; + typedef typename SchemaType::EncodingType EncodingType; + typedef typename SchemaType::SValue SValue; + typedef typename EncodingType::Ch Ch; + typedef GenericStringRef StringRefType; + typedef GenericValue ValueType; + + //! Constructor without output handler. + /*! + \param schemaDocument The schema document to conform to. + \param allocator Optional allocator for storing internal validation + states. \param schemaStackCapacity Optional initial capacity of schema path + stack. \param documentStackCapacity Optional initial capacity of document + path stack. + */ + GenericSchemaValidator( + const SchemaDocumentType &schemaDocument, StateAllocator *allocator = 0, + size_t schemaStackCapacity = kDefaultSchemaStackCapacity, + size_t documentStackCapacity = kDefaultDocumentStackCapacity) + : schemaDocument_(&schemaDocument), + root_(schemaDocument.GetRoot()), + stateAllocator_(allocator), + ownStateAllocator_(0), + schemaStack_(allocator, schemaStackCapacity), + documentStack_(allocator, documentStackCapacity), + outputHandler_(0), + error_(kObjectType), + currentError_(), + missingDependents_(), + valid_(true) +#if RAPIDJSON_SCHEMA_VERBOSE + , + depth_(0) +#endif + { + } + + //! Constructor with output handler. + /*! + \param schemaDocument The schema document to conform to. + \param allocator Optional allocator for storing internal validation + states. \param schemaStackCapacity Optional initial capacity of schema path + stack. \param documentStackCapacity Optional initial capacity of document + path stack. + */ + GenericSchemaValidator( + const SchemaDocumentType &schemaDocument, OutputHandler &outputHandler, + StateAllocator *allocator = 0, + size_t schemaStackCapacity = kDefaultSchemaStackCapacity, + size_t documentStackCapacity = kDefaultDocumentStackCapacity) + : schemaDocument_(&schemaDocument), + root_(schemaDocument.GetRoot()), + stateAllocator_(allocator), + ownStateAllocator_(0), + schemaStack_(allocator, schemaStackCapacity), + documentStack_(allocator, documentStackCapacity), + outputHandler_(&outputHandler), + error_(kObjectType), + currentError_(), + missingDependents_(), + valid_(true) +#if RAPIDJSON_SCHEMA_VERBOSE + , + depth_(0) +#endif + { + } + + //! Destructor. + ~GenericSchemaValidator() { + Reset(); + RAPIDJSON_DELETE(ownStateAllocator_); + } + + //! Reset the internal states. + void Reset() { + while (!schemaStack_.Empty()) PopSchema(); + documentStack_.Clear(); + error_.SetObject(); + currentError_.SetNull(); + missingDependents_.SetNull(); + valid_ = true; + } + + //! Checks whether the current state is valid. + // Implementation of ISchemaValidator + virtual bool IsValid() const { return valid_; } + + //! Gets the error object. + ValueType &GetError() { return error_; } + const ValueType &GetError() const { return error_; } + + //! Gets the JSON pointer pointed to the invalid schema. + PointerType GetInvalidSchemaPointer() const { + return schemaStack_.Empty() ? PointerType() : CurrentSchema().GetPointer(); + } + + //! Gets the keyword of invalid schema. + const Ch *GetInvalidSchemaKeyword() const { + return schemaStack_.Empty() ? 0 : CurrentContext().invalidKeyword; + } + + //! Gets the JSON pointer pointed to the invalid value. + PointerType GetInvalidDocumentPointer() const { + if (documentStack_.Empty()) { + return PointerType(); + } else { + return PointerType(documentStack_.template Bottom(), + documentStack_.GetSize() / sizeof(Ch)); + } + } + + void NotMultipleOf(int64_t actual, const SValue &expected) { + AddNumberError(SchemaType::GetMultipleOfString(), ValueType(actual).Move(), + expected); + } + void NotMultipleOf(uint64_t actual, const SValue &expected) { + AddNumberError(SchemaType::GetMultipleOfString(), ValueType(actual).Move(), + expected); + } + void NotMultipleOf(double actual, const SValue &expected) { + AddNumberError(SchemaType::GetMultipleOfString(), ValueType(actual).Move(), + expected); + } + void AboveMaximum(int64_t actual, const SValue &expected, bool exclusive) { + AddNumberError(SchemaType::GetMaximumString(), ValueType(actual).Move(), + expected, + exclusive ? &SchemaType::GetExclusiveMaximumString : 0); + } + void AboveMaximum(uint64_t actual, const SValue &expected, bool exclusive) { + AddNumberError(SchemaType::GetMaximumString(), ValueType(actual).Move(), + expected, + exclusive ? &SchemaType::GetExclusiveMaximumString : 0); + } + void AboveMaximum(double actual, const SValue &expected, bool exclusive) { + AddNumberError(SchemaType::GetMaximumString(), ValueType(actual).Move(), + expected, + exclusive ? &SchemaType::GetExclusiveMaximumString : 0); + } + void BelowMinimum(int64_t actual, const SValue &expected, bool exclusive) { + AddNumberError(SchemaType::GetMinimumString(), ValueType(actual).Move(), + expected, + exclusive ? &SchemaType::GetExclusiveMinimumString : 0); + } + void BelowMinimum(uint64_t actual, const SValue &expected, bool exclusive) { + AddNumberError(SchemaType::GetMinimumString(), ValueType(actual).Move(), + expected, + exclusive ? &SchemaType::GetExclusiveMinimumString : 0); + } + void BelowMinimum(double actual, const SValue &expected, bool exclusive) { + AddNumberError(SchemaType::GetMinimumString(), ValueType(actual).Move(), + expected, + exclusive ? &SchemaType::GetExclusiveMinimumString : 0); + } + + void TooLong(const Ch *str, SizeType length, SizeType expected) { + AddNumberError(SchemaType::GetMaxLengthString(), + ValueType(str, length, GetStateAllocator()).Move(), + SValue(expected).Move()); + } + void TooShort(const Ch *str, SizeType length, SizeType expected) { + AddNumberError(SchemaType::GetMinLengthString(), + ValueType(str, length, GetStateAllocator()).Move(), + SValue(expected).Move()); + } + void DoesNotMatch(const Ch *str, SizeType length) { + currentError_.SetObject(); + currentError_.AddMember(GetActualString(), + ValueType(str, length, GetStateAllocator()).Move(), + GetStateAllocator()); + AddCurrentError(SchemaType::GetPatternString()); + } + + void DisallowedItem(SizeType index) { + currentError_.SetObject(); + currentError_.AddMember(GetDisallowedString(), ValueType(index).Move(), + GetStateAllocator()); + AddCurrentError(SchemaType::GetAdditionalItemsString(), true); + } + void TooFewItems(SizeType actualCount, SizeType expectedCount) { + AddNumberError(SchemaType::GetMinItemsString(), + ValueType(actualCount).Move(), SValue(expectedCount).Move()); + } + void TooManyItems(SizeType actualCount, SizeType expectedCount) { + AddNumberError(SchemaType::GetMaxItemsString(), + ValueType(actualCount).Move(), SValue(expectedCount).Move()); + } + void DuplicateItems(SizeType index1, SizeType index2) { + ValueType duplicates(kArrayType); + duplicates.PushBack(index1, GetStateAllocator()); + duplicates.PushBack(index2, GetStateAllocator()); + currentError_.SetObject(); + currentError_.AddMember(GetDuplicatesString(), duplicates, + GetStateAllocator()); + AddCurrentError(SchemaType::GetUniqueItemsString(), true); + } + + void TooManyProperties(SizeType actualCount, SizeType expectedCount) { + AddNumberError(SchemaType::GetMaxPropertiesString(), + ValueType(actualCount).Move(), SValue(expectedCount).Move()); + } + void TooFewProperties(SizeType actualCount, SizeType expectedCount) { + AddNumberError(SchemaType::GetMinPropertiesString(), + ValueType(actualCount).Move(), SValue(expectedCount).Move()); + } + void StartMissingProperties() { currentError_.SetArray(); } + void AddMissingProperty(const SValue &name) { + currentError_.PushBack(ValueType(name, GetStateAllocator()).Move(), + GetStateAllocator()); + } + bool EndMissingProperties() { + if (currentError_.Empty()) return false; + ValueType error(kObjectType); + error.AddMember(GetMissingString(), currentError_, GetStateAllocator()); + currentError_ = error; + AddCurrentError(SchemaType::GetRequiredString()); + return true; + } + void PropertyViolations(ISchemaValidator **subvalidators, SizeType count) { + for (SizeType i = 0; i < count; ++i) + MergeError( + static_cast(subvalidators[i])->GetError()); + } + void DisallowedProperty(const Ch *name, SizeType length) { + currentError_.SetObject(); + currentError_.AddMember(GetDisallowedString(), + ValueType(name, length, GetStateAllocator()).Move(), + GetStateAllocator()); + AddCurrentError(SchemaType::GetAdditionalPropertiesString(), true); + } + + void StartDependencyErrors() { currentError_.SetObject(); } + void StartMissingDependentProperties() { missingDependents_.SetArray(); } + void AddMissingDependentProperty(const SValue &targetName) { + missingDependents_.PushBack( + ValueType(targetName, GetStateAllocator()).Move(), GetStateAllocator()); + } + void EndMissingDependentProperties(const SValue &sourceName) { + if (!missingDependents_.Empty()) + currentError_.AddMember(ValueType(sourceName, GetStateAllocator()).Move(), + missingDependents_, GetStateAllocator()); + } + void AddDependencySchemaError(const SValue &sourceName, + ISchemaValidator *subvalidator) { + currentError_.AddMember( + ValueType(sourceName, GetStateAllocator()).Move(), + static_cast(subvalidator)->GetError(), + GetStateAllocator()); + } + bool EndDependencyErrors() { + if (currentError_.ObjectEmpty()) return false; + ValueType error(kObjectType); + error.AddMember(GetErrorsString(), currentError_, GetStateAllocator()); + currentError_ = error; + AddCurrentError(SchemaType::GetDependenciesString()); + return true; + } + + void DisallowedValue() { + currentError_.SetObject(); + AddCurrentError(SchemaType::GetEnumString()); + } + void StartDisallowedType() { currentError_.SetArray(); } + void AddExpectedType(const typename SchemaType::ValueType &expectedType) { + currentError_.PushBack(ValueType(expectedType, GetStateAllocator()).Move(), + GetStateAllocator()); + } + void EndDisallowedType(const typename SchemaType::ValueType &actualType) { + ValueType error(kObjectType); + error.AddMember(GetExpectedString(), currentError_, GetStateAllocator()); + error.AddMember(GetActualString(), + ValueType(actualType, GetStateAllocator()).Move(), + GetStateAllocator()); + currentError_ = error; + AddCurrentError(SchemaType::GetTypeString()); + } + void NotAllOf(ISchemaValidator **subvalidators, SizeType count) { + for (SizeType i = 0; i < count; ++i) { + MergeError( + static_cast(subvalidators[i])->GetError()); + } + } + void NoneOf(ISchemaValidator **subvalidators, SizeType count) { + AddErrorArray(SchemaType::GetAnyOfString(), subvalidators, count); + } + void NotOneOf(ISchemaValidator **subvalidators, SizeType count) { + AddErrorArray(SchemaType::GetOneOfString(), subvalidators, count); + } + void Disallowed() { + currentError_.SetObject(); + AddCurrentError(SchemaType::GetNotString()); + } + +#define RAPIDJSON_STRING_(name, ...) \ + static const StringRefType &Get##name##String() { \ + static const Ch s[] = {__VA_ARGS__, '\0'}; \ + static const StringRefType v( \ + s, static_cast(sizeof(s) / sizeof(Ch) - 1)); \ + return v; \ + } + + RAPIDJSON_STRING_(InstanceRef, 'i', 'n', 's', 't', 'a', 'n', 'c', 'e', 'R', + 'e', 'f') + RAPIDJSON_STRING_(SchemaRef, 's', 'c', 'h', 'e', 'm', 'a', 'R', 'e', 'f') + RAPIDJSON_STRING_(Expected, 'e', 'x', 'p', 'e', 'c', 't', 'e', 'd') + RAPIDJSON_STRING_(Actual, 'a', 'c', 't', 'u', 'a', 'l') + RAPIDJSON_STRING_(Disallowed, 'd', 'i', 's', 'a', 'l', 'l', 'o', 'w', 'e', + 'd') + RAPIDJSON_STRING_(Missing, 'm', 'i', 's', 's', 'i', 'n', 'g') + RAPIDJSON_STRING_(Errors, 'e', 'r', 'r', 'o', 'r', 's') + RAPIDJSON_STRING_(Duplicates, 'd', 'u', 'p', 'l', 'i', 'c', 'a', 't', 'e', + 's') + +#undef RAPIDJSON_STRING_ + +#if RAPIDJSON_SCHEMA_VERBOSE +#define RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_() \ + RAPIDJSON_MULTILINEMACRO_BEGIN \ + *documentStack_.template Push() = '\0'; \ + documentStack_.template Pop(1); \ + internal::PrintInvalidDocument(documentStack_.template Bottom()); \ + RAPIDJSON_MULTILINEMACRO_END +#else +#define RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_() +#endif + +#define RAPIDJSON_SCHEMA_HANDLE_BEGIN_(method, arg1) \ + if (!valid_) return false; \ + if (!BeginValue() || !CurrentSchema().method arg1) { \ + RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_(); \ + return valid_ = false; \ + } + +#define RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(method, arg2) \ + for (Context *context = schemaStack_.template Bottom(); \ + context != schemaStack_.template End(); context++) { \ + if (context->hasher) \ + static_cast(context->hasher)->method arg2; \ + if (context->validators) \ + for (SizeType i_ = 0; i_ < context->validatorCount; i_++) \ + static_cast(context->validators[i_]) \ + ->method arg2; \ + if (context->patternPropertiesValidators) \ + for (SizeType i_ = 0; i_ < context->patternPropertiesValidatorCount; \ + i_++) \ + static_cast( \ + context->patternPropertiesValidators[i_]) \ + ->method arg2; \ + } + +#define RAPIDJSON_SCHEMA_HANDLE_END_(method, arg2) \ + return valid_ = EndValue() && (!outputHandler_ || outputHandler_->method arg2) + +#define RAPIDJSON_SCHEMA_HANDLE_VALUE_(method, arg1, arg2) \ + RAPIDJSON_SCHEMA_HANDLE_BEGIN_(method, arg1); \ + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(method, arg2); \ + RAPIDJSON_SCHEMA_HANDLE_END_(method, arg2) + + bool Null() { RAPIDJSON_SCHEMA_HANDLE_VALUE_(Null, (CurrentContext()), ()); } + bool Bool(bool b) { + RAPIDJSON_SCHEMA_HANDLE_VALUE_(Bool, (CurrentContext(), b), (b)); + } + bool Int(int i) { + RAPIDJSON_SCHEMA_HANDLE_VALUE_(Int, (CurrentContext(), i), (i)); + } + bool Uint(unsigned u) { + RAPIDJSON_SCHEMA_HANDLE_VALUE_(Uint, (CurrentContext(), u), (u)); + } + bool Int64(int64_t i) { + RAPIDJSON_SCHEMA_HANDLE_VALUE_(Int64, (CurrentContext(), i), (i)); + } + bool Uint64(uint64_t u) { + RAPIDJSON_SCHEMA_HANDLE_VALUE_(Uint64, (CurrentContext(), u), (u)); + } + bool Double(double d) { + RAPIDJSON_SCHEMA_HANDLE_VALUE_(Double, (CurrentContext(), d), (d)); + } + bool RawNumber(const Ch *str, SizeType length, bool copy) { + RAPIDJSON_SCHEMA_HANDLE_VALUE_( + String, (CurrentContext(), str, length, copy), (str, length, copy)); + } + bool String(const Ch *str, SizeType length, bool copy) { + RAPIDJSON_SCHEMA_HANDLE_VALUE_( + String, (CurrentContext(), str, length, copy), (str, length, copy)); + } + + bool StartObject() { + RAPIDJSON_SCHEMA_HANDLE_BEGIN_(StartObject, (CurrentContext())); + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(StartObject, ()); + return valid_ = !outputHandler_ || outputHandler_->StartObject(); + } + + bool Key(const Ch *str, SizeType len, bool copy) { + if (!valid_) return false; + AppendToken(str, len); + if (!CurrentSchema().Key(CurrentContext(), str, len, copy)) + return valid_ = false; + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(Key, (str, len, copy)); + return valid_ = !outputHandler_ || outputHandler_->Key(str, len, copy); + } + + bool EndObject(SizeType memberCount) { + if (!valid_) return false; + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(EndObject, (memberCount)); + if (!CurrentSchema().EndObject(CurrentContext(), memberCount)) + return valid_ = false; + RAPIDJSON_SCHEMA_HANDLE_END_(EndObject, (memberCount)); + } + + bool StartArray() { + RAPIDJSON_SCHEMA_HANDLE_BEGIN_(StartArray, (CurrentContext())); + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(StartArray, ()); + return valid_ = !outputHandler_ || outputHandler_->StartArray(); + } + + bool EndArray(SizeType elementCount) { + if (!valid_) return false; + RAPIDJSON_SCHEMA_HANDLE_PARALLEL_(EndArray, (elementCount)); + if (!CurrentSchema().EndArray(CurrentContext(), elementCount)) + return valid_ = false; + RAPIDJSON_SCHEMA_HANDLE_END_(EndArray, (elementCount)); + } + +#undef RAPIDJSON_SCHEMA_HANDLE_BEGIN_VERBOSE_ +#undef RAPIDJSON_SCHEMA_HANDLE_BEGIN_ +#undef RAPIDJSON_SCHEMA_HANDLE_PARALLEL_ +#undef RAPIDJSON_SCHEMA_HANDLE_VALUE_ + + // Implementation of ISchemaStateFactory + virtual ISchemaValidator *CreateSchemaValidator(const SchemaType &root) { + return new (GetStateAllocator().Malloc(sizeof(GenericSchemaValidator))) + GenericSchemaValidator(*schemaDocument_, root, + documentStack_.template Bottom(), + documentStack_.GetSize(), +#if RAPIDJSON_SCHEMA_VERBOSE + depth_ + 1, +#endif + &GetStateAllocator()); + } + + virtual void DestroySchemaValidator(ISchemaValidator *validator) { + GenericSchemaValidator *v = + static_cast(validator); + v->~GenericSchemaValidator(); + StateAllocator::Free(v); + } + + virtual void *CreateHasher() { + return new (GetStateAllocator().Malloc(sizeof(HasherType))) + HasherType(&GetStateAllocator()); + } + + virtual uint64_t GetHashCode(void *hasher) { + return static_cast(hasher)->GetHashCode(); + } + + virtual void DestroryHasher(void *hasher) { + HasherType *h = static_cast(hasher); + h->~HasherType(); + StateAllocator::Free(h); + } + + virtual void *MallocState(size_t size) { + return GetStateAllocator().Malloc(size); + } + + virtual void FreeState(void *p) { StateAllocator::Free(p); } + + private: + typedef typename SchemaType::Context Context; + typedef GenericValue, StateAllocator> HashCodeArray; + typedef internal::Hasher HasherType; + + GenericSchemaValidator( + const SchemaDocumentType &schemaDocument, const SchemaType &root, + const char *basePath, size_t basePathSize, +#if RAPIDJSON_SCHEMA_VERBOSE + unsigned depth, +#endif + StateAllocator *allocator = 0, + size_t schemaStackCapacity = kDefaultSchemaStackCapacity, + size_t documentStackCapacity = kDefaultDocumentStackCapacity) + : schemaDocument_(&schemaDocument), + root_(root), + stateAllocator_(allocator), + ownStateAllocator_(0), + schemaStack_(allocator, schemaStackCapacity), + documentStack_(allocator, documentStackCapacity), + outputHandler_(0), + error_(kObjectType), + currentError_(), + missingDependents_(), + valid_(true) +#if RAPIDJSON_SCHEMA_VERBOSE + , + depth_(depth) +#endif + { + if (basePath && basePathSize) + memcpy(documentStack_.template Push(basePathSize), basePath, + basePathSize); + } + + StateAllocator &GetStateAllocator() { + if (!stateAllocator_) + stateAllocator_ = ownStateAllocator_ = RAPIDJSON_NEW(StateAllocator)(); + return *stateAllocator_; + } + + bool BeginValue() { + if (schemaStack_.Empty()) + PushSchema(root_); + else { + if (CurrentContext().inArray) + internal::TokenHelper, + Ch>::AppendIndexToken(documentStack_, + CurrentContext() + .arrayElementIndex); + + if (!CurrentSchema().BeginValue(CurrentContext())) return false; + + SizeType count = CurrentContext().patternPropertiesSchemaCount; + const SchemaType **sa = CurrentContext().patternPropertiesSchemas; + typename Context::PatternValidatorType patternValidatorType = + CurrentContext().valuePatternValidatorType; + bool valueUniqueness = CurrentContext().valueUniqueness; + RAPIDJSON_ASSERT(CurrentContext().valueSchema); + PushSchema(*CurrentContext().valueSchema); + + if (count > 0) { + CurrentContext().objectPatternValidatorType = patternValidatorType; + ISchemaValidator **&va = CurrentContext().patternPropertiesValidators; + SizeType &validatorCount = + CurrentContext().patternPropertiesValidatorCount; + va = static_cast( + MallocState(sizeof(ISchemaValidator *) * count)); + for (SizeType i = 0; i < count; i++) + va[validatorCount++] = CreateSchemaValidator(*sa[i]); + } + + CurrentContext().arrayUniqueness = valueUniqueness; + } + return true; + } + + bool EndValue() { + if (!CurrentSchema().EndValue(CurrentContext())) return false; + +#if RAPIDJSON_SCHEMA_VERBOSE + GenericStringBuffer sb; + schemaDocument_->GetPointer(&CurrentSchema()).Stringify(sb); + + *documentStack_.template Push() = '\0'; + documentStack_.template Pop(1); + internal::PrintValidatorPointers(depth_, sb.GetString(), + documentStack_.template Bottom()); +#endif + + uint64_t h = + CurrentContext().arrayUniqueness + ? static_cast(CurrentContext().hasher)->GetHashCode() + : 0; + + PopSchema(); + + if (!schemaStack_.Empty()) { + Context &context = CurrentContext(); + if (context.valueUniqueness) { + HashCodeArray *a = + static_cast(context.arrayElementHashCodes); + if (!a) + CurrentContext().arrayElementHashCodes = a = + new (GetStateAllocator().Malloc(sizeof(HashCodeArray))) + HashCodeArray(kArrayType); + for (typename HashCodeArray::ConstValueIterator itr = a->Begin(); + itr != a->End(); ++itr) + if (itr->GetUint64() == h) { + DuplicateItems(static_cast(itr - a->Begin()), a->Size()); + RAPIDJSON_INVALID_KEYWORD_RETURN( + SchemaType::GetUniqueItemsString()); + } + a->PushBack(h, GetStateAllocator()); + } + } + + // Remove the last token of document pointer + while (!documentStack_.Empty() && + *documentStack_.template Pop(1) != '/') + ; + + return true; + } + + void AppendToken(const Ch *str, SizeType len) { + documentStack_.template Reserve( + 1 + + len * 2); // worst case all characters are escaped as two characters + *documentStack_.template PushUnsafe() = '/'; + for (SizeType i = 0; i < len; i++) { + if (str[i] == '~') { + *documentStack_.template PushUnsafe() = '~'; + *documentStack_.template PushUnsafe() = '0'; + } else if (str[i] == '/') { + *documentStack_.template PushUnsafe() = '~'; + *documentStack_.template PushUnsafe() = '1'; + } else + *documentStack_.template PushUnsafe() = str[i]; + } + } + + RAPIDJSON_FORCEINLINE void PushSchema(const SchemaType &schema) { + new (schemaStack_.template Push()) Context(*this, *this, &schema); + } + + RAPIDJSON_FORCEINLINE void PopSchema() { + Context *c = schemaStack_.template Pop(1); + if (HashCodeArray *a = + static_cast(c->arrayElementHashCodes)) { + a->~HashCodeArray(); + StateAllocator::Free(a); + } + c->~Context(); + } + + void AddErrorLocation(ValueType &result, bool parent) { + GenericStringBuffer sb; + PointerType instancePointer = GetInvalidDocumentPointer(); + ((parent && instancePointer.GetTokenCount() > 0) + ? PointerType(instancePointer.GetTokens(), + instancePointer.GetTokenCount() - 1) + : instancePointer) + .StringifyUriFragment(sb); + ValueType instanceRef(sb.GetString(), + static_cast(sb.GetSize() / sizeof(Ch)), + GetStateAllocator()); + result.AddMember(GetInstanceRefString(), instanceRef, GetStateAllocator()); + sb.Clear(); + memcpy(sb.Push(CurrentSchema().GetURI().GetStringLength()), + CurrentSchema().GetURI().GetString(), + CurrentSchema().GetURI().GetStringLength() * sizeof(Ch)); + GetInvalidSchemaPointer().StringifyUriFragment(sb); + ValueType schemaRef(sb.GetString(), + static_cast(sb.GetSize() / sizeof(Ch)), + GetStateAllocator()); + result.AddMember(GetSchemaRefString(), schemaRef, GetStateAllocator()); + } + + void AddError(ValueType &keyword, ValueType &error) { + typename ValueType::MemberIterator member = error_.FindMember(keyword); + if (member == error_.MemberEnd()) + error_.AddMember(keyword, error, GetStateAllocator()); + else { + if (member->value.IsObject()) { + ValueType errors(kArrayType); + errors.PushBack(member->value, GetStateAllocator()); + member->value = errors; + } + member->value.PushBack(error, GetStateAllocator()); + } + } + + void AddCurrentError(const typename SchemaType::ValueType &keyword, + bool parent = false) { + AddErrorLocation(currentError_, parent); + AddError(ValueType(keyword, GetStateAllocator(), false).Move(), + currentError_); + } + + void MergeError(ValueType &other) { + for (typename ValueType::MemberIterator it = other.MemberBegin(), + end = other.MemberEnd(); + it != end; ++it) { + AddError(it->name, it->value); + } + } + + void AddNumberError( + const typename SchemaType::ValueType &keyword, ValueType &actual, + const SValue &expected, + const typename SchemaType::ValueType &(*exclusive)() = 0) { + currentError_.SetObject(); + currentError_.AddMember(GetActualString(), actual, GetStateAllocator()); + currentError_.AddMember(GetExpectedString(), + ValueType(expected, GetStateAllocator()).Move(), + GetStateAllocator()); + if (exclusive) + currentError_.AddMember( + ValueType(exclusive(), GetStateAllocator()).Move(), true, + GetStateAllocator()); + AddCurrentError(keyword); + } + + void AddErrorArray(const typename SchemaType::ValueType &keyword, + ISchemaValidator **subvalidators, SizeType count) { + ValueType errors(kArrayType); + for (SizeType i = 0; i < count; ++i) + errors.PushBack( + static_cast(subvalidators[i])->GetError(), + GetStateAllocator()); + currentError_.SetObject(); + currentError_.AddMember(GetErrorsString(), errors, GetStateAllocator()); + AddCurrentError(keyword); + } + + const SchemaType &CurrentSchema() const { + return *schemaStack_.template Top()->schema; + } + Context &CurrentContext() { return *schemaStack_.template Top(); } + const Context &CurrentContext() const { + return *schemaStack_.template Top(); + } + + static const size_t kDefaultSchemaStackCapacity = 1024; + static const size_t kDefaultDocumentStackCapacity = 256; + const SchemaDocumentType *schemaDocument_; + const SchemaType &root_; + StateAllocator *stateAllocator_; + StateAllocator *ownStateAllocator_; + internal::Stack + schemaStack_; //!< stack to store the current path of schema + //!< (BaseSchemaType *) + internal::Stack + documentStack_; //!< stack to store the current path of validating + //!< document (Ch) + OutputHandler *outputHandler_; + ValueType error_; + ValueType currentError_; + ValueType missingDependents_; + bool valid_; +#if RAPIDJSON_SCHEMA_VERBOSE + unsigned depth_; +#endif +}; + +typedef GenericSchemaValidator SchemaValidator; + +/////////////////////////////////////////////////////////////////////////////// +// SchemaValidatingReader + +//! A helper class for parsing with validation. +/*! + This helper class is a functor, designed as a parameter of \ref + GenericDocument::Populate(). + + \tparam parseFlags Combination of \ref ParseFlag. + \tparam InputStream Type of input stream, implementing Stream concept. + \tparam SourceEncoding Encoding of the input stream. + \tparam SchemaDocumentType Type of schema document. + \tparam StackAllocator Allocator type for stack. +*/ +template +class SchemaValidatingReader { + public: + typedef typename SchemaDocumentType::PointerType PointerType; + typedef typename InputStream::Ch Ch; + typedef GenericValue ValueType; + + //! Constructor + /*! + \param is Input stream. + \param sd Schema document. + */ + SchemaValidatingReader(InputStream &is, const SchemaDocumentType &sd) + : is_(is), + sd_(sd), + invalidSchemaKeyword_(), + error_(kObjectType), + isValid_(true) {} + + template + bool operator()(Handler &handler) { + GenericReader + reader; + GenericSchemaValidator validator(sd_, handler); + parseResult_ = reader.template Parse(is_, validator); + + isValid_ = validator.IsValid(); + if (isValid_) { + invalidSchemaPointer_ = PointerType(); + invalidSchemaKeyword_ = 0; + invalidDocumentPointer_ = PointerType(); + error_.SetObject(); + } else { + invalidSchemaPointer_ = validator.GetInvalidSchemaPointer(); + invalidSchemaKeyword_ = validator.GetInvalidSchemaKeyword(); + invalidDocumentPointer_ = validator.GetInvalidDocumentPointer(); + error_.CopyFrom(validator.GetError(), allocator_); + } + + return parseResult_; + } + + const ParseResult &GetParseResult() const { return parseResult_; } + bool IsValid() const { return isValid_; } + const PointerType &GetInvalidSchemaPointer() const { + return invalidSchemaPointer_; + } + const Ch *GetInvalidSchemaKeyword() const { return invalidSchemaKeyword_; } + const PointerType &GetInvalidDocumentPointer() const { + return invalidDocumentPointer_; + } + const ValueType &GetError() const { return error_; } + + private: + InputStream &is_; + const SchemaDocumentType &sd_; + + ParseResult parseResult_; + PointerType invalidSchemaPointer_; + const Ch *invalidSchemaKeyword_; + PointerType invalidDocumentPointer_; + StackAllocator allocator_; + ValueType error_; + bool isValid_; +}; + +RAPIDJSON_NAMESPACE_END +RAPIDJSON_DIAG_POP + +#endif // RAPIDJSON_SCHEMA_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/stream.h b/src/livox_ros_driver2/3rdparty/rapidjson/stream.h new file mode 100644 index 0000000..a446c1a --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/stream.h @@ -0,0 +1,242 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#include "rapidjson.h" + +#ifndef RAPIDJSON_STREAM_H_ +#define RAPIDJSON_STREAM_H_ + +#include "encodings.h" + +RAPIDJSON_NAMESPACE_BEGIN + +/////////////////////////////////////////////////////////////////////////////// +// Stream + +/*! \class rapidjson::Stream + \brief Concept for reading and writing characters. + + For read-only stream, no need to implement PutBegin(), Put(), Flush() and +PutEnd(). + + For write-only stream, only need to implement Put() and Flush(). + +\code +concept Stream { + typename Ch; //!< Character type of the stream. + + //! Read the current character from stream without moving the read cursor. + Ch Peek() const; + + //! Read the current character from stream and moving the read cursor to +next character. Ch Take(); + + //! Get the current read cursor. + //! \return Number of characters read from start. + size_t Tell(); + + //! Begin writing operation at the current read pointer. + //! \return The begin writer pointer. + Ch* PutBegin(); + + //! Write a character. + void Put(Ch c); + + //! Flush the buffer. + void Flush(); + + //! End the writing operation. + //! \param begin The begin write pointer returned by PutBegin(). + //! \return Number of characters written. + size_t PutEnd(Ch* begin); +} +\endcode +*/ + +//! Provides additional information for stream. +/*! + By using traits pattern, this type provides a default configuration for + stream. For custom stream, this type can be specialized for other + configuration. See TEST(Reader, CustomStringStream) in readertest.cpp for + example. +*/ +template +struct StreamTraits { + //! Whether to make local copy of stream for optimization during parsing. + /*! + By default, for safety, streams do not use local copy optimization. + Stream that can be copied fast should specialize this, like + StreamTraits. + */ + enum { copyOptimization = 0 }; +}; + +//! Reserve n characters for writing to a stream. +template +inline void PutReserve(Stream &stream, size_t count) { + (void)stream; + (void)count; +} + +//! Write character to a stream, presuming buffer is reserved. +template +inline void PutUnsafe(Stream &stream, typename Stream::Ch c) { + stream.Put(c); +} + +//! Put N copies of a character to a stream. +template +inline void PutN(Stream &stream, Ch c, size_t n) { + PutReserve(stream, n); + for (size_t i = 0; i < n; i++) PutUnsafe(stream, c); +} + +/////////////////////////////////////////////////////////////////////////////// +// GenericStreamWrapper + +//! A Stream Wrapper +/*! \tThis string stream is a wrapper for any stream by just forwarding any + \treceived message to the origin stream. + \note implements Stream concept +*/ + +#if defined(_MSC_VER) && _MSC_VER <= 1800 +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(4702) // unreachable code +RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated +#endif + +template > +class GenericStreamWrapper { + public: + typedef typename Encoding::Ch Ch; + GenericStreamWrapper(InputStream &is) : is_(is) {} + + Ch Peek() const { return is_.Peek(); } + Ch Take() { return is_.Take(); } + size_t Tell() { return is_.Tell(); } + Ch *PutBegin() { return is_.PutBegin(); } + void Put(Ch ch) { is_.Put(ch); } + void Flush() { is_.Flush(); } + size_t PutEnd(Ch *ch) { return is_.PutEnd(ch); } + + // wrapper for MemoryStream + const Ch *Peek4() const { return is_.Peek4(); } + + // wrapper for AutoUTFInputStream + UTFType GetType() const { return is_.GetType(); } + bool HasBOM() const { return is_.HasBOM(); } + + protected: + InputStream &is_; +}; + +#if defined(_MSC_VER) && _MSC_VER <= 1800 +RAPIDJSON_DIAG_POP +#endif + +/////////////////////////////////////////////////////////////////////////////// +// StringStream + +//! Read-only string stream. +/*! \note implements Stream concept + */ +template +struct GenericStringStream { + typedef typename Encoding::Ch Ch; + + GenericStringStream(const Ch *src) : src_(src), head_(src) {} + + Ch Peek() const { return *src_; } + Ch Take() { return *src_++; } + size_t Tell() const { return static_cast(src_ - head_); } + + Ch *PutBegin() { + RAPIDJSON_ASSERT(false); + return 0; + } + void Put(Ch) { RAPIDJSON_ASSERT(false); } + void Flush() { RAPIDJSON_ASSERT(false); } + size_t PutEnd(Ch *) { + RAPIDJSON_ASSERT(false); + return 0; + } + + const Ch *src_; //!< Current read position. + const Ch *head_; //!< Original head of the string. +}; + +template +struct StreamTraits> { + enum { copyOptimization = 1 }; +}; + +//! String stream with UTF8 encoding. +typedef GenericStringStream> StringStream; + +/////////////////////////////////////////////////////////////////////////////// +// InsituStringStream + +//! A read-write string stream. +/*! This string stream is particularly designed for in-situ parsing. + \note implements Stream concept +*/ +template +struct GenericInsituStringStream { + typedef typename Encoding::Ch Ch; + + GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {} + + // Read + Ch Peek() { return *src_; } + Ch Take() { return *src_++; } + size_t Tell() { return static_cast(src_ - head_); } + + // Write + void Put(Ch c) { + RAPIDJSON_ASSERT(dst_ != 0); + *dst_++ = c; + } + + Ch *PutBegin() { return dst_ = src_; } + size_t PutEnd(Ch *begin) { return static_cast(dst_ - begin); } + void Flush() {} + + Ch *Push(size_t count) { + Ch *begin = dst_; + dst_ += count; + return begin; + } + void Pop(size_t count) { dst_ -= count; } + + Ch *src_; + Ch *dst_; + Ch *head_; +}; + +template +struct StreamTraits> { + enum { copyOptimization = 1 }; +}; + +//! Insitu string stream with UTF8 encoding. +typedef GenericInsituStringStream> InsituStringStream; + +RAPIDJSON_NAMESPACE_END + +#endif // RAPIDJSON_STREAM_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/stringbuffer.h b/src/livox_ros_driver2/3rdparty/rapidjson/stringbuffer.h new file mode 100644 index 0000000..42b2bb1 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/stringbuffer.h @@ -0,0 +1,130 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_STRINGBUFFER_H_ +#define RAPIDJSON_STRINGBUFFER_H_ + +#include "internal/stack.h" +#include "stream.h" + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS +#include // std::move +#endif + +#include "internal/stack.h" + +#if defined(__clang__) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(c++ 98 - compat) +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +//! Represents an in-memory output stream. +/*! + \tparam Encoding Encoding of the stream. + \tparam Allocator type for allocating memory buffer. + \note implements Stream concept +*/ +template +class GenericStringBuffer { + public: + typedef typename Encoding::Ch Ch; + + GenericStringBuffer(Allocator *allocator = 0, + size_t capacity = kDefaultCapacity) + : stack_(allocator, capacity) {} + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + GenericStringBuffer(GenericStringBuffer &&rhs) + : stack_(std::move(rhs.stack_)) {} + GenericStringBuffer &operator=(GenericStringBuffer &&rhs) { + if (&rhs != this) stack_ = std::move(rhs.stack_); + return *this; + } +#endif + + void Put(Ch c) { *stack_.template Push() = c; } + void PutUnsafe(Ch c) { *stack_.template PushUnsafe() = c; } + void Flush() {} + + void Clear() { stack_.Clear(); } + void ShrinkToFit() { + // Push and pop a null terminator. This is safe. + *stack_.template Push() = '\0'; + stack_.ShrinkToFit(); + stack_.template Pop(1); + } + + void Reserve(size_t count) { stack_.template Reserve(count); } + Ch *Push(size_t count) { return stack_.template Push(count); } + Ch *PushUnsafe(size_t count) { return stack_.template PushUnsafe(count); } + void Pop(size_t count) { stack_.template Pop(count); } + + const Ch *GetString() const { + // Push and pop a null terminator. This is safe. + *stack_.template Push() = '\0'; + stack_.template Pop(1); + + return stack_.template Bottom(); + } + + //! Get the size of string in bytes in the string buffer. + size_t GetSize() const { return stack_.GetSize(); } + + //! Get the length of string in Ch in the string buffer. + size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); } + + static const size_t kDefaultCapacity = 256; + mutable internal::Stack stack_; + + private: + // Prohibit copy constructor & assignment operator. + GenericStringBuffer(const GenericStringBuffer &); + GenericStringBuffer &operator=(const GenericStringBuffer &); +}; + +//! String buffer with UTF8 encoding +typedef GenericStringBuffer> StringBuffer; + +template +inline void PutReserve(GenericStringBuffer &stream, + size_t count) { + stream.Reserve(count); +} + +template +inline void PutUnsafe(GenericStringBuffer &stream, + typename Encoding::Ch c) { + stream.PutUnsafe(c); +} + +//! Implement specialized version of PutN() with memset() for better +//! performance. +template <> +inline void PutN(GenericStringBuffer> &stream, char c, size_t n) { + std::memset(stream.stack_.Push(n), c, n * sizeof(c)); +} + +RAPIDJSON_NAMESPACE_END + +#if defined(__clang__) +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_STRINGBUFFER_H_ diff --git a/src/livox_ros_driver2/3rdparty/rapidjson/writer.h b/src/livox_ros_driver2/3rdparty/rapidjson/writer.h new file mode 100644 index 0000000..69f9b05 --- /dev/null +++ b/src/livox_ros_driver2/3rdparty/rapidjson/writer.h @@ -0,0 +1,811 @@ +// Tencent is pleased to support the open source community by making RapidJSON +// available. +// +// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +// rights reserved. +// +// Licensed under the MIT License (the "License"); you may not use this file +// except in compliance with the License. You may obtain a copy of the License +// at +// +// http://opensource.org/licenses/MIT +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +// License for the specific language governing permissions and limitations under +// the License. + +#ifndef RAPIDJSON_WRITER_H_ +#define RAPIDJSON_WRITER_H_ + +#include // placement new +#include "internal/clzll.h" +#include "internal/dtoa.h" +#include "internal/itoa.h" +#include "internal/meta.h" +#include "internal/stack.h" +#include "internal/strfunc.h" +#include "stream.h" +#include "stringbuffer.h" + +#if defined(RAPIDJSON_SIMD) && defined(_MSC_VER) +#include +#pragma intrinsic(_BitScanForward) +#endif +#ifdef RAPIDJSON_SSE42 +#include +#elif defined(RAPIDJSON_SSE2) +#include +#elif defined(RAPIDJSON_NEON) +#include +#endif + +#ifdef __clang__ +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(padded) +RAPIDJSON_DIAG_OFF(unreachable - code) +RAPIDJSON_DIAG_OFF(c++ 98 - compat) +#elif defined(_MSC_VER) +RAPIDJSON_DIAG_PUSH +RAPIDJSON_DIAG_OFF(4127) // conditional expression is constant +#endif + +RAPIDJSON_NAMESPACE_BEGIN + +/////////////////////////////////////////////////////////////////////////////// +// WriteFlag + +/*! \def RAPIDJSON_WRITE_DEFAULT_FLAGS + \ingroup RAPIDJSON_CONFIG + \brief User-defined kWriteDefaultFlags definition. + + User can define this as any \c WriteFlag combinations. +*/ +#ifndef RAPIDJSON_WRITE_DEFAULT_FLAGS +#define RAPIDJSON_WRITE_DEFAULT_FLAGS kWriteNoFlags +#endif + +//! Combination of writeFlags +enum WriteFlag { + kWriteNoFlags = 0, //!< No flags are set. + kWriteValidateEncodingFlag = 1, //!< Validate encoding of JSON strings. + kWriteNanAndInfFlag = 2, //!< Allow writing of Infinity, -Infinity and NaN. + kWriteDefaultFlags = + RAPIDJSON_WRITE_DEFAULT_FLAGS //!< Default write flags. Can be customized + //!< by defining + //!< RAPIDJSON_WRITE_DEFAULT_FLAGS +}; + +//! JSON writer +/*! Writer implements the concept Handler. + It generates JSON text by events to an output os. + + User may programmatically calls the functions of a writer to generate JSON + text. + + On the other side, a writer can also be passed to objects that generates + events, + + for example Reader::Parse() and Document::Accept(). + + \tparam OutputStream Type of output stream. + \tparam SourceEncoding Encoding of source string. + \tparam TargetEncoding Encoding of output stream. + \tparam StackAllocator Type of allocator for allocating memory of stack. + \note implements Handler concept +*/ +template , + typename TargetEncoding = UTF8<>, + typename StackAllocator = CrtAllocator, + unsigned writeFlags = kWriteDefaultFlags> +class Writer { + public: + typedef typename SourceEncoding::Ch Ch; + + static const int kDefaultMaxDecimalPlaces = 324; + + //! Constructor + /*! \param os Output stream. + \param stackAllocator User supplied allocator. If it is null, it will + create a private one. \param levelDepth Initial capacity of stack. + */ + explicit Writer(OutputStream &os, StackAllocator *stackAllocator = 0, + size_t levelDepth = kDefaultLevelDepth) + : os_(&os), + level_stack_(stackAllocator, levelDepth * sizeof(Level)), + maxDecimalPlaces_(kDefaultMaxDecimalPlaces), + hasRoot_(false) {} + + explicit Writer(StackAllocator *allocator = 0, + size_t levelDepth = kDefaultLevelDepth) + : os_(0), + level_stack_(allocator, levelDepth * sizeof(Level)), + maxDecimalPlaces_(kDefaultMaxDecimalPlaces), + hasRoot_(false) {} + +#if RAPIDJSON_HAS_CXX11_RVALUE_REFS + Writer(Writer &&rhs) + : os_(rhs.os_), + level_stack_(std::move(rhs.level_stack_)), + maxDecimalPlaces_(rhs.maxDecimalPlaces_), + hasRoot_(rhs.hasRoot_) { + rhs.os_ = 0; + } +#endif + + //! Reset the writer with a new stream. + /*! + This function reset the writer with a new stream and default settings, + in order to make a Writer object reusable for output multiple JSONs. + + \param os New output stream. + \code + Writer writer(os1); + writer.StartObject(); + // ... + writer.EndObject(); + + writer.Reset(os2); + writer.StartObject(); + // ... + writer.EndObject(); + \endcode + */ + void Reset(OutputStream &os) { + os_ = &os; + hasRoot_ = false; + level_stack_.Clear(); + } + + //! Checks whether the output is a complete JSON. + /*! + A complete JSON has a complete root object or array. + */ + bool IsComplete() const { return hasRoot_ && level_stack_.Empty(); } + + int GetMaxDecimalPlaces() const { return maxDecimalPlaces_; } + + //! Sets the maximum number of decimal places for double output. + /*! + This setting truncates the output with specified number of decimal places. + + For example, + + \code + writer.SetMaxDecimalPlaces(3); + writer.StartArray(); + writer.Double(0.12345); // "0.123" + writer.Double(0.0001); // "0.0" + writer.Double(1.234567890123456e30); // "1.234567890123456e30" (do not + truncate significand for positive exponent) writer.Double(1.23e-4); // + "0.0" (do truncate significand for negative exponent) + writer.EndArray(); + \endcode + + The default setting does not truncate any decimal places. You can restore + to this setting by calling \code + writer.SetMaxDecimalPlaces(Writer::kDefaultMaxDecimalPlaces); + \endcode + */ + void SetMaxDecimalPlaces(int maxDecimalPlaces) { + maxDecimalPlaces_ = maxDecimalPlaces; + } + + /*!@name Implementation of Handler + \see Handler + */ + //@{ + + bool Null() { + Prefix(kNullType); + return EndValue(WriteNull()); + } + bool Bool(bool b) { + Prefix(b ? kTrueType : kFalseType); + return EndValue(WriteBool(b)); + } + bool Int(int i) { + Prefix(kNumberType); + return EndValue(WriteInt(i)); + } + bool Uint(unsigned u) { + Prefix(kNumberType); + return EndValue(WriteUint(u)); + } + bool Int64(int64_t i64) { + Prefix(kNumberType); + return EndValue(WriteInt64(i64)); + } + bool Uint64(uint64_t u64) { + Prefix(kNumberType); + return EndValue(WriteUint64(u64)); + } + + //! Writes the given \c double value to the stream + /*! + \param d The value to be written. + \return Whether it is succeed. + */ + bool Double(double d) { + Prefix(kNumberType); + return EndValue(WriteDouble(d)); + } + + bool RawNumber(const Ch *str, SizeType length, bool copy = false) { + RAPIDJSON_ASSERT(str != 0); + (void)copy; + Prefix(kNumberType); + return EndValue(WriteString(str, length)); + } + + bool String(const Ch *str, SizeType length, bool copy = false) { + RAPIDJSON_ASSERT(str != 0); + (void)copy; + Prefix(kStringType); + return EndValue(WriteString(str, length)); + } + +#if RAPIDJSON_HAS_STDSTRING + bool String(const std::basic_string &str) { + return String(str.data(), SizeType(str.size())); + } +#endif + + bool StartObject() { + Prefix(kObjectType); + new (level_stack_.template Push()) Level(false); + return WriteStartObject(); + } + + bool Key(const Ch *str, SizeType length, bool copy = false) { + return String(str, length, copy); + } + +#if RAPIDJSON_HAS_STDSTRING + bool Key(const std::basic_string &str) { + return Key(str.data(), SizeType(str.size())); + } +#endif + + bool EndObject(SizeType memberCount = 0) { + (void)memberCount; + RAPIDJSON_ASSERT(level_stack_.GetSize() >= + sizeof(Level)); // not inside an Object + RAPIDJSON_ASSERT(!level_stack_.template Top() + ->inArray); // currently inside an Array, not Object + RAPIDJSON_ASSERT(0 == + level_stack_.template Top()->valueCount % + 2); // Object has a Key without a Value + level_stack_.template Pop(1); + return EndValue(WriteEndObject()); + } + + bool StartArray() { + Prefix(kArrayType); + new (level_stack_.template Push()) Level(true); + return WriteStartArray(); + } + + bool EndArray(SizeType elementCount = 0) { + (void)elementCount; + RAPIDJSON_ASSERT(level_stack_.GetSize() >= sizeof(Level)); + RAPIDJSON_ASSERT(level_stack_.template Top()->inArray); + level_stack_.template Pop(1); + return EndValue(WriteEndArray()); + } + //@} + + /*! @name Convenience extensions */ + //@{ + + //! Simpler but slower overload. + bool String(const Ch *const &str) { + return String(str, internal::StrLen(str)); + } + bool Key(const Ch *const &str) { return Key(str, internal::StrLen(str)); } + + //@} + + //! Write a raw JSON value. + /*! + For user to write a stringified JSON as a value. + + \param json A well-formed JSON value. It should not contain null character + within [0, length - 1] range. \param length Length of the json. \param type + Type of the root of json. + */ + bool RawValue(const Ch *json, size_t length, Type type) { + RAPIDJSON_ASSERT(json != 0); + Prefix(type); + return EndValue(WriteRawValue(json, length)); + } + + //! Flush the output stream. + /*! + Allows the user to flush the output stream immediately. + */ + void Flush() { os_->Flush(); } + + protected: + //! Information for each nested level + struct Level { + Level(bool inArray_) : valueCount(0), inArray(inArray_) {} + size_t valueCount; //!< number of values in this level + bool inArray; //!< true if in array, otherwise in object + }; + + static const size_t kDefaultLevelDepth = 32; + + bool WriteNull() { + PutReserve(*os_, 4); + PutUnsafe(*os_, 'n'); + PutUnsafe(*os_, 'u'); + PutUnsafe(*os_, 'l'); + PutUnsafe(*os_, 'l'); + return true; + } + + bool WriteBool(bool b) { + if (b) { + PutReserve(*os_, 4); + PutUnsafe(*os_, 't'); + PutUnsafe(*os_, 'r'); + PutUnsafe(*os_, 'u'); + PutUnsafe(*os_, 'e'); + } else { + PutReserve(*os_, 5); + PutUnsafe(*os_, 'f'); + PutUnsafe(*os_, 'a'); + PutUnsafe(*os_, 'l'); + PutUnsafe(*os_, 's'); + PutUnsafe(*os_, 'e'); + } + return true; + } + + bool WriteInt(int i) { + char buffer[11]; + const char *end = internal::i32toa(i, buffer); + PutReserve(*os_, static_cast(end - buffer)); + for (const char *p = buffer; p != end; ++p) + PutUnsafe(*os_, static_cast(*p)); + return true; + } + + bool WriteUint(unsigned u) { + char buffer[10]; + const char *end = internal::u32toa(u, buffer); + PutReserve(*os_, static_cast(end - buffer)); + for (const char *p = buffer; p != end; ++p) + PutUnsafe(*os_, static_cast(*p)); + return true; + } + + bool WriteInt64(int64_t i64) { + char buffer[21]; + const char *end = internal::i64toa(i64, buffer); + PutReserve(*os_, static_cast(end - buffer)); + for (const char *p = buffer; p != end; ++p) + PutUnsafe(*os_, static_cast(*p)); + return true; + } + + bool WriteUint64(uint64_t u64) { + char buffer[20]; + char *end = internal::u64toa(u64, buffer); + PutReserve(*os_, static_cast(end - buffer)); + for (char *p = buffer; p != end; ++p) + PutUnsafe(*os_, static_cast(*p)); + return true; + } + + bool WriteDouble(double d) { + if (internal::Double(d).IsNanOrInf()) { + if (!(writeFlags & kWriteNanAndInfFlag)) return false; + if (internal::Double(d).IsNan()) { + PutReserve(*os_, 3); + PutUnsafe(*os_, 'N'); + PutUnsafe(*os_, 'a'); + PutUnsafe(*os_, 'N'); + return true; + } + if (internal::Double(d).Sign()) { + PutReserve(*os_, 9); + PutUnsafe(*os_, '-'); + } else + PutReserve(*os_, 8); + PutUnsafe(*os_, 'I'); + PutUnsafe(*os_, 'n'); + PutUnsafe(*os_, 'f'); + PutUnsafe(*os_, 'i'); + PutUnsafe(*os_, 'n'); + PutUnsafe(*os_, 'i'); + PutUnsafe(*os_, 't'); + PutUnsafe(*os_, 'y'); + return true; + } + + char buffer[25]; + char *end = internal::dtoa(d, buffer, maxDecimalPlaces_); + PutReserve(*os_, static_cast(end - buffer)); + for (char *p = buffer; p != end; ++p) + PutUnsafe(*os_, static_cast(*p)); + return true; + } + + bool WriteString(const Ch *str, SizeType length) { + static const typename OutputStream::Ch hexDigits[16] = { + '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'}; + static const char escape[256] = { +#define Z16 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 + // 0 1 2 3 4 5 6 7 8 9 A B C D E + // F + 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'b', 't', + 'n', 'u', 'f', 'r', 'u', 'u', // 00 + 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', + 'u', 'u', 'u', 'u', 'u', 'u', // 10 + 0, 0, '"', 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, // 20 + Z16, Z16, // 30~4F + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, '\\', 0, 0, 0, // 50 + Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16 // 60~FF +#undef Z16 + }; + + if (TargetEncoding::supportUnicode) + PutReserve(*os_, 2 + length * 6); // "\uxxxx..." + else + PutReserve(*os_, 2 + length * 12); // "\uxxxx\uyyyy..." + + PutUnsafe(*os_, '\"'); + GenericStringStream is(str); + while (ScanWriteUnescapedString(is, length)) { + const Ch c = is.Peek(); + if (!TargetEncoding::supportUnicode && static_cast(c) >= 0x80) { + // Unicode escaping + unsigned codepoint; + if (RAPIDJSON_UNLIKELY(!SourceEncoding::Decode(is, &codepoint))) + return false; + PutUnsafe(*os_, '\\'); + PutUnsafe(*os_, 'u'); + if (codepoint <= 0xD7FF || + (codepoint >= 0xE000 && codepoint <= 0xFFFF)) { + PutUnsafe(*os_, hexDigits[(codepoint >> 12) & 15]); + PutUnsafe(*os_, hexDigits[(codepoint >> 8) & 15]); + PutUnsafe(*os_, hexDigits[(codepoint >> 4) & 15]); + PutUnsafe(*os_, hexDigits[(codepoint)&15]); + } else { + RAPIDJSON_ASSERT(codepoint >= 0x010000 && codepoint <= 0x10FFFF); + // Surrogate pair + unsigned s = codepoint - 0x010000; + unsigned lead = (s >> 10) + 0xD800; + unsigned trail = (s & 0x3FF) + 0xDC00; + PutUnsafe(*os_, hexDigits[(lead >> 12) & 15]); + PutUnsafe(*os_, hexDigits[(lead >> 8) & 15]); + PutUnsafe(*os_, hexDigits[(lead >> 4) & 15]); + PutUnsafe(*os_, hexDigits[(lead)&15]); + PutUnsafe(*os_, '\\'); + PutUnsafe(*os_, 'u'); + PutUnsafe(*os_, hexDigits[(trail >> 12) & 15]); + PutUnsafe(*os_, hexDigits[(trail >> 8) & 15]); + PutUnsafe(*os_, hexDigits[(trail >> 4) & 15]); + PutUnsafe(*os_, hexDigits[(trail)&15]); + } + } else if ((sizeof(Ch) == 1 || static_cast(c) < 256) && + RAPIDJSON_UNLIKELY(escape[static_cast(c)])) { + is.Take(); + PutUnsafe(*os_, '\\'); + PutUnsafe(*os_, static_cast( + escape[static_cast(c)])); + if (escape[static_cast(c)] == 'u') { + PutUnsafe(*os_, '0'); + PutUnsafe(*os_, '0'); + PutUnsafe(*os_, hexDigits[static_cast(c) >> 4]); + PutUnsafe(*os_, hexDigits[static_cast(c) & 0xF]); + } + } else if (RAPIDJSON_UNLIKELY(!( + writeFlags & kWriteValidateEncodingFlag + ? Transcoder::Validate( + is, *os_) + : Transcoder::TranscodeUnsafe(is, + *os_)))) + return false; + } + PutUnsafe(*os_, '\"'); + return true; + } + + bool ScanWriteUnescapedString(GenericStringStream &is, + size_t length) { + return RAPIDJSON_LIKELY(is.Tell() < length); + } + + bool WriteStartObject() { + os_->Put('{'); + return true; + } + bool WriteEndObject() { + os_->Put('}'); + return true; + } + bool WriteStartArray() { + os_->Put('['); + return true; + } + bool WriteEndArray() { + os_->Put(']'); + return true; + } + + bool WriteRawValue(const Ch *json, size_t length) { + PutReserve(*os_, length); + GenericStringStream is(json); + while (RAPIDJSON_LIKELY(is.Tell() < length)) { + RAPIDJSON_ASSERT(is.Peek() != '\0'); + if (RAPIDJSON_UNLIKELY(!( + writeFlags & kWriteValidateEncodingFlag + ? Transcoder::Validate(is, + *os_) + : Transcoder::TranscodeUnsafe( + is, *os_)))) + return false; + } + return true; + } + + void Prefix(Type type) { + (void)type; + if (RAPIDJSON_LIKELY(level_stack_.GetSize() != + 0)) { // this value is not at root + Level *level = level_stack_.template Top(); + if (level->valueCount > 0) { + if (level->inArray) + os_->Put(','); // add comma if it is not the first element in array + else // in object + os_->Put((level->valueCount % 2 == 0) ? ',' : ':'); + } + if (!level->inArray && level->valueCount % 2 == 0) + RAPIDJSON_ASSERT(type == kStringType); // if it's in object, then even + // number should be a name + level->valueCount++; + } else { + RAPIDJSON_ASSERT(!hasRoot_); // Should only has one and only one root. + hasRoot_ = true; + } + } + + // Flush the value if it is the top level one. + bool EndValue(bool ret) { + if (RAPIDJSON_UNLIKELY(level_stack_.Empty())) // end of json text + Flush(); + return ret; + } + + OutputStream *os_; + internal::Stack level_stack_; + int maxDecimalPlaces_; + bool hasRoot_; + + private: + // Prohibit copy constructor & assignment operator. + Writer(const Writer &); + Writer &operator=(const Writer &); +}; + +// Full specialization for StringStream to prevent memory copying + +template <> +inline bool Writer::WriteInt(int i) { + char *buffer = os_->Push(11); + const char *end = internal::i32toa(i, buffer); + os_->Pop(static_cast(11 - (end - buffer))); + return true; +} + +template <> +inline bool Writer::WriteUint(unsigned u) { + char *buffer = os_->Push(10); + const char *end = internal::u32toa(u, buffer); + os_->Pop(static_cast(10 - (end - buffer))); + return true; +} + +template <> +inline bool Writer::WriteInt64(int64_t i64) { + char *buffer = os_->Push(21); + const char *end = internal::i64toa(i64, buffer); + os_->Pop(static_cast(21 - (end - buffer))); + return true; +} + +template <> +inline bool Writer::WriteUint64(uint64_t u) { + char *buffer = os_->Push(20); + const char *end = internal::u64toa(u, buffer); + os_->Pop(static_cast(20 - (end - buffer))); + return true; +} + +template <> +inline bool Writer::WriteDouble(double d) { + if (internal::Double(d).IsNanOrInf()) { + // Note: This code path can only be reached if + // (RAPIDJSON_WRITE_DEFAULT_FLAGS & kWriteNanAndInfFlag). + if (!(kWriteDefaultFlags & kWriteNanAndInfFlag)) return false; + if (internal::Double(d).IsNan()) { + PutReserve(*os_, 3); + PutUnsafe(*os_, 'N'); + PutUnsafe(*os_, 'a'); + PutUnsafe(*os_, 'N'); + return true; + } + if (internal::Double(d).Sign()) { + PutReserve(*os_, 9); + PutUnsafe(*os_, '-'); + } else + PutReserve(*os_, 8); + PutUnsafe(*os_, 'I'); + PutUnsafe(*os_, 'n'); + PutUnsafe(*os_, 'f'); + PutUnsafe(*os_, 'i'); + PutUnsafe(*os_, 'n'); + PutUnsafe(*os_, 'i'); + PutUnsafe(*os_, 't'); + PutUnsafe(*os_, 'y'); + return true; + } + + char *buffer = os_->Push(25); + char *end = internal::dtoa(d, buffer, maxDecimalPlaces_); + os_->Pop(static_cast(25 - (end - buffer))); + return true; +} + +#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) +template <> +inline bool Writer::ScanWriteUnescapedString(StringStream &is, + size_t length) { + if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length); + + if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false; + + const char *p = is.src_; + const char *end = is.head_ + length; + const char *nextAligned = reinterpret_cast( + (reinterpret_cast(p) + 15) & static_cast(~15)); + const char *endAligned = reinterpret_cast( + reinterpret_cast(end) & static_cast(~15)); + if (nextAligned > end) return true; + + while (p != nextAligned) + if (*p < 0x20 || *p == '\"' || *p == '\\') { + is.src_ = p; + return RAPIDJSON_LIKELY(is.Tell() < length); + } else + os_->PutUnsafe(*p++); + + // The rest of string using SIMD + static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"', + '\"', '\"', '\"', '\"', '\"', '\"', + '\"', '\"', '\"', '\"'}; + static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\', + '\\', '\\', '\\', '\\', '\\', '\\', + '\\', '\\', '\\', '\\'}; + static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, + 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, + 0x1F, 0x1F, 0x1F, 0x1F}; + const __m128i dq = + _mm_loadu_si128(reinterpret_cast(&dquote[0])); + const __m128i bs = + _mm_loadu_si128(reinterpret_cast(&bslash[0])); + const __m128i sp = + _mm_loadu_si128(reinterpret_cast(&space[0])); + + for (; p != endAligned; p += 16) { + const __m128i s = _mm_load_si128(reinterpret_cast(p)); + const __m128i t1 = _mm_cmpeq_epi8(s, dq); + const __m128i t2 = _mm_cmpeq_epi8(s, bs); + const __m128i t3 = _mm_cmpeq_epi8(_mm_max_epu8(s, sp), + sp); // s < 0x20 <=> max(s, 0x1F) == 0x1F + const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3); + unsigned short r = static_cast(_mm_movemask_epi8(x)); + if (RAPIDJSON_UNLIKELY(r != 0)) { // some of characters is escaped + SizeType len; +#ifdef _MSC_VER // Find the index of first escaped + unsigned long offset; + _BitScanForward(&offset, r); + len = offset; +#else + len = static_cast(__builtin_ffs(r) - 1); +#endif + char *q = reinterpret_cast(os_->PushUnsafe(len)); + for (size_t i = 0; i < len; i++) q[i] = p[i]; + + p += len; + break; + } + _mm_storeu_si128(reinterpret_cast<__m128i *>(os_->PushUnsafe(16)), s); + } + + is.src_ = p; + return RAPIDJSON_LIKELY(is.Tell() < length); +} +#elif defined(RAPIDJSON_NEON) +template <> +inline bool Writer::ScanWriteUnescapedString(StringStream &is, + size_t length) { + if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length); + + if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false; + + const char *p = is.src_; + const char *end = is.head_ + length; + const char *nextAligned = reinterpret_cast( + (reinterpret_cast(p) + 15) & static_cast(~15)); + const char *endAligned = reinterpret_cast( + reinterpret_cast(end) & static_cast(~15)); + if (nextAligned > end) return true; + + while (p != nextAligned) + if (*p < 0x20 || *p == '\"' || *p == '\\') { + is.src_ = p; + return RAPIDJSON_LIKELY(is.Tell() < length); + } else + os_->PutUnsafe(*p++); + + // The rest of string using SIMD + const uint8x16_t s0 = vmovq_n_u8('"'); + const uint8x16_t s1 = vmovq_n_u8('\\'); + const uint8x16_t s2 = vmovq_n_u8('\b'); + const uint8x16_t s3 = vmovq_n_u8(32); + + for (; p != endAligned; p += 16) { + const uint8x16_t s = vld1q_u8(reinterpret_cast(p)); + uint8x16_t x = vceqq_u8(s, s0); + x = vorrq_u8(x, vceqq_u8(s, s1)); + x = vorrq_u8(x, vceqq_u8(s, s2)); + x = vorrq_u8(x, vcltq_u8(s, s3)); + + x = vrev64q_u8(x); // Rev in 64 + uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0); // extract + uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1); // extract + + SizeType len = 0; + bool escaped = false; + if (low == 0) { + if (high != 0) { + uint32_t lz = RAPIDJSON_CLZLL(high); + len = 8 + (lz >> 3); + escaped = true; + } + } else { + uint32_t lz = RAPIDJSON_CLZLL(low); + len = lz >> 3; + escaped = true; + } + if (RAPIDJSON_UNLIKELY(escaped)) { // some of characters is escaped + char *q = reinterpret_cast(os_->PushUnsafe(len)); + for (size_t i = 0; i < len; i++) q[i] = p[i]; + + p += len; + break; + } + vst1q_u8(reinterpret_cast(os_->PushUnsafe(16)), s); + } + + is.src_ = p; + return RAPIDJSON_LIKELY(is.Tell() < length); +} +#endif // RAPIDJSON_NEON + +RAPIDJSON_NAMESPACE_END + +#if defined(_MSC_VER) || defined(__clang__) +RAPIDJSON_DIAG_POP +#endif + +#endif // RAPIDJSON_RAPIDJSON_H_ diff --git a/src/livox_ros_driver2/CHANGELOG.md b/src/livox_ros_driver2/CHANGELOG.md new file mode 100644 index 0000000..5a64e83 --- /dev/null +++ b/src/livox_ros_driver2/CHANGELOG.md @@ -0,0 +1,60 @@ +# Changelog + +All notable changes to this project will be documented in this file. +## [1.2.5] +### Added +- Support Mid-360s Lidar. + +## [1.2.4] +### Fixed +- Optimize framing performance + +## [1.2.3] +### Fixed +- Optimize framing logic and reduce CPU usage +- Fixed some known issues + +## [1.2.1] +### Fixed +- Fix offset time error regarding CustomMsg format message publishment. + +## [1.2.0] +### Added +- Revise the frame segmentation logic. +- (Notice!!!) Add Timestamp to each point in Livox pointcloud2 (PointXYZRTLT) format. The PointXYZRTL format has been updated to PointXYZRTLT format. Compatibility needs to be considered. +### Fixed +- Improve support for gPTP and GPS synchronizations. + +--- +## [1.1.3] +### Fixed +- Improve performance when running in ROS2 Humble. + +--- +## [1.1.2] +### Changed +- Change publish frequency range to [0.5Hz, 10 Hz]. +### Fixed +- Fix a high CPU-usage problem. + +--- +## [1.1.1] +### Added +- Offer valid line-number info in the point cloud data of MID-360 Lidar. +- Enable IMU by default. +### Changed +- Update the README slightly. + +--- +## [1.0.0] +### Added +- Support Mid-360 Lidar. +- Support for Ubuntu 22.04 ROS2 humble. +- Support multi-topic fuction, the suffix of the topic name corresponds to the ip address of each Lidar. +### Changed +- Remove the embedded SDK. +- Constraint: Livox ROS Driver 2 for ROS2 does not support message passing with PCL native data types. +### Fixed +- Fix IMU packet loss. +- Fix some conflicts with livox ros driver. +- Fixed HAP Lidar publishing PointCloud2 and CustomMsg format point clouds with no line number. diff --git a/src/livox_ros_driver2/CMakeLists.txt b/src/livox_ros_driver2/CMakeLists.txt new file mode 100644 index 0000000..99a8ccc --- /dev/null +++ b/src/livox_ros_driver2/CMakeLists.txt @@ -0,0 +1,336 @@ +# judge which cmake codes to use +if(ROS_EDITION STREQUAL "ROS1") + + # Copyright(c) 2019 livoxtech limited. + + cmake_minimum_required(VERSION 3.0) + + + #--------------------------------------------------------------------------------------- + # Start livox_ros_driver2 project + #--------------------------------------------------------------------------------------- + include(cmake/version.cmake) + project(livox_ros_driver2 VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX) + message(STATUS "livox_ros_driver2 version: ${LIVOX_ROS_DRIVER2_VERSION}") + + #--------------------------------------------------------------------------------------- + # Add ROS Version MACRO + #--------------------------------------------------------------------------------------- + add_definitions(-DBUILDING_ROS1) + + #--------------------------------------------------------------------------------------- + # find package and the dependecy + #--------------------------------------------------------------------------------------- + find_package(Boost 1.54 REQUIRED COMPONENTS + system + thread + chrono + ) + + ## Find catkin macros and libraries + ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) + ## is used, also find other catkin packages + find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + sensor_msgs + std_msgs + message_generation + rosbag + pcl_ros + ) + + ## Find pcl lib + find_package(PCL REQUIRED) + + ## Generate messages in the 'msg' folder + add_message_files(FILES + CustomPoint.msg + CustomMsg.msg + # Message2.msg + ) + + ## Generate added messages and services with any dependencies listed here + generate_messages(DEPENDENCIES + std_msgs + ) + + find_package(PkgConfig) + pkg_check_modules(APR apr-1) + if (APR_FOUND) + message(${APR_INCLUDE_DIRS}) + message(${APR_LIBRARIES}) + endif (APR_FOUND) + + ################################### + ## catkin specific configuration ## + ################################### + ## The catkin_package macro generates cmake config files for your package + ## Declare things to be passed to dependent projects + ## INCLUDE_DIRS: uncomment this if your package contains header files + ## LIBRARIES: libraries you create in this project that dependent projects als o need + ## CATKIN_DEPENDS: catkin_packages dependent projects also need + ## DEPENDS: system dependencies of this project that dependent projects also n eed + catkin_package(CATKIN_DEPENDS + roscpp rospy std_msgs message_runtime + pcl_ros + ) + + #--------------------------------------------------------------------------------------- + # Set default build to release + #--------------------------------------------------------------------------------------- + if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose Release or Debug" FORCE) + endif() + + #--------------------------------------------------------------------------------------- + # Compiler config + #--------------------------------------------------------------------------------------- + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) + + ## make sure the livox_lidar_sdk_static library is installed + find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_static.a /usr/local/lib) + + ## PCL library + link_directories(${PCL_LIBRARY_DIRS}) + add_definitions(${PCL_DEFINITIONS}) + + #--------------------------------------------------------------------------------------- + # generate excutable and add libraries + #--------------------------------------------------------------------------------------- + add_executable(${PROJECT_NAME}_node + "" + ) + + #--------------------------------------------------------------------------------------- + # precompile macro and compile option + #--------------------------------------------------------------------------------------- + target_compile_options(${PROJECT_NAME}_node + PRIVATE $<$:-Wall> + ) + + #--------------------------------------------------------------------------------------- + # add projects that depend on + #--------------------------------------------------------------------------------------- + add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp) + + #--------------------------------------------------------------------------------------- + # source file + #--------------------------------------------------------------------------------------- + target_sources(${PROJECT_NAME}_node + PRIVATE + src/driver_node.cpp + src/lds.cpp + src/lds_lidar.cpp + src/lddc.cpp + src/livox_ros_driver2.cpp + + src/comm/comm.cpp + src/comm/ldq.cpp + src/comm/semaphore.cpp + src/comm/lidar_imu_data_queue.cpp + src/comm/cache_index.cpp + src/comm/pub_handler.cpp + + src/parse_cfg_file/parse_cfg_file.cpp + src/parse_cfg_file/parse_livox_lidar_cfg.cpp + + src/call_back/lidar_common_callback.cpp + src/call_back/livox_lidar_callback.cpp + ) + + #--------------------------------------------------------------------------------------- + # include file + #--------------------------------------------------------------------------------------- + target_include_directories(${PROJECT_NAME}_node + PUBLIC + ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${APR_INCLUDE_DIRS} + 3rdparty + src + ) + + #--------------------------------------------------------------------------------------- + # link libraries + #--------------------------------------------------------------------------------------- + target_link_libraries(${PROJECT_NAME}_node + ${LIVOX_LIDAR_SDK_LIBRARY} + ${Boost_LIBRARY} + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + ${APR_LIBRARIES} + ) + + + #--------------------------------------------------------------------------------------- + # Install + #--------------------------------------------------------------------------------------- + + install(TARGETS ${PROJECT_NAME}_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY launch_ROS1/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch_ROS1 + ) + + #--------------------------------------------------------------------------------------- + # end of CMakeList.txt + #--------------------------------------------------------------------------------------- + + +else(ROS_EDITION STREQUAL "ROS2") + + # Copyright(c) 2020 livoxtech limited. + + cmake_minimum_required(VERSION 3.14) + project(livox_ros_driver2) + + # Default to C99 + if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) + endif() + + # Default to C++14 + if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + endif() + + list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules") + + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) + endif() + + # Printf version info + include(cmake/version.cmake) + project(${PROJECT_NAME} VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX) + message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}") + + #--------------------------------------------------------------------------------------- + # Add ROS Version MACRO + #--------------------------------------------------------------------------------------- + add_definitions(-DBUILDING_ROS2) + + # find dependencies + # uncomment the following section in order to fill in + # further dependencies manually. + # find_package( REQUIRED) + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + find_package(PCL REQUIRED) + find_package(std_msgs REQUIRED) + find_package(builtin_interfaces REQUIRED) + find_package(rosidl_default_generators REQUIRED) + + # check apr + find_package(PkgConfig) + pkg_check_modules(APR apr-1) + if (APR_FOUND) + message(${APR_INCLUDE_DIRS}) + message(${APR_LIBRARIES}) + endif (APR_FOUND) + + # generate custom msg headers + set(LIVOX_INTERFACES livox_interfaces2) + rosidl_generate_interfaces(${LIVOX_INTERFACES} + "msg/CustomPoint.msg" + "msg/CustomMsg.msg" + DEPENDENCIES builtin_interfaces std_msgs + LIBRARY_NAME ${PROJECT_NAME} + ) + + ## make sure the livox_lidar_sdk_shared library is installed + find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED) + + ## + find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR + NAMES "livox_lidar_api.h" "livox_lidar_def.h" + REQUIRED) + + ## PCL library + link_directories(${PCL_LIBRARY_DIRS}) + add_definitions(${PCL_DEFINITIONS}) + + # livox ros2 driver target + ament_auto_add_library(${PROJECT_NAME} SHARED + src/livox_ros_driver2.cpp + src/lddc.cpp + src/driver_node.cpp + src/lds.cpp + src/lds_lidar.cpp + + src/comm/comm.cpp + src/comm/ldq.cpp + src/comm/semaphore.cpp + src/comm/lidar_imu_data_queue.cpp + src/comm/cache_index.cpp + src/comm/pub_handler.cpp + + src/parse_cfg_file/parse_cfg_file.cpp + src/parse_cfg_file/parse_livox_lidar_cfg.cpp + + src/call_back/lidar_common_callback.cpp + src/call_back/livox_lidar_callback.cpp + ) + + target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS}) + + # get include directories of custom msg headers + if(HUMBLE_ROS STREQUAL "humble") + rosidl_get_typesupport_target(cpp_typesupport_target + ${LIVOX_INTERFACES} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}") + else() + set(LIVOX_INTERFACE_TARGET "${LIVOX_INTERFACES}__rosidl_typesupport_cpp") + add_dependencies(${PROJECT_NAME} ${LIVOX_INTERFACES}) + get_target_property(LIVOX_INTERFACES_INCLUDE_DIRECTORIES ${LIVOX_INTERFACE_TARGET} INTERFACE_INCLUDE_DIRECTORIES) + endif() + + # include file direcotry + target_include_directories(${PROJECT_NAME} PUBLIC + ${PCL_INCLUDE_DIRS} + ${APR_INCLUDE_DIRS} + ${LIVOX_LIDAR_SDK_INCLUDE_DIR} + ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES} # for custom msgs + 3rdparty + src + ) + + # link libraries + target_link_libraries(${PROJECT_NAME} + ${LIVOX_LIDAR_SDK_LIBRARY} + ${LIVOX_INTERFACE_TARGET} # for custom msgs + ${PPT_LIBRARY} + ${Boost_LIBRARY} + ${PCL_LIBRARIES} + ${APR_LIBRARIES} + ) + + rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "livox_ros::DriverNode" + EXECUTABLE ${PROJECT_NAME}_node + ) + + if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + endif() + + ament_auto_package(INSTALL_TO_SHARE + config + launch_ROS2 + ) + +endif() \ No newline at end of file diff --git a/src/livox_ros_driver2/LICENSE.txt b/src/livox_ros_driver2/LICENSE.txt new file mode 100644 index 0000000..9e0f5db --- /dev/null +++ b/src/livox_ros_driver2/LICENSE.txt @@ -0,0 +1,412 @@ +The following portions of the LIVOX’s Livox ROS Driver2 (“Software” referred to in the terms below) are made available to you under the terms of the MIT License provided below and is also available at https://opensource.org/licenses/MIT. + +livox_ros_driver2 +├── build.sh +├── cmake +│   └── version.cmake +├── CMakeLists.txt +├── config +│   ├── display_point_cloud_ROS1.rviz +│   ├── display_point_cloud_ROS2.rviz +│   ├── HAP_config.json +│   ├── MID360_config.json +│   └── mixed_HAP_MID360_config.json +├── launch_ROS1 +│   ├── msg_HAP.launch +│   ├── msg_MID360.launch +│   ├── msg_mixed.launch +│   ├── rviz_HAP.launch +│   ├── rviz_MID360.launch +│   └── rviz_mixed.launch +├── launch_ROS2 +│   ├── msg_HAP_launch.py +│   ├── msg_MID360_launch.py +│   ├── rviz_HAP_launch.py +│   ├── rviz_MID360_launch.py +│   └── rviz_mixed.py +├── msg +│   ├── CustomMsg.msg +│   └── CustomPoint.msg +├── package_ROS1.xml +├── package_ROS2.xml +├── package.xml +├── README.md +└── src + ├── call_back + │   ├── lidar_common_callback.cpp + │   ├── lidar_common_callback.h + │   ├── livox_lidar_callback.cpp + │   └── livox_lidar_callback.h + ├── comm + │   ├── cache_index.cpp + │   ├── cache_index.h + │   ├── comm.cpp + │   ├── comm.h + │   ├── ldq.cpp + │   ├── ldq.h + │   ├── lidar_imu_data_queue.cpp + │   ├── lidar_imu_data_queue.h + │   ├── pub_handler.cpp + │   ├── pub_handler.h + │   ├── semaphore.cpp + │   └── semaphore.h + ├── driver_node.cpp + ├── driver_node.h + ├── include + │   ├── livox_ros_driver2.h + │   ├── ros1_headers.h + │   ├── ros2_headers.h + │   └── ros_headers.h + ├── lddc.cpp + ├── lddc.h + ├── lds.cpp + ├── lds.h + ├── lds_lidar.cpp + ├── lds_lidar.h + ├── livox_ros_driver2.cpp + └── parse_cfg_file + ├── parse_cfg_file.cpp + ├── parse_cfg_file.h + ├── parse_livox_lidar_cfg.cpp + └── parse_livox_lidar_cfg.h + +--------------------------------- + +The MIT License (MIT) + +Copyright (c) 2022 Livox. All rights reserved. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. + +=============================================================== + +LIVOX’s Livox ROS Driver2 uses unmodified source code of RapidJSON (https://github.com/Tencent/rapidjson), which is also licensed under MIT license. A copy of the MIT license is provided below and is also available at https://opensource.org/licenses/MIT. + +livox_ros_driver2 +├── 3rdparty +│   └── rapidjson +│   ├── allocators.h +│   ├── cursorstreamwrapper.h +│   ├── document.h +│   ├── encodedstream.h +│   ├── encodings.h +│   ├── error +│   │   ├── en.h +│   │   └── error.h +│   ├── filereadstream.h +│   ├── filewritestream.h +│   ├── fwd.h +│   ├── internal +│   │   ├── biginteger.h +│   │   ├── clzll.h +│   │   ├── diyfp.h +│   │   ├── dtoa.h +│   │   ├── ieee754.h +│   │   ├── itoa.h +│   │   ├── meta.h +│   │   ├── pow10.h +│   │   ├── regex.h +│   │   ├── stack.h +│   │   ├── strfunc.h +│   │   ├── strtod.h +│   │   └── swap.h +│   ├── istreamwrapper.h +│   ├── memorybuffer.h +│   ├── memorystream.h +│   ├── msinttypes +│   │   ├── inttypes.h +│   │   └── stdint.h +│   ├── ostreamwrapper.h +│   ├── pointer.h +│   ├── prettywriter.h +│   ├── rapidjson.h +│   ├── reader.h +│   ├── schema.h +│   ├── stream.h +│   ├── stringbuffer.h +│   └── writer.h + +------------------------------------------------------------- + +Tencent is pleased to support the open source community by making RapidJSON +available. + +Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All +rights reserved. + +Licensed under the MIT License (the "License"); you may not use this file +except in compliance with the License. You may obtain a copy of the License +at + +http://opensource.org/licenses/MIT + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +License for the specific language governing permissions and limitations under +the License. + +=============================================================== + +LIVOX’s Livox ROS Driver2 uses unmodified libraries and interfaces of ROS (https://www.ros.org/), which is also licensed under 3-Clause-BSD license. A copy of the 3-Clause-BSD license is provided below and is also available at https://opensource.org/licenses/BSD-3-Clause. + +------------------------------------------------------------- + +The 3-Clause BSD License + +Copyright (c) 2001 - 2009, The Board of Trustees of the University of Illinois. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + +* Redistributions of source code must retain the above + copyright notice, this list of conditions and the + following disclaimer. + +* Redistributions in binary form must reproduce the + above copyright notice, this list of conditions + and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the University of Illinois + nor the names of its contributors may be used to + endorse or promote products derived from this + software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +=============================================================== + +LIVOX’s Livox ROS Driver2 uses unmodified libraries and interfaces of ROS2-rclcpp (https://github.com/ros2), which is also licensed under Apache License 2.0. A copy of the Apache License 2.0 is provided below and is also available at https://www.apache.org/licenses/LICENSE-2.0. + +------------------------------------------------------------- + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/livox_ros_driver2/README.md b/src/livox_ros_driver2/README.md new file mode 100644 index 0000000..ab7c0ad --- /dev/null +++ b/src/livox_ros_driver2/README.md @@ -0,0 +1,545 @@ +# Livox ROS Driver 2 + +Livox ROS Driver 2 is the 2nd-generation driver package used to connect LiDAR products produced by Livox, applicable for ROS (noetic recommended) and ROS2 (foxy or humble recommended). + + **Note :** + + As a debugging tool, Livox ROS Driver is not recommended for mass production but limited to test scenarios. You should optimize the code based on the original source to meet your various needs. + +## 1. Preparation + +### 1.1 OS requirements + + * Ubuntu 18.04 for ROS Melodic; + * Ubuntu 20.04 for ROS Noetic and ROS2 Foxy; + * Ubuntu 22.04 for ROS2 Humble; + + **Tips:** + + Colcon is a build tool used in ROS2. + + How to install colcon: [Colcon installation instructions](https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Colcon-Tutorial.html) + +### 1.2 Install ROS & ROS2 + +For ROS Melodic installation, please refer to: +[ROS Melodic installation instructions](https://wiki.ros.org/melodic/Installation) + +For ROS Noetic installation, please refer to: +[ROS Noetic installation instructions](https://wiki.ros.org/noetic/Installation) + +For ROS2 Foxy installation, please refer to: +[ROS Foxy installation instructions](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) + +For ROS2 Humble installation, please refer to: +[ROS Humble installation instructions](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) + +Desktop-Full installation is recommend. + +## 2. Build & Run Livox ROS Driver 2 + +### 2.1 Clone Livox ROS Driver 2 source code: + +```shell +git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 +``` + + **Note :** + + Be sure to clone the source code in a '[work_space]/src/' folder (as shown above), otherwise compilation errors will occur due to the compilation tool restriction. + +### 2.2 Build & install the Livox-SDK2 + + **Note :** + + Please follow the guidance of installation in the [Livox-SDK2/README.md](https://github.com/Livox-SDK/Livox-SDK2/blob/master/README.md) + +### 2.3 Build the Livox ROS Driver 2: + +#### For ROS (take Noetic as an example): +```shell +source /opt/ros/noetic/setup.sh +./build.sh ROS1 +``` + +#### For ROS2 Foxy: +```shell +source /opt/ros/foxy/setup.sh +./build.sh ROS2 +``` + +#### For ROS2 Humble: +```shell +source /opt/ros/humble/setup.sh +./build.sh humble +``` + +### 2.4 Run Livox ROS Driver 2: + +#### For ROS: + +```shell +source ../../devel/setup.sh +roslaunch livox_ros_driver2 [launch file] +``` + +in which, + +* **livox_ros_driver2** : is the ROS package name of Livox ROS Driver 2; +* **[launch file]** : is the ROS launch file you want to use; the 'launch_ROS1' folder contains several launch samples for your reference; + +An rviz launch example for HAP LiDAR would be: + +```shell +roslaunch livox_ros_driver2 rviz_HAP.launch +``` + +#### For ROS2: +```shell +source ../../install/setup.sh +ros2 launch livox_ros_driver2 [launch file] +``` + +in which, + +* **[launch file]** : is the ROS2 launch file you want to use; the 'launch_ROS2' folder contains several launch samples for your reference. + +A rviz launch example for HAP LiDAR would be: + +```shell +ros2 launch livox_ros_driver2 rviz_HAP_launch.py +``` + +## 3. Launch file and livox_ros_driver2 internal parameter configuration instructions + +### 3.1 Launch file configuration instructions + +Launch files of ROS are in the "ws_livox/src/livox_ros_driver2/launch_ROS1" directory and launch files of ROS2 are in the "ws_livox/src/livox_ros_driver2/launch_ROS2" directory. Different launch files have different configuration parameter values and are used in different scenarios: + +| launch file name | Description | +| ------------------------- | ------------------------------------------------------------ | +| rviz_HAP.launch | Connect to HAP LiDAR device
Publish pointcloud2 format data
Autoload rviz | +| msg_HAP.launch | Connect to HAP LiDAR device
Publish livox customized pointcloud data| +| rviz_MID360.launch | Connect to MID360 LiDAR device
Publish pointcloud2 format data
Autoload rviz| +| msg_MID360.launch | Connect to MID360 LiDAR device
Publish livox customized pointcloud data | +| rviz_mixed.launch | Connect to HAP and MID360 LiDAR device
Publish pointcloud2 format data
Autoload rviz| +| msg_mixed.launch | Connect to HAP and MID360 LiDAR device
Publish livox customized pointcloud data | + +### 3.2 Livox ros driver 2 internal main parameter configuration instructions + +All internal parameters of Livox_ros_driver2 are in the launch file. Below are detailed descriptions of the three commonly used parameters : + +| Parameter | Detailed description | Default | +| ------------ | ------------------------------------------------------------ | ------- | +| publish_freq | Set the frequency of point cloud publish
Floating-point data type, recommended values 5.0, 10.0, 20.0, 50.0, etc. The maximum publish frequency is 100.0 Hz.| 10.0 | +| multi_topic | If the LiDAR device has an independent topic to publish pointcloud data
0 -- All LiDAR devices use the same topic to publish pointcloud data
1 -- Each LiDAR device has its own topic to publish point cloud data | 0 | +| xfer_format | Set pointcloud format
0 -- Livox pointcloud2(PointXYZRTLT) pointcloud format
1 -- Livox customized pointcloud format
2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS) | 0 | + + **Note :** + + Other parameters not mentioned in this table are not suggested to be changed unless fully understood. + +    ***Livox_ros_driver2 pointcloud data detailed description :*** + +1. Livox pointcloud2 (PointXYZRTLT) point cloud format, as follows : + +```c +float32 x # X axis, unit:m +float32 y # Y axis, unit:m +float32 z # Z axis, unit:m +float32 intensity # the value is reflectivity, 0.0~255.0 +uint8 tag # livox tag +uint8 line # laser number in lidar +float64 timestamp # Timestamp of point +``` + **Note :** + + The number of points in the frame may be different, but each point provides a timestamp. + +2. Livox customized data package format, as follows : + +```c +std_msgs/Header header # ROS standard message header +uint64 timebase # The time of first point +uint32 point_num # Total number of pointclouds +uint8 lidar_id # Lidar device id number +uint8[3] rsvd # Reserved use +CustomPoint[] points # Pointcloud data +``` + +    Customized Point Cloud (CustomPoint) format in the above customized data package : + +```c +uint32 offset_time # offset time relative to the base time +float32 x # X axis, unit:m +float32 y # Y axis, unit:m +float32 z # Z axis, unit:m +uint8 reflectivity # reflectivity, 0~255 +uint8 tag # livox tag +uint8 line # laser number in lidar +``` + +3. The standard pointcloud2 (pcl :: PointXYZI) format in the PCL library (only ROS can publish): + +    Please refer to the pcl :: PointXYZI data structure in the point_types.hpp file of the PCL library. + +## 4. LiDAR config + +LiDAR Configurations (such as ip, port, data type... etc.) can be set via a json-style config file. Config files for single HAP, Mid360 and mixed-LiDARs are in the "config" folder. The parameter naming *'user_config_path'* in launch files indicates such json file path. + +1. Follow is a configuration example for HAP LiDAR (located in config/HAP_config.json): + +```json +{ + "lidar_summary_info" : { + "lidar_type": 8 # protocol type index, please don't revise this value + }, + "HAP": { + "device_type" : "HAP", + "lidar_ipaddr": "", + "lidar_net_info" : { + "cmd_data_port": 56000, # command port + "push_msg_port": 0, + "point_data_port": 57000, + "imu_data_port": 58000, + "log_data_port": 59000 + }, + "host_net_info" : { + "cmd_data_ip" : "192.168.1.5", # host ip (it can be revised) + "cmd_data_port": 56000, + "push_msg_ip": "", + "push_msg_port": 0, + "point_data_ip": "192.168.1.5", # host ip + "point_data_port": 57000, + "imu_data_ip" : "192.168.1.5", # host ip + "imu_data_port": 58000, + "log_data_ip" : "", + "log_data_port": 59000 + } + }, + "lidar_configs" : [ + { + "ip" : "192.168.1.100", # ip of the LiDAR you want to config + "pcl_data_type" : 1, + "pattern_mode" : 0, + "blind_spot_set" : 50, + "extrinsic_parameter" : { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} +``` + +The parameter attributes in the above json file are described in the following table : + +**LiDAR configuration parameter** +| Parameter | Type | Description | Default | +| :------------------------- | ------- | ------------------------------------------------------------ | --------------- | +| ip | String | Ip of the LiDAR you want to config | 192.168.1.100 | +| pcl_data_type | Int | Choose the resolution of the point cloud data to send
1 -- Cartesian coordinate data (32 bits)
2 -- Cartesian coordinate data (16 bits)
3 --Spherical coordinate data| 1 | +| pattern_mode | Int | Space scan pattern
0 -- non-repeating scanning pattern mode
1 -- repeating scanning pattern mode
2 -- repeating scanning pattern mode (low scanning rate) | 0 | +| blind_spot_set (Only for HAP LiDAR) | Int | Set blind spot
Range from 50 cm to 200 cm | 50 | +| extrinsic_parameter | | Set extrinsic parameter
The data types of "roll" "picth" "yaw" are float
The data types of "x" "y" "z" are int
| + +For more infomation about the HAP config, please refer to: +[HAP Config File Description](https://github.com/Livox-SDK/Livox-SDK2/wiki/hap-config-file-description) + +2. When connecting multiple LiDARs, add objects corresponding to different LiDARs to the "lidar_configs" array. Examples of mixed-LiDARs config file contents are as follows : + +```json +{ + "lidar_summary_info" : { + "lidar_type": 8 # protocol type index, please don't revise this value + }, + "HAP": { + "lidar_net_info" : { # HAP ports, please don't revise these values + "cmd_data_port": 56000, # HAP command port + "push_msg_port": 0, + "point_data_port": 57000, + "imu_data_port": 58000, + "log_data_port": 59000 + }, + "host_net_info" : { + "cmd_data_ip" : "192.168.1.5", # host ip + "cmd_data_port": 56000, + "push_msg_ip": "", + "push_msg_port": 0, + "point_data_ip": "192.168.1.5", # host ip + "point_data_port": 57000, + "imu_data_ip" : "192.168.1.5", # host ip + "imu_data_port": 58000, + "log_data_ip" : "", + "log_data_port": 59000 + } + }, + "MID360": { + "lidar_net_info" : { # Mid360 ports, please don't revise these values + "cmd_data_port": 56100, # Mid360 command port + "push_msg_port": 56200, + "point_data_port": 56300, + "imu_data_port": 56400, + "log_data_port": 56500 + }, + "host_net_info" : { + "cmd_data_ip" : "192.168.1.5", # host ip + "cmd_data_port": 56101, + "push_msg_ip": "192.168.1.5", # host ip + "push_msg_port": 56201, + "point_data_ip": "192.168.1.5", # host ip + "point_data_port": 56301, + "imu_data_ip" : "192.168.1.5", # host ip + "imu_data_port": 56401, + "log_data_ip" : "", + "log_data_port": 56501 + } + }, + "lidar_configs" : [ + { + "ip" : "192.168.1.100", # ip of the HAP you want to config + "pcl_data_type" : 1, + "pattern_mode" : 0, + "blind_spot_set" : 50, + "extrinsic_parameter" : { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + }, + { + "ip" : "192.168.1.12", # ip of the Mid360 you want to config + "pcl_data_type" : 1, + "pattern_mode" : 0, + "extrinsic_parameter" : { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} +``` +3. when multiple nics on the host connect to multiple LiDARs, you need to add objects corresponding to different LiDARs to the lidar_configs array. Run different luanch files separately, and the following is an example of mixing lidar configuration file contents: + +**MID360_config1:** +```json +{ + "lidar_summary_info" : { + "lidar_type": 8 # protocol type index，please don't revise this value + }, + "MID360": { + "lidar_net_info": { + "cmd_data_port": 56100, # command port + "push_msg_port": 56200, + "point_data_port": 56300, + "imu_data_port": 56400, + "log_data_port": 56500 + }, + "host_net_info": [ + { + "lidar_ip": ["192.168.1.100"], # Lidar ip + "host_ip": "192.168.1.5", # host ip + "cmd_data_port": 56101, + "push_msg_port": 56201, + "point_data_port": 56301, + "imu_data_port": 56401, + "log_data_port": 56501 + } + ] + }, + "lidar_configs": [ + { + "ip": "192.168.1.100", # ip of the LiDAR you want to config + "pcl_data_type": 1, + "pattern_mode": 0, + "extrinsic_parameter": { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} +``` +**MID360_config2:** +```json +{ + "lidar_summary_info" : { + "lidar_type": 8 # protocol type index，please don't revise this value + }, + "MID360": { + "lidar_net_info": { + "cmd_data_port": 56100, # command port + "push_msg_port": 56200, + "point_data_port": 56300, + "imu_data_port": 56400, + "log_data_port": 56500 + }, + "host_net_info": [ + { + "lidar_ip": ["192.168.2.100"], # Lidar ip + "host_ip": "192.168.2.5", # host ip + "cmd_data_port": 56101, + "push_msg_port": 56201, + "point_data_port": 56301, + "imu_data_port": 56401, + "log_data_port": 56501 + } + ] + }, + "lidar_configs": [ + { + "ip": "192.168.2.100", # ip of the LiDAR you want to config + "pcl_data_type": 1, + "pattern_mode": 0, + "extrinsic_parameter": { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} +``` +**Launch1:** +``` + + + + + + + + + + + + + + + + + + + + + + + + + # Mid360 MID360_config1 name + + + + + + + + + + + + + + + +``` +**Launch2:** +``` + + + + + + + + + + + + + + + + + + + + + + + + + # Mid360 MID360_config2 name + + + + + + + + + + + + + + + + +``` + +## 5. Supported LiDAR list + +* HAP +* Mid360 +* (more types are comming soon...) + +## 6. FAQ + +### 6.1 launch with "livox_lidar_rviz_HAP.launch" but no point cloud display on the grid? + +Please check the "Global Options - Fixed Frame" field in the RViz "Display" pannel. Set the field value to "livox_frame" and check the "PointCloud2" option in the pannel. + +### 6.2 launch with command "ros2 launch livox_lidar_rviz_HAP_launch.py" but cannot open shared object file "liblivox_sdk_shared.so" ? + +Please add '/usr/local/lib' to the env LD_LIBRARY_PATH. + +* If you want to add to current terminal: + + ```shell + export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib + ``` + +* If you want to add to current user: + + ```shell + vim ~/.bashrc + export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib + source ~/.bashrc + ``` diff --git a/src/livox_ros_driver2/build.sh b/src/livox_ros_driver2/build.sh new file mode 100755 index 0000000..3ce316c --- /dev/null +++ b/src/livox_ros_driver2/build.sh @@ -0,0 +1,70 @@ +#!/bin/bash + +readonly VERSION_ROS1="ROS1" +readonly VERSION_ROS2="ROS2" +readonly VERSION_HUMBLE="humble" + +pushd `pwd` > /dev/null +cd `dirname $0` +echo "Working Path: "`pwd` + +ROS_VERSION="" +ROS_HUMBLE="" + +# Set working ROS version +if [ "$1" = "ROS2" ]; then + ROS_VERSION=${VERSION_ROS2} +elif [ "$1" = "humble" ]; then + ROS_VERSION=${VERSION_ROS2} + ROS_HUMBLE=${VERSION_HUMBLE} +elif [ "$1" = "ROS1" ]; then + ROS_VERSION=${VERSION_ROS1} +else + echo "Invalid Argument" + exit +fi +echo "ROS version is: "$ROS_VERSION + +# clear `build/` folder. +# TODO: Do not clear these folders, if the last build is based on the same ROS version. +rm -rf ../../build/ +rm -rf ../../devel/ +rm -rf ../../install/ +# clear src/CMakeLists.txt if it exists. +if [ -f ../CMakeLists.txt ]; then + rm -f ../CMakeLists.txt +fi + +# exit + +# substitute the files/folders: CMakeList.txt, package.xml(s) +if [ ${ROS_VERSION} = ${VERSION_ROS1} ]; then + if [ -f package.xml ]; then + rm package.xml + fi + cp -f package_ROS1.xml package.xml +elif [ ${ROS_VERSION} = ${VERSION_ROS2} ]; then + if [ -f package.xml ]; then + rm package.xml + fi + cp -f package_ROS2.xml package.xml + cp -rf launch_ROS2/ launch/ +fi + +# build +pushd `pwd` > /dev/null +if [ $ROS_VERSION = ${VERSION_ROS1} ]; then + cd ../../ + catkin_make -DROS_EDITION=${VERSION_ROS1} +elif [ $ROS_VERSION = ${VERSION_ROS2} ]; then + cd ../../ + colcon build --cmake-args -DROS_EDITION=${VERSION_ROS2} -DHUMBLE_ROS=${ROS_HUMBLE} +fi +popd > /dev/null + +# remove the substituted folders/files +if [ $ROS_VERSION = ${VERSION_ROS2} ]; then + rm -rf launch/ +fi + +popd > /dev/null diff --git a/src/livox_ros_driver2/cmake/version.cmake b/src/livox_ros_driver2/cmake/version.cmake new file mode 100644 index 0000000..908c19b --- /dev/null +++ b/src/livox_ros_driver2/cmake/version.cmake @@ -0,0 +1,16 @@ +#--------------------------------------------------------------------------------------- +# Get livox_ros_driver2 version from include/livox_ros_driver2.h +#--------------------------------------------------------------------------------------- +file(READ "${CMAKE_CURRENT_LIST_DIR}/../src/include/livox_ros_driver2.h" LIVOX_ROS_DRIVER2_VERSION_FILE) +string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MAJOR ([0-9]+)" _ "${LIVOX_ROS_DRIVER2_VERSION_FILE}") +set(ver_major ${CMAKE_MATCH_1}) + +string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MINOR ([0-9]+)" _ "${LIVOX_ROS_DRIVER2_VERSION_FILE}") +set(ver_minor ${CMAKE_MATCH_1}) +string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_PATCH ([0-9]+)" _ "${LIVOX_ROS_DRIVER2_VERSION_FILE}") +set(ver_patch ${CMAKE_MATCH_1}) + +if (NOT DEFINED ver_major OR NOT DEFINED ver_minor OR NOT DEFINED ver_patch) + message(FATAL_ERROR "Could not extract valid version from include/livox_ros_driver2.h") +endif() +set (LIVOX_ROS_DRIVER2_VERSION "${ver_major}.${ver_minor}.${ver_patch}") diff --git a/src/livox_ros_driver2/config/HAP_config.json b/src/livox_ros_driver2/config/HAP_config.json new file mode 100644 index 0000000..090ab9f --- /dev/null +++ b/src/livox_ros_driver2/config/HAP_config.json @@ -0,0 +1,42 @@ +{ + "lidar_summary_info" : { + "lidar_type": 8 + }, + "HAP": { + "lidar_net_info" : { + "cmd_data_port": 56000, + "push_msg_port": 0, + "point_data_port": 57000, + "imu_data_port": 58000, + "log_data_port": 59000 + }, + "host_net_info" : { + "cmd_data_ip" : "192.168.1.5", + "cmd_data_port": 56000, + "push_msg_ip": "", + "push_msg_port": 0, + "point_data_ip": "192.168.1.5", + "point_data_port": 57000, + "imu_data_ip" : "192.168.1.5", + "imu_data_port": 58000, + "log_data_ip" : "", + "log_data_port": 59000 + } + }, + "lidar_configs" : [ + { + "ip" : "192.168.1.100", + "pcl_data_type" : 1, + "pattern_mode" : 0, + "extrinsic_parameter" : { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} + diff --git a/src/livox_ros_driver2/config/MID360_config.json b/src/livox_ros_driver2/config/MID360_config.json new file mode 100644 index 0000000..2e0a371 --- /dev/null +++ b/src/livox_ros_driver2/config/MID360_config.json @@ -0,0 +1,41 @@ +{ + "lidar_summary_info": { + "lidar_type": 8 + }, + "MID360": { + "lidar_net_info": { + "cmd_data_port": 56100, + "push_msg_port": 56200, + "point_data_port": 56300, + "imu_data_port": 56400, + "log_data_port": 56500 + }, + "host_net_info": { + "cmd_data_ip": "192.168.1.5", + "cmd_data_port": 56101, + "push_msg_ip": "192.168.1.5", + "push_msg_port": 56201, + "point_data_ip": "192.168.1.5", + "point_data_port": 56301, + "imu_data_ip": "192.168.1.5", + "imu_data_port": 56401, + "log_data_ip": "", + "log_data_port": 56501 + } + }, + "lidar_configs": [ + { + "ip": "192.168.1.192", + "pcl_data_type": 1, + "pattern_mode": 0, + "extrinsic_parameter": { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} \ No newline at end of file diff --git a/src/livox_ros_driver2/config/MID360s_config.json b/src/livox_ros_driver2/config/MID360s_config.json new file mode 100644 index 0000000..100d595 --- /dev/null +++ b/src/livox_ros_driver2/config/MID360s_config.json @@ -0,0 +1,41 @@ +{ + "lidar_summary_info": { + "lidar_type": 8 + }, + "Mid360s": { + "lidar_net_info": { + "cmd_data_port": 56100, + "push_msg_port": 56200, + "point_data_port": 56300, + "imu_data_port": 56400, + "log_data_port": 56500 + }, + "host_net_info": [ + { + "host_ip": "192.168.1.50", + "cmd_data_port": 56101, + "push_msg_port": 56201, + "point_data_port": 56301, + "imu_data_port": 56401, + "log_data_port": 56501 + } + ] + }, + "lidar_configs": [ + { + "_comment": "192, 163", + "ip": "192.168.1.192", + "pcl_data_type": 1, + "pattern_mode": 0, + "timestamp_type": 0, + "extrinsic_parameter": { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} diff --git a/src/livox_ros_driver2/config/display_point_cloud_ROS1.rviz b/src/livox_ros_driver2/config/display_point_cloud_ROS1.rviz new file mode 100644 index 0000000..6239170 --- /dev/null +++ b/src/livox_ros_driver2/config/display_point_cloud_ROS1.rviz @@ -0,0 +1,172 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /PointCloud21 + Splitter Ratio: 0.500694990158081 + Tree Height: 728 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.4569999873638153 + Min Value: -0.367000013589859 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 1 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.004999999888241291 + Style: Points + Topic: /livox/lidar + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: livox_frame + Frame Rate: 40 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 25.80008888244629 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.2672550082206726 + Y: 0.061853598803281784 + Z: 0.15087400376796722 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5597995519638062 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.065610408782959 + Saved: + - Class: rviz/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 1.1103999614715576 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.5703970193862915 +Window Geometry: + Displays: + collapsed: false + Height: 1025 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001c400000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005730000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 diff --git a/src/livox_ros_driver2/config/display_point_cloud_ROS2.rviz b/src/livox_ros_driver2/config/display_point_cloud_ROS2.rviz new file mode 100644 index 0000000..c044254 --- /dev/null +++ b/src/livox_ros_driver2/config/display_point_cloud_ROS2.rviz @@ -0,0 +1,137 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 796 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0.20000000298023224 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 150 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /livox/lidar + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: livox_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 14.215983390808105 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9503982067108154 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.6603963375091553 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1025 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015f000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c3000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1853 + X: 67 + Y: 27 diff --git a/src/livox_ros_driver2/config/mixed_HAP_MID360_config.json b/src/livox_ros_driver2/config/mixed_HAP_MID360_config.json new file mode 100644 index 0000000..0539b1e --- /dev/null +++ b/src/livox_ros_driver2/config/mixed_HAP_MID360_config.json @@ -0,0 +1,76 @@ +{ + "lidar_summary_info" : { + "lidar_type": 8 + }, + "HAP": { + "lidar_net_info" : { + "cmd_data_port": 56000, + "push_msg_port": 0, + "point_data_port": 57000, + "imu_data_port": 58000, + "log_data_port": 59000 + }, + "host_net_info" : { + "cmd_data_ip" : "192.168.1.5", + "cmd_data_port": 56000, + "push_msg_ip": "", + "push_msg_port": 0, + "point_data_ip": "192.168.1.5", + "point_data_port": 57000, + "imu_data_ip" : "192.168.1.5", + "imu_data_port": 58000, + "log_data_ip" : "", + "log_data_port": 59000 + } + }, + "MID360": { + "lidar_net_info" : { + "cmd_data_port": 56100, + "push_msg_port": 56200, + "point_data_port": 56300, + "imu_data_port": 56400, + "log_data_port": 56500 + }, + "host_net_info" : { + "cmd_data_ip" : "192.168.1.5", + "cmd_data_port": 56101, + "push_msg_ip": "192.168.1.5", + "push_msg_port": 56201, + "point_data_ip": "192.168.1.5", + "point_data_port": 56301, + "imu_data_ip" : "192.168.1.5", + "imu_data_port": 56401, + "log_data_ip" : "", + "log_data_port": 56501 + } + }, + "lidar_configs" : [ + { + "ip" : "192.168.1.100", + "pcl_data_type" : 1, + "pattern_mode" : 0, + "extrinsic_parameter" : { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + }, + { + "ip" : "192.168.1.12", + "pcl_data_type" : 1, + "pattern_mode" : 0, + "extrinsic_parameter" : { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} + diff --git a/src/livox_ros_driver2/launch_ROS1/msg_HAP.launch b/src/livox_ros_driver2/launch_ROS1/msg_HAP.launch new file mode 100644 index 0000000..005e038 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS1/msg_HAP.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/livox_ros_driver2/launch_ROS1/msg_MID360.launch b/src/livox_ros_driver2/launch_ROS1/msg_MID360.launch new file mode 100644 index 0000000..1e08745 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS1/msg_MID360.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/livox_ros_driver2/launch_ROS1/msg_MID360s.launch b/src/livox_ros_driver2/launch_ROS1/msg_MID360s.launch new file mode 100644 index 0000000..3559f45 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS1/msg_MID360s.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/livox_ros_driver2/launch_ROS1/msg_mixed.launch b/src/livox_ros_driver2/launch_ROS1/msg_mixed.launch new file mode 100644 index 0000000..bd4fe79 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS1/msg_mixed.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/livox_ros_driver2/launch_ROS1/rviz_HAP.launch b/src/livox_ros_driver2/launch_ROS1/rviz_HAP.launch new file mode 100644 index 0000000..572098a --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS1/rviz_HAP.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/livox_ros_driver2/launch_ROS1/rviz_MID360.launch b/src/livox_ros_driver2/launch_ROS1/rviz_MID360.launch new file mode 100644 index 0000000..44c8bff --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS1/rviz_MID360.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/livox_ros_driver2/launch_ROS1/rviz_MID360s.launch b/src/livox_ros_driver2/launch_ROS1/rviz_MID360s.launch new file mode 100644 index 0000000..3e577e7 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS1/rviz_MID360s.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/livox_ros_driver2/launch_ROS1/rviz_mixed.launch b/src/livox_ros_driver2/launch_ROS1/rviz_mixed.launch new file mode 100644 index 0000000..d1851e8 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS1/rviz_mixed.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/livox_ros_driver2/launch_ROS2/mid360s_calib_launch.py b/src/livox_ros_driver2/launch_ROS2/mid360s_calib_launch.py new file mode 100644 index 0000000..564806e --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS2/mid360s_calib_launch.py @@ -0,0 +1,44 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +# xfer_format=0 : PointCloud2 (캘리브레이션 용도) +# direct_visual_lidar_calibration / capture_single.py 사용 시 + +xfer_format = 0 # PointCloud2 (PointXYZRTL) +multi_topic = 0 +data_src = 0 +publish_freq = 10.0 +output_type = 0 +frame_id = 'livox_frame' +lvx_file_path = '/home/livox/livox_test.lvx' +cmdline_bd_code = 'livox0000000001' + +cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' +cur_config_path = cur_path + '../config' +user_config_path = os.path.join(cur_config_path, 'MID360s_config.json') + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code} +] + + +def generate_launch_description(): + livox_driver = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + parameters=livox_ros2_params + ) + + return LaunchDescription([livox_driver]) diff --git a/src/livox_ros_driver2/launch_ROS2/mid360s_fastlivo_launch.py b/src/livox_ros_driver2/launch_ROS2/mid360s_fastlivo_launch.py new file mode 100644 index 0000000..88a3324 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS2/mid360s_fastlivo_launch.py @@ -0,0 +1,44 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +# xfer_format=1 : CustomMsg (FAST-LIVO2 용도) +# fast_livo 노드 사용 시 + +xfer_format = 1 # Livox CustomMsg +multi_topic = 0 +data_src = 0 +publish_freq = 10.0 +output_type = 0 +frame_id = 'livox_frame' +lvx_file_path = '/home/livox/livox_test.lvx' +cmdline_bd_code = 'livox0000000001' + +cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' +cur_config_path = cur_path + '../config' +user_config_path = os.path.join(cur_config_path, 'MID360s_config.json') + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code} +] + + +def generate_launch_description(): + livox_driver = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + parameters=livox_ros2_params + ) + + return LaunchDescription([livox_driver]) diff --git a/src/livox_ros_driver2/launch_ROS2/msg_HAP_launch.py b/src/livox_ros_driver2/launch_ROS2/msg_HAP_launch.py new file mode 100644 index 0000000..2ff3d66 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS2/msg_HAP_launch.py @@ -0,0 +1,55 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = 'livox_frame' +lvx_file_path = '/home/livox/livox_test.lvx' +cmdline_bd_code = 'livox0000000001' + +cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' +cur_config_path = cur_path + '../config' +rviz_config_path = os.path.join(cur_config_path, 'livox_lidar.rviz') +user_config_path = os.path.join(cur_config_path, 'HAP_config.json') +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code} +] + + +def generate_launch_description(): + livox_driver = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + parameters=livox_ros2_params + ) + + return LaunchDescription([ + livox_driver, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ]) diff --git a/src/livox_ros_driver2/launch_ROS2/msg_MID360_launch.py b/src/livox_ros_driver2/launch_ROS2/msg_MID360_launch.py new file mode 100644 index 0000000..8492c49 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS2/msg_MID360_launch.py @@ -0,0 +1,54 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = 'livox_frame' +lvx_file_path = '/home/livox/livox_test.lvx' +cmdline_bd_code = 'livox0000000001' + +cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' +cur_config_path = cur_path + '../config' +user_config_path = os.path.join(cur_config_path, 'MID360_config.json') +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code} +] + + +def generate_launch_description(): + livox_driver = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + parameters=livox_ros2_params + ) + + return LaunchDescription([ + livox_driver, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ]) diff --git a/src/livox_ros_driver2/launch_ROS2/msg_MID360s_launch.py b/src/livox_ros_driver2/launch_ROS2/msg_MID360s_launch.py new file mode 100644 index 0000000..1c9e4ca --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS2/msg_MID360s_launch.py @@ -0,0 +1,54 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = 'livox_frame' +lvx_file_path = '/home/livox/livox_test.lvx' +cmdline_bd_code = 'livox0000000001' + +cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' +cur_config_path = cur_path + '../config' +user_config_path = os.path.join(cur_config_path, 'MID360s_config.json') +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code} +] + + +def generate_launch_description(): + livox_driver = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + parameters=livox_ros2_params + ) + + return LaunchDescription([ + livox_driver, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ]) diff --git a/src/livox_ros_driver2/launch_ROS2/rviz_HAP_launch.py b/src/livox_ros_driver2/launch_ROS2/rviz_HAP_launch.py new file mode 100644 index 0000000..834a545 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS2/rviz_HAP_launch.py @@ -0,0 +1,63 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = 'livox_frame' +lvx_file_path = '/home/livox/livox_test.lvx' +cmdline_bd_code = 'livox0000000001' + +cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' +cur_config_path = cur_path + '../config' +rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz') +user_config_path = os.path.join(cur_config_path, 'HAP_config.json') +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code} +] + + +def generate_launch_description(): + livox_driver = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + parameters=livox_ros2_params + ) + + livox_rviz = Node( + package='rviz2', + executable='rviz2', + output='screen', + arguments=['--display-config', rviz_config_path] + ) + + return LaunchDescription([ + livox_driver, + livox_rviz, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ]) diff --git a/src/livox_ros_driver2/launch_ROS2/rviz_MID360_launch.py b/src/livox_ros_driver2/launch_ROS2/rviz_MID360_launch.py new file mode 100644 index 0000000..31d3480 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS2/rviz_MID360_launch.py @@ -0,0 +1,63 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = 'livox_frame' +lvx_file_path = '/home/livox/livox_test.lvx' +cmdline_bd_code = 'livox0000000001' + +cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' +cur_config_path = cur_path + '../config' +rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz') +user_config_path = os.path.join(cur_config_path, 'MID360_config.json') +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code} +] + + +def generate_launch_description(): + livox_driver = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + parameters=livox_ros2_params + ) + + livox_rviz = Node( + package='rviz2', + executable='rviz2', + output='screen', + arguments=['--display-config', rviz_config_path] + ) + + return LaunchDescription([ + livox_driver, + livox_rviz, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ]) diff --git a/src/livox_ros_driver2/launch_ROS2/rviz_MID360s_launch.py b/src/livox_ros_driver2/launch_ROS2/rviz_MID360s_launch.py new file mode 100644 index 0000000..532900f --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS2/rviz_MID360s_launch.py @@ -0,0 +1,63 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format (FAST-LIVO2 requires CustomMsg) +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 50.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = 'livox_frame' +lvx_file_path = '/home/livox/livox_test.lvx' +cmdline_bd_code = 'livox0000000001' + +cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' +cur_config_path = cur_path + '../config' +rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz') +user_config_path = os.path.join(cur_config_path, 'MID360s_config.json') +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code} +] + + +def generate_launch_description(): + livox_driver = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + parameters=livox_ros2_params + ) + + livox_rviz = Node( + package='rviz2', + executable='rviz2', + output='screen', + arguments=['--display-config', rviz_config_path] + ) + + return LaunchDescription([ + livox_driver, + livox_rviz, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ]) diff --git a/src/livox_ros_driver2/launch_ROS2/rviz_mixed.py b/src/livox_ros_driver2/launch_ROS2/rviz_mixed.py new file mode 100644 index 0000000..46a39c7 --- /dev/null +++ b/src/livox_ros_driver2/launch_ROS2/rviz_mixed.py @@ -0,0 +1,63 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = 'livox_frame' +lvx_file_path = '/home/livox/livox_test.lvx' +cmdline_bd_code = 'livox0000000001' + +cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' +cur_config_path = cur_path + '../config' +rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz') +user_config_path = os.path.join(cur_config_path, 'mixed_HAP_MID360_config.json') +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code} +] + + +def generate_launch_description(): + livox_driver = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + parameters=livox_ros2_params + ) + + livox_rviz = Node( + package='rviz2', + executable='rviz2', + output='screen', + arguments=['--display-config', rviz_config_path] + ) + + return LaunchDescription([ + livox_driver, + livox_rviz, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ]) diff --git a/src/livox_ros_driver2/msg/CustomMsg.msg b/src/livox_ros_driver2/msg/CustomMsg.msg new file mode 100644 index 0000000..91f044b --- /dev/null +++ b/src/livox_ros_driver2/msg/CustomMsg.msg @@ -0,0 +1,9 @@ +# Livox publish pointcloud msg format. + +std_msgs/Header header # ROS standard message header +uint64 timebase # The time of first point +uint32 point_num # Total number of pointclouds +uint8 lidar_id # Lidar device id number +uint8[3] rsvd # Reserved use +CustomPoint[] points # Pointcloud data + diff --git a/src/livox_ros_driver2/msg/CustomPoint.msg b/src/livox_ros_driver2/msg/CustomPoint.msg new file mode 100644 index 0000000..34719b2 --- /dev/null +++ b/src/livox_ros_driver2/msg/CustomPoint.msg @@ -0,0 +1,10 @@ +# Livox costom pointcloud format. + +uint32 offset_time # offset time relative to the base time +float32 x # X axis, unit:m +float32 y # Y axis, unit:m +float32 z # Z axis, unit:m +uint8 reflectivity # reflectivity, 0~255 +uint8 tag # livox tag +uint8 line # laser number in lidar + diff --git a/src/livox_ros_driver2/package_ROS1.xml b/src/livox_ros_driver2/package_ROS1.xml new file mode 100644 index 0000000..9af2178 --- /dev/null +++ b/src/livox_ros_driver2/package_ROS1.xml @@ -0,0 +1,82 @@ + + + livox_ros_driver2 + 1.0.0 + The ROS device driver for Livox 3D LiDARs + + + + + Livox Dev Team + + + + + + MIT + + + + + + + + + + + + + Livox Dev Team + + + + + + + + + + + + + + + + + + + + + + catkin + + roscpp + rospy + std_msgs + message_generation + rosbag + pcl_ros + + roscpp + rospy + std_msgs + rosbag + pcl_ros + + roscpp + rospy + std_msgs + message_runtime + rosbag + pcl_ros + + sensor_msgs + git + apr + + + + + + + diff --git a/src/livox_ros_driver2/package_ROS2.xml b/src/livox_ros_driver2/package_ROS2.xml new file mode 100644 index 0000000..96f5762 --- /dev/null +++ b/src/livox_ros_driver2/package_ROS2.xml @@ -0,0 +1,35 @@ + + + + livox_ros_driver2 + 1.0.0 + The ROS device driver for Livox 3D LiDARs, for ROS2 + feng + MIT + + ament_cmake_auto + rosidl_default_generators + rosidl_interface_packages + + rclcpp + rclcpp_components + std_msgs + sensor_msgs + rcutils + pcl_conversions + rcl_interfaces + libpcl-all-dev + + rosbag2 + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + git + apr + + + ament_cmake + + diff --git a/src/livox_ros_driver2/src/call_back/lidar_common_callback.cpp b/src/livox_ros_driver2/src/call_back/lidar_common_callback.cpp new file mode 100644 index 0000000..43efb24 --- /dev/null +++ b/src/livox_ros_driver2/src/call_back/lidar_common_callback.cpp @@ -0,0 +1,73 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "lidar_common_callback.h" + +#include "../lds_lidar.h" + +#include + +namespace livox_ros { + +void LidarCommonCallback::OnLidarPointClounCb(PointFrame* frame, void* client_data) { + if (frame == nullptr) { + printf("LidarPointCloudCb frame is nullptr.\n"); + return; + } + + if (client_data == nullptr) { + printf("Lidar point cloud cb failed, client data is nullptr.\n"); + return; + } + + if (frame->lidar_num ==0) { + printf("LidarPointCloudCb lidar_num:%u.\n", frame->lidar_num); + return; + } + + LdsLidar *lds_lidar = static_cast(client_data); + + //printf("Lidar point cloud, lidar_num:%u.\n", frame->lidar_num); + + lds_lidar->StoragePointData(frame); +} + +void LidarCommonCallback::LidarImuDataCallback(ImuData* imu_data, void *client_data) { + if (imu_data == nullptr) { + printf("Imu data is nullptr.\n"); + return; + } + if (client_data == nullptr) { + printf("Lidar point cloud cb failed, client data is nullptr.\n"); + return; + } + + LdsLidar *lds_lidar = static_cast(client_data); + lds_lidar->StorageImuData(imu_data); +} + +} // namespace livox_ros + + + diff --git a/src/livox_ros_driver2/src/call_back/lidar_common_callback.h b/src/livox_ros_driver2/src/call_back/lidar_common_callback.h new file mode 100644 index 0000000..4ff6803 --- /dev/null +++ b/src/livox_ros_driver2/src/call_back/lidar_common_callback.h @@ -0,0 +1,40 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_ +#define LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_ + +#include "comm/comm.h" + +namespace livox_ros { + +class LidarCommonCallback { + public: + static void OnLidarPointClounCb(PointFrame* frame, void* client_data); + static void LidarImuDataCallback(ImuData* imu_data, void *client_data); +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_ diff --git a/src/livox_ros_driver2/src/call_back/livox_lidar_callback.cpp b/src/livox_ros_driver2/src/call_back/livox_lidar_callback.cpp new file mode 100644 index 0000000..8b68d16 --- /dev/null +++ b/src/livox_ros_driver2/src/call_back/livox_lidar_callback.cpp @@ -0,0 +1,334 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "livox_lidar_callback.h" + +#include "livox_lidar_api.h" +#include +#include +#include + +namespace livox_ros { + +void LivoxLidarCallback::LidarInfoChangeCallback(const uint32_t handle, + const LivoxLidarInfo* info, + void* client_data) { + if (client_data == nullptr) { + std::cout << "lidar info change callback failed, client data is nullptr" << std::endl; + return; + } + LdsLidar* lds_lidar = static_cast(client_data); + + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + if (lidar_device == nullptr) { + std::cout << "found lidar not defined in the user-defined config, ip: " << IpNumToString(handle) << std::endl; + // add lidar device + uint8_t index = 0; + int8_t ret = lds_lidar->cache_index_.GetFreeIndex(kLivoxLidarType, handle, index); + if (ret != 0) { + std::cout << "failed to add lidar device, lidar ip: " << IpNumToString(handle) << std::endl; + return; + } + LidarDevice *p_lidar = &(lds_lidar->lidars_[index]); + p_lidar->lidar_type = kLivoxLidarType; + } else { + // set the lidar according to the user-defined config + const UserLivoxLidarConfig& config = lidar_device->livox_config; + + // lock for modify the lidar device set_bits + { + std::lock_guard lock(lds_lidar->config_mutex_); + if (config.pcl_data_type != -1 ) { + lidar_device->livox_config.set_bits |= kConfigDataType; + SetLivoxLidarPclDataType(handle, static_cast(config.pcl_data_type), + LivoxLidarCallback::SetDataTypeCallback, lds_lidar); + std::cout << "set pcl data type, handle: " << handle << ", data type: " + << static_cast(config.pcl_data_type) << std::endl; + } + if (config.pattern_mode != -1) { + lidar_device->livox_config.set_bits |= kConfigScanPattern; + SetLivoxLidarScanPattern(handle, static_cast(config.pattern_mode), + LivoxLidarCallback::SetPatternModeCallback, lds_lidar); + std::cout << "set scan pattern, handle: " << handle << ", scan pattern: " + << static_cast(config.pattern_mode) << std::endl; + } + if (config.blind_spot_set != -1) { + lidar_device->livox_config.set_bits |= kConfigBlindSpot; + SetLivoxLidarBlindSpot(handle, config.blind_spot_set, + LivoxLidarCallback::SetBlindSpotCallback, lds_lidar); + + std::cout << "set blind spot, handle: " << handle << ", blind spot distance: " + << config.blind_spot_set << std::endl; + } + if (config.dual_emit_en != -1) { + lidar_device->livox_config.set_bits |= kConfigDualEmit; + SetLivoxLidarDualEmit(handle, (config.dual_emit_en == 0 ? false : true), + LivoxLidarCallback::SetDualEmitCallback, lds_lidar); + std::cout << "set dual emit mode, handle: " << handle << ", enable dual emit: " + << static_cast(config.dual_emit_en) << std::endl; + } + } // free lock for set_bits + + // set extrinsic params into lidar + LivoxLidarInstallAttitude attitude { + config.extrinsic_param.roll, + config.extrinsic_param.pitch, + config.extrinsic_param.yaw, + config.extrinsic_param.x, + config.extrinsic_param.y, + config.extrinsic_param.z + }; + SetLivoxLidarInstallAttitude(config.handle, &attitude, + LivoxLidarCallback::SetAttitudeCallback, lds_lidar); + } + + std::cout << "begin to change work mode to 'Normal', handle: " << handle << std::endl; + SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr); + EnableLivoxLidarImuData(handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar); + return; +} + +void LivoxLidarCallback::WorkModeChangedCallback(livox_status status, + uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data) { + if (status != kLivoxLidarStatusSuccess) { + std::cout << "failed to change work mode, handle: " << handle << ", try again..."<< std::endl; + std::this_thread::sleep_for(std::chrono::seconds(1)); + SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr); + return; + } + std::cout << "successfully change work mode, handle: " << handle << std::endl; + return; +} + +void LivoxLidarCallback::SetDataTypeCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data) { + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + if (lidar_device == nullptr) { + std::cout << "failed to set data type since no lidar device found, handle: " + << handle << std::endl; + return; + } + LdsLidar* lds_lidar = static_cast(client_data); + + if (status == kLivoxLidarStatusSuccess) { + std::lock_guard lock(lds_lidar->config_mutex_); + lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDataType)); + if (!lidar_device->livox_config.set_bits) { + lidar_device->connect_state = kConnectStateSampling; + } + std::cout << "successfully set data type, handle: " << handle + << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl; + } else if (status == kLivoxLidarStatusTimeout) { + const UserLivoxLidarConfig& config = lidar_device->livox_config; + SetLivoxLidarPclDataType(handle, static_cast(config.pcl_data_type), + LivoxLidarCallback::SetDataTypeCallback, client_data); + std::cout << "set data type timeout, handle: " << handle + << ", try again..." << std::endl; + } else { + std::cout << "failed to set data type, handle: " << handle + << ", return code: " << response->ret_code + << ", error key: " << response->error_key << std::endl; + } + return; +} + +void LivoxLidarCallback::SetPatternModeCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data) { + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + if (lidar_device == nullptr) { + std::cout << "failed to set pattern mode since no lidar device found, handle: " + << handle << std::endl; + return; + } + LdsLidar* lds_lidar = static_cast(client_data); + + if (status == kLivoxLidarStatusSuccess) { + std::lock_guard lock(lds_lidar->config_mutex_); + lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigScanPattern)); + if (!lidar_device->livox_config.set_bits) { + lidar_device->connect_state = kConnectStateSampling; + } + std::cout << "successfully set pattern mode, handle: " << handle + << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl; + } else if (status == kLivoxLidarStatusTimeout) { + const UserLivoxLidarConfig& config = lidar_device->livox_config; + SetLivoxLidarScanPattern(handle, static_cast(config.pattern_mode), + LivoxLidarCallback::SetPatternModeCallback, client_data); + std::cout << "set pattern mode timeout, handle: " << handle + << ", try again..." << std::endl; + } else { + std::cout << "failed to set pattern mode, handle: " << handle + << ", return code: " << response->ret_code + << ", error key: " << response->error_key << std::endl; + } + return; +} + +void LivoxLidarCallback::SetBlindSpotCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data) { + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + if (lidar_device == nullptr) { + std::cout << "failed to set blind spot since no lidar device found, handle: " + << handle << std::endl; + return; + } + LdsLidar* lds_lidar = static_cast(client_data); + + if (status == kLivoxLidarStatusSuccess) { + std::lock_guard lock(lds_lidar->config_mutex_); + lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigBlindSpot)); + if (!lidar_device->livox_config.set_bits) { + lidar_device->connect_state = kConnectStateSampling; + } + std::cout << "successfully set blind spot, handle: " << handle + << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl; + } else if (status == kLivoxLidarStatusTimeout) { + const UserLivoxLidarConfig& config = lidar_device->livox_config; + SetLivoxLidarBlindSpot(handle, config.blind_spot_set, + LivoxLidarCallback::SetBlindSpotCallback, client_data); + std::cout << "set blind spot timeout, handle: " << handle + << ", try again..." << std::endl; + } else { + std::cout << "failed to set blind spot, handle: " << handle + << ", return code: " << response->ret_code + << ", error key: " << response->error_key << std::endl; + } + return; +} + +void LivoxLidarCallback::SetDualEmitCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data) { + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + if (lidar_device == nullptr) { + std::cout << "failed to set dual emit mode since no lidar device found, handle: " + << handle << std::endl; + return; + } + + LdsLidar* lds_lidar = static_cast(client_data); + if (status == kLivoxLidarStatusSuccess) { + std::lock_guard lock(lds_lidar->config_mutex_); + lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDualEmit)); + if (!lidar_device->livox_config.set_bits) { + lidar_device->connect_state = kConnectStateSampling; + } + std::cout << "successfully set dual emit mode, handle: " << handle + << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl; + } else if (status == kLivoxLidarStatusTimeout) { + const UserLivoxLidarConfig& config = lidar_device->livox_config; + SetLivoxLidarDualEmit(handle, config.dual_emit_en, + LivoxLidarCallback::SetDualEmitCallback, client_data); + std::cout << "set dual emit mode timeout, handle: " << handle + << ", try again..." << std::endl; + } else { + std::cout << "failed to set dual emit mode, handle: " << handle + << ", return code: " << response->ret_code + << ", error key: " << response->error_key << std::endl; + } + return; +} + +void LivoxLidarCallback::SetAttitudeCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data) { + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + if (lidar_device == nullptr) { + std::cout << "failed to set dual emit mode since no lidar device found, handle: " + << handle << std::endl; + return; + } + + LdsLidar* lds_lidar = static_cast(client_data); + if (status == kLivoxLidarStatusSuccess) { + std::cout << "successfully set lidar attitude, ip: " << IpNumToString(handle) << std::endl; + } else if (status == kLivoxLidarStatusTimeout) { + std::cout << "set lidar attitude timeout, ip: " << IpNumToString(handle) + << ", try again..." << std::endl; + const UserLivoxLidarConfig& config = lidar_device->livox_config; + LivoxLidarInstallAttitude attitude { + config.extrinsic_param.roll, + config.extrinsic_param.pitch, + config.extrinsic_param.yaw, + config.extrinsic_param.x, + config.extrinsic_param.y, + config.extrinsic_param.z + }; + SetLivoxLidarInstallAttitude(config.handle, &attitude, + LivoxLidarCallback::SetAttitudeCallback, lds_lidar); + } else { + std::cout << "failed to set lidar attitude, ip: " << IpNumToString(handle) << std::endl; + } +} + +void LivoxLidarCallback::EnableLivoxLidarImuDataCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data) { + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + if (lidar_device == nullptr) { + std::cout << "failed to set pattern mode since no lidar device found, handle: " + << handle << std::endl; + return; + } + LdsLidar* lds_lidar = static_cast(client_data); + + if (response == nullptr) { + std::cout << "failed to get response since no lidar IMU sensor found, handle: " + << handle << std::endl; + return; + } + + if (status == kLivoxLidarStatusSuccess) { + std::cout << "successfully enable Livox Lidar imu, ip: " << IpNumToString(handle) << std::endl; + } else if (status == kLivoxLidarStatusTimeout) { + std::cout << "enable Livox Lidar imu timeout, ip: " << IpNumToString(handle) + << ", try again..." << std::endl; + EnableLivoxLidarImuData(handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar); + } else { + std::cout << "failed to enable Livox Lidar imu, ip: " << IpNumToString(handle) << std::endl; + } +} + +LidarDevice* LivoxLidarCallback::GetLidarDevice(const uint32_t handle, void* client_data) { + if (client_data == nullptr) { + std::cout << "failed to get lidar device, client data is nullptr" << std::endl; + return nullptr; + } + + LdsLidar* lds_lidar = static_cast(client_data); + uint8_t index = 0; + int8_t ret = lds_lidar->cache_index_.GetIndex(kLivoxLidarType, handle, index); + if (ret != 0) { + return nullptr; + } + + return &(lds_lidar->lidars_[index]); +} + +} // namespace livox_ros diff --git a/src/livox_ros_driver2/src/call_back/livox_lidar_callback.h b/src/livox_ros_driver2/src/call_back/livox_lidar_callback.h new file mode 100644 index 0000000..e1e30b2 --- /dev/null +++ b/src/livox_ros_driver2/src/call_back/livox_lidar_callback.h @@ -0,0 +1,69 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_ +#define LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_ + +#include "../lds.h" +#include "../lds_lidar.h" +#include "../comm/comm.h" + + +namespace livox_ros { + +class LivoxLidarCallback { + public: + static void LidarInfoChangeCallback(const uint32_t handle, + const LivoxLidarInfo* info, + void* client_data); + static void WorkModeChangedCallback(livox_status status, + uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data); + static void SetDataTypeCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data); + static void SetPatternModeCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data); + static void SetBlindSpotCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data); + static void SetDualEmitCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data); + static void SetAttitudeCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data); + static void EnableLivoxLidarImuDataCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, + void *client_data); + + private: + static LidarDevice* GetLidarDevice(const uint32_t handle, void* client_data); +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_ diff --git a/src/livox_ros_driver2/src/comm/cache_index.cpp b/src/livox_ros_driver2/src/comm/cache_index.cpp new file mode 100644 index 0000000..33e6500 --- /dev/null +++ b/src/livox_ros_driver2/src/comm/cache_index.cpp @@ -0,0 +1,121 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "cache_index.h" +#include "livox_lidar_def.h" + +namespace livox_ros { + +CacheIndex::CacheIndex() { + std::array index_cache = {0}; + index_cache_.swap(index_cache); +} + +int8_t CacheIndex::GetFreeIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) { + std::string key; + int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key); + if (ret != 0) { + return -1; + } + { + std::lock_guard lock(index_mutex_); + if (map_index_.find(key) != map_index_.end()) { + index = map_index_[key]; + return 0; + } + } + + { + printf("GetFreeIndex key:%s.\n", key.c_str()); + std::lock_guard lock(index_mutex_); + for (size_t i = 0; i < kMaxSourceLidar; ++i) { + if (!index_cache_[i]) { + index_cache_[i] = 1; + map_index_[key] = static_cast(i); + index = static_cast(i); + return 0; + } + } + } + return -1; +} + +int8_t CacheIndex::GenerateIndexKey(const uint8_t livox_lidar_type, const uint32_t handle, std::string& key) { + if (livox_lidar_type == kLivoxLidarType) { + key = "livox_lidar_" + std::to_string(handle); + } else { + printf("Can not generate index, the livox lidar type is unknown, the livox lidar type:%u\n", livox_lidar_type); + return -1; + } + return 0; +} + +int8_t CacheIndex::GetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) { + std::string key; + int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key); + if (ret != 0) { + return -1; + } + + if (map_index_.find(key) != map_index_.end()) { + std::lock_guard lock(index_mutex_); + index = map_index_[key]; + return 0; + } + printf("Can not get index, the livox lidar type:%u, handle:%u\n", livox_lidar_type, handle); + return -1; +} + +int8_t CacheIndex::LvxGetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) { + std::string key; + int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key); + if (ret != 0) { + return -1; + } + + if (map_index_.find(key) != map_index_.end()) { + index = map_index_[key]; + return 0; + } + + return GetFreeIndex(livox_lidar_type, handle, index); +} + +void CacheIndex::ResetIndex(LidarDevice *lidar) { + std::string key; + int8_t ret = GenerateIndexKey(lidar->lidar_type, lidar->handle, key); + if (ret != 0) { + printf("Reset index failed, can not generate index key, lidar type:%u, handle:%u.\n", lidar->lidar_type, lidar->handle); + return; + } + + if (map_index_.find(key) != map_index_.end()) { + uint8_t index = map_index_[key]; + std::lock_guard lock(index_mutex_); + map_index_.erase(key); + index_cache_[index] = 0; + } +} + +} // namespace diff --git a/src/livox_ros_driver2/src/comm/cache_index.h b/src/livox_ros_driver2/src/comm/cache_index.h new file mode 100644 index 0000000..9451eda --- /dev/null +++ b/src/livox_ros_driver2/src/comm/cache_index.h @@ -0,0 +1,54 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_CACHE_INDEX_H_ +#define LIVOX_ROS_DRIVER_CACHE_INDEX_H_ + +#include +#include +#include +#include + +#include "comm/comm.h" + +namespace livox_ros { + +class CacheIndex { + public: + CacheIndex(); + int8_t GetFreeIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index); + int8_t GetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index); + int8_t GenerateIndexKey(const uint8_t livox_lidar_type, const uint32_t handle, std::string& key); + int8_t LvxGetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index); + void ResetIndex(LidarDevice *lidar); + + private: + std::mutex index_mutex_; + std::map map_index_; /* key:handle/slot, val:index */ + std::array index_cache_; +}; + +} // namespace livox_ros + +# endif // LIVOX_ROS_DRIVER_CACHE_INDEX_H_ diff --git a/src/livox_ros_driver2/src/comm/comm.cpp b/src/livox_ros_driver2/src/comm/comm.cpp new file mode 100644 index 0000000..b3490cc --- /dev/null +++ b/src/livox_ros_driver2/src/comm/comm.cpp @@ -0,0 +1,70 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "comm/comm.h" +#include +#include + +namespace livox_ros { + +/** Common function --------------------------------------------------------- */ +bool IsFilePathValid(const char *path_str) { + int str_len = strlen(path_str); + + if ((str_len > kPathStrMinSize) && (str_len < kPathStrMaxSize)) { + return true; + } else { + return false; + } +} + +uint32_t CalculatePacketQueueSize(const double publish_freq) { + uint32_t queue_size = 10; + if (publish_freq > 10.0) { + queue_size = static_cast(publish_freq) + 1; + } + return queue_size; +} + +std::string IpNumToString(uint32_t ip_num) { + struct in_addr ip; + ip.s_addr = ip_num; + return std::string(inet_ntoa(ip)); +} + +uint32_t IpStringToNum(std::string ip_string) { + return static_cast(inet_addr(ip_string.c_str())); +} + +std::string ReplacePeriodByUnderline(std::string str) { + std::size_t pos = str.find("."); + while (pos != std::string::npos) { + str.replace(pos, 1, "_"); + pos = str.find("."); + } + return str; +} + +} // namespace livox_ros + diff --git a/src/livox_ros_driver2/src/comm/comm.h b/src/livox_ros_driver2/src/comm/comm.h new file mode 100644 index 0000000..492b39e --- /dev/null +++ b/src/livox_ros_driver2/src/comm/comm.h @@ -0,0 +1,304 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER2_COMM_H_ +#define LIVOX_ROS_DRIVER2_COMM_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "lidar_imu_data_queue.h" + +namespace livox_ros { + +/** Max lidar data source num */ +const uint8_t kMaxSourceLidar = 32; + + +/** Eth packet relative info parama */ +const uint32_t kMaxPointPerEthPacket = 100; +const uint32_t kMinEthPacketQueueSize = 32; /**< must be 2^n */ +const uint32_t kMaxEthPacketQueueSize = 131072; /**< must be 2^n */ +const uint32_t kImuEthPacketQueueSize = 256; + +/** Max packet length according to Ethernet MTU */ +const uint32_t KEthPacketMaxLength = 1500; +const uint32_t KEthPacketHeaderLength = 18; /**< (sizeof(LivoxEthPacket) - 1) */ +const uint32_t KCartesianPointSize = 13; +const uint32_t KSphericalPointSzie = 9; + +const uint64_t kRosTimeMax = 4294967296000000000; /**< 2^32 * 1000000000ns */ +const int64_t kPacketTimeGap = 1000000; /**< 1ms = 1000000ns */ +/**< the threshold of packet continuous */ +const int64_t kMaxPacketTimeGap = 1700000; +/**< the threshold of device disconect */ +const int64_t kDeviceDisconnectThreshold = 1000000000; +const uint32_t kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */ +const uint32_t kNsTolerantFrameTimeDeviation = 1000000; /**< 1ms = 1000000ns */ +const uint32_t kRatioOfMsToNs = 1000000; /**< 1ms = 1000000ns */ + +const int kPathStrMinSize = 4; /**< Must more than 4 char */ +const int kPathStrMaxSize = 256; /**< Must less than 256 char */ +const int kBdCodeSize = 15; + +const uint32_t kPointXYZRSize = 16; +const uint32_t kPointXYZRTRSize = 18; + +const double PI = 3.14159265358979323846; + +constexpr uint32_t kMaxBufferSize = 0x8000; // 32k bytes + +/** Device Line Number **/ +const uint8_t kLineNumberDefault = 1; +const uint8_t kLineNumberMid360 = 4; +const uint8_t kLineNumberHAP = 6; + +// SDK related +typedef enum { + kIndustryLidarType = 1, + kVehicleLidarType = 2, + kDirectLidarType = 4, + kLivoxLidarType = 8 +} LidarProtoType; + +// SDK related +/** Timestamp sync mode define. */ +typedef enum { + kTimestampTypeNoSync = 0, /**< No sync signal mode. */ + kTimestampTypeGptpOrPtp = 1, /**< gPTP or PTP sync mode */ + kTimestampTypeGps = 2 /**< GPS sync mode. */ +} TimestampType; + +/** Lidar connect state */ +typedef enum { + kConnectStateOff = 0, + kConnectStateOn = 1, + kConnectStateConfig = 2, + kConnectStateSampling = 3, +} LidarConnectState; + +/** Device data source type */ +typedef enum { + kSourceRawLidar = 0, /**< Data from raw lidar. */ + kSourceRawHub = 1, /**< Data from lidar hub. */ + kSourceLvxFile, /**< Data from parse lvx file. */ + kSourceUndef, +} LidarDataSourceType; + +typedef enum { kCoordinateCartesian = 0, kCoordinateSpherical } CoordinateType; + +typedef enum { + kConfigDataType = 1 << 0, + kConfigScanPattern = 1 << 1, + kConfigBlindSpot = 1 << 2, + kConfigDualEmit = 1 << 3, + kConfigUnknown +} LivoxLidarConfigCodeBit; + +typedef enum { + kNoneExtrinsicParameter, + kExtrinsicParameterFromLidar, + kExtrinsicParameterFromXml +} ExtrinsicParameterType; + +typedef struct { + uint8_t lidar_type {}; +} LidarSummaryInfo; + +/** 8bytes stamp to uint64_t stamp */ +typedef union { + struct { + uint32_t low; + uint32_t high; + } stamp_word; + + uint8_t stamp_bytes[8]; + int64_t stamp; +} LdsStamp; + +#pragma pack(1) + +typedef struct { + float x; /**< X axis, Unit:m */ + float y; /**< Y axis, Unit:m */ + float z; /**< Z axis, Unit:m */ + float reflectivity; /**< Reflectivity */ + uint8_t tag; /**< Livox point tag */ + uint8_t line; /**< Laser line id */ + double timestamp; /**< Timestamp of point*/ +} LivoxPointXyzrtlt; + +typedef struct { + float x; + float y; + float z; + float intensity; + uint8_t tag; + uint8_t line; + uint64_t offset_time; +} PointXyzlt; + +typedef struct { + uint32_t handle; + uint8_t lidar_type; ////refer to LivoxLidarType + uint32_t points_num; + PointXyzlt* points; +} PointPacket; + +typedef struct { + uint64_t base_time[kMaxSourceLidar] {}; + uint8_t lidar_num {}; + PointPacket lidar_point[kMaxSourceLidar] {}; +} PointFrame; + +#pragma pack() + +typedef struct { + LidarProtoType lidar_type; + uint32_t handle; + uint64_t base_time; + uint32_t points_num; + std::vector points; +} StoragePacket; + +typedef struct { + LidarProtoType lidar_type; + uint32_t handle; + bool extrinsic_enable; + uint32_t point_num; + uint8_t data_type; + uint8_t line_num; + uint64_t time_stamp; + uint64_t point_interval; + std::vector raw_data; +} RawPacket; + +typedef struct { + StoragePacket *storage_packet; + volatile uint32_t rd_idx; + volatile uint32_t wr_idx; + uint32_t mask; + uint32_t size; /**< must be power of 2. */ +} LidarDataQueue; + +/*****************************/ +/* About Extrinsic Parameter */ +typedef struct { + float roll; /**< Roll angle, unit: degree. */ + float pitch; /**< Pitch angle, unit: degree. */ + float yaw; /**< Yaw angle, unit: degree. */ + int32_t x; /**< X translation, unit: mm. */ + int32_t y; /**< Y translation, unit: mm. */ + int32_t z; /**< Z translation, unit: mm. */ +} ExtParameter; + +typedef float TranslationVector[3]; /**< x, y, z translation, unit: mm. */ +typedef float RotationMatrix[3][3]; + +typedef struct { + TranslationVector trans; + RotationMatrix rotation; +} ExtParameterDetailed; + +typedef struct { + LidarProtoType lidar_type; + uint32_t handle; + ExtParameter param; +} LidarExtParameter; + +/** Configuration in json config file for livox lidar */ +typedef struct { + char broadcast_code[16]; + bool enable_connect; + bool enable_fan; + uint32_t return_mode; + uint32_t coordinate; + uint32_t imu_rate; + uint32_t extrinsic_parameter_source; + bool enable_high_sensitivity; +} UserRawConfig; + +typedef struct { + bool enable_fan; + uint32_t return_mode; + uint32_t coordinate; /**< 0 for CartesianCoordinate; others for SphericalCoordinate. */ + uint32_t imu_rate; + uint32_t extrinsic_parameter_source; + bool enable_high_sensitivity; + volatile uint32_t set_bits; + volatile uint32_t get_bits; +} UserConfig; + +typedef struct { + uint32_t handle; + int8_t pcl_data_type; + int8_t pattern_mode; + int32_t blind_spot_set; + int8_t dual_emit_en; + ExtParameter extrinsic_param; + volatile uint32_t set_bits; + volatile uint32_t get_bits; +} UserLivoxLidarConfig; + +/** Lidar data source info abstract */ +typedef struct { + uint8_t lidar_type; + uint32_t handle; + // union { + // uint8_t slot : 4; //slot for LivoxLidarType::kVehicleLidarType + // uint8_t handle : 4; // handle for LivoxLidarType::kIndustryLidarType + // }; + uint8_t data_src; /**< From raw lidar or livox file. */ + volatile LidarConnectState connect_state; + // DeviceInfo info; + + LidarDataQueue data; + LidarImuDataQueue imu_data; + + uint32_t firmware_ver; /**< Firmware version of lidar */ + UserLivoxLidarConfig livox_config; +} LidarDevice; + +constexpr uint32_t kMaxProductType = 10; +constexpr uint32_t kDeviceTypeLidarMid70 = 6; + +/***********************************/ +/* Global function for general use */ +bool IsFilePathValid(const char *path_str); +uint32_t CalculatePacketQueueSize(const double publish_freq); +std::string IpNumToString(uint32_t ip_num); +uint32_t IpStringToNum(std::string ip_string); +std::string ReplacePeriodByUnderline(std::string str); + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER2_COMM_H_ diff --git a/src/livox_ros_driver2/src/comm/ldq.cpp b/src/livox_ros_driver2/src/comm/ldq.cpp new file mode 100644 index 0000000..d70bcee --- /dev/null +++ b/src/livox_ros_driver2/src/comm/ldq.cpp @@ -0,0 +1,150 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include +#include + +#include "ldq.h" + +namespace livox_ros { + +/* for pointcloud queue process */ +bool InitQueue(LidarDataQueue *queue, uint32_t queue_size) { + if (queue == nullptr) { + // ROS_WARN("RosDriver Queue: Initialization failed - invalid queue."); + return false; + } + + if (!IsPowerOf2(queue_size)) { + queue_size = RoundupPowerOf2(queue_size); + printf("Init queue, real query size:%u.\n", queue_size); + } + + if (queue->storage_packet) { + delete[] queue->storage_packet; + queue->storage_packet = nullptr; + } + + queue->storage_packet = new StoragePacket[queue_size]; + if (queue->storage_packet == nullptr) { + // ROS_WARN("RosDriver Queue: Initialization failed - failed to allocate memory."); + return false; + } + + queue->rd_idx = 0; + queue->wr_idx = 0; + queue->size = queue_size; + queue->mask = queue_size - 1; + + return true; +} + +bool DeInitQueue(LidarDataQueue *queue) { + if (queue == nullptr) { + // ROS_WARN("RosDriver Queue: Deinitialization failed - invalid queue."); + return false; + } + + if (queue->storage_packet) { + delete[] queue->storage_packet; + } + + queue->rd_idx = 0; + queue->wr_idx = 0; + queue->size = 0; + queue->mask = 0; + + return true; +} + +void ResetQueue(LidarDataQueue *queue) { + queue->rd_idx = 0; + queue->wr_idx = 0; +} + +bool QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet) { + if (queue == nullptr || storage_packet == nullptr) { + // ROS_WARN("RosDriver Queue: Invalid pointer parameters."); + return false; + } + + if (QueueIsEmpty(queue)) { + // ROS_WARN("RosDriver Queue: Pop failed, since the queue is empty."); + return false; + } + + uint32_t rd_idx = queue->rd_idx & queue->mask; + + storage_packet->base_time = queue->storage_packet[rd_idx].base_time; + storage_packet->points_num = queue->storage_packet[rd_idx].points_num; + storage_packet->points.resize(queue->storage_packet[rd_idx].points_num); + + memcpy(storage_packet->points.data(), queue->storage_packet[rd_idx].points.data(), (storage_packet->points_num) * sizeof(PointXyzlt)); + return true; +} + +void QueuePopUpdate(LidarDataQueue *queue) { + queue->rd_idx++; +} + +bool QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet) { + if (!QueuePrePop(queue, storage_packet)) { + return false; + } + QueuePopUpdate(queue); + + return true; +} + +uint32_t QueueUsedSize(LidarDataQueue *queue) { + return queue->wr_idx - queue->rd_idx; +} + +uint32_t QueueUnusedSize(LidarDataQueue *queue) { + return (queue->size - QueueUsedSize(queue)); +} + +bool QueueIsFull(LidarDataQueue *queue) { + return ((queue->wr_idx - queue->rd_idx) > queue->mask); +} + +bool QueueIsEmpty(LidarDataQueue *queue) { + return (queue->rd_idx == queue->wr_idx); +} + +uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, const uint64_t base_time) { + uint32_t wr_idx = queue->wr_idx & queue->mask; + PointPacket* lidar_point_data = reinterpret_cast(data); + queue->storage_packet[wr_idx].base_time = base_time; + queue->storage_packet[wr_idx].points_num = lidar_point_data->points_num; + + queue->storage_packet[wr_idx].points.clear(); + queue->storage_packet[wr_idx].points.resize(lidar_point_data->points_num); + memcpy(queue->storage_packet[wr_idx].points.data(), lidar_point_data->points, sizeof(PointXyzlt) * (lidar_point_data->points_num)); + + queue->wr_idx++; + return 1; +} + +} // namespace livox_ros diff --git a/src/livox_ros_driver2/src/comm/ldq.h b/src/livox_ros_driver2/src/comm/ldq.h new file mode 100644 index 0000000..9830a31 --- /dev/null +++ b/src/livox_ros_driver2/src/comm/ldq.h @@ -0,0 +1,66 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_LDQ_H_ +#define LIVOX_ROS_DRIVER_LDQ_H_ + +#include +#include + +#include "comm/comm.h" + +namespace livox_ros { + +inline static bool IsPowerOf2(uint32_t size) { + return (size != 0) && ((size & (size - 1)) == 0); +} + +inline static uint32_t RoundupPowerOf2(uint32_t size) { + uint32_t power2_val = 0; + for (int i = 0; i < 32; i++) { + power2_val = ((uint32_t)1) << i; + if (size <= power2_val) { + break; + } + } + + return power2_val; +} + +/** queue operate function */ +bool InitQueue(LidarDataQueue *queue, uint32_t queue_size); +bool DeInitQueue(LidarDataQueue *queue); +void ResetQueue(LidarDataQueue *queue); +bool QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet); +void QueuePopUpdate(LidarDataQueue *queue); +bool QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet); +uint32_t QueueUsedSize(LidarDataQueue *queue); +uint32_t QueueUnusedSize(LidarDataQueue *queue); +bool QueueIsFull(LidarDataQueue *queue); +bool QueueIsEmpty(LidarDataQueue *queue); +uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, const uint64_t base_time); + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LDQ_H_ diff --git a/src/livox_ros_driver2/src/comm/lidar_imu_data_queue.cpp b/src/livox_ros_driver2/src/comm/lidar_imu_data_queue.cpp new file mode 100644 index 0000000..43de94b --- /dev/null +++ b/src/livox_ros_driver2/src/comm/lidar_imu_data_queue.cpp @@ -0,0 +1,70 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "lidar_imu_data_queue.h" + +namespace livox_ros { + +void LidarImuDataQueue::Push(ImuData* imu_data) { + ImuData data; + data.lidar_type = imu_data->lidar_type; + data.handle = imu_data->handle; + data.time_stamp = imu_data->time_stamp; + + data.gyro_x = imu_data->gyro_x; + data.gyro_y = imu_data->gyro_y; + data.gyro_z = imu_data->gyro_z; + + data.acc_x = imu_data->acc_x; + data.acc_y = imu_data->acc_y; + data.acc_z = imu_data->acc_z; + + std::lock_guard lock(mutex_); + imu_data_queue_.push_back(std::move(data)); +} + +bool LidarImuDataQueue::Pop(ImuData& imu_data) { + std::lock_guard lock(mutex_); + if (imu_data_queue_.empty()) { + return false; + } + imu_data = imu_data_queue_.front(); + imu_data_queue_.pop_front(); + return true; +} + +bool LidarImuDataQueue::Empty() { + std::lock_guard lock(mutex_); + return imu_data_queue_.empty(); +} + +void LidarImuDataQueue::Clear() { + std::list tmp_imu_data_queue; + { + std::lock_guard lock(mutex_); + imu_data_queue_.swap(tmp_imu_data_queue); + } +} + +} // namespace livox_ros \ No newline at end of file diff --git a/src/livox_ros_driver2/src/comm/lidar_imu_data_queue.h b/src/livox_ros_driver2/src/comm/lidar_imu_data_queue.h new file mode 100644 index 0000000..83091f3 --- /dev/null +++ b/src/livox_ros_driver2/src/comm/lidar_imu_data_queue.h @@ -0,0 +1,77 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_ +#define LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_ + +#include +#include +#include + +namespace livox_ros { + +// Based on the IMU Data Type in Livox communication protocol +// TODO: add a link to the protocol +typedef struct { + float gyro_x; /**< Gyroscope X axis, Unit:rad/s */ + float gyro_y; /**< Gyroscope Y axis, Unit:rad/s */ + float gyro_z; /**< Gyroscope Z axis, Unit:rad/s */ + float acc_x; /**< Accelerometer X axis, Unit:g */ + float acc_y; /**< Accelerometer Y axis, Unit:g */ + float acc_z; /**< Accelerometer Z axis, Unit:g */ +} RawImuPoint; + +typedef struct { + uint8_t lidar_type; + uint32_t handle; + uint8_t slot; + // union { + // uint8_t handle; + // uint8_t slot; + // }; + uint64_t time_stamp; + float gyro_x; /**< Gyroscope X axis, Unit:rad/s */ + float gyro_y; /**< Gyroscope Y axis, Unit:rad/s */ + float gyro_z; /**< Gyroscope Z axis, Unit:rad/s */ + float acc_x; /**< Accelerometer X axis, Unit:g */ + float acc_y; /**< Accelerometer Y axis, Unit:g */ + float acc_z; /**< Accelerometer Z axis, Unit:g */ +} ImuData; + +class LidarImuDataQueue { + public: + void Push(ImuData* imu_data); + bool Pop(ImuData& imu_data); + bool Empty(); + void Clear(); + + private: + std::mutex mutex_; + std::list imu_data_queue_; +}; + +} // namespace + +#endif // LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_ + diff --git a/src/livox_ros_driver2/src/comm/pub_handler.cpp b/src/livox_ros_driver2/src/comm/pub_handler.cpp new file mode 100644 index 0000000..aa59d37 --- /dev/null +++ b/src/livox_ros_driver2/src/comm/pub_handler.cpp @@ -0,0 +1,457 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "pub_handler.h" +#include "livox_lidar_api.h" +#include +#include +#include +#include + +namespace livox_ros { + +std::atomic PubHandler::is_timestamp_sync_; + +PubHandler &pub_handler() { + static PubHandler handler; + return handler; +} + +void PubHandler::Init() { +} + +void PubHandler::Uninit() { + if (lidar_listen_id_ > 0) { + LivoxLidarRemovePointCloudObserver(lidar_listen_id_); + lidar_listen_id_ = 0; + } + + RequestExit(); + + if (point_process_thread_ && + point_process_thread_->joinable()) { + point_process_thread_->join(); + point_process_thread_ = nullptr; + } else { + /* */ + } +} + +void PubHandler::RequestExit() { + is_quit_.store(true); +} + +void PubHandler::SetPointCloudConfig(const double publish_freq) { + publish_interval_ = (kNsPerSecond / (publish_freq * 10)) * 10; + publish_interval_tolerance_ = publish_interval_ - kNsTolerantFrameTimeDeviation; + publish_interval_ms_ = publish_interval_ / kRatioOfMsToNs; + if (!point_process_thread_) { + point_process_thread_ = std::make_shared(&PubHandler::RawDataProcess, this); + } + return; +} + +void PubHandler::SetImuDataCallback(ImuDataCallback cb, void* client_data) { + imu_client_data_ = client_data; + imu_callback_ = cb; +} + +void PubHandler::AddLidarsExtParam(LidarExtParameter& lidar_param) { + std::unique_lock lock(packet_mutex_); + uint32_t id = 0; + GetLidarId(lidar_param.lidar_type, lidar_param.handle, id); + lidar_extrinsics_[id] = lidar_param; +} + +void PubHandler::ClearAllLidarsExtrinsicParams() { + std::unique_lock lock(packet_mutex_); + lidar_extrinsics_.clear(); +} + +void PubHandler::SetPointCloudsCallback(PointCloudsCallback cb, void* client_data) { + pub_client_data_ = client_data; + points_callback_ = cb; + lidar_listen_id_ = LivoxLidarAddPointCloudObserver(OnLivoxLidarPointCloudCallback, this); +} + +void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t dev_type, + LivoxLidarEthernetPacket *data, void *client_data) { + PubHandler* self = (PubHandler*)client_data; + if (!self) { + return; + } + + if (data->time_type != kTimestampTypeNoSync) { + is_timestamp_sync_.store(true); + } else { + is_timestamp_sync_.store(false); + } + + if (data->data_type == kLivoxLidarImuData) { + if (self->imu_callback_) { + RawImuPoint* imu = (RawImuPoint*) data->data; + ImuData imu_data; + imu_data.lidar_type = static_cast(LidarProtoType::kLivoxLidarType); + imu_data.handle = handle; + imu_data.time_stamp = GetEthPacketTimestamp(data->time_type, + data->timestamp, sizeof(data->timestamp)); + imu_data.gyro_x = imu->gyro_x; + imu_data.gyro_y = imu->gyro_y; + imu_data.gyro_z = imu->gyro_z; + imu_data.acc_x = imu->acc_x; + imu_data.acc_y = imu->acc_y; + imu_data.acc_z = imu->acc_z; + self->imu_callback_(&imu_data, self->imu_client_data_); + } + return; + } + RawPacket packet = {}; + packet.handle = handle; + packet.lidar_type = LidarProtoType::kLivoxLidarType; + packet.extrinsic_enable = false; + if (dev_type == LivoxLidarDeviceType::kLivoxLidarTypeIndustrialHAP) { + packet.line_num = kLineNumberHAP; + } else if (dev_type == LivoxLidarDeviceType::kLivoxLidarTypeMid360||dev_type==LivoxLidarDeviceType::kLivoxLidarTypeMid360s) { + packet.line_num = kLineNumberMid360; + } else { + packet.line_num = kLineNumberDefault; + } + packet.data_type = data->data_type; + packet.point_num = data->dot_num; + packet.point_interval = data->time_interval * 100 / data->dot_num; //ns + packet.time_stamp = GetEthPacketTimestamp(data->time_type, + data->timestamp, sizeof(data->timestamp)); + uint32_t length = data->length - sizeof(LivoxLidarEthernetPacket) + 1; + packet.raw_data.insert(packet.raw_data.end(), data->data, data->data + length); + { + std::unique_lock lock(self->packet_mutex_); + self->raw_packet_queue_.push_back(packet); + } + self->packet_condition_.notify_one(); + + return; +} + +void PubHandler::PublishPointCloud() { + //publish point + if (points_callback_) { + points_callback_(&frame_, pub_client_data_); + } + return; +} + +void PubHandler::CheckTimer(uint32_t id) { + + if (PubHandler::is_timestamp_sync_.load()) { // Enable time synchronization + auto& process_handler = lidar_process_handlers_[id]; + uint64_t recent_time_ms = process_handler->GetRecentTimeStamp() / kRatioOfMsToNs; + if ((recent_time_ms % publish_interval_ms_ != 0) || recent_time_ms == 0) { + return; + } + + uint64_t diff = process_handler->GetRecentTimeStamp() - process_handler->GetLidarBaseTime(); + if (diff < publish_interval_tolerance_) { + return; + } + + frame_.base_time[frame_.lidar_num] = process_handler->GetLidarBaseTime(); + points_[id].clear(); + process_handler->GetLidarPointClouds(points_[id]); + if (points_[id].empty()) { + return; + } + PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num]; + lidar_point.lidar_type = LidarProtoType::kLivoxLidarType; // TODO: + lidar_point.handle = id; + lidar_point.points_num = points_[id].size(); + lidar_point.points = points_[id].data(); + frame_.lidar_num++; + + if (frame_.lidar_num != 0) { + PublishPointCloud(); + frame_.lidar_num = 0; + } + } else { // Disable time synchronization + auto now_time = std::chrono::high_resolution_clock::now(); + //First Set + static bool first = true; + if (first) { + last_pub_time_ = now_time; + first = false; + return; + } + if (now_time - last_pub_time_ < std::chrono::nanoseconds(publish_interval_)) { + return; + } + last_pub_time_ += std::chrono::nanoseconds(publish_interval_); + for (auto &process_handler : lidar_process_handlers_) { + frame_.base_time[frame_.lidar_num] = process_handler.second->GetLidarBaseTime(); + uint32_t handle = process_handler.first; + points_[handle].clear(); + process_handler.second->GetLidarPointClouds(points_[handle]); + if (points_[handle].empty()) { + continue; + } + PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num]; + lidar_point.lidar_type = LidarProtoType::kLivoxLidarType; // TODO: + lidar_point.handle = handle; + lidar_point.points_num = points_[handle].size(); + lidar_point.points = points_[handle].data(); + frame_.lidar_num++; + } + PublishPointCloud(); + frame_.lidar_num = 0; + } + return; +} + +void PubHandler::RawDataProcess() { + RawPacket raw_data; + while (!is_quit_.load()) { + { + std::unique_lock lock(packet_mutex_); + if (raw_packet_queue_.empty()) { + packet_condition_.wait_for(lock, std::chrono::milliseconds(500)); + if (raw_packet_queue_.empty()) { + continue; + } + } + raw_data = raw_packet_queue_.front(); + raw_packet_queue_.pop_front(); + } + uint32_t id = 0; + GetLidarId(raw_data.lidar_type, raw_data.handle, id); + if (lidar_process_handlers_.find(id) == lidar_process_handlers_.end()) { + lidar_process_handlers_[id].reset(new LidarPubHandler()); + } + auto &process_handler = lidar_process_handlers_[id]; + if (lidar_extrinsics_.find(id) != lidar_extrinsics_.end()) { + lidar_process_handlers_[id]->SetLidarsExtParam(lidar_extrinsics_[id]); + } + process_handler->PointCloudProcess(raw_data); + CheckTimer(id); + } +} + +bool PubHandler::GetLidarId(LidarProtoType lidar_type, uint32_t handle, uint32_t& id) { + if (lidar_type == kLivoxLidarType) { + id = handle; + return true; + } + return false; +} + +uint64_t PubHandler::GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size) { + LdsStamp time; + memcpy(time.stamp_bytes, time_stamp, size); + + if (timestamp_type == kTimestampTypeGptpOrPtp || + timestamp_type == kTimestampTypeGps) { + return time.stamp; + } + + return std::chrono::high_resolution_clock::now().time_since_epoch().count(); +} + +/*******************************/ +/* LidarPubHandler Definitions*/ +LidarPubHandler::LidarPubHandler() : is_set_extrinsic_params_(false) {} + +uint64_t LidarPubHandler::GetLidarBaseTime() { + if (points_clouds_.empty()) { + return 0; + } + return points_clouds_.at(0).offset_time; +} + +void LidarPubHandler::GetLidarPointClouds(std::vector& points_clouds) { + std::lock_guard lock(mutex_); + points_clouds.swap(points_clouds_); +} + +uint64_t LidarPubHandler::GetRecentTimeStamp() { + if (points_clouds_.empty()) { + return 0; + } + return points_clouds_.back().offset_time; +} + +uint32_t LidarPubHandler::GetLidarPointCloudsSize() { + std::lock_guard lock(mutex_); + return points_clouds_.size(); +} + +//convert to standard format and extrinsic compensate +void LidarPubHandler::PointCloudProcess(RawPacket & pkt) { + if (pkt.lidar_type == LidarProtoType::kLivoxLidarType) { + LivoxLidarPointCloudProcess(pkt); + } else { + static bool flag = false; + if (!flag) { + std::cout << "error, unsupported protocol type: " << static_cast(pkt.lidar_type) << std::endl; + flag = true; + } + } +} + +void LidarPubHandler::LivoxLidarPointCloudProcess(RawPacket & pkt) { + switch (pkt.data_type) { + case kLivoxLidarCartesianCoordinateHighData: + ProcessCartesianHighPoint(pkt); + break; + case kLivoxLidarCartesianCoordinateLowData: + ProcessCartesianLowPoint(pkt); + break; + case kLivoxLidarSphericalCoordinateData: + ProcessSphericalPoint(pkt); + break; + default: + std::cout << "unknown data type: " << static_cast(pkt.data_type) + << " !!" << std::endl; + break; + } +} + +void LidarPubHandler::SetLidarsExtParam(LidarExtParameter lidar_param) { + if (is_set_extrinsic_params_) { + return; + } + extrinsic_.trans[0] = lidar_param.param.x; + extrinsic_.trans[1] = lidar_param.param.y; + extrinsic_.trans[2] = lidar_param.param.z; + + double cos_roll = cos(static_cast(lidar_param.param.roll * PI / 180.0)); + double cos_pitch = cos(static_cast(lidar_param.param.pitch * PI / 180.0)); + double cos_yaw = cos(static_cast(lidar_param.param.yaw * PI / 180.0)); + double sin_roll = sin(static_cast(lidar_param.param.roll * PI / 180.0)); + double sin_pitch = sin(static_cast(lidar_param.param.pitch * PI / 180.0)); + double sin_yaw = sin(static_cast(lidar_param.param.yaw * PI / 180.0)); + + extrinsic_.rotation[0][0] = cos_pitch * cos_yaw; + extrinsic_.rotation[0][1] = sin_roll * sin_pitch * cos_yaw - cos_roll * sin_yaw; + extrinsic_.rotation[0][2] = cos_roll * sin_pitch * cos_yaw + sin_roll * sin_yaw; + + extrinsic_.rotation[1][0] = cos_pitch * sin_yaw; + extrinsic_.rotation[1][1] = sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw; + extrinsic_.rotation[1][2] = cos_roll * sin_pitch * sin_yaw - sin_roll * cos_yaw; + + extrinsic_.rotation[2][0] = -sin_pitch; + extrinsic_.rotation[2][1] = sin_roll * cos_pitch; + extrinsic_.rotation[2][2] = cos_roll * cos_pitch; + + is_set_extrinsic_params_ = true; +} + +void LidarPubHandler::ProcessCartesianHighPoint(RawPacket & pkt) { + LivoxLidarCartesianHighRawPoint* raw = (LivoxLidarCartesianHighRawPoint*)pkt.raw_data.data(); + PointXyzlt point = {}; + for (uint32_t i = 0; i < pkt.point_num; i++) { + if (pkt.extrinsic_enable) { + point.x = raw[i].x / 1000.0; + point.y = raw[i].y / 1000.0; + point.z = raw[i].z / 1000.0; + } else { + point.x = (raw[i].x * extrinsic_.rotation[0][0] + + raw[i].y * extrinsic_.rotation[0][1] + + raw[i].z * extrinsic_.rotation[0][2] + extrinsic_.trans[0]) / 1000.0; + point.y = (raw[i].x* extrinsic_.rotation[1][0] + + raw[i].y * extrinsic_.rotation[1][1] + + raw[i].z * extrinsic_.rotation[1][2] + extrinsic_.trans[1]) / 1000.0; + point.z = (raw[i].x * extrinsic_.rotation[2][0] + + raw[i].y * extrinsic_.rotation[2][1] + + raw[i].z * extrinsic_.rotation[2][2] + extrinsic_.trans[2]) / 1000.0; + } + point.intensity = raw[i].reflectivity; + point.line = i % pkt.line_num; + point.tag = raw[i].tag; + point.offset_time = pkt.time_stamp + i * pkt.point_interval; + std::lock_guard lock(mutex_); + points_clouds_.push_back(point); + } +} + +void LidarPubHandler::ProcessCartesianLowPoint(RawPacket & pkt) { + LivoxLidarCartesianLowRawPoint* raw = (LivoxLidarCartesianLowRawPoint*)pkt.raw_data.data(); + PointXyzlt point = {}; + for (uint32_t i = 0; i < pkt.point_num; i++) { + if (pkt.extrinsic_enable) { + point.x = raw[i].x / 100.0; + point.y = raw[i].y / 100.0; + point.z = raw[i].z / 100.0; + } else { + point.x = (raw[i].x * extrinsic_.rotation[0][0] + + raw[i].y * extrinsic_.rotation[0][1] + + raw[i].z * extrinsic_.rotation[0][2] + extrinsic_.trans[0]) / 100.0; + point.y = (raw[i].x* extrinsic_.rotation[1][0] + + raw[i].y * extrinsic_.rotation[1][1] + + raw[i].z * extrinsic_.rotation[1][2] + extrinsic_.trans[1]) / 100.0; + point.z = (raw[i].x * extrinsic_.rotation[2][0] + + raw[i].y * extrinsic_.rotation[2][1] + + raw[i].z * extrinsic_.rotation[2][2] + extrinsic_.trans[2]) / 100.0; + } + point.intensity = raw[i].reflectivity; + point.line = i % pkt.line_num; + point.tag = raw[i].tag; + point.offset_time = pkt.time_stamp + i * pkt.point_interval; + std::lock_guard lock(mutex_); + points_clouds_.push_back(point); + } +} + +void LidarPubHandler::ProcessSphericalPoint(RawPacket& pkt) { + LivoxLidarSpherPoint* raw = (LivoxLidarSpherPoint*)pkt.raw_data.data(); + PointXyzlt point = {}; + for (uint32_t i = 0; i < pkt.point_num; i++) { + double radius = raw[i].depth / 1000.0; + double theta = raw[i].theta / 100.0 / 180 * PI; + double phi = raw[i].phi / 100.0 / 180 * PI; + double src_x = radius * sin(theta) * cos(phi); + double src_y = radius * sin(theta) * sin(phi); + double src_z = radius * cos(theta); + if (pkt.extrinsic_enable) { + point.x = src_x; + point.y = src_y; + point.z = src_z; + } else { + point.x = src_x * extrinsic_.rotation[0][0] + + src_y * extrinsic_.rotation[0][1] + + src_z * extrinsic_.rotation[0][2] + (extrinsic_.trans[0] / 1000.0); + point.y = src_x * extrinsic_.rotation[1][0] + + src_y * extrinsic_.rotation[1][1] + + src_z * extrinsic_.rotation[1][2] + (extrinsic_.trans[1] / 1000.0); + point.z = src_x * extrinsic_.rotation[2][0] + + src_y * extrinsic_.rotation[2][1] + + src_z * extrinsic_.rotation[2][2] + (extrinsic_.trans[2] / 1000.0); + } + + point.intensity = raw[i].reflectivity; + point.line = i % pkt.line_num; + point.tag = raw[i].tag; + point.offset_time = pkt.time_stamp + i * pkt.point_interval; + std::lock_guard lock(mutex_); + points_clouds_.push_back(point); + } +} + +} // namespace livox_ros diff --git a/src/livox_ros_driver2/src/comm/pub_handler.h b/src/livox_ros_driver2/src/comm/pub_handler.h new file mode 100644 index 0000000..9b70d6b --- /dev/null +++ b/src/livox_ros_driver2/src/comm/pub_handler.h @@ -0,0 +1,137 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_DRIVER_PUB_HANDLER_H_ +#define LIVOX_DRIVER_PUB_HANDLER_H_ + +#include +#include +#include // std::condition_variable +#include +#include +#include +#include +#include // std::mutex +#include + +#include "livox_lidar_api.h" +#include "comm/comm.h" + +namespace livox_ros { + +class LidarPubHandler { + public: + LidarPubHandler(); + ~ LidarPubHandler() {} + + void PointCloudProcess(RawPacket& pkt); + void SetLidarsExtParam(LidarExtParameter param); + void GetLidarPointClouds(std::vector& points_clouds); + + uint64_t GetRecentTimeStamp(); + uint32_t GetLidarPointCloudsSize(); + uint64_t GetLidarBaseTime(); + + private: + void LivoxLidarPointCloudProcess(RawPacket & pkt); + void ProcessCartesianHighPoint(RawPacket & pkt); + void ProcessCartesianLowPoint(RawPacket & pkt); + void ProcessSphericalPoint(RawPacket & pkt); + std::vector points_clouds_; + ExtParameterDetailed extrinsic_ = { + {0, 0, 0}, + { + {1, 0, 0}, + {0, 1, 1}, + {0, 0, 1} + } + }; + std::mutex mutex_; + std::atomic_bool is_set_extrinsic_params_; +}; + +class PubHandler { + public: + using PointCloudsCallback = std::function; + using ImuDataCallback = std::function; + using TimePoint = std::chrono::high_resolution_clock::time_point; + + PubHandler() {} + + ~ PubHandler() { Uninit(); } + + void Uninit(); + void RequestExit(); + void Init(); + void SetPointCloudConfig(const double publish_freq); + void SetPointCloudsCallback(PointCloudsCallback cb, void* client_data); + void AddLidarsExtParam(LidarExtParameter& extrinsic_params); + void ClearAllLidarsExtrinsicParams(); + void SetImuDataCallback(ImuDataCallback cb, void* client_data); + + private: + //thread to process raw data + void RawDataProcess(); + std::atomic is_quit_{false}; + std::shared_ptr point_process_thread_; + std::mutex packet_mutex_; + std::condition_variable packet_condition_; + + //publish callback + void CheckTimer(uint32_t id); + void PublishPointCloud(); + static void OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t dev_type, + LivoxLidarEthernetPacket *data, void *client_data); + + static bool GetLidarId(LidarProtoType lidar_type, uint32_t handle, uint32_t& id); + static uint64_t GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size); + + PointCloudsCallback points_callback_; + void* pub_client_data_ = nullptr; + + ImuDataCallback imu_callback_; + void* imu_client_data_ = nullptr; + + PointFrame frame_; + + std::deque raw_packet_queue_; + + //pub config + uint64_t publish_interval_ = 100000000; //100 ms + uint64_t publish_interval_tolerance_ = 100000000; //100 ms + uint64_t publish_interval_ms_ = 100; //100 ms + TimePoint last_pub_time_; + + std::map> lidar_process_handlers_; + std::map> points_; + std::map lidar_extrinsics_; + static std::atomic is_timestamp_sync_; + uint16_t lidar_listen_id_ = 0; +}; + +PubHandler &pub_handler(); + +} // namespace livox_ros + +#endif // LIVOX_DRIVER_PUB_HANDLER_H_ \ No newline at end of file diff --git a/src/livox_ros_driver2/src/comm/semaphore.cpp b/src/livox_ros_driver2/src/comm/semaphore.cpp new file mode 100644 index 0000000..941815a --- /dev/null +++ b/src/livox_ros_driver2/src/comm/semaphore.cpp @@ -0,0 +1,41 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "semaphore.h" + +namespace livox_ros { + +void Semaphore::Signal() { + std::unique_lock lock(mutex_); + ++count_; + cv_.notify_one(); +} + +void Semaphore::Wait() { + std::unique_lock lock(mutex_); + cv_.wait(lock, [=] { return count_ > 0; }); + --count_; +} + +} // namespace livox_ros diff --git a/src/livox_ros_driver2/src/comm/semaphore.h b/src/livox_ros_driver2/src/comm/semaphore.h new file mode 100644 index 0000000..b8085cd --- /dev/null +++ b/src/livox_ros_driver2/src/comm/semaphore.h @@ -0,0 +1,51 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_SEMAPHORE_H_ +#define LIVOX_ROS_DRIVER_SEMAPHORE_H_ + +#include +#include + +namespace livox_ros { + +class Semaphore { + public: + explicit Semaphore(int count = 0) : count_(count) { + } + void Signal(); + void Wait(); + int GetCount() { + return count_; + } + + private: + std::mutex mutex_; + std::condition_variable cv_; + volatile int count_; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_SEMAPHORE_H_ diff --git a/src/livox_ros_driver2/src/driver_node.cpp b/src/livox_ros_driver2/src/driver_node.cpp new file mode 100644 index 0000000..24f0aba --- /dev/null +++ b/src/livox_ros_driver2/src/driver_node.cpp @@ -0,0 +1,46 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "driver_node.h" +#include "lddc.h" + +namespace livox_ros { + +DriverNode& DriverNode::GetNode() noexcept { + return *this; +} + +DriverNode::~DriverNode() { + lddc_ptr_->lds_->RequestExit(); + exit_signal_.set_value(); + pointclouddata_poll_thread_->join(); + imudata_poll_thread_->join(); +} + +} // namespace livox_ros + + + + + diff --git a/src/livox_ros_driver2/src/driver_node.h b/src/livox_ros_driver2/src/driver_node.h new file mode 100644 index 0000000..aaf4f81 --- /dev/null +++ b/src/livox_ros_driver2/src/driver_node.h @@ -0,0 +1,78 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_DRIVER_NODE_H +#define LIVOX_DRIVER_NODE_H + +#include "include/ros_headers.h" + +namespace livox_ros { + +class Lddc; + +#ifdef BUILDING_ROS1 +class DriverNode final : public ros::NodeHandle { + public: + DriverNode() = default; + DriverNode(const DriverNode &) = delete; + ~DriverNode(); + DriverNode &operator=(const DriverNode &) = delete; + + DriverNode& GetNode() noexcept; + + void PointCloudDataPollThread(); + void ImuDataPollThread(); + + std::unique_ptr lddc_ptr_; + std::shared_ptr pointclouddata_poll_thread_; + std::shared_ptr imudata_poll_thread_; + std::shared_future future_; + std::promise exit_signal_; +}; + +#elif defined BUILDING_ROS2 +class DriverNode final : public rclcpp::Node { + public: + explicit DriverNode(const rclcpp::NodeOptions& options); + DriverNode(const DriverNode &) = delete; + ~DriverNode(); + DriverNode &operator=(const DriverNode &) = delete; + + DriverNode& GetNode() noexcept; + + private: + void PointCloudDataPollThread(); + void ImuDataPollThread(); + + std::unique_ptr lddc_ptr_; + std::shared_ptr pointclouddata_poll_thread_; + std::shared_ptr imudata_poll_thread_; + std::shared_future future_; + std::promise exit_signal_; +}; +#endif + +} // namespace livox_ros + +#endif // LIVOX_DRIVER_NODE_H \ No newline at end of file diff --git a/src/livox_ros_driver2/src/include/livox_ros_driver2.h b/src/livox_ros_driver2/src/include/livox_ros_driver2.h new file mode 100644 index 0000000..2de7eda --- /dev/null +++ b/src/livox_ros_driver2/src/include/livox_ros_driver2.h @@ -0,0 +1,40 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER2_INClUDE_H_ +#define LIVOX_ROS_DRIVER2_INClUDE_H_ + +#define LIVOX_ROS_DRIVER2_VER_MAJOR 1 +#define LIVOX_ROS_DRIVER2_VER_MINOR 2 +#define LIVOX_ROS_DRIVER2_VER_PATCH 5 + +#define GET_STRING(n) GET_STRING_DIRECT(n) +#define GET_STRING_DIRECT(n) #n + +#define LIVOX_ROS_DRIVER2_VERSION_STRING \ + GET_STRING(LIVOX_ROS_DRIVER2_VER_MAJOR) \ + "." GET_STRING(LIVOX_ROS_DRIVER2_VER_MINOR) "." GET_STRING( \ + LIVOX_ROS_DRIVER2_VER_PATCH) + +#endif // LIVOX_ROS_DRIVER2_INClUDE_H_ diff --git a/src/livox_ros_driver2/src/include/ros1_headers.h b/src/livox_ros_driver2/src/include/ros1_headers.h new file mode 100644 index 0000000..6510859 --- /dev/null +++ b/src/livox_ros_driver2/src/include/ros1_headers.h @@ -0,0 +1,54 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +// Denoting headers specifically used for building ROS1 Driver. + +#ifndef ROS1_HEADERS_H_ +#define ROS1_HEADERS_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include "livox_ros_driver2/CustomMsg.h" +#include "livox_ros_driver2/CustomPoint.h" + + +#define DRIVER_DEBUG(node, ...) ROS_DEBUG(__VA_ARGS__) +#define DRIVER_INFO(node, ...) ROS_INFO(__VA_ARGS__) +#define DRIVER_WARN(node, ...) ROS_WARN(__VA_ARGS__) +#define DRIVER_ERROR(node, ...) ROS_ERROR(__VA_ARGS__) +#define DRIVER_FATAL(node, ...) ROS_FATAL(__VA_ARGS__) + +#define DRIVER_DEBUG_EXTRA(node, EXTRA, ...) ROS_DEBUG_##EXTRA(__VA_ARGS__) +#define DRIVER_INFO_EXTRA(node, EXTRA, ...) ROS_INFO_##EXTRA(__VA_ARGS__) +#define DRIVER_WARN_EXTRA(node, EXTRA, ...) ROS_WARN_##EXTRA(__VA_ARGS__) +#define DRIVER_ERROR_EXTRA(node, EXTRA, ...) ROS_ERROR_##EXTRA(__VA_ARGS__) +#define DRIVER_FATAL_EXTRA(node, EXTRA, ...) ROS_FATAL_##EXTRA(__VA_ARGS__) + +#endif // ROS1_HEADERS_H_ diff --git a/src/livox_ros_driver2/src/include/ros2_headers.h b/src/livox_ros_driver2/src/include/ros2_headers.h new file mode 100644 index 0000000..1d11236 --- /dev/null +++ b/src/livox_ros_driver2/src/include/ros2_headers.h @@ -0,0 +1,52 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +// Denoting headers specifically used for building ROS1 Driver. + +#ifndef ROS2_HEADERS_H_ +#define ROS2_HEADERS_H_ + +#include +#include + +#include +#include +#include +#include +#include "livox_ros_driver2/msg/custom_point.hpp" +#include "livox_ros_driver2/msg/custom_msg.hpp" + +#define DRIVER_DEBUG(node, ...) RCLCPP_DEBUG((node).get_logger(), __VA_ARGS__) +#define DRIVER_INFO(node, ...) RCLCPP_INFO((node).get_logger(), __VA_ARGS__) +#define DRIVER_WARN(node, ...) RCLCPP_WARN((node).get_logger(), __VA_ARGS__) +#define DRIVER_ERROR(node, ...) RCLCPP_ERROR((node).get_logger(), __VA_ARGS__) +#define DRIVER_FATAL(node, ...) RCLCPP_FATAL((node).get_logger(), __VA_ARGS__) + +#define DRIVER_DEBUG_EXTRA(node, EXTRA, ...) RCLCPP_DEBUG_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_INFO_EXTRA(node, EXTRA, ...) RCLCPP_INFO_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_WARN_EXTRA(node, EXTRA, ...) RCLCPP_WARN_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_ERROR_EXTRA(node, EXTRA, ...) RCLCPP_ERROR_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_FATAL_EXTRA(node, EXTRA, ...) RCLCPP_FATAL_##EXTRA((node).get_logger(), __VA_ARGS__) + +#endif // ROS2_HEADERS_H_ diff --git a/src/livox_ros_driver2/src/include/ros_headers.h b/src/livox_ros_driver2/src/include/ros_headers.h new file mode 100644 index 0000000..5c8b281 --- /dev/null +++ b/src/livox_ros_driver2/src/include/ros_headers.h @@ -0,0 +1,34 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef ROS_HEADERS_H_ +#define ROS_HEADERS_H_ + +#ifdef BUILDING_ROS1 +#include "ros1_headers.h" +#elif defined BUILDING_ROS2 +#include "ros2_headers.h" +#endif + +#endif // ROS_HEADERS_H_ diff --git a/src/livox_ros_driver2/src/lddc.cpp b/src/livox_ros_driver2/src/lddc.cpp new file mode 100644 index 0000000..7204f67 --- /dev/null +++ b/src/livox_ros_driver2/src/lddc.cpp @@ -0,0 +1,745 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "lddc.h" +#include "comm/ldq.h" +#include "comm/comm.h" + +#include +#include +#include +#include +#include + +#include "include/ros_headers.h" + +#include "driver_node.h" +#include "lds_lidar.h" + +namespace livox_ros { + +/** Lidar Data Distribute Control--------------------------------------------*/ +#ifdef BUILDING_ROS1 +Lddc::Lddc(int format, int multi_topic, int data_src, int output_type, + double frq, std::string &frame_id, bool lidar_bag, bool imu_bag) + : transfer_format_(format), + use_multi_topic_(multi_topic), + data_src_(data_src), + output_type_(output_type), + publish_frq_(frq), + frame_id_(frame_id), + enable_lidar_bag_(lidar_bag), + enable_imu_bag_(imu_bag) { + publish_period_ns_ = kNsPerSecond / publish_frq_; + lds_ = nullptr; + memset(private_pub_, 0, sizeof(private_pub_)); + memset(private_imu_pub_, 0, sizeof(private_imu_pub_)); + global_pub_ = nullptr; + global_imu_pub_ = nullptr; + cur_node_ = nullptr; + bag_ = nullptr; +} +#elif defined BUILDING_ROS2 +Lddc::Lddc(int format, int multi_topic, int data_src, int output_type, + double frq, std::string &frame_id) + : transfer_format_(format), + use_multi_topic_(multi_topic), + data_src_(data_src), + output_type_(output_type), + publish_frq_(frq), + frame_id_(frame_id) { + publish_period_ns_ = kNsPerSecond / publish_frq_; + lds_ = nullptr; + pointt = nullptr; + shm_initialized_ = false; +#if 0 + bag_ = nullptr; +#endif +} +#endif + +Lddc::~Lddc() { +#ifdef BUILDING_ROS1 + if (global_pub_) { + delete global_pub_; + } + + if (global_imu_pub_) { + delete global_imu_pub_; + } +#endif + + PrepareExit(); + +#ifdef BUILDING_ROS1 + for (uint32_t i = 0; i < kMaxSourceLidar; i++) { + if (private_pub_[i]) { + delete private_pub_[i]; + } + } + + for (uint32_t i = 0; i < kMaxSourceLidar; i++) { + if (private_imu_pub_[i]) { + delete private_imu_pub_[i]; + } + } +#endif + std::cout << "lddc destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl; +#ifdef BUILDING_ROS2 + if (pointt != nullptr && pointt != MAP_FAILED) { + munmap(pointt, sizeof(time_stamp)); + pointt = nullptr; + } +#endif +} + +int Lddc::RegisterLds(Lds *lds) { + if (lds_ == nullptr) { + lds_ = lds; + return 0; + } else { + return -1; + } +} + +void Lddc::DistributePointCloudData(void) { + if (!lds_) { + std::cout << "lds is not registered" << std::endl; + return; + } + if (lds_->IsRequestExit()) { + std::cout << "DistributePointCloudData is RequestExit" << std::endl; + return; + } + + lds_->pcd_semaphore_.Wait(); + for (uint32_t i = 0; i < lds_->lidar_count_; i++) { + uint32_t lidar_id = i; + LidarDevice *lidar = &lds_->lidars_[lidar_id]; + LidarDataQueue *p_queue = &lidar->data; + if ((kConnectStateSampling != lidar->connect_state) || (p_queue == nullptr)) { + continue; + } + PollingLidarPointCloudData(lidar_id, lidar); + } +} + +void Lddc::DistributeImuData(void) { + if (!lds_) { + std::cout << "lds is not registered" << std::endl; + return; + } + if (lds_->IsRequestExit()) { + std::cout << "DistributeImuData is RequestExit" << std::endl; + return; + } + + lds_->imu_semaphore_.Wait(); + for (uint32_t i = 0; i < lds_->lidar_count_; i++) { + uint32_t lidar_id = i; + LidarDevice *lidar = &lds_->lidars_[lidar_id]; + LidarImuDataQueue *p_queue = &lidar->imu_data; + if ((kConnectStateSampling != lidar->connect_state) || (p_queue == nullptr)) { + continue; + } + PollingLidarImuData(lidar_id, lidar); + } +} + +void Lddc::PollingLidarPointCloudData(uint8_t index, LidarDevice *lidar) { + LidarDataQueue *p_queue = &lidar->data; + if (p_queue == nullptr || p_queue->storage_packet == nullptr) { + return; + } + +#ifdef BUILDING_ROS2 + if (!shm_initialized_) { + const char *user_name = getlogin(); + std::string path_for_time_stamp = "/home/" + std::string(user_name) + "/timeshare"; + const char *shared_file_name = path_for_time_stamp.c_str(); + int fd = open(shared_file_name, O_CREAT | O_RDWR | O_TRUNC, 0666); + if (fd == -1) { + fprintf(stderr, "[lddc] Failed to open shared memory file: %s\n", shared_file_name); + } else { + fprintf(stderr, "[lddc] Shared memory file opened, fd=%d\n", fd); + lseek(fd, sizeof(time_stamp), SEEK_SET); + write(fd, "", 1); + pointt = (time_stamp *)mmap(NULL, sizeof(time_stamp), + PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + if (pointt == MAP_FAILED) { + fprintf(stderr, "[lddc] mmap failed\n"); + pointt = nullptr; + } + close(fd); + } + shm_initialized_ = true; + } +#endif + + while (!lds_->IsRequestExit() && !QueueIsEmpty(p_queue)) { + if (kPointCloud2Msg == transfer_format_) { + PublishPointcloud2(p_queue, index); + } else if (kLivoxCustomMsg == transfer_format_) { + PublishCustomPointcloud(p_queue, index); + } else if (kPclPxyziMsg == transfer_format_) { + PublishPclMsg(p_queue, index); + } + } +} + +void Lddc::PollingLidarImuData(uint8_t index, LidarDevice *lidar) { + LidarImuDataQueue& p_queue = lidar->imu_data; + while (!lds_->IsRequestExit() && !p_queue.Empty()) { + PublishImuData(p_queue, index); + } +} + +void Lddc::PrepareExit(void) { +#ifdef BUILDING_ROS1 + if (bag_) { + DRIVER_INFO(*cur_node_, "Waiting to save the bag file!"); + bag_->close(); + DRIVER_INFO(*cur_node_, "Save the bag file successfully!"); + bag_ = nullptr; + } +#endif + if (lds_) { + lds_->PrepareExit(); + lds_ = nullptr; + } +} + +void Lddc::PublishPointcloud2(LidarDataQueue *queue, uint8_t index) { + while(!QueueIsEmpty(queue)) { + StoragePacket pkg; + QueuePop(queue, &pkg); + if (pkg.points.empty()) { + printf("Publish point cloud2 failed, the pkg points is empty.\n"); + continue; + } + + PointCloud2 cloud; + uint64_t timestamp = 0; + InitPointcloud2Msg(pkg, cloud, timestamp); + PublishPointcloud2Data(index, timestamp, cloud); +#ifdef BUILDING_ROS2 + if (pointt != nullptr && pointt != MAP_FAILED) { + pointt->low = static_cast(timestamp); + } +#endif + } +} + +void Lddc::PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index) { + while(!QueueIsEmpty(queue)) { + StoragePacket pkg; + QueuePop(queue, &pkg); + if (pkg.points.empty()) { + printf("Publish custom point cloud failed, the pkg points is empty.\n"); + continue; + } + + CustomMsg livox_msg; + InitCustomMsg(livox_msg, pkg, index); + FillPointsToCustomMsg(livox_msg, pkg); + PublishCustomPointData(livox_msg, index); +#ifdef BUILDING_ROS2 + if (pointt != nullptr && pointt != MAP_FAILED) { + pointt->low = static_cast(pkg.base_time); + } +#endif + } +} + +/* for pcl::pxyzi */ +void Lddc::PublishPclMsg(LidarDataQueue *queue, uint8_t index) { +#ifdef BUILDING_ROS2 + static bool first_log = true; + if (first_log) { + std::cout << "error: message type 'pcl::PointCloud' is NOT supported in ROS2, " + << "please modify the 'xfer_format' field in the launch file" + << std::endl; + } + first_log = false; + return; +#endif + while(!QueueIsEmpty(queue)) { + StoragePacket pkg; + QueuePop(queue, &pkg); + if (pkg.points.empty()) { + printf("Publish point cloud failed, the pkg points is empty.\n"); + continue; + } + + PointCloud cloud; + uint64_t timestamp = 0; + InitPclMsg(pkg, cloud, timestamp); + FillPointsToPclMsg(pkg, cloud); + PublishPclData(index, timestamp, cloud); + } + return; +} + +void Lddc::InitPointcloud2MsgHeader(PointCloud2& cloud) { + cloud.header.frame_id.assign(frame_id_); + cloud.height = 1; + cloud.width = 0; + cloud.fields.resize(7); + cloud.fields[0].offset = 0; + cloud.fields[0].name = "x"; + cloud.fields[0].count = 1; + cloud.fields[0].datatype = PointField::FLOAT32; + cloud.fields[1].offset = 4; + cloud.fields[1].name = "y"; + cloud.fields[1].count = 1; + cloud.fields[1].datatype = PointField::FLOAT32; + cloud.fields[2].offset = 8; + cloud.fields[2].name = "z"; + cloud.fields[2].count = 1; + cloud.fields[2].datatype = PointField::FLOAT32; + cloud.fields[3].offset = 12; + cloud.fields[3].name = "intensity"; + cloud.fields[3].count = 1; + cloud.fields[3].datatype = PointField::FLOAT32; + cloud.fields[4].offset = 16; + cloud.fields[4].name = "tag"; + cloud.fields[4].count = 1; + cloud.fields[4].datatype = PointField::UINT8; + cloud.fields[5].offset = 17; + cloud.fields[5].name = "line"; + cloud.fields[5].count = 1; + cloud.fields[5].datatype = PointField::UINT8; + cloud.fields[6].offset = 18; + cloud.fields[6].name = "timestamp"; + cloud.fields[6].count = 1; + cloud.fields[6].datatype = PointField::FLOAT64; + cloud.point_step = sizeof(LivoxPointXyzrtlt); +} + +void Lddc::InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint64_t& timestamp) { + InitPointcloud2MsgHeader(cloud); + + cloud.point_step = sizeof(LivoxPointXyzrtlt); + + cloud.width = pkg.points_num; + cloud.row_step = cloud.width * cloud.point_step; + + cloud.is_bigendian = false; + cloud.is_dense = true; + + if (!pkg.points.empty()) { + timestamp = pkg.base_time; + } + + #ifdef BUILDING_ROS1 + cloud.header.stamp = ros::Time( timestamp / 1000000000.0); + #elif defined BUILDING_ROS2 + cloud.header.stamp = rclcpp::Time(timestamp); + #endif + + std::vector points; + for (size_t i = 0; i < pkg.points_num; ++i) { + LivoxPointXyzrtlt point; + point.x = pkg.points[i].x; + point.y = pkg.points[i].y; + point.z = pkg.points[i].z; + point.reflectivity = pkg.points[i].intensity; + point.tag = pkg.points[i].tag; + point.line = pkg.points[i].line; + point.timestamp = static_cast(pkg.points[i].offset_time); + points.push_back(std::move(point)); + } + cloud.data.resize(pkg.points_num * sizeof(LivoxPointXyzrtlt)); + memcpy(cloud.data.data(), points.data(), pkg.points_num * sizeof(LivoxPointXyzrtlt)); +} + +void Lddc::PublishPointcloud2Data(const uint8_t index, const uint64_t timestamp, const PointCloud2& cloud) { +#ifdef BUILDING_ROS1 + PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index); +#elif defined BUILDING_ROS2 + Publisher::SharedPtr publisher_ptr = + std::dynamic_pointer_cast>(GetCurrentPublisher(index)); +#endif + + if (kOutputToRos == output_type_) { + publisher_ptr->publish(cloud); + } else { +#ifdef BUILDING_ROS1 + if (bag_ && enable_lidar_bag_) { + bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), cloud); + } +#endif + } +} + +void Lddc::InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index) { + livox_msg.header.frame_id.assign(frame_id_); + +#ifdef BUILDING_ROS1 + static uint32_t msg_seq = 0; + livox_msg.header.seq = msg_seq; + ++msg_seq; +#endif + + uint64_t timestamp = 0; + if (!pkg.points.empty()) { + timestamp = pkg.base_time; + } + livox_msg.timebase = timestamp; + +#ifdef BUILDING_ROS1 + livox_msg.header.stamp = ros::Time(timestamp / 1000000000.0); +#elif defined BUILDING_ROS2 + livox_msg.header.stamp = rclcpp::Time(timestamp); +#endif + + livox_msg.point_num = pkg.points_num; + if (lds_->lidars_[index].lidar_type == kLivoxLidarType) { + livox_msg.lidar_id = lds_->lidars_[index].handle; + } else { + printf("Init custom msg lidar id failed, the index:%u.\n", index); + livox_msg.lidar_id = 0; + } +} + +void Lddc::FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg) { + uint32_t points_num = pkg.points_num; + const std::vector& points = pkg.points; + for (uint32_t i = 0; i < points_num; ++i) { + CustomPoint point; + point.x = points[i].x; + point.y = points[i].y; + point.z = points[i].z; + point.reflectivity = points[i].intensity; + point.tag = points[i].tag; + point.line = points[i].line; + point.offset_time = static_cast(points[i].offset_time - pkg.base_time); + + livox_msg.points.push_back(std::move(point)); + } +} + +void Lddc::PublishCustomPointData(const CustomMsg& livox_msg, const uint8_t index) { +#ifdef BUILDING_ROS1 + PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index); +#elif defined BUILDING_ROS2 + Publisher::SharedPtr publisher_ptr = std::dynamic_pointer_cast>(GetCurrentPublisher(index)); +#endif + + if (kOutputToRos == output_type_) { + publisher_ptr->publish(livox_msg); + } else { +#ifdef BUILDING_ROS1 + if (bag_ && enable_lidar_bag_) { + bag_->write(publisher_ptr->getTopic(), ros::Time(livox_msg.timebase / 1000000000.0), livox_msg); + } +#endif + } +} + +void Lddc::InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp) { +#ifdef BUILDING_ROS1 + cloud.header.frame_id.assign(frame_id_); + cloud.height = 1; + cloud.width = pkg.points_num; + + if (!pkg.points.empty()) { + timestamp = pkg.base_time; + } + cloud.header.stamp = timestamp / 1000.0; // to pcl ros time stamp +#elif defined BUILDING_ROS2 + std::cout << "warning: pcl::PointCloud is not supported in ROS2, " + << "please check code logic" + << std::endl; +#endif + return; +} + +void Lddc::FillPointsToPclMsg(const StoragePacket& pkg, PointCloud& pcl_msg) { +#ifdef BUILDING_ROS1 + if (pkg.points.empty()) { + return; + } + + uint32_t points_num = pkg.points_num; + const std::vector& points = pkg.points; + for (uint32_t i = 0; i < points_num; ++i) { + pcl::PointXYZI point; + point.x = points[i].x; + point.y = points[i].y; + point.z = points[i].z; + point.intensity = points[i].intensity; + + pcl_msg.points.push_back(std::move(point)); + } +#elif defined BUILDING_ROS2 + std::cout << "warning: pcl::PointCloud is not supported in ROS2, " + << "please check code logic" + << std::endl; +#endif + return; +} + +void Lddc::PublishPclData(const uint8_t index, const uint64_t timestamp, const PointCloud& cloud) { +#ifdef BUILDING_ROS1 + PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index); + if (kOutputToRos == output_type_) { + publisher_ptr->publish(cloud); + } else { + if (bag_ && enable_lidar_bag_) { + bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), cloud); + } + } +#elif defined BUILDING_ROS2 + std::cout << "warning: pcl::PointCloud is not supported in ROS2, " + << "please check code logic" + << std::endl; +#endif + return; +} + +void Lddc::InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp) { + imu_msg.header.frame_id = "livox_frame"; + + timestamp = imu_data.time_stamp; +#ifdef BUILDING_ROS1 + imu_msg.header.stamp = ros::Time(timestamp / 1000000000.0); // to ros time stamp +#elif defined BUILDING_ROS2 + imu_msg.header.stamp = rclcpp::Time(timestamp); // to ros time stamp +#endif + + imu_msg.angular_velocity.x = imu_data.gyro_x; + imu_msg.angular_velocity.y = imu_data.gyro_y; + imu_msg.angular_velocity.z = imu_data.gyro_z; + imu_msg.linear_acceleration.x = imu_data.acc_x; + imu_msg.linear_acceleration.y = imu_data.acc_y; + imu_msg.linear_acceleration.z = imu_data.acc_z; +} + +void Lddc::PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index) { + ImuData imu_data; + if (!imu_data_queue.Pop(imu_data)) { + //printf("Publish imu data failed, imu data queue pop failed.\n"); + return; + } + + ImuMsg imu_msg; + uint64_t timestamp; + InitImuMsg(imu_data, imu_msg, timestamp); + +#ifdef BUILDING_ROS1 + PublisherPtr publisher_ptr = GetCurrentImuPublisher(index); +#elif defined BUILDING_ROS2 + Publisher::SharedPtr publisher_ptr = std::dynamic_pointer_cast>(GetCurrentImuPublisher(index)); +#endif + + if (kOutputToRos == output_type_) { + publisher_ptr->publish(imu_msg); + } else { +#ifdef BUILDING_ROS1 + if (bag_ && enable_imu_bag_) { + bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), imu_msg); + } +#endif + } +} + +#ifdef BUILDING_ROS2 +std::shared_ptr Lddc::CreatePublisher(uint8_t msg_type, + std::string &topic_name, uint32_t queue_size) { + if (kPointCloud2Msg == msg_type) { + DRIVER_INFO(*cur_node_, + "%s publish use PointCloud2 format", topic_name.c_str()); + return cur_node_->create_publisher(topic_name, queue_size); + } else if (kLivoxCustomMsg == msg_type) { + DRIVER_INFO(*cur_node_, + "%s publish use livox custom format", topic_name.c_str()); + return cur_node_->create_publisher(topic_name, queue_size); + } +#if 0 + else if (kPclPxyziMsg == msg_type) { + DRIVER_INFO(*cur_node_, + "%s publish use pcl PointXYZI format", topic_name.c_str()); + return cur_node_->create_publisher(topic_name, queue_size); + } +#endif + else if (kLivoxImuMsg == msg_type) { + DRIVER_INFO(*cur_node_, + "%s publish use imu format", topic_name.c_str()); + return cur_node_->create_publisher(topic_name, + queue_size); + } else { + PublisherPtr null_publisher(nullptr); + return null_publisher; + } +} +#endif + +#ifdef BUILDING_ROS1 +PublisherPtr Lddc::GetCurrentPublisher(uint8_t index) { + ros::Publisher **pub = nullptr; + uint32_t queue_size = kMinEthPacketQueueSize; + + if (use_multi_topic_) { + pub = &private_pub_[index]; + queue_size = queue_size / 8; // queue size is 4 for only one lidar + } else { + pub = &global_pub_; + queue_size = queue_size * 8; // shared queue size is 256, for all lidars + } + + if (*pub == nullptr) { + char name_str[48]; + memset(name_str, 0, sizeof(name_str)); + if (use_multi_topic_) { + std::string ip_string = IpNumToString(lds_->lidars_[index].handle); + snprintf(name_str, sizeof(name_str), "livox/lidar_%s", + ReplacePeriodByUnderline(ip_string).c_str()); + DRIVER_INFO(*cur_node_, "Support multi topics."); + } else { + DRIVER_INFO(*cur_node_, "Support only one topic."); + snprintf(name_str, sizeof(name_str), "livox/lidar"); + } + + *pub = new ros::Publisher; + if (kPointCloud2Msg == transfer_format_) { + **pub = + cur_node_->GetNode().advertise(name_str, queue_size); + DRIVER_INFO(*cur_node_, + "%s publish use PointCloud2 format, set ROS publisher queue size %d", + name_str, queue_size); + } else if (kLivoxCustomMsg == transfer_format_) { + **pub = cur_node_->GetNode().advertise(name_str, + queue_size); + DRIVER_INFO(*cur_node_, + "%s publish use livox custom format, set ROS publisher queue size %d", + name_str, queue_size); + } else if (kPclPxyziMsg == transfer_format_) { + **pub = cur_node_->GetNode().advertise(name_str, queue_size); + DRIVER_INFO(*cur_node_, + "%s publish use pcl PointXYZI format, set ROS publisher queue " + "size %d", + name_str, queue_size); + } + } + + return *pub; +} + +PublisherPtr Lddc::GetCurrentImuPublisher(uint8_t handle) { + ros::Publisher **pub = nullptr; + uint32_t queue_size = kMinEthPacketQueueSize; + + if (use_multi_topic_) { + pub = &private_imu_pub_[handle]; + queue_size = queue_size * 2; // queue size is 64 for only one lidar + } else { + pub = &global_imu_pub_; + queue_size = queue_size * 8; // shared queue size is 256, for all lidars + } + + if (*pub == nullptr) { + char name_str[48]; + memset(name_str, 0, sizeof(name_str)); + if (use_multi_topic_) { + DRIVER_INFO(*cur_node_, "Support multi topics."); + std::string ip_string = IpNumToString(lds_->lidars_[handle].handle); + snprintf(name_str, sizeof(name_str), "livox/imu_%s", + ReplacePeriodByUnderline(ip_string).c_str()); + } else { + DRIVER_INFO(*cur_node_, "Support only one topic."); + snprintf(name_str, sizeof(name_str), "livox/imu"); + } + + *pub = new ros::Publisher; + **pub = cur_node_->GetNode().advertise(name_str, queue_size); + DRIVER_INFO(*cur_node_, "%s publish imu data, set ROS publisher queue size %d", name_str, + queue_size); + } + + return *pub; +} +#elif defined BUILDING_ROS2 +std::shared_ptr Lddc::GetCurrentPublisher(uint8_t handle) { + uint32_t queue_size = kMinEthPacketQueueSize; + if (use_multi_topic_) { + if (!private_pub_[handle]) { + char name_str[48]; + memset(name_str, 0, sizeof(name_str)); + + std::string ip_string = IpNumToString(lds_->lidars_[handle].handle); + snprintf(name_str, sizeof(name_str), "livox/lidar_%s", + ReplacePeriodByUnderline(ip_string).c_str()); + std::string topic_name(name_str); + queue_size = queue_size * 2; // queue size is 64 for only one lidar + private_pub_[handle] = CreatePublisher(transfer_format_, topic_name, queue_size); + } + return private_pub_[handle]; + } else { + if (!global_pub_) { + std::string topic_name("livox/lidar"); + queue_size = queue_size * 8; // shared queue size is 256, for all lidars + global_pub_ = CreatePublisher(transfer_format_, topic_name, queue_size); + } + return global_pub_; + } +} + +std::shared_ptr Lddc::GetCurrentImuPublisher(uint8_t handle) { + uint32_t queue_size = kMinEthPacketQueueSize; + if (use_multi_topic_) { + if (!private_imu_pub_[handle]) { + char name_str[48]; + memset(name_str, 0, sizeof(name_str)); + std::string ip_string = IpNumToString(lds_->lidars_[handle].handle); + snprintf(name_str, sizeof(name_str), "livox/imu_%s", + ReplacePeriodByUnderline(ip_string).c_str()); + std::string topic_name(name_str); + queue_size = queue_size * 2; // queue size is 64 for only one lidar + private_imu_pub_[handle] = CreatePublisher(kLivoxImuMsg, topic_name, + queue_size); + } + return private_imu_pub_[handle]; + } else { + if (!global_imu_pub_) { + std::string topic_name("livox/imu"); + queue_size = queue_size * 8; // shared queue size is 256, for all lidars + global_imu_pub_ = CreatePublisher(kLivoxImuMsg, topic_name, queue_size); + } + return global_imu_pub_; + } +} +#endif + +void Lddc::CreateBagFile(const std::string &file_name) { +#ifdef BUILDING_ROS1 + if (!bag_) { + bag_ = new rosbag::Bag; + bag_->open(file_name, rosbag::bagmode::Write); + DRIVER_INFO(*cur_node_, "Create bag file :%s!", file_name.c_str()); + } +#endif +} + +} // namespace livox_ros diff --git a/src/livox_ros_driver2/src/lddc.h b/src/livox_ros_driver2/src/lddc.h new file mode 100644 index 0000000..7a5a95c --- /dev/null +++ b/src/livox_ros_driver2/src/lddc.h @@ -0,0 +1,183 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER2_LDDC_H_ +#define LIVOX_ROS_DRIVER2_LDDC_H_ + +#include "include/livox_ros_driver2.h" + +#include "driver_node.h" +#include "lds.h" + +#ifdef BUILDING_ROS2 +#include +#include +#include +#endif + +namespace livox_ros { + +#ifdef BUILDING_ROS2 +typedef struct { + int64_t high; + int64_t low; +} time_stamp; +#endif + +/** Send pointcloud message Data to ros subscriber or save them in rosbag file */ +typedef enum { + kOutputToRos = 0, + kOutputToRosBagFile = 1, +} DestinationOfMessageOutput; + +/** The message type of transfer */ +typedef enum { + kPointCloud2Msg = 0, + kLivoxCustomMsg = 1, + kPclPxyziMsg = 2, + kLivoxImuMsg = 3, +} TransferType; + +/** Type-Definitions based on ROS versions */ +#ifdef BUILDING_ROS1 +using Publisher = ros::Publisher; +using PublisherPtr = ros::Publisher*; +using PointCloud2 = sensor_msgs::PointCloud2; +using PointField = sensor_msgs::PointField; +using CustomMsg = livox_ros_driver2::CustomMsg; +using CustomPoint = livox_ros_driver2::CustomPoint; +using ImuMsg = sensor_msgs::Imu; +#elif defined BUILDING_ROS2 +template using Publisher = rclcpp::Publisher; +using PublisherPtr = std::shared_ptr; +using PointCloud2 = sensor_msgs::msg::PointCloud2; +using PointField = sensor_msgs::msg::PointField; +using CustomMsg = livox_ros_driver2::msg::CustomMsg; +using CustomPoint = livox_ros_driver2::msg::CustomPoint; +using ImuMsg = sensor_msgs::msg::Imu; +#endif + +using PointCloud = pcl::PointCloud; + +class DriverNode; + +class Lddc final { + public: +#ifdef BUILDING_ROS1 + Lddc(int format, int multi_topic, int data_src, int output_type, double frq, + std::string &frame_id, bool lidar_bag, bool imu_bag); +#elif defined BUILDING_ROS2 + Lddc(int format, int multi_topic, int data_src, int output_type, double frq, + std::string &frame_id); +#endif + ~Lddc(); + + int RegisterLds(Lds *lds); + void DistributePointCloudData(void); + void DistributeImuData(void); + void CreateBagFile(const std::string &file_name); + void PrepareExit(void); + + uint8_t GetTransferFormat(void) { return transfer_format_; } + uint8_t IsMultiTopic(void) { return use_multi_topic_; } + void SetRosNode(livox_ros::DriverNode *node) { cur_node_ = node; } + + // void SetRosPub(ros::Publisher *pub) { global_pub_ = pub; }; // NOT USED + void SetPublishFrq(uint32_t frq) { publish_frq_ = frq; } + + public: + Lds *lds_; +#ifdef BUILDING_ROS2 + time_stamp *pointt; +#endif + + private: + void PollingLidarPointCloudData(uint8_t index, LidarDevice *lidar); + void PollingLidarImuData(uint8_t index, LidarDevice *lidar); + + void PublishPointcloud2(LidarDataQueue *queue, uint8_t index); + void PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index); + void PublishPclMsg(LidarDataQueue *queue, uint8_t index); + + void PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index); + + void InitPointcloud2MsgHeader(PointCloud2& cloud); + void InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint64_t& timestamp); + void PublishPointcloud2Data(const uint8_t index, uint64_t timestamp, const PointCloud2& cloud); + + void InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index); + void FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg); + void PublishCustomPointData(const CustomMsg& livox_msg, const uint8_t index); + + void InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp); + void FillPointsToPclMsg(const StoragePacket& pkg, PointCloud& pcl_msg); + void PublishPclData(const uint8_t index, const uint64_t timestamp, const PointCloud& cloud); + + void InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp); + + void FillPointsToPclMsg(PointCloud& pcl_msg, LivoxPointXyzrtlt* src_point, uint32_t num); + void FillPointsToCustomMsg(CustomMsg& livox_msg, LivoxPointXyzrtlt* src_point, uint32_t num, + uint32_t offset_time, uint32_t point_interval, uint32_t echo_num); + +#ifdef BUILDING_ROS2 + PublisherPtr CreatePublisher(uint8_t msg_type, std::string &topic_name, uint32_t queue_size); +#endif + + PublisherPtr GetCurrentPublisher(uint8_t index); + PublisherPtr GetCurrentImuPublisher(uint8_t index); + + private: + uint8_t transfer_format_; + uint8_t use_multi_topic_; + uint8_t data_src_; + uint8_t output_type_; + double publish_frq_; + uint32_t publish_period_ns_; + std::string frame_id_; + +#ifdef BUILDING_ROS1 + bool enable_lidar_bag_; + bool enable_imu_bag_; + PublisherPtr private_pub_[kMaxSourceLidar]; + PublisherPtr global_pub_; + PublisherPtr private_imu_pub_[kMaxSourceLidar]; + PublisherPtr global_imu_pub_; + rosbag::Bag *bag_; +#elif defined BUILDING_ROS2 + PublisherPtr private_pub_[kMaxSourceLidar]; + PublisherPtr global_pub_; + PublisherPtr private_imu_pub_[kMaxSourceLidar]; + PublisherPtr global_imu_pub_; +#endif + + livox_ros::DriverNode *cur_node_; + +#ifdef BUILDING_ROS2 + bool shm_initialized_; +#endif +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER2_LDDC_H_ diff --git a/src/livox_ros_driver2/src/lds.cpp b/src/livox_ros_driver2/src/lds.cpp new file mode 100644 index 0000000..ca98462 --- /dev/null +++ b/src/livox_ros_driver2/src/lds.cpp @@ -0,0 +1,200 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include +#include +#include +#include +#include +#include + +#include "lds.h" +#include "comm/ldq.h" + +namespace livox_ros { + +CacheIndex Lds::cache_index_; + +/* Member function --------------------------------------------------------- */ +Lds::Lds(const double publish_freq, const uint8_t data_src) + : lidar_count_(kMaxSourceLidar), + pcd_semaphore_(0), + imu_semaphore_(0), + publish_freq_(publish_freq), + data_src_(data_src), + request_exit_(false) { + ResetLds(data_src_); +} + +Lds::~Lds() { + lidar_count_ = 0; + ResetLds(0); + printf("lds destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"); +} + +void Lds::ResetLidar(LidarDevice *lidar, uint8_t data_src) { + //cache_index_.ResetIndex(lidar); + DeInitQueue(&lidar->data); + lidar->imu_data.Clear(); + + lidar->data_src = data_src; + lidar->connect_state = kConnectStateOff; +} + +void Lds::SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src) { + lidar->data_src = data_src; +} + +void Lds::ResetLds(uint8_t data_src) { + lidar_count_ = kMaxSourceLidar; + for (uint32_t i = 0; i < kMaxSourceLidar; i++) { + ResetLidar(&lidars_[i], data_src); + } +} + +void Lds::RequestExit() { + request_exit_ = true; +} + +bool Lds::IsAllQueueEmpty() { + for (int i = 0; i < lidar_count_; i++) { + if (!QueueIsEmpty(&lidars_[i].data)) { + return false; + } + } + return true; +} + +bool Lds::IsAllQueueReadStop() { + for (int i = 0; i < lidar_count_; i++) { + uint32_t data_size = QueueUsedSize(&lidars_[i].data); + if (data_size) { + return false; + } + } + return true; +} + +void Lds::StorageImuData(ImuData* imu_data) { + uint32_t device_num = 0; + if (imu_data->lidar_type == kLivoxLidarType) { + device_num = imu_data->handle; + } else { + printf("Storage imu data failed, unknown lidar type:%u.\n", imu_data->lidar_type); + return; + } + + uint8_t index = 0; + int ret = cache_index_.GetIndex(imu_data->lidar_type, device_num, index); + if (ret != 0) { + printf("Storage point data failed, can not get index, lidar type:%u, device_num:%u.\n", imu_data->lidar_type, device_num); + return; + } + + LidarDevice *p_lidar = &lidars_[index]; + LidarImuDataQueue* imu_queue = &p_lidar->imu_data; + imu_queue->Push(imu_data); + if (!imu_queue->Empty()) { + if (imu_semaphore_.GetCount() <= 0) { + imu_semaphore_.Signal(); + } + } +} + +void Lds::StorageLvxPointData(PointFrame* frame) { + if (frame == nullptr) { + return; + } + + uint8_t lidar_number = frame->lidar_num; + for (uint i = 0; i < lidar_number; ++i) { + PointPacket& lidar_point = frame->lidar_point[i]; + + uint64_t base_time = frame->base_time[i]; + uint8_t index = 0; + int8_t ret = cache_index_.LvxGetIndex(lidar_point.lidar_type, lidar_point.handle, index); + if (ret != 0) { + printf("Storage lvx point data failed, lidar type:%u, device num:%u.\n", lidar_point.lidar_type, lidar_point.handle); + continue; + } + + lidars_[index].connect_state = kConnectStateSampling; + + PushLidarData(&lidar_point, index, base_time); + } +} + +void Lds::StoragePointData(PointFrame* frame) { + if (frame == nullptr) { + return; + } + + uint8_t lidar_number = frame->lidar_num; + for (uint i = 0; i < lidar_number; ++i) { + PointPacket& lidar_point = frame->lidar_point[i]; + //printf("StoragePointData, lidar_type:%u, point_num:%lu.\n", lidar_point.lidar_type, lidar_point.points_num); + + uint64_t base_time = frame->base_time[i]; + + uint8_t index = 0; + int8_t ret = cache_index_.GetIndex(lidar_point.lidar_type, lidar_point.handle, index); + if (ret != 0) { + printf("Storage point data failed, lidar type:%u, handle:%u.\n", lidar_point.lidar_type, lidar_point.handle); + continue; + } + PushLidarData(&lidar_point, index, base_time); + } +} + +void Lds::PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint64_t base_time) { + if (lidar_data == nullptr) { + return; + } + + LidarDevice *p_lidar = &lidars_[index]; + LidarDataQueue *queue = &p_lidar->data; + + if (nullptr == queue->storage_packet) { + uint32_t queue_size = CalculatePacketQueueSize(publish_freq_); + InitQueue(queue, queue_size); + printf("Lidar[%u] storage queue size: %u\n", index, queue_size); + } + + if (!QueueIsFull(queue)) { + QueuePushAny(queue, (uint8_t *)lidar_data, base_time); + if (!QueueIsEmpty(queue)) { + if (pcd_semaphore_.GetCount() <= 0) { + pcd_semaphore_.Signal(); + } + } + } else { + if (pcd_semaphore_.GetCount() <= 0) { + pcd_semaphore_.Signal(); + } + } +} + +void Lds::PrepareExit(void) {} + +} // namespace livox_ros diff --git a/src/livox_ros_driver2/src/lds.h b/src/livox_ros_driver2/src/lds.h new file mode 100644 index 0000000..c61eeea --- /dev/null +++ b/src/livox_ros_driver2/src/lds.h @@ -0,0 +1,83 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +// livox lidar data source + +#ifndef LIVOX_ROS_DRIVER_LDS_H_ +#define LIVOX_ROS_DRIVER_LDS_H_ + +#include + +#include "comm/semaphore.h" +#include "comm/comm.h" +#include "comm/cache_index.h" + +namespace livox_ros { +/** + * Lidar data source abstract. + */ +class Lds { + public: + Lds(const double publish_freq, const uint8_t data_src); + virtual ~Lds(); + + void StorageImuData(ImuData* imu_data); + void StoragePointData(PointFrame* frame); + void StorageLvxPointData(PointFrame* frame); + + int8_t GetHandle(const uint8_t lidar_type, const PointPacket* lidar_point); + void PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint64_t base_time); + + static void ResetLidar(LidarDevice *lidar, uint8_t data_src); + static void SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src); + void ResetLds(uint8_t data_src); + + void RequestExit(); + + bool IsAllQueueEmpty(); + bool IsAllQueueReadStop(); + + void CleanRequestExit() { request_exit_ = false; } + bool IsRequestExit() { return request_exit_; } + virtual void PrepareExit(void); + + // get publishing frequency + double GetLdsFrequency() { return publish_freq_; } + + public: + uint8_t lidar_count_; /**< Lidar access handle. */ + LidarDevice lidars_[kMaxSourceLidar]; /**< The index is the handle */ + Semaphore pcd_semaphore_; + Semaphore imu_semaphore_; + static CacheIndex cache_index_; + protected: + double publish_freq_; + uint8_t data_src_; + private: + volatile bool request_exit_; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LDS_H_ diff --git a/src/livox_ros_driver2/src/lds_lidar.cpp b/src/livox_ros_driver2/src/lds_lidar.cpp new file mode 100644 index 0000000..a500ef9 --- /dev/null +++ b/src/livox_ros_driver2/src/lds_lidar.cpp @@ -0,0 +1,214 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "lds_lidar.h" + +#include +#include +#include +#include +#include + +#ifdef WIN32 +#include +#include +#pragma comment(lib, "Ws2_32.lib") +#else +#include +#include +#include +#include +#endif // WIN32 +#include "livox_lidar_api.h" +#include "comm/comm.h" +#include "comm/pub_handler.h" + +#include "parse_cfg_file/parse_cfg_file.h" +#include "parse_cfg_file/parse_livox_lidar_cfg.h" + +#include "call_back/lidar_common_callback.h" +#include "call_back/livox_lidar_callback.h" + +using namespace std; + +namespace livox_ros { + +/** Const varible ------------------------------------------------------------*/ +/** For callback use only */ +LdsLidar *g_lds_ldiar = nullptr; + +/** Global function for common use -------------------------------------------*/ + +/** Lds lidar function -------------------------------------------------------*/ +LdsLidar::LdsLidar(double publish_freq) + : Lds(publish_freq, kSourceRawLidar), + auto_connect_mode_(true), + whitelist_count_(0), + is_initialized_(false) { + memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_)); + ResetLdsLidar(); +} + +LdsLidar::~LdsLidar() {} + +void LdsLidar::ResetLdsLidar(void) { ResetLds(kSourceRawLidar); } + + + +bool LdsLidar::InitLdsLidar(const std::string& path_name) { + if (is_initialized_) { + printf("Lds is already inited!\n"); + return false; + } + + if (g_lds_ldiar == nullptr) { + g_lds_ldiar = this; + } + + path_ = path_name; + if (!InitLidars()) { + return false; + } + SetLidarPubHandle(); + if (!Start()) { + return false; + } + is_initialized_ = true; + return true; +} + +bool LdsLidar::InitLidars() { + if (!ParseSummaryConfig()) { + return false; + } + std::cout << "config lidar type: " << static_cast(lidar_summary_info_.lidar_type) << std::endl; + + if (lidar_summary_info_.lidar_type & kLivoxLidarType) { + if (!InitLivoxLidar()) { + return false; + } + } + return true; +} + + +bool LdsLidar::Start() { + if (lidar_summary_info_.lidar_type & kLivoxLidarType) { + if (!LivoxLidarStart()) { + return false; + } + } + return true; +} + +bool LdsLidar::ParseSummaryConfig() { + return ParseCfgFile(path_).ParseSummaryInfo(lidar_summary_info_); +} + +bool LdsLidar::InitLivoxLidar() { +#ifdef BUILDING_ROS2 + DisableLivoxSdkConsoleLogger(); +#endif + + // parse user config + LivoxLidarConfigParser parser(path_); + std::vector user_configs; + if (!parser.Parse(user_configs)) { + std::cout << "failed to parse user-defined config" << std::endl; + } + + // SDK initialization + if (!LivoxLidarSdkInit(path_.c_str())) { + std::cout << "Failed to init livox lidar sdk." << std::endl; + return false; + } + + // fill in lidar devices + for (auto& config : user_configs) { + uint8_t index = 0; + int8_t ret = g_lds_ldiar->cache_index_.GetFreeIndex(kLivoxLidarType, config.handle, index); + if (ret != 0) { + std::cout << "failed to get free index, lidar ip: " << IpNumToString(config.handle) << std::endl; + continue; + } + LidarDevice *p_lidar = &(g_lds_ldiar->lidars_[index]); + p_lidar->lidar_type = kLivoxLidarType; + p_lidar->livox_config = config; + p_lidar->handle = config.handle; + + LidarExtParameter lidar_param; + lidar_param.handle = config.handle; + lidar_param.lidar_type = kLivoxLidarType; + if (config.pcl_data_type == kLivoxLidarCartesianCoordinateLowData) { + // temporary resolution + lidar_param.param.roll = config.extrinsic_param.roll; + lidar_param.param.pitch = config.extrinsic_param.pitch; + lidar_param.param.yaw = config.extrinsic_param.yaw; + lidar_param.param.x = config.extrinsic_param.x / 10; + lidar_param.param.y = config.extrinsic_param.y / 10; + lidar_param.param.z = config.extrinsic_param.z / 10; + } else { + lidar_param.param.roll = config.extrinsic_param.roll; + lidar_param.param.pitch = config.extrinsic_param.pitch; + lidar_param.param.yaw = config.extrinsic_param.yaw; + lidar_param.param.x = config.extrinsic_param.x; + lidar_param.param.y = config.extrinsic_param.y; + lidar_param.param.z = config.extrinsic_param.z; + } + pub_handler().AddLidarsExtParam(lidar_param); + } + + SetLivoxLidarInfoChangeCallback(LivoxLidarCallback::LidarInfoChangeCallback, g_lds_ldiar); + return true; +} + +void LdsLidar::SetLidarPubHandle() { + pub_handler().SetPointCloudsCallback(LidarCommonCallback::OnLidarPointClounCb, g_lds_ldiar); + pub_handler().SetImuDataCallback(LidarCommonCallback::LidarImuDataCallback, g_lds_ldiar); + + double publish_freq = Lds::GetLdsFrequency(); + pub_handler().SetPointCloudConfig(publish_freq); +} + +bool LdsLidar::LivoxLidarStart() { + return true; +} + +int LdsLidar::DeInitLdsLidar(void) { + if (!is_initialized_) { + printf("LiDAR data source is not exit"); + return -1; + } + + if (lidar_summary_info_.lidar_type & kLivoxLidarType) { + LivoxLidarSdkUninit(); + printf("Livox Lidar SDK Deinit completely!\n"); + } + + return 0; +} + +void LdsLidar::PrepareExit(void) { DeInitLdsLidar(); } + +} // namespace livox_ros diff --git a/src/livox_ros_driver2/src/lds_lidar.h b/src/livox_ros_driver2/src/lds_lidar.h new file mode 100644 index 0000000..72e58a0 --- /dev/null +++ b/src/livox_ros_driver2/src/lds_lidar.h @@ -0,0 +1,94 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +/** Livox LiDAR data source, data from dependent lidar */ + +#ifndef LIVOX_ROS_DRIVER_LDS_LIDAR_H_ +#define LIVOX_ROS_DRIVER_LDS_LIDAR_H_ + +#include +#include +#include + +#include "lds.h" +#include "comm/comm.h" + +#include "livox_lidar_def.h" + +#include "rapidjson/document.h" + +namespace livox_ros { + +class LdsLidar final : public Lds { + public: + static LdsLidar *GetInstance(double publish_freq) { + printf("LdsLidar *GetInstance\n"); + static LdsLidar lds_lidar(publish_freq); + return &lds_lidar; + } + + bool InitLdsLidar(const std::string& path_name); + bool Start(); + + int DeInitLdsLidar(void); + private: + LdsLidar(double publish_freq); + LdsLidar(const LdsLidar &) = delete; + ~LdsLidar(); + LdsLidar &operator=(const LdsLidar &) = delete; + + bool ParseSummaryConfig(); + + bool InitLidars(); + bool InitLivoxLidar(); // for new SDK + + bool LivoxLidarStart(); + + void ResetLdsLidar(void); + + void SetLidarPubHandle(); + + // auto connect mode + void EnableAutoConnectMode(void) { auto_connect_mode_ = true; } + void DisableAutoConnectMode(void) { auto_connect_mode_ = false; } + bool IsAutoConnectMode(void) { return auto_connect_mode_; } + + virtual void PrepareExit(void); + + public: + std::mutex config_mutex_; + + private: + std::string path_; + LidarSummaryInfo lidar_summary_info_; + + bool auto_connect_mode_; + uint32_t whitelist_count_; + volatile bool is_initialized_; + char broadcast_code_whitelist_[kMaxLidarCount][kBroadcastCodeSize]; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LDS_LIDAR_H_ diff --git a/src/livox_ros_driver2/src/lds_lvx.cpp b/src/livox_ros_driver2/src/lds_lvx.cpp new file mode 100644 index 0000000..7b7f810 --- /dev/null +++ b/src/livox_ros_driver2/src/lds_lvx.cpp @@ -0,0 +1,88 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "lds_lvx.h" + +namespace livox_ros { + +// std::condition_variable LdsLvx::cv_; +// std::mutex LdsLvx::mtx_; +std::atomic_bool LdsLvx::is_file_end_(false); + +LdsLvx::LdsLvx(double publish_freq) : Lds(publish_freq, kSourceLvxFile), is_initialized_(false) { +} + +LdsLvx::~LdsLvx() { +} + +int LdsLvx::Init(const char *lvx_path) { + if (is_initialized_) { + printf("Livox file data source is already inited!\n"); + return -1; + } + +#ifdef BUILDING_ROS2 + DisableLivoxSdkConsoleLogger(); +#endif + + printf("Lds lvx init lvx_path:%s.\n", lvx_path); + + is_initialized_ = true; + return 0; +} + +void LdsLvx::OnPointCloudsFrameCallback(uint32_t frame_index, uint32_t total_frame, PointFrame *point_cloud_frame, void *client_data) { + if (!point_cloud_frame) { + printf("Point clouds frame call back failed, point cloud frame is nullptr.\n"); + return; + } + + LdsLvx* lds = static_cast(client_data); + if (lds == nullptr) { + printf("Point clouds frame call back failed, client data is nullptr.\n"); + return; + } + + lds->StorageLvxPointData(point_cloud_frame); + + if (frame_index == total_frame) { + is_file_end_.store(true); + } +} + +void LdsLvx::ReadLvxFile() { +} + +} // namespace livox_ros + + diff --git a/src/livox_ros_driver2/src/lds_lvx.h b/src/livox_ros_driver2/src/lds_lvx.h new file mode 100644 index 0000000..43336aa --- /dev/null +++ b/src/livox_ros_driver2/src/lds_lvx.h @@ -0,0 +1,68 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +// livox lidar lvx data source + +#ifndef LIVOX_ROS_DRIVER_LDS_LVX_H_ +#define LIVOX_ROS_DRIVER_LDS_LVX_H_ + +#include +#include + +#include "lds.h" +#include "comm/comm.h" + +#include "livox_lidar_api.h" +#include "livox_lidar_def.h" + +namespace livox_ros { +/** + * Lidar data source abstract. + */ +class LdsLvx final : public Lds { + public: + static LdsLvx *GetInstance(double publish_freq) { + static LdsLvx lds_lvx(publish_freq); + return &lds_lvx; + } + + int Init(const char *lvx_path); + + void ReadLvxFile(); + + private: + LdsLvx(double publish_freq); + LdsLvx(const LdsLvx &) = delete; + ~LdsLvx(); + LdsLvx &operator=(const LdsLvx &) = delete; + + static void OnPointCloudsFrameCallback(uint32_t frame_index, uint32_t total_frame, PointFrame *point_cloud_frame, void *client_data); + + private: + volatile bool is_initialized_; + static std::atomic_bool is_file_end_; +}; + +} // namespace livox_ros +#endif diff --git a/src/livox_ros_driver2/src/livox_ros_driver2.cpp b/src/livox_ros_driver2/src/livox_ros_driver2.cpp new file mode 100644 index 0000000..6dda05f --- /dev/null +++ b/src/livox_ros_driver2/src/livox_ros_driver2.cpp @@ -0,0 +1,235 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include +#include +#include +#include +#include + +#include "include/livox_ros_driver2.h" +#include "include/ros_headers.h" +#include "driver_node.h" +#include "lddc.h" +#include "lds_lidar.h" + +using namespace livox_ros; + +#ifdef BUILDING_ROS1 +int main(int argc, char **argv) { + /** Ros related */ + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { + ros::console::notifyLoggerLevelsChanged(); + } + + ros::init(argc, argv, "livox_lidar_publisher"); + + // ros::NodeHandle livox_node; + livox_ros::DriverNode livox_node; + + DRIVER_INFO(livox_node, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING); + + /** Init default system parameter */ + int xfer_format = kPointCloud2Msg; + int multi_topic = 0; + int data_src = kSourceRawLidar; + double publish_freq = 10.0; /* Hz */ + int output_type = kOutputToRos; + std::string frame_id = "livox_frame"; + bool lidar_bag = true; + bool imu_bag = false; + + livox_node.GetNode().getParam("xfer_format", xfer_format); + livox_node.GetNode().getParam("multi_topic", multi_topic); + livox_node.GetNode().getParam("data_src", data_src); + livox_node.GetNode().getParam("publish_freq", publish_freq); + livox_node.GetNode().getParam("output_data_type", output_type); + livox_node.GetNode().getParam("frame_id", frame_id); + livox_node.GetNode().getParam("enable_lidar_bag", lidar_bag); + livox_node.GetNode().getParam("enable_imu_bag", imu_bag); + + printf("data source:%u.\n", data_src); + + if (publish_freq > 100.0) { + publish_freq = 100.0; + } else if (publish_freq < 0.5) { + publish_freq = 0.5; + } else { + publish_freq = publish_freq; + } + + livox_node.future_ = livox_node.exit_signal_.get_future(); + + /** Lidar data distribute control and lidar data source set */ + livox_node.lddc_ptr_ = std::make_unique(xfer_format, multi_topic, data_src, output_type, + publish_freq, frame_id, lidar_bag, imu_bag); + livox_node.lddc_ptr_->SetRosNode(&livox_node); + + if (data_src == kSourceRawLidar) { + DRIVER_INFO(livox_node, "Data Source is raw lidar."); + + std::string user_config_path; + livox_node.getParam("user_config_path", user_config_path); + DRIVER_INFO(livox_node, "Config file : %s", user_config_path.c_str()); + + LdsLidar *read_lidar = LdsLidar::GetInstance(publish_freq); + livox_node.lddc_ptr_->RegisterLds(static_cast(read_lidar)); + + if ((read_lidar->InitLdsLidar(user_config_path))) { + DRIVER_INFO(livox_node, "Init lds lidar successfully!"); + } else { + DRIVER_ERROR(livox_node, "Init lds lidar failed!"); + } + } else { + DRIVER_ERROR(livox_node, "Invalid data src (%d), please check the launch file", data_src); + } + + livox_node.pointclouddata_poll_thread_ = std::make_shared(&DriverNode::PointCloudDataPollThread, &livox_node); + livox_node.imudata_poll_thread_ = std::make_shared(&DriverNode::ImuDataPollThread, &livox_node); + while (ros::ok()) { usleep(10000); } + + return 0; +} + +#elif defined BUILDING_ROS2 +namespace livox_ros +{ +DriverNode::DriverNode(const rclcpp::NodeOptions & node_options) +: Node("livox_driver_node", node_options) +{ + DRIVER_INFO(*this, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING); + + /** Init default system parameter */ + int xfer_format = kPointCloud2Msg; + int multi_topic = 0; + int data_src = kSourceRawLidar; + double publish_freq = 10.0; /* Hz */ + int output_type = kOutputToRos; + std::string frame_id; + + this->declare_parameter("xfer_format", xfer_format); + this->declare_parameter("multi_topic", 0); + this->declare_parameter("data_src", data_src); + this->declare_parameter("publish_freq", 10.0); + this->declare_parameter("output_data_type", output_type); + this->declare_parameter("frame_id", "frame_default"); + this->declare_parameter("user_config_path", "path_default"); + this->declare_parameter("cmdline_input_bd_code", "000000000000001"); + this->declare_parameter("lvx_file_path", "/home/livox/livox_test.lvx"); + + this->get_parameter("xfer_format", xfer_format); + this->get_parameter("multi_topic", multi_topic); + this->get_parameter("data_src", data_src); + this->get_parameter("publish_freq", publish_freq); + this->get_parameter("output_data_type", output_type); + this->get_parameter("frame_id", frame_id); + + if (publish_freq > 100.0) { + publish_freq = 100.0; + } else if (publish_freq < 0.5) { + publish_freq = 0.5; + } else { + publish_freq = publish_freq; + } + + future_ = exit_signal_.get_future(); + + /** Lidar data distribute control and lidar data source set */ + lddc_ptr_ = std::make_unique(xfer_format, multi_topic, data_src, output_type, publish_freq, frame_id); + lddc_ptr_->SetRosNode(this); + + if (data_src == kSourceRawLidar) { + DRIVER_INFO(*this, "Data Source is raw lidar."); + + std::string user_config_path; + this->get_parameter("user_config_path", user_config_path); + DRIVER_INFO(*this, "Config file : %s", user_config_path.c_str()); + + std::string cmdline_bd_code; + this->get_parameter("cmdline_input_bd_code", cmdline_bd_code); + + LdsLidar *read_lidar = LdsLidar::GetInstance(publish_freq); + lddc_ptr_->RegisterLds(static_cast(read_lidar)); + + if ((read_lidar->InitLdsLidar(user_config_path))) { + DRIVER_INFO(*this, "Init lds lidar success!"); + } else { + DRIVER_ERROR(*this, "Init lds lidar fail!"); + } + } else { + DRIVER_ERROR(*this, "Invalid data src (%d), please check the launch file", data_src); + } + + pointclouddata_poll_thread_ = std::make_shared(&DriverNode::PointCloudDataPollThread, this); + imudata_poll_thread_ = std::make_shared(&DriverNode::ImuDataPollThread, this); +} + +} // namespace livox_ros + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(livox_ros::DriverNode) + +#endif // defined BUILDING_ROS2 + + +void DriverNode::PointCloudDataPollThread() +{ + std::future_status status; + std::this_thread::sleep_for(std::chrono::seconds(3)); + do { + lddc_ptr_->DistributePointCloudData(); + status = future_.wait_for(std::chrono::microseconds(0)); + } while (status == std::future_status::timeout); +} + +void DriverNode::ImuDataPollThread() +{ + std::future_status status; + std::this_thread::sleep_for(std::chrono::seconds(3)); + do { + lddc_ptr_->DistributeImuData(); + status = future_.wait_for(std::chrono::microseconds(0)); + } while (status == std::future_status::timeout); +} + + + + + + + + + + + + + + + + + + + + + diff --git a/src/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.cpp b/src/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.cpp new file mode 100644 index 0000000..d967087 --- /dev/null +++ b/src/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.cpp @@ -0,0 +1,67 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "parse_cfg_file.h" + +#include +#include +#include + +namespace livox_ros { + +ParseCfgFile::ParseCfgFile(const std::string& path) : path_(path) {} + +bool ParseCfgFile::ParseSummaryInfo(LidarSummaryInfo& lidar_summary_info) { + FILE* raw_file = std::fopen(path_.c_str(), "rb"); + if (!raw_file) { + std::cout << "parse summary info failed, can not open file: " << path_ << std::endl; + return false; + } + + char read_buffer[kMaxBufferSize]; + rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer)); + rapidjson::Document doc; + do { + if (doc.ParseStream(config_file).HasParseError()) { + break; + } + if (!doc.HasMember("lidar_summary_info") || !doc["lidar_summary_info"].IsObject()) { + break; + } + const rapidjson::Value &object = doc["lidar_summary_info"]; + if (!object.HasMember("lidar_type") || !object["lidar_type"].IsUint()) { + break; + } + lidar_summary_info.lidar_type = static_cast(object["lidar_type"].GetUint()); + std::fclose(raw_file); + return true; + } while (false); + + std::cout << "parse lidar type failed." << std::endl; + std::fclose(raw_file); + return false; +} + +} // namespace livox_ros + diff --git a/src/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.h b/src/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.h new file mode 100644 index 0000000..8d2db1c --- /dev/null +++ b/src/livox_ros_driver2/src/parse_cfg_file/parse_cfg_file.h @@ -0,0 +1,52 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_ +#define LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_ + +#include "../comm/comm.h" + +#include "rapidjson/document.h" +#include "rapidjson/filereadstream.h" +#include "rapidjson/stringbuffer.h" + +#include +#include + +namespace livox_ros { + +class ParseCfgFile { + public: + explicit ParseCfgFile(const std::string& path); + ~ParseCfgFile() {} + + bool ParseSummaryInfo(LidarSummaryInfo& lidar_summary_info); + + private: + const std::string path_; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_ diff --git a/src/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.cpp b/src/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.cpp new file mode 100644 index 0000000..9360733 --- /dev/null +++ b/src/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.cpp @@ -0,0 +1,156 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "parse_livox_lidar_cfg.h" +#include + +namespace livox_ros { + +bool LivoxLidarConfigParser::Parse(std::vector &lidar_configs) { + FILE* raw_file = std::fopen(path_.c_str(), "rb"); + if (!raw_file) { + std::cout << "failed to open config file: " << path_ << std::endl; + return false; + } + + lidar_configs.clear(); + char read_buffer[kMaxBufferSize]; + rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer)); + rapidjson::Document doc; + + do { + if (doc.ParseStream(config_file).HasParseError()) { + std::cout << "failed to parse config jason" << std::endl; + break; + } + if (!doc.HasMember("lidar_configs") || + !doc["lidar_configs"].IsArray() || + 0 == doc["lidar_configs"].Size()) { + std::cout << "there is no user-defined config" << std::endl; + break; + } + if (!ParseUserConfigs(doc, lidar_configs)) { + std::cout << "failed to parse basic configs" << std::endl; + break; + } + return true; + } while (false); + + std::fclose(raw_file); + return false; +} + +bool LivoxLidarConfigParser::ParseUserConfigs(const rapidjson::Document &doc, + std::vector &user_configs) { + const rapidjson::Value &lidar_configs = doc["lidar_configs"]; + for (auto &config : lidar_configs.GetArray()) { + if (!config.HasMember("ip")) { + continue; + } + UserLivoxLidarConfig user_config; + + // parse user configs + user_config.handle = IpStringToNum(std::string(config["ip"].GetString())); + if (!config.HasMember("pcl_data_type")) { + user_config.pcl_data_type = -1; + } else { + user_config.pcl_data_type = static_cast(config["pcl_data_type"].GetInt()); + } + if (!config.HasMember("pattern_mode")) { + user_config.pattern_mode = -1; + } else { + user_config.pattern_mode = static_cast(config["pattern_mode"].GetInt()); + } + if (!config.HasMember("blind_spot_set")) { + user_config.blind_spot_set = -1; + } else { + user_config.blind_spot_set = static_cast(config["blind_spot_set"].GetInt()); + } + if (!config.HasMember("dual_emit_en")) { + user_config.dual_emit_en = -1; + } else { + user_config.dual_emit_en = static_cast(config["dual_emit_en"].GetInt()); + } + if (!config.HasMember("extrinsic_parameter")) { + memset(&user_config.extrinsic_param, 0, sizeof(user_config.extrinsic_param)); + } else { + auto &value = config["extrinsic_parameter"]; + if (!ParseExtrinsics(value, user_config.extrinsic_param)) { + memset(&user_config.extrinsic_param, 0, sizeof(user_config.extrinsic_param)); + std::cout << "failed to parse extrinsic parameters, ip: " + << IpNumToString(user_config.handle) << std::endl; + } + } + user_config.set_bits = 0; + user_config.get_bits = 0; + + user_configs.push_back(user_config); + } + + if (0 == user_configs.size()) { + std::cout << "no valid base configs" << std::endl; + return false; + } + std::cout << "successfully parse base config, counts: " + << user_configs.size() << std::endl; + return true; +} + +bool LivoxLidarConfigParser::ParseExtrinsics(const rapidjson::Value &value, + ExtParameter ¶m) { + if (!value.HasMember("roll")) { + param.roll = 0.0f; + } else { + param.roll = static_cast(value["roll"].GetFloat()); + } + if (!value.HasMember("pitch")) { + param.pitch = 0.0f; + } else { + param.pitch = static_cast(value["pitch"].GetFloat()); + } + if (!value.HasMember("yaw")) { + param.yaw = 0.0f; + } else { + param.yaw = static_cast(value["yaw"].GetFloat()); + } + if (!value.HasMember("x")) { + param.x = 0; + } else { + param.x = static_cast(value["x"].GetInt()); + } + if (!value.HasMember("y")) { + param.y = 0; + } else { + param.y = static_cast(value["y"].GetInt()); + } + if (!value.HasMember("z")) { + param.z = 0; + } else { + param.z = static_cast(value["z"].GetInt()); + } + + return true; +} + +} // namespace livox_ros diff --git a/src/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.h b/src/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.h new file mode 100644 index 0000000..b1be73b --- /dev/null +++ b/src/livox_ros_driver2/src/parse_cfg_file/parse_livox_lidar_cfg.h @@ -0,0 +1,57 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_ +#define LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_ + +#include "comm/comm.h" + +#include "rapidjson/document.h" +#include "rapidjson/filereadstream.h" +#include "rapidjson/stringbuffer.h" + +#include +#include +#include + +namespace livox_ros { + +class LivoxLidarConfigParser { + public: + explicit LivoxLidarConfigParser(const std::string& path) : path_(path) {} + ~LivoxLidarConfigParser() {} + + bool Parse(std::vector &lidar_configs); + + private: + bool ParseUserConfigs(const rapidjson::Document &doc, + std::vector &user_configs); + bool ParseExtrinsics(const rapidjson::Value &value, ExtParameter ¶m); + + const std::string path_; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_ diff --git a/src/rpg_vikit/.gitignore b/src/rpg_vikit/.gitignore new file mode 100644 index 0000000..ae1037d --- /dev/null +++ b/src/rpg_vikit/.gitignore @@ -0,0 +1 @@ +vikit_py/build diff --git a/src/rpg_vikit/README.md b/src/rpg_vikit/README.md new file mode 100644 index 0000000..fdc4157 --- /dev/null +++ b/src/rpg_vikit/README.md @@ -0,0 +1,69 @@ +# Vikit: Vision-Kit for Robotics + +Vikit is a versatile collection of C++ tools and utilities designed for computer vision and robotics projects. This version has been modernized for the ROS2 ecosystem (specifically tested on Jazzy and Humble) with a focus on high-performance parameter handling and cross-node compatibility. + +--- + +## 🔒 Disclaimer & Acknowledgments + +### Usage Policy +This software is provided for **educational and research purposes only**. It shall **not be used for any commercial purposes**. + +### Acknowledgments +We extend our deepest gratitude to the original developers and contributors of the projects that served as the foundation for this kit: +* [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) + +--- + +## 🚀 Key Improvements in ROS2 + +### Optimized Parameter Fetching Architecture +One of the most significant challenges in migrating from ROS1 to ROS2 is the removal of the global Parameter Server. In ROS2, parameters are local to each node. Vikit addresses this through a multi-tiered fetching strategy in `params_helper.hpp`: + +1. **Native Node Access**: Direct, high-speed access to parameters owned by the current node handle. +2. **`SyncParametersClient` (High Performance)**: For cross-node parameter access (e.g., retrieving camera intrinsics from a central `parameter_blackboard`). This utilizes optimized ROS2 Service calls to achieve microsecond-level latency, avoiding the overhead of CLI tools. +3. **Command-Line Fallback**: A robust fallback mechanism using `popen` to interface with the ROS2 CLI (`ros2 param get`), ensuring parameter retrieval even in complex edge cases where service clients might be restricted. + +This architecture ensures that vision components can load dozens of camera parameters nearly instantaneously, a critical requirement for real-time SLAM and VIO systems. + +--- + +## 🛠 Installation Guide + +### Prerequisites: Sophus +Vikit relies on Sophus for Lie groups. It is recommended to use version `1.22.10`. + +```bash +git clone https://github.com/strasdat/Sophus.git -b 1.22.10 +cd Sophus && mkdir build && cd build +cmake .. && make -j$(nproc) +sudo make install +``` + +### Building `vikit_common` +`vikit_common` is a pure CMake package and can be installed globally. + +```bash +cd vikit_common +mkdir build && cd build +cmake .. && make -j$(nproc) +sudo make install +``` + +### Building `vikit_ros` +`vikit_ros` is integrated into the ROS2 workspace and should be built using `colcon`. + +```bash +# Move vikit_ros to your workspace src directory +cd ~/ros2_ws +colcon build --symlink-install --packages-select vikit_ros +``` + +--- + +## 📅 Maintenance Info +* **Last Update**: December 2025 +* **Target Systems**: Ubuntu 22.04 (Humble) / 24.04 (Jazzy) +* **Compiler**: C++17 compliant (GCC 9+) diff --git a/src/rpg_vikit/vikit_common/CMakeLists.txt b/src/rpg_vikit/vikit_common/CMakeLists.txt new file mode 100644 index 0000000..3889d0a --- /dev/null +++ b/src/rpg_vikit/vikit_common/CMakeLists.txt @@ -0,0 +1,115 @@ +SET(PROJECT_NAME vikit_common) +PROJECT(${PROJECT_NAME}) +CMAKE_MINIMUM_REQUIRED (VERSION 3.0) +SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo +SET(CMAKE_VERBOSE_MAKEFILE OFF) +SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") +SET(USE_ROS False) # Set False if you want to build this package without Catkin + +# Set build flags. Set IS_ARM on odroid board as environment variable +SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") +IF(DEFINED ENV{ARM_ARCHITECTURE}) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv8-a") +ELSE() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") +ENDIF() +message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") +if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") +else() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv8-a") +endif() + +IF(CMAKE_COMPILER_IS_GNUCC) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +ELSE() + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +ENDIF() +SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") + +# Add plain cmake packages +FIND_PACKAGE(OpenCV REQUIRED) +FIND_PACKAGE(Eigen REQUIRED) +FIND_PACKAGE(Sophus REQUIRED) +FIND_PACKAGE(fmt 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() + +# Include dirs +INCLUDE_DIRECTORIES( + include + ${Eigen_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Sophus_INCLUDE_DIRS} + fmt +) + +IF(USE_ROS) + FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp cmake_modules) + LIST(APPEND INCLUDE_DIRECTORIES ${catkin_INCLUDE_DIRS}) + catkin_package( + DEPENDS Eigen OpenCV Sophus + CATKIN_DEPENDS roscpp + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + ) +ELSE() + SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) + SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) +ENDIF() + +# Set Sourcefiles +LIST(APPEND SOURCEFILES src/atan_camera.cpp + src/omni_camera.cpp + src/math_utils.cpp + src/vision.cpp + src/performance_monitor.cpp + src/robust_cost.cpp + src/user_input_thread.cpp + src/pinhole_camera.cpp + src/equidistant_camera.cpp + src/polynomial_camera.cpp + src/homography.cpp + src/img_align.cpp) + +# Create vikit library +ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCEFILES}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} + ${OpenCV_LIBS} + ${Sophus_LIBRARIES} + fmt::fmt) + +IF(USE_ROS) + TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${catkin_LIBRARIES}) +ENDIF() + +# Tests +ADD_EXECUTABLE(test_vk_common_camera test/test_camera.cpp) +TARGET_LINK_LIBRARIES(test_vk_common_camera ${PROJECT_NAME} ${OpenCV_LIBS}) + +ADD_EXECUTABLE(test_vk_common_triangulation test/test_triangulation.cpp) +TARGET_LINK_LIBRARIES(test_vk_common_triangulation ${PROJECT_NAME} ${OpenCV_LIBS}) + +ADD_EXECUTABLE(test_vk_common_patch_score test/test_patch_score.cpp) +TARGET_LINK_LIBRARIES(test_vk_common_patch_score ${PROJECT_NAME} ${OpenCV_LIBS}) + + +################################################################################ +# Create the vikit_commonConfig.cmake file for other cmake projects. +IF(NOT USE_ROS) + # In CMake 3.6 and later, reading the LOCATION property from a target is no longer allowed. + # GET_TARGET_PROPERTY( FULL_LIBRARY_NAME ${PROJECT_NAME} LOCATION ) + set(VIKIT_COMMON_LOCATION $) + SET(vikit_common_LIBRARIES ${FULL_LIBRARY_NAME} ) + SET(vikit_common_LIBRARY_DIR ${PROJECT_BINARY_DIR} ) + SET(vikit_common_INCLUDE_DIR "${PROJECT_SOURCE_DIR}/include") + CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/vikit_commonConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/vikit_commonConfig.cmake @ONLY IMMEDIATE ) + export( PACKAGE vikit_common ) + + INSTALL(DIRECTORY include/vikit DESTINATION ${CMAKE_INSTALL_PREFIX}/include FILES_MATCHING PATTERN "*.h" ) + INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib ) +ENDIF() \ No newline at end of file diff --git a/src/rpg_vikit/vikit_common/CMakeModules/FindEigen.cmake b/src/rpg_vikit/vikit_common/CMakeModules/FindEigen.cmake new file mode 100644 index 0000000..8587367 --- /dev/null +++ b/src/rpg_vikit/vikit_common/CMakeModules/FindEigen.cmake @@ -0,0 +1,81 @@ +############################################################################### +# +# CMake script for finding the Eigen library. +# +# http://eigen.tuxfamily.org/index.php?title=Main_Page +# +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD +# license. +# +# +# Input variables: +# +# - Eigen_ROOT_DIR (optional): When specified, header files and libraries +# will be searched for in `${Eigen_ROOT_DIR}/include` and +# `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order +# will be ignored. When unspecified, the default CMake search order is used. +# This variable can be specified either as a CMake or environment variable. +# If both are set, preference is given to the CMake variable. +# Use this variable for finding packages installed in a nonstandard location, +# or for enforcing that one of multiple package installations is picked up. +# +# Cache variables (not intended to be used in CMakeLists.txt files) +# +# - Eigen_INCLUDE_DIR: Absolute path to package headers. +# +# +# Output variables: +# +# - Eigen_FOUND: Boolean that indicates if the package was found +# - Eigen_INCLUDE_DIRS: Paths to the necessary header files +# - Eigen_VERSION: Version of Eigen library found +# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen +# +# +# Example usage: +# +# # Passing the version means Eigen_FOUND will only be TRUE if a +# # version >= the provided version is found. +# find_package(Eigen 3.1.2) +# if(NOT Eigen_FOUND) +# # Error handling +# endif() +# ... +# add_definitions(${Eigen_DEFINITIONS}) +# ... +# include_directories(${Eigen_INCLUDE_DIRS} ...) +# +############################################################################### + +find_package(PkgConfig) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + + +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" + "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" + PATH_SUFFIXES eigen3 include/eigen3 include) + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") +endif(EIGEN_FOUND) + + +set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +set(Eigen_FOUND ${EIGEN_FOUND}) +set(Eigen_VERSION ${EIGEN_VERSION}) +set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) diff --git a/src/rpg_vikit/vikit_common/bin/test_vk_common_camera b/src/rpg_vikit/vikit_common/bin/test_vk_common_camera new file mode 100755 index 0000000..b98f9ac Binary files /dev/null and b/src/rpg_vikit/vikit_common/bin/test_vk_common_camera differ diff --git a/src/rpg_vikit/vikit_common/bin/test_vk_common_patch_score b/src/rpg_vikit/vikit_common/bin/test_vk_common_patch_score new file mode 100755 index 0000000..89736bf Binary files /dev/null and b/src/rpg_vikit/vikit_common/bin/test_vk_common_patch_score differ diff --git a/src/rpg_vikit/vikit_common/bin/test_vk_common_triangulation b/src/rpg_vikit/vikit_common/bin/test_vk_common_triangulation new file mode 100755 index 0000000..238351e Binary files /dev/null and b/src/rpg_vikit/vikit_common/bin/test_vk_common_triangulation differ diff --git a/src/rpg_vikit/vikit_common/include/vikit/abstract_camera.h b/src/rpg_vikit/vikit_common/include/vikit/abstract_camera.h new file mode 100644 index 0000000..e4b44b4 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/abstract_camera.h @@ -0,0 +1,85 @@ +/* + * abstract_camera.h + * + * Created on: Jul 23, 2012 + * Author: cforster + */ + +#ifndef ABSTRACT_CAMERA_H_ +#define ABSTRACT_CAMERA_H_ + +#include + +namespace vk +{ + +using namespace std; +using namespace Eigen; + +class AbstractCamera +{ +protected: + + int width_; // TODO cannot be const because of omni-camera model + int height_; + double scale_; + +public: + + AbstractCamera() {}; // need this constructor for omni camera + AbstractCamera(int width, int height, double scale) : width_(width), height_(height), scale_(scale){}; + + virtual ~AbstractCamera() {}; + + /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. + virtual Vector3d + cam2world(const double& x, const double& y) const = 0; + + /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. + virtual Vector3d + cam2world(const Vector2d& px) const = 0; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const = 0; + + /// projects unit plane coordinates to camera coordinates + virtual Vector2d + world2cam(const Vector2d& uv) const = 0; + + virtual double + errorMultiplier2() const = 0; + + virtual double + errorMultiplier() const = 0; + + virtual double fx() const = 0; + virtual double fy() const = 0; + virtual double cx() const = 0; + virtual double cy() const = 0; + + inline int width() const { return width_; } + + inline int height() const { return height_; } + + inline double scale() const { return scale_; } + + inline bool isInFrame(const Vector2i & obs, int boundary=0) const + { + if(obs[0]>=boundary && obs[0]=boundary && obs[1]= boundary && obs[0] < width()/(1<= boundary && obs[1] // memset +#include // memset +#include +#include + +namespace vk { +namespace aligned_mem { + + /// Check if the pointer is aligned to the specified byte granularity + inline bool + is_aligned8(const void* ptr) + { + return ((reinterpret_cast(ptr)) & 0x7) == 0; + } + + inline bool + is_aligned16(const void* ptr) + { + return ((reinterpret_cast(ptr)) & 0xF) == 0; + } + + template struct placement_delete + { + enum { Size = (1<= Size) { + placement_delete::free(buf+Size,M-Size); + placement_delete::destruct(buf); + } else { + placement_delete::free(buf, M); + } + } + }; + + template struct placement_delete + { + static inline void free(T*, size_t ) {} + }; + + inline void * aligned_alloc(size_t count, size_t alignment){ + void * mem = NULL; + assert(posix_memalign(&mem, alignment, count) == 0); + return mem; + } + + inline void aligned_free(void * memory) { + free(memory); + } + + template + inline T * aligned_alloc(size_t count, size_t alignment){ + void * data = aligned_alloc(sizeof(T)* count, alignment); + return new (data) T[count]; + } + + template + inline void aligned_free(T * memory, size_t count){ + placement_delete::free(memory, count); + aligned_free(memory); + } + + template inline void memfill(T* data, int n, const T val) + { + T* de = data + n; + for(;data < de; data++) + *data=val; + } + + template<> inline void memfill(unsigned char* data, int n, const unsigned char val) + { + memset(data, val, n); + } + + template<> inline void memfill(signed char* data, int n, const signed char val) + { + memset(data, val, n); + } + + template<> inline void memfill(char* data, int n, const char val) + { + memset(data, val, n); + } + + template + struct AlignedMem { + T* mem; + size_t count; + AlignedMem(size_t c) : count(c) { + mem = aligned_alloc(count, N); + } + ~AlignedMem() { + aligned_free(mem, count); + } + T* data() { return mem; } + const T* data() const { return mem; } + }; + +} // namespace aligned_mem +} // namespace vikit + + +#endif // VIKIT_ALIGNED_MEM_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/atan_camera.h b/src/rpg_vikit/vikit_common/include/vikit/atan_camera.h new file mode 100644 index 0000000..48b0b23 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/atan_camera.h @@ -0,0 +1,96 @@ +/* + * atan_camera.h + * + * Created on: Aug 21, 2012 + * Author: cforster + * + * This class implements the FOV distortion model of Deverneay and Faugeras, + * Straight lines have to be straight, 2001. + * + * The code is an implementation of the ATAN class in PTAM by Georg Klein using Eigen. + */ + +#ifndef ATAN_CAMERA_H_ +#define ATAN_CAMERA_H_ + +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +class ATANCamera : public AbstractCamera { + +private: + double fx_, fy_; //!< focal length + double fx_inv_, fy_inv_; //!< inverse focal length + double cx_, cy_; //!< projection center + double s_, s_inv_; //!< distortion model coeff + double tans_; //!< distortion model coeff + double tans_inv_; //!< distortion model coeff + bool distortion_; //!< use distortion model? + + //! Radial distortion transformation factor: returns ration of distorted / undistorted radius. + inline double rtrans_factor(double r) const + { + if(r < 0.001 || s_ == 0.0) + return 1.0; + else + return (s_inv_* atan(r * tans_) / r); + }; + + //! Inverse radial distortion: returns un-distorted radius from distorted. + inline double invrtrans(double r) const + { + if(s_ == 0.0) + return r; + return (tan(r * s_) * tans_inv_); + }; + +public: + + ATANCamera(double width, double height, double fx, double fy, double dx, double dy, double s); + + ~ATANCamera(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + const Vector2d focal_length() const + { + return Vector2d(fx_, fy_); + } + + virtual double errorMultiplier2() const + { + return fx_; + } + + virtual double errorMultiplier() const + { + return 4*fx_*fy_; + } + + virtual double fx() const { return fx_; }; + virtual double fy() const { return fy_; }; + virtual double cx() const { return cx_; }; + virtual double cy() const { return cy_; }; +}; + +} // end namespace vk + +#endif /* ATAN_CAMERA_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/blender_utils.h b/src/rpg_vikit/vikit_common/include/vikit/blender_utils.h new file mode 100644 index 0000000..c858b56 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/blender_utils.h @@ -0,0 +1,133 @@ +/* + * blender_utils.h + * + * Created on: Feb 13, 2014 + * Author: cforster + */ + +#ifndef VIKIT_BLENDER_UTILS_H_ +#define VIKIT_BLENDER_UTILS_H_ + +#include +#include +#include +#include +#include +#include +#include + +namespace vk { +namespace blender_utils { + +void loadBlenderDepthmap( + const std::string file_name, + const vk::AbstractCamera& cam, + cv::Mat& img) +{ + std::ifstream file_stream(file_name.c_str()); + assert(file_stream.is_open()); + img = cv::Mat(cam.height(), cam.width(), CV_32FC1); + float * img_ptr = img.ptr(); + float depth; + for(int y=0; y> depth; + // blender: + Eigen::Vector2d uv(vk::project2d(cam.cam2world(x,y))); + *img_ptr = depth * sqrt(uv[0]*uv[0] + uv[1]*uv[1] + 1.0); + + // povray + // *img_ptr = depth/100.0; // depth is in [cm], we want [m] + + if(file_stream.peek() == '\n' && x != cam.width()-1 && y != cam.height()-1) + printf("WARNING: did not read the full depthmap!\n"); + } + } +} + +bool getDepthmapNormalAtPoint( + const Vector2i& px, + const cv::Mat& depth, + const int halfpatch_size, + const vk::AbstractCamera& cam, + Vector3d& normal) +{ + assert(cam.width() == depth.cols && cam.height() == depth.rows); + if(!cam.isInFrame(px, halfpatch_size+1)) + return false; + + const size_t n_meas = (halfpatch_size*2+1)*(halfpatch_size*2+1); + list pts; + for(int y = px[1]-halfpatch_size; y<=px[1]+halfpatch_size; ++y) + for(int x = px[0]-halfpatch_size; x<=px[0]+halfpatch_size; ++x) + pts.push_back(cam.cam2world(x,y)*depth.at(y,x)); + + assert(n_meas == pts.size()); + Matrix A; A.resize(n_meas, Eigen::NoChange); + Matrix b; b.resize(n_meas, Eigen::NoChange); + + size_t i = 0; + for(list::iterator it=pts.begin(); it!=pts.end(); ++it) + { + A.row(i) << it->x(), it->y(), it->z(), 1.0; + b[i] = 0; + ++i; + } + + JacobiSVD svd(A, ComputeThinU | ComputeThinV); + + Matrix V = svd.matrixV(); + normal = V.block<3,1>(0,3); + normal.normalize(); + return true; +} + +namespace file_format +{ + +class ImageNameAndPose +{ +public: + ImageNameAndPose() {} + virtual ~ImageNameAndPose() {} + double timestamp_; + std::string image_name_; + Eigen::Vector3d t_; + Eigen::Quaterniond q_; + friend std::ostream& operator <<(std::ostream& out, const ImageNameAndPose& pair); + friend std::istream& operator >>(std::istream& in, ImageNameAndPose& pair); +}; + +std::ostream& operator <<(std::ostream& out, const ImageNameAndPose& gt) +{ + out << gt.timestamp_ << " " << gt.image_name_ << " " + << gt.t_.x() << " " << gt.t_.y() << " " << gt.t_.z() << " " + << gt.q_.x() << " " << gt.q_.y() << " " << gt.q_.z() << " " << gt.q_.w() << " " << std::endl; + return out; +} + +std::istream& operator >>(std::istream& in, ImageNameAndPose& gt) +{ + in >> gt.timestamp_; + in >> gt.image_name_; + double tx, ty, tz, qx, qy, qz, qw; + in >> tx; + in >> ty; + in >> tz; + in >> qx; + in >> qy; + in >> qz; + in >> qw; + gt.t_ = Eigen::Vector3d(tx, ty, tz); + gt.q_ = Eigen::Quaterniond(qw, qx, qy, qz); + gt.q_.normalize(); + return in; +} + +} // namespace file_format +} // namespace blender_utils +} // namespace vk + +#endif // VIKIT_BLENDER_UTILS_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/equidistant_camera.h b/src/rpg_vikit/vikit_common/include/vikit/equidistant_camera.h new file mode 100644 index 0000000..72a5e43 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/equidistant_camera.h @@ -0,0 +1,145 @@ +/* + * equidistant_camera.h + * + * Created on: January 26, 2023 + * Author: xuankuzcr + */ + +#ifndef EQUIDISTANT_CAMERA_H_ +#define EQUIDISTANT_CAMERA_H_ + +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +class EquidistantCamera : public AbstractCamera { + +private: + const double fx_, fy_; + const double cx_, cy_; + bool distortion_; //!< is it pure pinhole model or has it equidistant distortion + double k1_, k2_, k3_, k4_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + EquidistantCamera(double width, double height, double scale, + double fx, double fy, double cx, double cy, + double k1=0.0, double k2=0.0, double k3=0.0, double k4=0.0); + + ~EquidistantCamera(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + const Vector2d focal_length() const + { + return Vector2d(fx_, fy_); + } + + inline double thetad_from_theta(const double theta) const + { + const double theta2 = theta * theta; + const double theta4 = theta2 * theta2; + const double theta6 = theta4 * theta2; + const double theta8 = theta4 * theta4; + const double thetad = theta * (1.0 + k1_ * theta2 + k2_ * theta4 + + k3_ * theta6 + k4_ * theta8); + return thetad; + } + + inline double deriv_thetad_from_theta(const double theta) const + { + const double theta2 = theta * theta; + const double theta4 = theta2 * theta2; + const double theta6 = theta4 * theta2; + const double theta8 = theta4 * theta4; + return 1 + 3 * k1_ * theta2 + 5 * k2_ * theta4 + 7 * k3_ * theta6 + + 9 * k4_ * theta8; + } + + inline Eigen::Matrix2d jacobian_2x2(const Eigen::Vector2d& uv) const + { + const double r = uv.norm(); + if (r < 1e-8) + { + return Eigen::Matrix2d::Identity(); + } + + const double inv_r = 1.0 / r; + const double r2 = r * r; + const double dr_du = uv(0) * inv_r; + const double dr_dv = uv(1) * inv_r; + + const double theta = std::atan(r); + const double dtheta_dr = 1.0 / (1 + r * r); + + const double thetad = thetad_from_theta(theta); + const double dthetad_dtheta = deriv_thetad_from_theta(theta); + const double dthetad_dr = dthetad_dtheta * dtheta_dr; + + const double scaling = thetad / r; + const double dscaling_du = (dthetad_dr * dr_du * r - dr_du * thetad) / r2; + const double dscaling_dv = (dthetad_dr * dr_dv * r - dr_dv * thetad) / r2; + + const double dx_du = dscaling_du * uv(0) + scaling; + const double dx_dv = dscaling_dv * uv(0); + const double dy_du = dscaling_du * uv(1); + const double dy_dv = dscaling_dv * uv(1) + scaling; + Eigen::Matrix2d jac; + jac << dx_du, dx_dv, dy_du, dy_dv; + return jac; + } + + inline Eigen::Matrix jacobian_2x3(const Eigen::Vector3d& p) const + { + Eigen::Matrix jac; + const double x = p[0]; + const double y = p[1]; + const double z_inv = 1./p[2]; + const double z_inv_2 = z_inv * z_inv; + jac(0,0) = fx_ * z_inv; + jac(0,1) = 0.0; + jac(0,2) = -fx_ * x * z_inv_2; + jac(1,0) = 0.0; + jac(1,1) = fy_ * z_inv; + jac(1,2) = -fy_ * y * z_inv_2; + return jac; + } + + virtual double errorMultiplier2() const + { + return fabs(fx_); + } + + virtual double errorMultiplier() const + { + return fabs(4.0*fx_*fy_); + } + + virtual double fx() const { return fx_; }; + virtual double fy() const { return fy_; }; + virtual double cx() const { return cx_; }; + virtual double cy() const { return cy_; }; +}; + +} // end namespace vk + + +#endif /* #define EQUIDISTANT_CAMERA_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/file_reader.h b/src/rpg_vikit/vikit_common/include/vikit/file_reader.h new file mode 100644 index 0000000..3c71b56 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/file_reader.h @@ -0,0 +1,101 @@ +/** + * This file is part of dvo. + * + * Copyright 2012 Christian Kerl (Technical University of Munich) + * For more information see . + * + * dvo 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 3 of the License, or + * (at your option) any later version. + * + * dvo 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 dvo. If not, see . + */ + +#ifndef VIKIT_FILE_READER_H_ +#define VIKIT_FILE_READER_H_ + +#include +#include + +namespace vk +{ + +/** + * Entry has to support the following operator + * std::istream& operator >>(std::istream&, Entry&); + */ +template +class FileReader +{ +public: + FileReader(const std::string& file) : + hasEntry_(false), + file_(file), + file_stream_(file.c_str()) + {} + + virtual ~FileReader() + { + file_stream_.close(); + } + + void skip(int num_lines) + { + for(int idx = 0; idx < num_lines; ++idx) + { + if(!file_stream_.good()) continue; + file_stream_.ignore(1024, '\n'); + assert(file_stream_.gcount() < 1024); + } + } + + void skipComments() + { + while(file_stream_.good() && file_stream_.peek() == '#') + skip(1); + } + + /// Moves to the next entry in the file. Returns true, if there was a next entry, false otherwise. + bool next() + { + if(file_stream_.good() && !file_stream_.eof()) + { + file_stream_ >> entry_; + hasEntry_ = true; + return true; + } + return false; + } + + /// Read all entries at once. + void readAllEntries(std::vector& entries) + { + if(!hasEntry()) next(); + do + entries.push_back(entry()); + while(next()); + } + + /// Gets the current entry + const Entry& entry() const { return entry_; } + + /// Determines whether the first entry was read + const bool& hasEntry() const { return hasEntry_; } + +private: + bool hasEntry_; + std::string file_; + std::ifstream file_stream_; + Entry entry_; +}; + +} // end namespace vk + +#endif // VIKIT_FILE_READER_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/file_reader_types.h b/src/rpg_vikit/vikit_common/include/vikit/file_reader_types.h new file mode 100644 index 0000000..b4482bf --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/file_reader_types.h @@ -0,0 +1,97 @@ +/* + * file_reader_types.h + * + * Created on: Jul 1, 2014 + * Author: cforster + */ + +#ifndef VIKIT_FILE_READER_TYPES_H_ +#define VIKIT_FILE_READER_TYPES_H_ + +#include +#include +namespace vk { + +/// Common types +namespace file_format { + +/// IMU rotational velocity and linear acceleration +class ImuRotvelLinacc +{ +public: + ImuRotvelLinacc() {} + virtual ~ImuRotvelLinacc() {} + double timestamp_; //!< timestamp in seconds + Eigen::Vector3d w_; //!< angular velocity + Eigen::Vector3d a_; //!< linear acceleration + friend std::ostream& operator <<(std::ostream& out, const ImuRotvelLinacc& pair); + friend std::istream& operator >>(std::istream& in, ImuRotvelLinacc& pair); +}; + +std::ostream& operator <<(std::ostream& out, const ImuRotvelLinacc& gt) +{ + out << gt.timestamp_ << " " + << gt.w_.x() << " " << gt.w_.y() << " " << gt.w_.z() << " " + << gt.a_.x() << " " << gt.a_.y() << " " << gt.a_.z() << std::endl; + return out; +} + +std::istream& operator >>(std::istream& in, ImuRotvelLinacc& gt) +{ + double wx, wy, wz, ax, ay, az; + in >> gt.timestamp_; + in >> wx; + in >> wy; + in >> wz; + in >> ax; + in >> ay; + in >> az; + gt.w_ = Eigen::Vector3d(wx, wy, wz); + gt.a_ = Eigen::Vector3d(ax, ay, az); + return in; +} + + +/// Timestamp with Position and Orientation +class PoseStamped +{ +public: + PoseStamped() {} + virtual ~PoseStamped() {} + double timestamp_; //!< timestamp in seconds + Eigen::Vector3d t_; //!< position + Eigen::Quaterniond q_; //!< orientation + friend std::ostream& operator <<(std::ostream& out, const ImuRotvelLinacc& pair); + friend std::istream& operator >>(std::istream& in, ImuRotvelLinacc& pair); +}; + +std::ostream& operator <<(std::ostream& out, const PoseStamped& gt) +{ + out << gt.timestamp_ << " " + << gt.t_.x() << " " << gt.t_.y() << " " << gt.t_.z() << " " + << gt.q_.x() << " " << gt.q_.y() << " " << gt.q_.z() << " " << gt.q_.w()<< " " + << std::endl; + return out; +} + +std::istream& operator >>(std::istream& in, PoseStamped& gt) +{ + in >> gt.timestamp_; + double tx, ty, tz, qx, qy, qz, qw; + in >> tx; + in >> ty; + in >> tz; + in >> qx; + in >> qy; + in >> qz; + in >> qw; + gt.t_ = Eigen::Vector3d(tx, ty, tz); + gt.q_ = Eigen::Quaterniond(qw, qx, qy, qz); + gt.q_.normalize(); + return in; +} + +} // namespace file_format +} // namespace vk + +#endif // VIKIT_FILE_READER_TYPES_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/homography.h b/src/rpg_vikit/vikit_common/include/vikit/homography.h new file mode 100644 index 0000000..7631787 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/homography.h @@ -0,0 +1,85 @@ +/* + * homography.cpp + * Adaptation of PTAM-GPL HomographyInit class. + * https://github.com/Oxford-PTAM/PTAM-GPL + * Licence: GPLv3 + * Copyright 2008 Isis Innovation Limited + * + * Created on: Sep 2, 2012 + * by: cforster + * + * This class implements the homography decomposition of Faugeras and Lustman's + * 1988 tech report. Code converted to Eigen from PTAM. + * + */ + +#ifndef HOMOGRAPHY_H_ +#define HOMOGRAPHY_H_ + +#include +#include +#include +#include +#include +namespace vk { + +using namespace Eigen; +using namespace std; + +struct HomographyDecomposition +{ + Vector3d t; + Matrix3d R; + double d; + Vector3d n; + + // Resolved Composition + Sophus::SE3 T; //!< second from first + int score; +}; + +class Homography +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Homography (const vector >& _fts1, + const vector >& _fts2, + double _error_multiplier2, + double _thresh_in_px); + + void + calcFromPlaneParams (const Vector3d & normal, + const Vector3d & point_on_plane); + + void + calcFromMatches (); + + size_t + computeMatchesInliers (); + + bool + computeSE3fromMatches (); + + bool + decompose (); + + void + findBestDecomposition (); + + double thresh; + double error_multiplier2; + const vector >& fts_c1; //!< Features on first image on unit plane + const vector >& fts_c2; //!< Features on second image on unit plane + vector inliers; + Sophus::SE3 T_c2_from_c1; //!< Relative translation and rotation of two images + Eigen::Matrix3d H_c2_from_c1; //!< Homography + vector decompositions; +}; + + + + +} /* end namespace vk */ + +#endif /* HOMOGRAPHY_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/img_align.h b/src/rpg_vikit/vikit_common/include/vikit/img_align.h new file mode 100644 index 0000000..ceea10a --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/img_align.h @@ -0,0 +1,153 @@ +/* + * img_align.h + * + * Created on: Aug 22, 2012 + * Author: cforster + */ + +#ifndef IMG_ALIGN_H_ +#define IMG_ALIGN_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; +using namespace vk; +using namespace Sophus; + +//! Forward Compositional Image Alignment +class ForwardCompositionalSE3 : public NLLSSolver<6, Sophus::SE3> { + +protected: + vector& cam_pyr_; + vector& depth_pyr_; + vector& img_pyr_; + vector& tpl_pyr_; + vector& img_pyr_dx_; + vector& img_pyr_dy_; + int level_; + int n_levels_; + PerformanceMonitor permon_; + bool display_; + bool log_; + double res_thresh_; + + virtual double + computeResiduals (const Sophus::SE3& model, bool linearize_system, bool compute_weight_scale = false); + + virtual int + solve(); + + virtual void + update(const ModelType& old_model, ModelType& new_model); + + virtual void + startIteration(); + + virtual void + finishIteration(); + +public: + cv::Mat resimg_; + + ForwardCompositionalSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + Sophus::SE3& init_model, + int n_levels, + int n_iter = 50, + float res_thresh = 0.2, + bool display = true, + Method method = LevenbergMarquardt, + int test_id = 0); + + ForwardCompositionalSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + int n_levels, + int n_iter = 50, + float res_thresh = 0.2, + bool display = true, + Method method = LevenbergMarquardt, + int test_id = 0); + + void + runOptimization(Sophus::SE3& model, int levelBegin = -1, int levelEnd = -1); + +}; + + +//! Efficient Second Order Minimization (ESM) +class SecondOrderMinimisationSE3 : public NLLSSolver<6, Sophus::SE3> { + +protected: + vector& cam_pyr_; + vector& depth_pyr_; + vector& img_pyr_; + vector& tpl_pyr_; + vector& img_pyr_dx_; + vector& img_pyr_dy_; + vector& tpl_pyr_dx_; + vector& tpl_pyr_dy_; + int level_; + PerformanceMonitor permon_; + bool display_; + bool log_; + float res_thresh_; + + virtual double + computeResiduals (const Sophus::SE3& model, bool linearize_system, bool compute_weight_scale = false); + + virtual int + solve(); + + virtual void + update(const ModelType& old_model, ModelType& new_model); + + virtual void + startIteration(); + + virtual void + finishIteration(); + +public: + cv::Mat resimg_; + + SecondOrderMinimisationSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + vector& tpl_pyr_dx, + vector& tpl_pyr_dy, + Sophus::SE3& init_model, + int n_levels, + int n_iter = 50, + float res_thresh = 0.2, + bool display = true, + Method method = LevenbergMarquardt, + int test_id = -1); +}; + +} // end namespace ImgAlign + +#endif /* IMG_ALIGN_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/math_utils.h b/src/rpg_vikit/vikit_common/include/vikit/math_utils.h new file mode 100644 index 0000000..87d6832 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/math_utils.h @@ -0,0 +1,171 @@ +/* + * math_utils.h + * + * Created on: Jul 20, 2012 + * Author: cforster + */ + +#ifndef MATH_UTILS_H_ +#define MATH_UTILS_H_ + + +#include +#include +#include + +namespace vk +{ + +using namespace Eigen; +using namespace std; +using namespace Sophus; + +Vector3d triangulateFeatureNonLin( + const Eigen::Matrix3d& R, + const Vector3d& t, + const Vector3d& feature1, + const Vector3d& feature2); + +/// Assumes the bearing vectors f_c and f_r are on the epipolar plane, i.e. +/// perfect triangulation without noise! +bool depthFromTriangulationExact( + const Eigen::Matrix3d& R_r_c, + const Vector3d& t_r_c, + const Vector3d& f_r, + const Vector3d& f_c, + double& depth_in_r, + double& depth_in_c); + +double reprojError( + const Vector3d& f1, + const Vector3d& f2, + double error_multiplier2); + +double computeInliers( + const vector& features1, + const vector& features2, + const Eigen::Matrix3d& R, + const Vector3d& t, + const double reproj_thresh, + double error_multiplier2, + vector& xyz_vec, + vector& inliers, + vector& outliers); + +void computeInliersOneView( + const vector & feature_sphere_vec, + const vector & xyz_vec, + const Eigen::Matrix3d &R, + const Vector3d &t, + const double reproj_thresh, + const double error_multiplier2, + vector& inliers, + vector& outliers); + +//! Direct Cosine Matrix to Roll Pitch Yaw +Vector3d dcm2rpy(const Eigen::Matrix3d &R); + +//! Roll Pitch Yaw to Direct Cosine Matrix +Eigen::Matrix3d rpy2dcm(const Vector3d &rpy); + +//! Angle Axis parametrization to Quaternion +Quaterniond angax2quat(const Vector3d& n, const double& angle); + +//! Angle Axis parametrization to Matrix representation +Eigen::Matrix3d angax2dcm(const Vector3d& n, const double& angle); + +double sampsonusError( + const Vector2d &v2Dash, + const Eigen::Matrix3d& m3Essential, + const Vector2d& v2); + +inline Eigen::Matrix3d sqew(const Vector3d& v) +{ + Eigen::Matrix3d v_sqew; + v_sqew << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + return v_sqew; +} + +inline double norm_max(const Eigen::VectorXd & v) +{ + double max = -1; + for (int i=0; imax){ + max = abs; + } + } + return max; +} + +inline Vector2d project2d(const Vector3d& v) +{ + return v.head<2>()/v[2]; +} + +inline Vector3d unproject2d(const Vector2d& v) +{ + return Vector3d(v[0], v[1], 1.0); +} + +inline Vector3d project3d(const Vector4d& v) +{ + return v.head<3>()/v[3]; +} + +inline Vector4d unproject3d(const Vector3d& v) +{ + return Vector4d(v[0], v[1], v[2], 1.0); +} + +template +T getMedian(vector& data_vec) +{ + assert(!data_vec.empty()); + typename vector::iterator it = data_vec.begin()+floor(data_vec.size()/2); + nth_element(data_vec.begin(), it, data_vec.end()); + return *it; +} + +inline double pyrFromZero_d(double x_0, int level) +{ + return x_0/(1< & frame_jac) +{ + const double x = xyz[0]; + const double y = xyz[1]; + const double z = xyz[2]; + const double z_2 = z*z; + + frame_jac(0,0) = -1./z *focal_length; + frame_jac(0,1) = 0; + frame_jac(0,2) = x/z_2 *focal_length; + frame_jac(0,3) = x*y/z_2 * focal_length; + frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length; + frame_jac(0,5) = y/z *focal_length; + + frame_jac(1,0) = 0; + frame_jac(1,1) = -1./z *focal_length; + frame_jac(1,2) = y/z_2 *focal_length; + frame_jac(1,3) = (1+y*y/z_2) *focal_length; + frame_jac(1,4) = -x*y/z_2 *focal_length; + frame_jac(1,5) = -x/z *focal_length; +} + +} // end namespace vk + +#endif /* MATH_UTILS_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/nlls_solver.h b/src/rpg_vikit/vikit_common/include/vikit/nlls_solver.h new file mode 100644 index 0000000..e7349c4 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/nlls_solver.h @@ -0,0 +1,169 @@ +/* + * Abstract Nonlinear Least-Squares Solver Class + * + * nlls_solver.h + * + * Created on: Nov 5, 2012 + * Author: cforster + */ + +#ifndef LM_SOLVER_H_ +#define LM_SOLVER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +/** + * \brief Abstract Class for solving nonlinear least-squares (NLLS) problems. + * + * The function implements two algorithms: Levenberg Marquardt and Gauss Newton + * + * Example implementations of this function can be found in the rpl_examples + * package: img_align_2d.cpp, img_align_3d.cpp + * + * Template Parameters: + * D : dimension of the residual + * T : type of the model, e.g. SE2, SE3 + */ + +template +class NLLSSolver { + +public: + typedef T ModelType; + enum Method{GaussNewton, LevenbergMarquardt}; + enum ScaleEstimatorType{UnitScale, TDistScale, MADScale, NormalScale}; + enum WeightFunctionType{UnitWeight, TDistWeight, TukeyWeight, HuberWeight}; + +protected: + Eigen::Matrix H_; //!< Hessian approximation + Eigen::Matrix Jres_; //!< Jacobian x Residual + Eigen::Matrix x_; //!< update step + bool have_prior_; + ModelType prior_; + Eigen::Matrix I_prior_; //!< Prior information matrix (inverse covariance) + double chi2_; + double rho_; + Method method_; + + /// If the flag linearize_system is set, the function must also compute the + /// Jacobian and set the member variables H_, Jres_ + virtual double + computeResiduals (const ModelType& model, + bool linearize_system, + bool compute_weight_scale) = 0; + + /// Solve the linear system H*x = Jres. This function must set the update + /// step in the member variable x_. Must return true if the system could be + /// solved and false if it was singular. + virtual int + solve () = 0; + + virtual void + update (const ModelType& old_model, ModelType& new_model) = 0; + + virtual void + applyPrior (const ModelType& current_model) { } + + virtual void + startIteration () { } + + virtual void + finishIteration () { } + + virtual void + finishTrial () { } + +public: + + /// Damping parameter. If mu > 0, coefficient matrix is positive definite, this + /// ensures that x is a descent direction. If mu is large, x is a short step in + /// the steepest direction. This is good if the current iterate is far from the + /// solution. If mu is small, LM approximates gauss newton iteration and we + /// have (almost) quadratic convergence in the final stages. + double mu_init_, mu_; + double nu_init_, nu_; //!< Increase factor of mu after fail + size_t n_iter_init_, n_iter_; //!< Number of Iterations + size_t n_trials_; //!< Number of trials + size_t n_trials_max_; //!< Max number of trials + size_t n_meas_; //!< Number of measurements + bool stop_; //!< Stop flag + bool verbose_; //!< Output Statistics + double eps_; //!< Stop if update norm is smaller than eps + size_t iter_; //!< Current Iteration + + // robust least squares + bool use_weights_; + float scale_; + robust_cost::ScaleEstimatorPtr scale_estimator_; + robust_cost::WeightFunctionPtr weight_function_; + + NLLSSolver() : + have_prior_(false), + method_(LevenbergMarquardt), + mu_init_(0.01f), + mu_(mu_init_), + nu_init_(2.0), + nu_(nu_init_), + n_iter_init_(15), + n_iter_(n_iter_init_), + n_trials_(0), + n_trials_max_(5), + n_meas_(0), + stop_(false), + verbose_(true), + eps_(0.0000000001), + iter_(0), + use_weights_(false), + scale_(0.0), + scale_estimator_(NULL), + weight_function_(NULL) + { } + + virtual ~NLLSSolver() {} + + /// Calls the GaussNewton or LevenbergMarquardt optimization strategy + void optimize(ModelType& model); + + /// Gauss Newton optimization strategy + void optimizeGaussNewton(ModelType& model); + + /// Levenberg Marquardt optimization strategy + void optimizeLevenbergMarquardt(ModelType& model); + + /// Specify the robust cost that should be used and the appropriate scale estimator + void setRobustCostFunction( + ScaleEstimatorType scale_estimator, + WeightFunctionType weight_function); + + /// Add prior to optimization. + void setPrior( + const ModelType& prior, + const Eigen::Matrix& Information); + + /// Reset all parameters to restart the optimization + void reset(); + + /// Get the squared error + const double& getChi2() const; + + /// The Information matrix is equal to the inverse covariance matrix. + const Eigen::Matrix& getInformationMatrix() const; +}; + +} // end namespace vk + +#include "nlls_solver_impl.hpp" + +#endif /* LM_SOLVER_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/nlls_solver_impl.hpp b/src/rpg_vikit/vikit_common/include/vikit/nlls_solver_impl.hpp new file mode 100644 index 0000000..63a3b74 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/nlls_solver_impl.hpp @@ -0,0 +1,324 @@ +/* + * Abstract Nonlinear Least-Squares Solver Class + * + * nlls_solver.h + * + * Created on: Nov 5, 2012 + * Author: cforster + */ + +#ifndef LM_SOLVER_IMPL_HPP_ +#define LM_SOLVER_IMPL_HPP_ + +#include +#include + +template +void vk::NLLSSolver::optimize(ModelType& model) +{ + if(method_ == GaussNewton) + optimizeGaussNewton(model); + else if(method_ == LevenbergMarquardt) + optimizeLevenbergMarquardt(model); +} + +template +void vk::NLLSSolver::optimizeGaussNewton(ModelType& model) +{ + // Compute weight scale + if(use_weights_) + computeResiduals(model, false, true); + + // Save the old model to rollback in case of unsuccessful update + ModelType old_model(model); + + // perform iterative estimation + for (iter_ = 0; iter_::Identity(D,D); + Jres_.setZero(); + + // compute initial error + n_meas_ = 0; + computeResiduals(model, true, false); + + // add damping term: + H_ += (H_.diagonal()*mu_).asDiagonal(); + + // add prior + if(have_prior_) + applyPrior(model); + + // solve the linear system + if(solve()) + { + // update the model + update(model, new_model); + + // compute error with new model and compare to old error + n_meas_ = 0; + new_chi2 = computeResiduals(new_model, false, false); + rho_ = chi2_-new_chi2; + } + else + { + // matrix was singular and could not be computed + cout << "Matrix is close to singular!" << endl; + cout << "H = " << H_ << endl; + cout << "Jres = " << Jres_ << endl; + rho_ = -1; + } + + if(rho_>0) + { + // update decrased the error -> success + model = new_model; + chi2_ = new_chi2; + stop_ = vk::norm_max(x_)<=eps_; + mu_ *= max(1./3., min(1.-pow(2*rho_-1,3), 2./3.)); + nu_ = 2.; + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Success" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << endl; + } + } + else + { + // update increased the error -> fail + mu_ *= nu_; + nu_ *= 2.; + ++n_trials_; + if (n_trials_ >= n_trials_max_) + stop_ = true; + + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Failure" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << endl; + } + } + + finishTrial(); + + } while(!(rho_>0 || stop_)); + if (stop_) + break; + + finishIteration(); + } +} + + +template +void vk::NLLSSolver::setRobustCostFunction( + ScaleEstimatorType scale_estimator, + WeightFunctionType weight_function) +{ + switch(scale_estimator) + { + case TDistScale: + if(verbose_) + printf("Using TDistribution Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::TDistributionScaleEstimator()); + use_weights_=true; + break; + case MADScale: + if(verbose_) + printf("Using MAD Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::MADScaleEstimator()); + use_weights_=true; + break; + case NormalScale: + if(verbose_) + printf("Using Normal Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::NormalDistributionScaleEstimator()); + use_weights_=true; + break; + default: + if(verbose_) + printf("Using Unit Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::UnitScaleEstimator()); + use_weights_=false; + } + + switch(weight_function) + { + case TDistWeight: + if(verbose_) + printf("Using TDistribution Weight Function\n"); + weight_function_.reset(new robust_cost::TDistributionWeightFunction()); + break; + case TukeyWeight: + if(verbose_) + printf("Using Tukey Weight Function\n"); + weight_function_.reset(new robust_cost::TukeyWeightFunction()); + break; + case HuberWeight: + if(verbose_) + printf("Using Huber Weight Function\n"); + weight_function_.reset(new robust_cost::HuberWeightFunction()); + break; + default: + if(verbose_) + printf("Using Unit Weight Function\n"); + weight_function_.reset(new robust_cost::UnitWeightFunction()); + } +} + +template +void vk::NLLSSolver::setPrior( + const T& prior, + const Eigen::Matrix& Information) +{ + have_prior_ = true; + prior_ = prior; + I_prior_ = Information; +} + +template +void vk::NLLSSolver::reset() +{ + have_prior_ = false; + chi2_ = 1e10; + mu_ = mu_init_; + nu_ = nu_init_; + n_meas_ = 0; + n_iter_ = n_iter_init_; + iter_ = 0; + stop_ = false; +} + +template +inline const double& vk::NLLSSolver::getChi2() const +{ + return chi2_; +} + +template +inline const Eigen::Matrix& vk::NLLSSolver::getInformationMatrix() const +{ + return H_; +} + +#endif /* LM_SOLVER_IMPL_HPP_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/omni_camera.h b/src/rpg_vikit/vikit_common/include/vikit/omni_camera.h new file mode 100644 index 0000000..a17e286 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/omni_camera.h @@ -0,0 +1,87 @@ +/* + * OcamProjector.h + * + * Created on: Sep 22, 2010 + * Author: laurent kneip + */ + +#ifndef OCAMPROJECTOR_H_ +#define OCAMPROJECTOR_H_ + +#include +#include +#include +#include +#include + +#define CMV_MAX_BUF 1024 +#define MAX_POL_LENGTH 64 + +namespace vk { + +using namespace std; +using namespace Eigen; + +struct ocam_model +{ + double pol[MAX_POL_LENGTH]; // the polynomial coefficients: pol[0] + x"pol[1] + x^2*pol[2] + ... + x^(N-1)*pol[N-1] + int length_pol; // length of polynomial + double invpol[MAX_POL_LENGTH]; // the coefficients of the inverse polynomial + int length_invpol; // length of inverse polynomial + double xc; // row coordinate of the center + double yc; // column coordinate of the center + double c; // affine parameter + double d; // affine parameter + double e; // affine parameter + int width; // image width + int height; // image height +}; + +class OmniCamera : public AbstractCamera { + +private: + struct ocam_model ocamModel; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + double error_multiplier_; + + OmniCamera(){} + OmniCamera(string calibFile); + ~OmniCamera(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + double + computeErrorMultiplier(); + + virtual double errorMultiplier2() const + { + return sqrt(error_multiplier_)/2; + } + + virtual double errorMultiplier() const + { + return error_multiplier_; + } + + virtual double fx() const { return 0.0; }; + virtual double fy() const { return 0.0; }; + virtual double cx() const { return 0.0; }; + virtual double cy() const { return 0.0; }; +}; + +} // end namespace vk + +#endif /* OCAMPROJECTOR_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/patch_score.h b/src/rpg_vikit/vikit_common/include/vikit/patch_score.h new file mode 100644 index 0000000..8f0516c --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/patch_score.h @@ -0,0 +1,225 @@ +/* + * patch_score.h + * + * Created on: Dec 5, 2013 + * Author: cforster + */ + +#ifndef VIKIT_PATCH_SCORE_H_ +#define VIKIT_PATCH_SCORE_H_ + +#include + +#if __SSE2__ +#include +#endif + +namespace vk { +namespace patch_score { + + +#if __SSE2__ +// Horizontal sum of uint16s stored in an XMM register +inline int SumXMM_16(__m128i &target) +{ + unsigned short int sums_store[8]; + _mm_storeu_si128((__m128i*)sums_store, target); + return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3] + + sums_store[4] + sums_store[5] + sums_store[6] + sums_store[7]; +} +// Horizontal sum of uint32s stored in an XMM register +inline int SumXMM_32(__m128i &target) +{ + unsigned int sums_store[4]; + _mm_storeu_si128((__m128i*)sums_store, target); + return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3]; +} +#endif + +/// Zero Mean Sum of Squared Differences Cost +template +class ZMSSD { +public: + + static const int patch_size_ = 2*HALF_PATCH_SIZE; + static const int patch_area_ = patch_size_*patch_size_; + static const int threshold_ = 2000*patch_area_; + uint8_t* ref_patch_; + int sumA_, sumAA_; + + ZMSSD(uint8_t* ref_patch) : + ref_patch_(ref_patch) + { + uint32_t sumA_uint=0, sumAA_uint=0; + for(int r = 0; r < patch_area_; r++) + { + uint8_t n = ref_patch_[r]; + sumA_uint += n; + sumAA_uint += n*n; + } + sumA_ = sumA_uint; + sumAA_ = sumAA_uint; + } + + static int threshold() { return threshold_; } + + int computeScore(uint8_t* cur_patch) const + { + uint32_t sumB_uint = 0; + uint32_t sumBB_uint = 0; + uint32_t sumAB_uint = 0; + for(int r = 0; r < patch_area_; r++) + { + const uint8_t cur_pixel = cur_patch[r]; + sumB_uint += cur_pixel; + sumBB_uint += cur_pixel*cur_pixel; + sumAB_uint += cur_pixel * ref_patch_[r]; + } + const int sumB = sumB_uint; + const int sumBB = sumBB_uint; + const int sumAB = sumAB_uint; + return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_; + } + + int computeScore(uint8_t* cur_patch, int stride) const + { + int sumB, sumBB, sumAB; +#if __SSE2__ + if(patch_size_ == 8) + { + // From PTAM-GPL, Copyright 2008 Isis Innovation Limited + __m128i xImageAsEightBytes; + __m128i xImageAsWords; + __m128i xTemplateAsEightBytes; + __m128i xTemplateAsWords; + __m128i xZero; + __m128i xImageSums; // These sums are 8xuint16 + __m128i xImageSqSums; // These sums are 4xint32 + __m128i xCrossSums; // These sums are 4xint32 + __m128i xProduct; + + xImageSums = _mm_setzero_si128(); + xImageSqSums = _mm_setzero_si128(); + xCrossSums = _mm_setzero_si128(); + xZero = _mm_setzero_si128(); + + uint8_t* imagepointer = cur_patch; + uint8_t* templatepointer = ref_patch_; + long unsigned int cur_stride = stride; + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + imagepointer += cur_stride; + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer); + templatepointer += 16; + xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer); + xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero); + xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums); + xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords); + xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums); + xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero); + xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords); + xCrossSums = _mm_add_epi32(xProduct, xCrossSums); + + sumB = SumXMM_16(xImageSums); + sumAB = SumXMM_32(xCrossSums); + sumBB = SumXMM_32(xImageSqSums); + } + else +#endif + { + uint32_t sumB_uint = 0; + uint32_t sumBB_uint = 0; + uint32_t sumAB_uint = 0; + for(int y=0, r=0; y < patch_size_; ++y) + { + uint8_t* cur_patch_ptr = cur_patch + y*stride; + for(int x=0; x < patch_size_; ++x, ++r) + { + const uint8_t cur_px = cur_patch_ptr[x]; + sumB_uint += cur_px; + sumBB_uint += cur_px * cur_px; + sumAB_uint += cur_px * ref_patch_[r]; + } + } + sumB = sumB_uint; + sumBB = sumBB_uint; + sumAB = sumAB_uint; + } + return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_; + } +}; + +} // namespace patch_score +} // namespace vk + +#endif // VIKIT_PATCH_SCORE_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/performance_monitor.h b/src/rpg_vikit/vikit_common/include/vikit/performance_monitor.h new file mode 100644 index 0000000..faf07b8 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/performance_monitor.h @@ -0,0 +1,53 @@ +/* + * performance_monitor.h + * + * Created on: Aug 26, 2011 + * Author: Christian Forster + */ + +#ifndef VIKIT_PERFORMANCE_MONITOR_H +#define VIKIT_PERFORMANCE_MONITOR_H + +#include +#include +#include +#include +#include + +namespace vk +{ + +struct LogItem +{ + double data; + bool set; +}; + +class PerformanceMonitor +{ +public: + PerformanceMonitor(); + ~PerformanceMonitor(); + void init(const std::string& trace_name, const std::string& trace_dir); + void addTimer(const std::string& name); + void addLog(const std::string& name); + void writeToFile(); + void startTimer(const std::string& name); + void stopTimer(const std::string& name); + double getTime(const std::string& name) const; + void log(const std::string& name, double data); + +private: + std::map timers_; + std::map logs_; + std::string trace_name_; // +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +class PinholeCamera : public AbstractCamera { + +private: + const double fx_, fy_; + const double cx_, cy_; + bool distortion_; //!< is it pure pinhole model or has it radial distortion? + double d_[5]; //!< distortion parameters, see http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html + cv::Mat cvK_, cvD_; + cv::Mat undist_map1_, undist_map2_; + bool use_optimization_; + Matrix3d K_; + Matrix3d K_inv_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + PinholeCamera(double width, double height, double scale, + double fx, double fy, double cx, double cy, + double d0=0.0, double d1=0.0, double d2=0.0, double d3=0.0, double d4=0.0); + + ~PinholeCamera(); + + void + initUnistortionMap(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + const Vector2d focal_length() const + { + return Vector2d(fx_, fy_); + } + + virtual double errorMultiplier2() const + { + return fabs(fx_); + } + + virtual double errorMultiplier() const + { + return fabs(4.0*fx_*fy_); + } + + inline const Matrix3d& K() const { return K_; }; + inline const Matrix3d& K_inv() const { return K_inv_; }; + virtual double fx() const { return fx_; }; + virtual double fy() const { return fy_; }; + virtual double cx() const { return cx_; }; + virtual double cy() const { return cy_; }; + inline double d0() const { return d_[0]; }; + inline double d1() const { return d_[1]; }; + inline double d2() const { return d_[2]; }; + inline double d3() const { return d_[3]; }; + inline double d4() const { return d_[4]; }; + + void undistortImage(const cv::Mat& raw, cv::Mat& rectified); + +}; + +} // end namespace vk + + +#endif /* PINHOLE_CAMERA_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/polynomial_camera.h b/src/rpg_vikit/vikit_common/include/vikit/polynomial_camera.h new file mode 100644 index 0000000..ab9e307 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/polynomial_camera.h @@ -0,0 +1,153 @@ +/* + * polynomial_camera.h + * + * Created on: January 26, 2023 + * Author: xuankuzcr + */ + +#ifndef POLYNOMIAL_CAMERA_H_ +#define POLYNOMIAL_CAMERA_H_ + +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +class PolynomialCamera : public AbstractCamera { + +private: + const double fx_, fy_; + const double cx_, cy_; + const double skew_; + bool distortion_; //!< is it pure pinhole model or has it equidistant distortion + double k2_, k3_, k4_, k5_, k6_, k7_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + // PolynomialCamera(double width, double height, double scale, + // double fx, double fy, double cx, double cy, double skew, + // double k2=0.0, double k3=0.0, double k4=0.0, double k5=0.0, double k6=0.0, double k7=0.0); + + PolynomialCamera(double width, double height, // double scale, + double fx, double fy, double cx, double cy, double skew, + double k2=0.0, double k3=0.0, double k4=0.0, double k5=0.0, double k6=0.0, double k7=0.0); + + ~PolynomialCamera(); + + virtual Vector3d + cam2world(const double& x, const double& y) const; + + virtual Vector3d + cam2world(const Vector2d& px) const; + + virtual Vector2d + world2cam(const Vector3d& xyz_c) const; + + virtual Vector2d + world2cam(const Vector2d& uv) const; + + const Vector2d focal_length() const + { + return Vector2d(fx_, fy_); + } + + inline double thetad_from_theta(const double theta) const + { + const double theta2 = theta * theta; + const double theta3 = theta2 * theta; + const double theta4 = theta3 * theta; + const double theta5 = theta4 * theta; + const double theta6 = theta5 * theta; + const double theta7 = theta6 * theta; + const double thetad = theta + k2_ * theta2 + k3_ * theta3 + + k4_ * theta4 + k5_ * theta5 + k6_ * theta6 + k7_ * theta7; + return thetad; + } + + inline double deriv_thetad_from_theta(const double theta) const + { + const double theta2 = theta * theta; + const double theta3 = theta2 * theta; + const double theta4 = theta3 * theta; + const double theta5 = theta4 * theta; + const double theta6 = theta5 * theta; + return 1 + 2 * k2_ * theta + 3 * k3_ * theta2 + 4 * k4_ * theta3 + 5 * k5_ * theta4 + + 6 * k6_ * theta5 + 7 * k7_ * theta6; + } + + inline Eigen::Matrix2d jacobian_2x2(const Eigen::Vector2d& uv) const + { + const double r = uv.norm(); + if (r < 1e-8) + { + return Eigen::Matrix2d::Identity(); + } + + const double inv_r = 1.0 / r; + const double r2 = r * r; + const double dr_du = uv(0) * inv_r; + const double dr_dv = uv(1) * inv_r; + + const double theta = std::atan(r); + const double dtheta_dr = 1.0 / (1 + r * r); + + const double thetad = thetad_from_theta(theta); + const double dthetad_dtheta = deriv_thetad_from_theta(theta); + const double dthetad_dr = dthetad_dtheta * dtheta_dr; + + const double scaling = thetad / r; + const double dscaling_du = (dthetad_dr * dr_du * r - dr_du * thetad) / r2; + const double dscaling_dv = (dthetad_dr * dr_dv * r - dr_dv * thetad) / r2; + + const double dx_du = dscaling_du * uv(0) + scaling; + const double dx_dv = dscaling_dv * uv(0); + const double dy_du = dscaling_du * uv(1); + const double dy_dv = dscaling_dv * uv(1) + scaling; + Eigen::Matrix2d jac; + jac << dx_du, dx_dv, dy_du, dy_dv; + return jac; + } + + inline Eigen::Matrix jacobian_2x3(const Eigen::Vector3d& p) const + { + Eigen::Matrix jac; + const double x = p[0]; + const double y = p[1]; + const double z_inv = 1./p[2]; + const double z_inv_2 = z_inv * z_inv; + jac(0,0) = fx_ * z_inv; + jac(0,1) = skew_ * z_inv; + jac(0,2) = -fx_ * x * z_inv_2; + jac(1,0) = 0.0; + jac(1,1) = fy_ * z_inv; + jac(1,2) = -fy_ * y * z_inv_2; + return jac; + } + + virtual double errorMultiplier2() const + { + return fabs(fx_); + } + + virtual double errorMultiplier() const + { + return fabs(4.0*fx_*fy_); + } + + virtual double fx() const { return fx_; }; + virtual double fy() const { return fy_; }; + virtual double cx() const { return cx_; }; + virtual double cy() const { return cy_; }; +}; + +} // end namespace vk + + +#endif /* #define POLYNOMIAL_CAMERA_H_ */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/ringbuffer.h b/src/rpg_vikit/vikit_common/include/vikit/ringbuffer.h new file mode 100644 index 0000000..caa2dc6 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/ringbuffer.h @@ -0,0 +1,130 @@ +// This file is part of VisionTools. +// +// Copyright 2011 Hauke Strasdat (Imperial College London) +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef VISIONTOOLS_RING_BUFFER_H +#define VISIONTOOLS_RING_BUFFER_H + +#include +#include +#include + +namespace vk +{ + +template +class RingBuffer +{ +public: + RingBuffer (int size); + + void + push_back (const T & elem); + + bool + empty () const; + + T + get (int i); + + T + getSum () const; + + T + getMean () const; + + int size() + { + return num_elem_; + } + +private: + std::vector arr_; + int begin_; + int end_; + int num_elem_; + int arr_size_; +}; + +template +RingBuffer +::RingBuffer(int size) : + arr_(size), + begin_(0), + end_(-1), + num_elem_(0), + arr_size_(size) +{} + +template +bool RingBuffer +::empty() const +{ + return arr_.empty(); +} + +template +void RingBuffer +::push_back(const T & elem) +{ + if (num_elem_ +T RingBuffer +::get(int i) +{ + assert(i +T RingBuffer +::getSum() const +{ + T sum=0; + for(int i=0; i +T RingBuffer +::getMean() const +{ + if(num_elem_ == 0) + return 0; + return getSum()/num_elem_; +} + +} + +#endif diff --git a/src/rpg_vikit/vikit_common/include/vikit/robust_cost.h b/src/rpg_vikit/vikit_common/include/vikit/robust_cost.h new file mode 100644 index 0000000..d03881a --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/robust_cost.h @@ -0,0 +1,156 @@ +/** +* This file is part of dvo. +* +* Copyright 2012 Christian Kerl (Technical University of Munich) +* For more information see . +* +* dvo 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 3 of the License, or +* (at your option) any later version. +* +* dvo 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 dvo. If not, see . +*/ + +#ifndef VIKIT_ROBUST_COST_H_ +#define VIKIT_ROBUST_COST_H_ + +#include +#include +#include +namespace vk { +namespace robust_cost { + +// interface for scale estimators +class ScaleEstimator +{ +public: + virtual ~ScaleEstimator() {}; + virtual float compute(std::vector& errors) const = 0; +}; +typedef std::shared_ptr ScaleEstimatorPtr; + +class UnitScaleEstimator : public ScaleEstimator +{ +public: + UnitScaleEstimator() {} + virtual ~UnitScaleEstimator() {} + virtual float compute(std::vector& errors) const { return 1.0f; }; +}; + +// estimates scale by fitting a t-distribution to the data with the given degrees of freedom +class TDistributionScaleEstimator : public ScaleEstimator +{ +public: + TDistributionScaleEstimator(const float dof = DEFAULT_DOF); + virtual ~TDistributionScaleEstimator() {}; + virtual float compute(std::vector& errors) const; + + static const float DEFAULT_DOF; + static const float INITIAL_SIGMA; +protected: + float dof_; + float initial_sigma_; +}; + +// estimates scale by computing the median absolute deviation +class MADScaleEstimator : public ScaleEstimator +{ +public: + MADScaleEstimator() {}; + virtual ~MADScaleEstimator() {}; + virtual float compute(std::vector& errors) const; + +private: + static const float NORMALIZER;; +}; + +// estimates scale by computing the standard deviation +class NormalDistributionScaleEstimator : public ScaleEstimator +{ +public: + NormalDistributionScaleEstimator() {}; + virtual ~NormalDistributionScaleEstimator() {}; + virtual float compute(std::vector& errors) const; +private: +}; + +/** + * Interface for weight functions. A weight function is the first derivative of a symmetric robust function p(sqrt(t)). + * The errors are assumed to be normalized to unit variance. + * + * See: + * "Lucas-Kanade 20 Years On: A Unifying Framework: Part 2" - Page 23, Equation (54) + */ +class WeightFunction +{ +public: + virtual ~WeightFunction() {}; + virtual float value(const float& x) const = 0; + virtual void configure(const float& param) {}; +}; +typedef std::shared_ptr WeightFunctionPtr; + +class UnitWeightFunction : public WeightFunction +{ +public: + UnitWeightFunction() {}; + virtual ~UnitWeightFunction() {}; + virtual float value(const float& x) const { return 1.0f; }; +}; + +/** + * Tukey's hard re-descending function. + * + * See: + * http://en.wikipedia.org/wiki/Redescending_M-estimator + */ +class TukeyWeightFunction : public WeightFunction +{ +public: + TukeyWeightFunction(const float b = DEFAULT_B); + virtual ~TukeyWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_B; +private: + float b_square; +}; + +class TDistributionWeightFunction : public WeightFunction +{ +public: + TDistributionWeightFunction(const float dof = DEFAULT_DOF); + virtual ~TDistributionWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_DOF; +private: + float dof_; + float normalizer_; +}; + +class HuberWeightFunction : public WeightFunction +{ +public: + HuberWeightFunction(const float k = DEFAULT_K); + virtual ~HuberWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_K; +private: + float k; +}; + +} // namespace robust_cost +} // namespace vk +#endif // VIKIT_ROBUST_COST_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/sample.h b/src/rpg_vikit/vikit_common/include/vikit/sample.h new file mode 100644 index 0000000..a0a047f --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/sample.h @@ -0,0 +1,50 @@ +#ifndef VIKIT_SAMPLE_H_ +#define VIKIT_SAMPLE_H_ + +#include +#include + +namespace vk { + +class Sample +{ +public: + static void setTimeBasedSeed(); + static int uniform(int from, int to); + static double uniform(); + static double gaussian(double sigma); + static std::ranlux24 gen_real; + static std::mt19937 gen_int; +}; + +std::ranlux24 Sample::gen_real; +std::mt19937 Sample::gen_int; + +void Sample::setTimeBasedSeed() +{ + unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); + gen_real = std::ranlux24(seed); + gen_int = std::mt19937(seed); +} + +int Sample::uniform(int from, int to) +{ + std::uniform_int_distribution distribution(from, to); + return distribution(gen_int); +} + +double Sample::uniform() +{ + std::uniform_real_distribution distribution(0.0, 1.0); + return distribution(gen_real); +} + +double Sample::gaussian(double stddev) +{ + std::normal_distribution distribution(0.0, stddev); + return distribution(gen_real); +} + +} // namespace vk + +#endif // VIKIT_SAMPLE_H_ diff --git a/src/rpg_vikit/vikit_common/include/vikit/timer.h b/src/rpg_vikit/vikit_common/include/vikit/timer.h new file mode 100644 index 0000000..45bbc7f --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/timer.h @@ -0,0 +1,81 @@ +#ifndef TIMER_H +#define TIMER_H + +#include +#include +#include + +namespace vk +{ + +class Timer +{ +private: + timeval start_time_; + double time_; + double accumulated_; +public: + + /// The constructor directly starts the timer. + Timer() : + time_(0.0), + accumulated_(0.0) + { + start(); + } + + ~Timer() + {} + + inline void start() + { + accumulated_ = 0.0; + gettimeofday(&start_time_, NULL); + } + + inline void resume() + { + gettimeofday(&start_time_, NULL); + } + + inline double stop() + { + timeval end_time; + gettimeofday(&end_time, NULL); + long seconds = end_time.tv_sec - start_time_.tv_sec; + long useconds = end_time.tv_usec - start_time_.tv_usec; + time_ = ((seconds) + useconds*0.000001) + accumulated_; + accumulated_ = time_; + return time_; + } + + inline double getTime() const + { + return time_; + } + + inline void reset() + { + time_ = 0.0; + accumulated_ = 0.0; + } + + static double getCurrentTime() + { + timeval time_now; + gettimeofday(&time_now, NULL); + return time_now.tv_sec + time_now.tv_usec*0.000001; + } + + static double getCurrentSecond() + { + timeval time_now; + gettimeofday(&time_now, NULL); + return time_now.tv_sec; + } + +}; + +} // end namespace vk + +#endif diff --git a/src/rpg_vikit/vikit_common/include/vikit/user_input_thread.h b/src/rpg_vikit/vikit_common/include/vikit/user_input_thread.h new file mode 100644 index 0000000..a92cd15 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/user_input_thread.h @@ -0,0 +1,54 @@ +#ifndef USER_INPUT_THREAD_H +#define USER_INPUT_THREAD_H + +#include +#include + +namespace vk { + +/// A class that starts its own thread and listens to the console input. The +/// console input can then be inquired using the getInput() function. +class UserInputThread +{ +public: + UserInputThread(); + ~UserInputThread(); + + /// Returns the latest acquired user input. Default is set to 0. + /// Once this function is called, the input state is reset to the default. + char getInput(); + + /// Stop the thread + void stop(); + +private: + + /// Main loop that waits for new user input + void acquireUserInput(); + + /// Initialize new terminal i/o settings + void initTermios(int echo); + + /// Restore old terminal i/o settings + void resetTermios(); + + /// Read 1 character - echo defines echo mode + int getch_(int echo); + + /// Read 1 character without echo + int getch(); + + /// Read 1 character with echo + int getche(); + + bool stop_; + std::thread * user_input_thread_; + char input_; + + struct termios original_terminal_settings_; + struct termios old_terminal_settings_, new_terminal_settings_; +}; + +} // end namespace vk + +#endif /* USER_INPUT_THREAD_H */ diff --git a/src/rpg_vikit/vikit_common/include/vikit/vision.h b/src/rpg_vikit/vikit_common/include/vikit/vision.h new file mode 100644 index 0000000..b1e5304 --- /dev/null +++ b/src/rpg_vikit/vikit_common/include/vikit/vision.h @@ -0,0 +1,77 @@ +/* + * vision.h + * + * Created on: May 14, 2013 + * Author: cforster + */ + +#ifndef VIKIT_VISION_H_ +#define VIKIT_VISION_H_ + +#include +#include +#include + +namespace vk +{ + +//! Return value between 0 and 1 +//! WARNING This function does not check whether the x/y is within the border +inline float +interpolateMat_32f(const cv::Mat& mat, float u, float v) +{ + assert(mat.type()==CV_32F); + float x = floor(u); + float y = floor(v); + float subpix_x = u-x; + float subpix_y = v-y; + float wx0 = 1.0-subpix_x; + float wx1 = subpix_x; + float wy0 = 1.0-subpix_y; + float wy1 = subpix_y; + + float val00 = mat.at(y,x); + float val10 = mat.at(y,x+1); + float val01 = mat.at(y+1,x); + float val11 = mat.at(y+1,x+1); + return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11; +} + +//! Return value between 0 and 255 +//! WARNING This function does not check whether the x/y is within the border +inline float +interpolateMat_8u(const cv::Mat& mat, float u, float v) +{ + assert(mat.type()==CV_8U); + int x = floor(u); + int y = floor(v); + float subpix_x = u-x; + float subpix_y = v-y; + + float w00 = (1.0f-subpix_x)*(1.0f-subpix_y); + float w01 = (1.0f-subpix_x)*subpix_y; + float w10 = subpix_x*(1.0f-subpix_y); + float w11 = 1.0f - w00 - w01 - w10; + + const int stride = mat.step.p[0]; + unsigned char* ptr = mat.data + y*stride + x; + return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1]; +} + +void halfSample(const cv::Mat& in, cv::Mat& out); + +float shiTomasiScore(const cv::Mat& img, int u, int v); + +void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst); + +#ifdef __SSE2__ + +/// Used to convert a Kinect depthmap +/// Code by Christian Kerl DVO, GPL Licence +void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale); + +#endif + +} // namespace vk + +#endif // VIKIT_VISION_H_ diff --git a/src/rpg_vikit/vikit_common/lib/libvikit_common.so b/src/rpg_vikit/vikit_common/lib/libvikit_common.so new file mode 100755 index 0000000..d2d644e Binary files /dev/null and b/src/rpg_vikit/vikit_common/lib/libvikit_common.so differ diff --git a/src/rpg_vikit/vikit_common/package.xml b/src/rpg_vikit/vikit_common/package.xml new file mode 100644 index 0000000..4717938 --- /dev/null +++ b/src/rpg_vikit/vikit_common/package.xml @@ -0,0 +1,30 @@ + + + vikit_common + 0.0.0 + + The vikit_common package + + + cforster + + + GPLv3 + + + ament_cmake + + + rclcpp + cmake_modules + + + rclcpp + + + + + + + + \ No newline at end of file diff --git a/src/rpg_vikit/vikit_common/src/atan_camera.cpp b/src/rpg_vikit/vikit_common/src/atan_camera.cpp new file mode 100644 index 0000000..c35bb54 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/atan_camera.cpp @@ -0,0 +1,86 @@ +/* + * atan_camera.cpp + * + * Created on: Aug 21, 2012 + * Author: cforster + */ + + +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +ATANCamera:: +ATANCamera(double width, double height, + double fx, double fy, + double cx, double cy, + double s) : + AbstractCamera(width, height, 1.0), + fx_(width*fx), fy_(height*fy), + fx_inv_(1.0/fx_), fy_inv_(1.0/fy_), + cx_(cx*width - 0.5), cy_(cy*height - 0.5), + s_(s), s_inv_(1.0/s_) +{ + if(s_ != 0.0) + { + tans_ = 2.0 * tan(s_ / 2.0); + tans_inv_ = 1.0 / tans_; + s_inv_ = 1.0 / s_; + distortion_ = true; + } + else + { + s_inv_ = 0.0; + tans_ = 0.0; + distortion_ = false; + } +} + +ATANCamera:: +~ATANCamera() +{} + +Vector3d ATANCamera:: +cam2world(const double& x, const double& y) const +{ + Vector2d dist_cam((x - cx_) * fx_inv_, + (y - cy_) * fy_inv_); + double dist_r = dist_cam.norm(); + double r = invrtrans(dist_r); + double d_factor; + if(dist_r > 0.01) + d_factor = r / dist_r; + else + d_factor = 1.0; + return unproject2d(d_factor * dist_cam).normalized(); +} + +Vector3d ATANCamera:: +cam2world (const Vector2d& px) const +{ + return cam2world(px[0], px[1]); +} + +Vector2d ATANCamera:: +world2cam(const Vector3d& xyz_c) const +{ + return world2cam(project2d(xyz_c)); +} + +Vector2d ATANCamera:: +world2cam(const Vector2d& uv) const +{ + double r = uv.norm(); + double factor = rtrans_factor(r); + Vector2d dist_cam = factor * uv; + return Vector2d(cx_ + fx_ * dist_cam[0], + cy_ + fy_ * dist_cam[1]); +} + +} /* end vk */ diff --git a/src/rpg_vikit/vikit_common/src/equidistant_camera.cpp b/src/rpg_vikit/vikit_common/src/equidistant_camera.cpp new file mode 100644 index 0000000..2fa5224 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/equidistant_camera.cpp @@ -0,0 +1,111 @@ +/* + * equidistant_camera.cpp + * + * Created on: January 26, 2023 + * Author: xuankuzcr + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +EquidistantCamera:: +EquidistantCamera(double width, double height, double scale, + double fx, double fy, + double cx, double cy, + double k1, double k2, double k3, double k4) : + AbstractCamera(width * scale , height * scale, scale), + fx_(fx * scale), fy_(fy * scale), cx_(cx * scale), cy_(cy * scale), + distortion_(fabs(k1) > 0.0000001) +{ + cout << "scale: " << scale << endl; + k1_ = k1; k2_ = k2; k3_ = k3; k4_ = k4; +} + +EquidistantCamera:: +~EquidistantCamera() +{} + +Vector3d EquidistantCamera:: +cam2world(const double& u, const double& v) const +{ + Vector3d xyz; + if(!distortion_) + { + xyz[0] = (u - cx_)/fx_; + xyz[1] = (v - cy_)/fy_; + xyz[2] = 1.0; + } + else + { + double x = (u - cx_)/fx_; + double y = (v - cy_)/fy_; + const double thetad = std::sqrt(x * x + y * y); + double theta = thetad; + for (int i = 0; i < 5; ++i) + { + const double theta2 = theta * theta; + const double theta4 = theta2 * theta2; + const double theta6 = theta4 * theta2; + const double theta8 = theta4 * theta4; + theta = thetad / + (1.0 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8); + } + const double scaling = std::tan(theta) / thetad; + x *= scaling; + y *= scaling; + xyz[0] = x; + xyz[1] = y; + xyz[2] = 1.0; + } + return xyz.normalized(); +} + +Vector3d EquidistantCamera:: +cam2world (const Vector2d& uv) const +{ + return cam2world(uv[0], uv[1]); +} + +Vector2d EquidistantCamera:: +world2cam(const Vector3d& xyz) const +{ + return world2cam(project2d(xyz)); +} + +Vector2d EquidistantCamera:: +world2cam(const Vector2d& uv) const +{ + Vector2d px; + if(!distortion_) + { + px[0] = fx_*uv[0] + cx_; + px[1] = fy_*uv[1] + cy_; + } + else + { + double xd, yd; + const double r = uv.norm(); + if (r < 1e-8) + { + return uv; + } + const double theta = std::atan(r); + const double thetad = thetad_from_theta(theta); + const double scaling = thetad / r; + xd = uv[0] * scaling; + yd = uv[1] * scaling; + px[0] = xd*fx_ + cx_; + px[1] = yd*fy_ + cy_; + } + return px; +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/homography.cpp b/src/rpg_vikit/vikit_common/src/homography.cpp new file mode 100644 index 0000000..785f519 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/homography.cpp @@ -0,0 +1,283 @@ +/* + * homography.cpp + * Adaptation of PTAM-GPL HomographyInit class. + * https://github.com/Oxford-PTAM/PTAM-GPL + * Licence: GPLv3 + * Copyright 2008 Isis Innovation Limited + * + * Created on: Sep 2, 2012 + * by: cforster + */ + +#include +#include +#include + +namespace vk { + +Homography:: +Homography(const vector >& _fts1, + const vector >& _fts2, + double _error_multiplier2, + double _thresh_in_px) : + thresh(_thresh_in_px), + error_multiplier2(_error_multiplier2), + fts_c1(_fts1), + fts_c2(_fts2) +{ +} + +void Homography:: +calcFromPlaneParams(const Vector3d& n_c1, const Vector3d& xyz_c1) +{ + double d = n_c1.dot(xyz_c1); // normal distance from plane to KF + H_c2_from_c1 = T_c2_from_c1.rotationMatrix() + (T_c2_from_c1.translation()*n_c1.transpose())/d; +} + +void Homography:: +calcFromMatches() +{ + vector src_pts(fts_c1.size()), dst_pts(fts_c1.size()); + for(size_t i=0; i(0,0); + H_c2_from_c1(0,1) = cvH.at(0,1); + H_c2_from_c1(0,2) = cvH.at(0,2); + H_c2_from_c1(1,0) = cvH.at(1,0); + H_c2_from_c1(1,1) = cvH.at(1,1); + H_c2_from_c1(1,2) = cvH.at(1,2); + H_c2_from_c1(2,0) = cvH.at(2,0); + H_c2_from_c1(2,1) = cvH.at(2,1); + H_c2_from_c1(2,2) = cvH.at(2,2); +} + +size_t Homography:: +computeMatchesInliers() +{ + inliers.clear(); inliers.resize(fts_c1.size()); + size_t n_inliers = 0; + for(size_t i=0; i svd(H_c2_from_c1, ComputeThinU | ComputeThinV); + + Vector3d singular_values = svd.singularValues(); + + double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT) + double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book. + double d3 = fabs(singular_values[2]); + + Matrix3d U = svd.matrixU(); + Matrix3d V = svd.matrixV(); // VT^T + + double s = U.determinant() * V.determinant(); + + double dPrime_PM = d2; + + int nCase; + if(d1 != d2 && d2 != d3) + nCase = 1; + else if( d1 == d2 && d2 == d3) + nCase = 3; + else + nCase = 2; + + if(nCase != 1) + { + printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. "); + return false; + } + + double x1_PM; + double x2; + double x3_PM; + + // All below deals with the case = 1 case. + // Case 1 implies (d1 != d3) + { // Eq. 12 + x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3)); + x2 = 0; + x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3)); + }; + + double e1[4] = {1.0,-1.0, 1.0,-1.0}; + double e3[4] = {1.0, 1.0,-1.0,-1.0}; + + Vector3d np; + HomographyDecomposition decomp; + + // Case 1, d' > 0: + decomp.d = s * dPrime_PM; + for(size_t signs=0; signs<4; signs++) + { + // Eq 13 + decomp.R = Matrix3d::Identity(); + double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2; + double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2; + decomp.R(0,0) = dCosTheta; + decomp.R(0,2) = -dSinTheta; + decomp.R(2,0) = dSinTheta; + decomp.R(2,2) = dCosTheta; + + // Eq 14 + decomp.t[0] = (d1 - d3) * x1_PM * e1[signs]; + decomp.t[1] = 0.0; + decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs]; + + np[0] = x1_PM * e1[signs]; + np[1] = x2; + np[2] = x3_PM * e3[signs]; + decomp.n = V * np; + + decompositions.push_back(decomp); + } + + // Case 1, d' < 0: + decomp.d = s * -dPrime_PM; + for(size_t signs=0; signs<4; signs++) + { + // Eq 15 + decomp.R = -1 * Matrix3d::Identity(); + double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2; + double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2; + decomp.R(0,0) = dCosPhi; + decomp.R(0,2) = dSinPhi; + decomp.R(2,0) = dSinPhi; + decomp.R(2,2) = -dCosPhi; + + // Eq 16 + decomp.t[0] = (d1 + d3) * x1_PM * e1[signs]; + decomp.t[1] = 0.0; + decomp.t[2] = (d1 + d3) * x3_PM * e3[signs]; + + np[0] = x1_PM * e1[signs]; + np[1] = x2; + np[2] = x3_PM * e3[signs]; + decomp.n = V * np; + + decompositions.push_back(decomp); + } + + // Save rotation and translation of the decomposition + for(unsigned int i=0; i(R, t); + } + return true; +} + +bool operator<(const HomographyDecomposition lhs, const HomographyDecomposition rhs) +{ + return lhs.score < rhs.score; +} + +void Homography:: +findBestDecomposition() +{ + assert(decompositions.size() == 8); + for(size_t i=0; i 0.0) + nPositive++; + } + decom.score = -nPositive; + } + + sort(decompositions.begin(), decompositions.end()); + decompositions.resize(4); + + for(size_t i=0; i 0.0) + nPositive++; + }; + decom.score = -nPositive; + } + + sort(decompositions.begin(), decompositions.end()); + decompositions.resize(2); + + // According to Faugeras and Lustman, ambiguity exists if the two scores are equal + // but in practive, better to look at the ratio! + double dRatio = (double) decompositions[1].score / (double) decompositions[0].score; + + if(dRatio < 0.9) // no ambiguity! + decompositions.erase(decompositions.begin() + 1); + else // two-way ambiguity. Resolve by sampsonus score of all points. + { + double dErrorSquaredLimit = thresh * thresh * 4; + double adSampsonusScores[2]; + for(size_t i=0; i<2; i++) + { + Sophus::SE3 T = decompositions[i].T; + Sophus::Matrix3d Essential = T.rotationMatrix() * sqew(T.translation()); + double dSumError = 0; + for(size_t m=0; m < fts_c1.size(); m++ ) + { + double d = sampsonusError(fts_c1[m], Essential, fts_c2[m]); + if(d > dErrorSquaredLimit) + d = dErrorSquaredLimit; + dSumError += d; + } + adSampsonusScores[i] = dSumError; + } + + if(adSampsonusScores[0] <= adSampsonusScores[1]) + decompositions.erase(decompositions.begin() + 1); + else + decompositions.erase(decompositions.begin()); + } +} + + +} /* end namespace vk */ diff --git a/src/rpg_vikit/vikit_common/src/img_align.cpp b/src/rpg_vikit/vikit_common/src/img_align.cpp new file mode 100644 index 0000000..f84ae8b --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/img_align.cpp @@ -0,0 +1,443 @@ +/* + * img_align.cpp + * + * Created on: Aug 22, 2012 + * Author: cforster + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +namespace vk { + +/******************************************************************************* + * Forward Compositional + */ +ForwardCompositionalSE3:: +ForwardCompositionalSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + Sophus::SE3& init_model, + int n_levels, + int n_iter, + float res_thresh, + bool display, + Method method, + int test_id) : + cam_pyr_(cam_pyr), + depth_pyr_(depth_pyr), + img_pyr_(img_pyr), + tpl_pyr_(tpl_pyr), + img_pyr_dx_(img_pyr_dx), + img_pyr_dy_(img_pyr_dy), + display_(display), + log_(test_id < 0), + res_thresh_(res_thresh) +{ + n_iter_ = n_iter; + method_ = method; + + // Init Performance Monitor +#if 0 + if(log_) + { + permon_.init("forward", ament_index_cpp::get_package_share_directory("rpl_examples") + "/trace/img_align/data", + test_id, true); + permon_.addLog("iter"); + permon_.addLog("level"); + permon_.addLog("mu"); + permon_.addLog("chi2"); + permon_.addLog("trials"); + } +#endif + + runOptimization(init_model); + +} + +ForwardCompositionalSE3:: +ForwardCompositionalSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + int n_levels, + int n_iter, + float res_thresh, + bool display, + Method method, + int test_id) : + cam_pyr_(cam_pyr), + depth_pyr_(depth_pyr), + img_pyr_(img_pyr), + tpl_pyr_(tpl_pyr), + img_pyr_dx_(img_pyr_dx), + img_pyr_dy_(img_pyr_dy), + display_(display), + log_(test_id < 0), + res_thresh_(res_thresh) +{ + n_iter_ = n_iter; + method_ = method; + + // Init Performance Monitor +#if 0 + if(log_) + { + permon_.init("forward", ament_index_cpp::get_package_share_directory("rpl_examples") + "/trace/img_align/data", + test_id, true); + permon_.addLog("iter"); + permon_.addLog("level"); + permon_.addLog("mu"); + permon_.addLog("chi2"); + permon_.addLog("trials"); + } +#endif + +} + +void ForwardCompositionalSE3:: +runOptimization(Sophus::SE3& model, int levelBegin, int levelEnd) +{ + if(levelBegin < 0 || levelBegin > n_levels_-1) + levelBegin = n_levels_-1; + if(levelEnd < 0) + levelEnd = 1; + + // Perform Pyramidal optimization + for(level_=levelBegin; level_>=levelEnd; --level_) + { + mu_ = 0.1; + cout << endl << "PYRAMID LEVEL " << level_ + << endl << "---------------" << endl; + optimize(model); + } +} + +double ForwardCompositionalSE3:: +computeResiduals (const Sophus::SE3& model, bool linearize_system, bool compute_weight_scale) +{ + // Warp the image such that it aligns with the template image + double chi2 = 0; + size_t n_pixels = 0; + + if(linearize_system) + resimg_ = cv::Mat(tpl_pyr_[level_].size(), CV_32F, cv::Scalar(1)); + + for( int v=0; v(v,u); + Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]); + Vector3d xyz_img(model*xyz_tpl); + Vector2f uv_img_pyr = cam_pyr_[level_].world2cam(xyz_img).cast(); // apply cam model + if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast(), 2) ) + { + // compare image values + float intensity_tpl = + tpl_pyr_[level_].at(v,u); + float intensity_img = + interpolateMat_32f(img_pyr_[level_], uv_img_pyr[0], uv_img_pyr[1]); + + // compute residual (opposite to 2d case because of other jacobian) + float res = intensity_tpl-intensity_img; + + // robustification + if(res > res_thresh_) res = res_thresh_; + if(res < -res_thresh_) res = -res_thresh_; + chi2 += res*res; + n_pixels++; + + if(linearize_system) + { + // get gradient of warped image (~gradient at warped position) + float dx = 0.5*interpolateMat_32f(img_pyr_dx_[level_], uv_img_pyr[0], uv_img_pyr[1]); + float dy = 0.5*interpolateMat_32f(img_pyr_dy_[level_], uv_img_pyr[0], uv_img_pyr[1]); + + // evaluate jacobian + Eigen::Matrix frame_jac; + frameJac_xyz2uv(xyz_img, cam_pyr_[level_].fx(), frame_jac); + + // compute steppest descent images + Vector6d J = dx*frame_jac.row(0) + dy*frame_jac.row(1); + + // compute Hessian and + H_ += J*J.transpose(); + Jres_ += J*res; + + resimg_.at(v,u) = -res; + } + } + } + } + chi2 /= n_pixels; + return chi2; +} + +int ForwardCompositionalSE3:: +solve() +{ + x_ = H_.ldlt().solve(-Jres_); + if((bool) std::isnan((double) x_[0])) + return 0; + return 1; +} + +void ForwardCompositionalSE3:: +update(const ModelType& old_model, ModelType& new_model) +{ + new_model = Sophus::SE3::exp(x_)*(old_model); +} + +void ForwardCompositionalSE3:: +startIteration() +{ +#if 0 + if(log_) + permon_.newMeasurement(); +#endif +} + +void ForwardCompositionalSE3:: +finishIteration() +{ +#if 0 + if(log_) + { + permon_.log("iter", iter_); + permon_.log("level", level_); + permon_.log("mu", mu_); + permon_.log("chi2", chi2_); + permon_.log("trials", n_trials_); + } +#endif + + if(display_) + { + cv::namedWindow("residuals", cv::WINDOW_AUTOSIZE); + cv::imshow("residuals", resimg_*3); + cv::waitKey(0); + } +} + +/******************************************************************************* + * Efficient Second Order Minimization (ESM) + */ +SecondOrderMinimisationSE3:: +SecondOrderMinimisationSE3( vector& cam_pyr, + vector& depth_pyr, + vector& img_pyr, + vector& tpl_pyr, + vector& img_pyr_dx, + vector& img_pyr_dy, + vector& tpl_pyr_dx, + vector& tpl_pyr_dy, + Sophus::SE3& init_model, + int n_levels, + int n_iter, + float res_thresh, + bool display, + Method method, + int test_id) : + cam_pyr_(cam_pyr), + depth_pyr_(depth_pyr), + img_pyr_(img_pyr), + tpl_pyr_(tpl_pyr), + img_pyr_dx_(img_pyr_dx), + img_pyr_dy_(img_pyr_dy), + tpl_pyr_dx_(tpl_pyr_dx), + tpl_pyr_dy_(tpl_pyr_dy), + display_(display), + log_(test_id < 0), + res_thresh_(res_thresh) +{ + n_iter_ = n_iter; + method_ = method; + verbose_ = false; + +#if 0 + if(log_) + { + // Init Performance Monitor + permon_.init("esm", ament_index_cpp::get_package_share_directory("rpl_examples") + "/trace/img_align/data", + test_id, true); + permon_.addLog("iter"); + permon_.addLog("level"); + permon_.addLog("mu"); + permon_.addLog("chi2"); + permon_.addLog("trials"); + } +#endif + + // perform pyramidal optimization + for(level_=n_levels-1; level_>2; --level_) + //level_ = n_levels-1; + { + // Optimize + mu_ = 0.01f; + if(display_) + { + cout << endl << "PYRAMID LEVEL " << level_ + << endl << "patch-width = " << img_pyr_[level_].cols + << endl << "---------------" << endl; + } + optimize(init_model); + } +} + +double SecondOrderMinimisationSE3:: +computeResiduals (const Sophus::SE3& model, bool linearize_system, bool compute_weight_scale) +{ + // Warp the image such that it aligns with the template image + double chi2 = 0; + size_t n_pixels = 0; + + // TODO: to improve access speed, use a pointer and increment every iteration + + // Compute Warp + cv::Mat mask = cv::Mat_(tpl_pyr_[level_].rows, tpl_pyr_[level_].cols, false); + cv::Mat img_warped = cv::Mat_(tpl_pyr_[level_].rows, tpl_pyr_[level_].cols, 1.0); + + for( int v=0; v(v,u); + Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]); + Vector3d xyz_img(model*xyz_tpl); + Vector2f uv_img_pyr = cam_pyr_[level_].world2cam(xyz_img).cast(); // apply cam model + if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast(), 1) ) + { + img_warped.at(v,u) = interpolateMat_32f(img_pyr_[level_], uv_img_pyr[0], uv_img_pyr[1]); + + if( cam_pyr_[level_].isInFrame(uv_img_pyr.cast(), 2) ) + mask.at(v,u) = true; + } + } + } + + // Compute Warp derivative + cv::Mat img_warped_dx, img_warped_dy; + cv::Sobel(img_warped, img_warped_dx, CV_32F, 1, 0, 1); + cv::Sobel(img_warped, img_warped_dy, CV_32F, 0, 1, 1); + + // Compute Jacobian + if(linearize_system) + resimg_ = cv::Mat_(tpl_pyr_[level_].size(), 1.0); + + for( int v=0; v(v,u)) + { + // compare image values + float intensity_tpl = tpl_pyr_[level_].at(v,u); + float intensity_img = img_warped.at(v,u); + + // compute residual (opposite to 2d case because of other jacobian) + float res = intensity_tpl-intensity_img; + + // robustification + if(res > res_thresh_) res = res_thresh_; + if(res < -res_thresh_) res = -res_thresh_; + chi2 += res*res; + n_pixels++; + + if(linearize_system) + { + // 0.25 because we have two 0.5 factors. First from adding the two gradients + // and the second when we compute the sobel mask + float dx = 0.25*(tpl_pyr_dx_[level_].at(v,u) + img_warped_dx.at(v,u)); + float dy = 0.25*(tpl_pyr_dy_[level_].at(v,u) + img_warped_dy.at(v,u)); + + // evaluate jacobian + cv::Vec3f cv_float3 = depth_pyr_[level_].at(v,u); + Sophus::Vector3d xyz_tpl(cv_float3[0], cv_float3[1], cv_float3[2]); + Sophus::Vector3d xyz_img(model*xyz_tpl); + Eigen::Matrix frame_jac; + frameJac_xyz2uv(xyz_tpl, cam_pyr_[level_].fx(), frame_jac); + + // compute steppest descent images + Vector6d J = dx*frame_jac.row(0) + dy*frame_jac.row(1); + + // compute Hessian + H_ += J*J.transpose(); + Jres_ += J*res; + resimg_.at(v,u) = res; + } + } + } + } + chi2 /= n_pixels; + return chi2; +} + +int SecondOrderMinimisationSE3:: +solve() +{ + x_ = H_.ldlt().solve(-Jres_); + if((bool) std::isnan((double) x_[0])) + return 0; + return 1; +} + +void SecondOrderMinimisationSE3:: +update(const ModelType& old_model, ModelType& new_model) +{ + new_model = Sophus::SE3::exp(x_)*old_model; +} + +void SecondOrderMinimisationSE3:: +startIteration() +{ +#if 0 + if(log_) + permon_.newMeasurement(); +#endif +} + +void SecondOrderMinimisationSE3:: +finishIteration() +{ +#if 0 + if(log_) + { + permon_.log("iter", iter_); + permon_.log("level", level_); + permon_.log("mu", mu_); + permon_.log("chi2", chi2_); + permon_.log("trials", n_trials_); + } +#endif + + if(display_) + { + cv::namedWindow("residuals", cv::WINDOW_AUTOSIZE); + cv::imshow("residuals", resimg_*3); + cv::waitKey(0); + } +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/math_utils.cpp b/src/rpg_vikit/vikit_common/src/math_utils.cpp new file mode 100644 index 0000000..36dc658 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/math_utils.cpp @@ -0,0 +1,202 @@ +/* + * math_utils.cpp + * + * Created on: Jul 20, 2012 + * Author: cforster + */ + +#include + +namespace vk { + +using namespace Eigen; + +Vector3d +triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, + const Vector3d& feature1, const Vector3d& feature2 ) +{ + Vector3d f2 = R * feature2; + Vector2d b; + b[0] = t.dot(feature1); + b[1] = t.dot(f2); + Matrix2d A; + A(0,0) = feature1.dot(feature1); + A(1,0) = feature1.dot(f2); + A(0,1) = -A(1,0); + A(1,1) = -f2.dot(f2); + Vector2d lambda = A.inverse() * b; + Vector3d xm = lambda[0] * feature1; + Vector3d xn = t + lambda[1] * f2; + return ( xm + xn )/2; +} + +bool +depthFromTriangulationExact( + const Matrix3d& R_r_c, + const Vector3d& t_r_c, + const Vector3d& f_r, + const Vector3d& f_c, + double& depth_in_r, + double& depth_in_c) +{ + // bearing vectors (f_r, f_c) do not need to be unit length + const Vector3d f_c_in_r(R_r_c*f_c); + const double a = f_c_in_r.dot(f_r) / t_r_c.dot(f_r); + const double b = f_c_in_r.dot(t_r_c); + const double denom = (a*b - f_c_in_r.dot(f_c_in_r)); + + if(abs(denom) < 0.000001) + return false; + + depth_in_c = (b-a*t_r_c.dot(t_r_c)) / denom; + depth_in_r = (t_r_c + f_c_in_r*depth_in_c).norm(); + return true; +} + +double +reprojError(const Vector3d& f1, + const Vector3d& f2, + double error_multiplier2) +{ + Vector2d e = project2d(f1) - project2d(f2); + return error_multiplier2 * e.norm(); +} + +double +computeInliers(const vector& features1, // c1 + const vector& features2, // c2 + const Matrix3d& R, // R_c1_c2 + const Vector3d& t, // c1_t + const double reproj_thresh, + double error_multiplier2, + vector& xyz_vec, // in frame c1 + vector& inliers, + vector& outliers) +{ + inliers.clear(); inliers.reserve(features1.size()); + outliers.clear(); outliers.reserve(features1.size()); + xyz_vec.clear(); xyz_vec.reserve(features1.size()); + double tot_error = 0; + //triangulate all features and compute reprojection errors and inliers + for(size_t j=0; j reproj_thresh || e2 > reproj_thresh) + outliers.push_back(j); + else + { + inliers.push_back(j); + tot_error += e1+e2; + } + } + return tot_error; +} + +void +computeInliersOneView(const vector & feature_sphere_vec, + const vector & xyz_vec, + const Matrix3d &R, + const Vector3d &t, + const double reproj_thresh, + const double error_multiplier2, + vector& inliers, + vector& outliers) +{ + inliers.clear(); inliers.reserve(xyz_vec.size()); + outliers.clear(); outliers.reserve(xyz_vec.size()); + for(size_t j = 0; j < xyz_vec.size(); j++ ) + { + double e = reprojError(feature_sphere_vec[j], + R.transpose() * ( xyz_vec[j] - t ), + error_multiplier2); + if(e < reproj_thresh) + inliers.push_back(j); + else + outliers.push_back(j); + } +} + +Vector3d +dcm2rpy(const Matrix3d &R) +{ + Vector3d rpy; + rpy[1] = atan2( -R(2,0), sqrt( pow( R(0,0), 2 ) + pow( R(1,0), 2 ) ) ); + if( fabs( rpy[1] - M_PI/2 ) < 0.00001 ) + { + rpy[2] = 0; + rpy[0] = -atan2( R(0,1), R(1,1) ); + } + else + { + if( fabs( rpy[1] + M_PI/2 ) < 0.00001 ) + { + rpy[2] = 0; + rpy[0] = -atan2( R(0,1), R(1,1) ); + } + else + { + rpy[2] = atan2( R(1,0)/cos(rpy[1]), R(0,0)/cos(rpy[1]) ); + rpy[0] = atan2( R(2,1)/cos(rpy[1]), R(2,2)/cos(rpy[1]) ); + } + } + return rpy; +} + +Matrix3d +rpy2dcm(const Vector3d &rpy) +{ + Matrix3d R1; + R1(0,0) = 1.0; R1(0,1) = 0.0; R1(0,2) = 0.0; + R1(1,0) = 0.0; R1(1,1) = cos(rpy[0]); R1(1,2) = -sin(rpy[0]); + R1(2,0) = 0.0; R1(2,1) = -R1(1,2); R1(2,2) = R1(1,1); + + Matrix3d R2; + R2(0,0) = cos(rpy[1]); R2(0,1) = 0.0; R2(0,2) = sin(rpy[1]); + R2(1,0) = 0.0; R2(1,1) = 1.0; R2(1,2) = 0.0; + R2(2,0) = -R2(0,2); R2(2,1) = 0.0; R2(2,2) = R2(0,0); + + Matrix3d R3; + R3(0,0) = cos(rpy[2]); R3(0,1) = -sin(rpy[2]); R3(0,2) = 0.0; + R3(1,0) = -R3(0,1); R3(1,1) = R3(0,0); R3(1,2) = 0.0; + R3(2,0) = 0.0; R3(2,1) = 0.0; R3(2,2) = 1.0; + + return R3 * R2 * R1; +} + +Quaterniond +angax2quat(const Vector3d& n, const double& angle) +{ + // n must be normalized! + double s(sin(angle/2)); + return Quaterniond( cos(angle/2), n[0]*s, n[1]*s, n[2]*s ); +} + + +Matrix3d +angax2dcm(const Vector3d& n, const double& angle) +{ + // n must be normalized + Matrix3d sqewn(sqew(n)); + return Matrix3d(Matrix3d::Identity() + sqewn*sin(angle) + sqewn*sqewn*(1-cos(angle))); +} + +double +sampsonusError(const Vector2d &v2Dash, const Matrix3d& Essential, const Vector2d& v2) +{ + Vector3d v3Dash = unproject2d(v2Dash); + Vector3d v3 = unproject2d(v2); + + double dError = v3Dash.transpose() * Essential * v3; + + Vector3d fv3 = Essential * v3; + Vector3d fTv3Dash = Essential.transpose() * v3Dash; + + Vector2d fv3Slice = fv3.head<2>(); + Vector2d fTv3DashSlice = fTv3Dash.head<2>(); + + return (dError * dError / (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice))); +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/omni_camera.cpp b/src/rpg_vikit/vikit_common/src/omni_camera.cpp new file mode 100644 index 0000000..b4fdf39 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/omni_camera.cpp @@ -0,0 +1,199 @@ +/* + * OcamProjector.cpp + * + * Created on: Sep 22, 2010 + * Author: laurent kneip + */ + +#include +#include +#include + +namespace vk { + +OmniCamera:: +OmniCamera( string calibFile ) +{ + double *pol = ocamModel.pol; + double *invpol = ocamModel.invpol; + double *xc = &ocamModel.xc; + double *yc = &ocamModel.yc; + double *c = &ocamModel.c; + double *d = &ocamModel.d; + double *e = &ocamModel.e; + int *width = &ocamModel.width; + int *height = &ocamModel.height; + int *length_pol = &ocamModel.length_pol; + int *length_invpol = &ocamModel.length_invpol; + FILE *f; + char buf[CMV_MAX_BUF]; + int i; + + printf("Initialize OmniCamera: Read Calibration %s\n", calibFile.c_str()); + + //Open file + if( !( f = fopen( (char*) calibFile.c_str(), "r" ) ) ) + { + printf("Initialize OmniCamera: Cannot read calibration file."); + return; + } + + //Read polynomial coefficients + char* dummy = fgets( buf, CMV_MAX_BUF, f ); + int result = fscanf( f, "\n" ); + result = fscanf( f, "%d", length_pol ); + for( i = 0; i < *length_pol; i++ ) + result = fscanf( f, " %lf", &pol[i] ); + + //Read inverse polynomial coefficients + result = fscanf( f, "\n" ); + dummy = fgets( buf, CMV_MAX_BUF, f ); + result = fscanf( f, "\n" ); + result = fscanf( f, "%d", length_invpol ); + for( i = 0; i < *length_invpol; i++ ) + result = fscanf( f, " %lf", &invpol[i] ); + + //Read center coordinates + result = fscanf( f, "\n" ); + dummy = fgets( buf, CMV_MAX_BUF, f ); + result = fscanf( f, "\n" ); + result = fscanf( f, "%lf %lf\n", xc, yc ); + + //Read affine coefficients + dummy = fgets( buf, CMV_MAX_BUF, f ); + result = fscanf( f, "\n" ); + result = fscanf( f, "%lf %lf %lf\n", c, d, e ); + + //Read image size + dummy = fgets( buf, CMV_MAX_BUF, f ); + result = fscanf( f, "\n" ); + result = fscanf( f, "%d %d", height, width ); + + fclose(f); + + width_ = *width; + height_ = *height; + error_multiplier_ = computeErrorMultiplier(); +} + +OmniCamera:: +~OmniCamera() +{} + +Vector3d OmniCamera:: +cam2world(const double& u, const double& v) const +{ + Vector3d xyz; + + // Important: we exchange x and y since regular pinhole model is working with x along the columns and y along the rows + // Davide's framework is doing exactly the opposite + + double invdet = 1 / ( ocamModel.c - ocamModel.d * ocamModel.e ); + + xyz[0] = invdet * ( ( v - ocamModel.xc ) - ocamModel.d * ( u - ocamModel.yc ) ); + xyz[1] = invdet * ( -ocamModel.e * ( v - ocamModel.xc ) + ocamModel.c * ( u - ocamModel.yc ) ); + + double r = sqrt( pow( xyz[0], 2 ) + pow( xyz[1], 2 ) ); //distance [pixels] of the point from the image center + xyz[2] = ocamModel.pol[0]; + double r_i = 1; + + for( int i = 1; i < ocamModel.length_pol; i++ ) + { + r_i *= r; + xyz[2] += r_i * ocamModel.pol[i]; + } + + xyz.normalize(); + + // change back to pinhole model: + double temp = xyz[0]; + xyz[0] = xyz[1]; + xyz[1] = temp; + xyz[2] = -xyz[2]; + + return xyz; +} + +Vector3d OmniCamera:: +cam2world (const Vector2d& px) const +{ + return cam2world(px[0], px[1]); +} + +Vector2d OmniCamera:: +world2cam(const Vector3d& xyz_c) const +{ + Vector2d uv; + + // transform world-coordinates to Davide's camera frame + Vector3d worldCoordinates_bis; + worldCoordinates_bis[0] = xyz_c[1]; + worldCoordinates_bis[1] = xyz_c[0]; + worldCoordinates_bis[2] = -xyz_c[2]; + + double norm = sqrt( pow( worldCoordinates_bis[0], 2 ) + pow( worldCoordinates_bis[1], 2 ) ); + double theta = atan( worldCoordinates_bis[2]/norm ); + + // Important: we exchange x and y since Pirmin's stuff is working with x along the columns and y along the rows, + // Davide's framework is doing exactly the opposite + double rho; + double t_i; + double x; + double y; + + if(norm != 0) + { + rho = ocamModel.invpol[0]; + + t_i = 1; + + for( int i = 1; i < ocamModel.length_invpol; i++ ) + { + t_i *= theta; + rho += t_i * ocamModel.invpol[i]; + } + + x = worldCoordinates_bis[0] * rho/norm; + y = worldCoordinates_bis[1] * rho/norm; + + // we exchange 0 and 1 in order to have pinhole model again + uv[1] = x * ocamModel.c + y * ocamModel.d + ocamModel.xc; + uv[0] = x * ocamModel.e + y + ocamModel.yc; + } + else + { + // we exchange 0 and 1 in order to have pinhole model again + uv[1] = ocamModel.xc; + uv[0] = ocamModel.yc; + } + + return uv; +} + +Vector2d OmniCamera:: +world2cam(const Vector2d& uv) const +{ + return world2cam(unproject2d(uv).normalized()); +} + +double OmniCamera:: +computeErrorMultiplier() +{ + Vector3d vector1 = cam2world( .5*width_, .5*height_ ); + Vector3d vector2 = cam2world( .5*width_ + .5, .5*height_ ); + vector1 = vector1/vector1.norm(); + vector2 = vector2/vector2.norm(); + + double factor1 = .5/( 1 - vector1.dot(vector2) ); + + vector1 = cam2world( width_, .5*height_ ); + vector2 = cam2world( -.5 + (double) width_ , .5*height_ ); + vector1 = vector1/vector1.norm(); + vector2 = vector2/vector2.norm(); + + double factor2 = .5/( 1 - vector1.dot(vector2) ); + + return ( factor2 + factor1 ) * .5; +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/performance_monitor.cpp b/src/rpg_vikit/vikit_common/src/performance_monitor.cpp new file mode 100644 index 0000000..5e92ade --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/performance_monitor.cpp @@ -0,0 +1,161 @@ +/* + * performance_monitor.cpp + * + * Created on: Aug 26, 2011 + * Author: Christian Forster + */ + +#include +#include +#include + +namespace vk +{ +using namespace std; + +PerformanceMonitor::PerformanceMonitor() +{} + +PerformanceMonitor::~PerformanceMonitor() +{ + ofs_.flush(); + ofs_.close(); +} + +void PerformanceMonitor::init( + const string& trace_name, + const string& trace_dir) +{ + trace_name_ = trace_name; + trace_dir_ = trace_dir; + string filename(trace_dir + "/" + trace_name + ".csv"); + ofs_.open(filename.c_str()); + if(!ofs_.is_open()) + { + printf("Tracefile = %s\n", filename.c_str()); + throw runtime_error("Could not open tracefile."); + } + traceHeader(); +} + +void PerformanceMonitor::addTimer(const string& name) +{ + timers_.insert(make_pair(name, Timer())); +} + +void PerformanceMonitor::addLog(const string& name) +{ + logs_.insert(make_pair(name, LogItem())); +} + +void PerformanceMonitor::writeToFile() +{ + trace(); + + for(auto it = timers_.begin(); it!=timers_.end(); ++it) + it->second.reset(); + for(auto it=logs_.begin(); it!=logs_.end(); ++it) + { + it->second.set = false; + it->second.data = -1; + } +} + +void PerformanceMonitor::startTimer(const string& name) +{ + auto t = timers_.find(name); + if(t == timers_.end()) { + printf("Timer = %s\n", name.c_str()); + throw std::runtime_error("startTimer: Timer not registered"); + } + t->second.start(); +} + +void PerformanceMonitor::stopTimer(const string& name) +{ + auto t = timers_.find(name); + if(t == timers_.end()) { + printf("Timer = %s\n", name.c_str()); + throw std::runtime_error("stopTimer: Timer not registered"); + } + t->second.stop(); +} + +double PerformanceMonitor::getTime(const string& name) const +{ + auto t = timers_.find(name); + if(t == timers_.end()) { + printf("Timer = %s\n", name.c_str()); + throw std::runtime_error("Timer not registered"); + } + return t->second.getTime(); +} + +void PerformanceMonitor::log(const string& name, double data) +{ + auto l = logs_.find(name); + if(l == logs_.end()) { + printf("Logger = %s\n", name.c_str()); + throw std::runtime_error("Logger not registered"); + } + l->second.data = data; + l->second.set = true; +} + +void PerformanceMonitor::trace() +{ + char buffer[128]; + bool first_value = true; + if(!ofs_.is_open()) + throw std::runtime_error("Performance monitor not correctly initialized"); + ofs_.precision(15); + ofs_.setf(std::ios::fixed, std::ios::floatfield ); + for(auto it = timers_.begin(); it!=timers_.end(); ++it) + { + if(first_value) { + ofs_ << it->second.getTime(); + first_value = false; + } + else + ofs_ << "," << it->second.getTime(); + } + for(auto it=logs_.begin(); it!=logs_.end(); ++it) + { + if(first_value) { + ofs_ << it->second.data; + first_value = false; + } + else + ofs_ << "," << it->second.data; + } + ofs_ << "\n"; +} + +void PerformanceMonitor::traceHeader() +{ + if(!ofs_.is_open()) + throw std::runtime_error("Performance monitor not correctly initialized"); + bool first_value = true; + for(auto it = timers_.begin(); it!=timers_.end(); ++it) + { + if(first_value) { + ofs_ << it->first; + first_value = false; + } + else + ofs_ << "," << it->first; + } + for(auto it=logs_.begin(); it!=logs_.end(); ++it) + { + if(first_value) { + ofs_ << it->first; + first_value = false; + } + else + ofs_ << "," << it->first; + } + ofs_ << "\n"; +} + +} // namespace vk + diff --git a/src/rpg_vikit/vikit_common/src/pinhole_camera.cpp b/src/rpg_vikit/vikit_common/src/pinhole_camera.cpp new file mode 100644 index 0000000..4308abf --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/pinhole_camera.cpp @@ -0,0 +1,118 @@ +/* + * pinhole_camera.cpp + * + * Created on: Jul 24, 2012 + * Author: cforster + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +PinholeCamera:: +PinholeCamera(double width, double height, double scale, + double fx, double fy, + double cx, double cy, + double d0, double d1, double d2, double d3, double d4) : + AbstractCamera(width * scale , height * scale, scale), + fx_(fx * scale), fy_(fy * scale), cx_(cx * scale), cy_(cy * scale), + distortion_(fabs(d0) > 0.0000001), + undist_map1_(height_, width_, CV_16SC2), + undist_map2_(height_, width_, CV_16SC2), + use_optimization_(false) +{ + cout << "scale: " << scale << endl; + d_[0] = d0; d_[1] = d1; d_[2] = d2; d_[3] = d3; d_[4] = d4; + cvK_ = (cv::Mat_(3, 3) << fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0); + cvD_ = (cv::Mat_(1, 5) << d_[0], d_[1], d_[2], d_[3], d_[4]); + cv::initUndistortRectifyMap(cvK_, cvD_, cv::Mat_::eye(3,3), cvK_, + cv::Size(width_, height_), CV_16SC2, undist_map1_, undist_map2_); + K_ << fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0; + K_inv_ = K_.inverse(); +} + +PinholeCamera:: +~PinholeCamera() +{} + +Vector3d PinholeCamera:: +cam2world(const double& u, const double& v) const +{ + Vector3d xyz; + if(!distortion_) + { + xyz[0] = (u - cx_)/fx_; + xyz[1] = (v - cy_)/fy_; + xyz[2] = 1.0; + } + else + { + cv::Point2f uv(u,v), px; + const cv::Mat src_pt(1, 1, CV_32FC2, &uv.x); + cv::Mat dst_pt(1, 1, CV_32FC2, &px.x); + cv::undistortPoints(src_pt, dst_pt, cvK_, cvD_); + xyz[0] = px.x; + xyz[1] = px.y; + xyz[2] = 1.0; + } + return xyz.normalized(); +} + +Vector3d PinholeCamera:: +cam2world (const Vector2d& uv) const +{ + return cam2world(uv[0], uv[1]); +} + +Vector2d PinholeCamera:: +world2cam(const Vector3d& xyz) const +{ + return world2cam(project2d(xyz)); +} + +Vector2d PinholeCamera:: +world2cam(const Vector2d& uv) const +{ + Vector2d px; + if(!distortion_) + { + px[0] = fx_*uv[0] + cx_; + px[1] = fy_*uv[1] + cy_; + } + else + { + double x, y, r2, r4, r6, a1, a2, a3, cdist, xd, yd; + x = uv[0]; + y = uv[1]; + r2 = x*x + y*y; + r4 = r2*r2; + r6 = r4*r2; + a1 = 2*x*y; + a2 = r2 + 2*x*x; + a3 = r2 + 2*y*y; + cdist = 1 + d_[0]*r2 + d_[1]*r4 + d_[4]*r6; + xd = x*cdist + d_[2]*a1 + d_[3]*a2; + yd = y*cdist + d_[2]*a3 + d_[3]*a1; + px[0] = xd*fx_ + cx_; + px[1] = yd*fy_ + cy_; + } + return px; +} + +void PinholeCamera:: +undistortImage(const cv::Mat& raw, cv::Mat& rectified) +{ + if(distortion_) + cv::remap(raw, rectified, undist_map1_, undist_map2_, cv::INTER_LINEAR); + else + rectified = raw.clone(); +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/polynomial_camera.cpp b/src/rpg_vikit/vikit_common/src/polynomial_camera.cpp new file mode 100644 index 0000000..91dfbf3 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/polynomial_camera.cpp @@ -0,0 +1,152 @@ +/* + * polynomial_camera.cpp + * + * Created on: January 26, 2023 + * Author: xuankuzcr + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +// PolynomialCamera:: +// PolynomialCamera(double width, double height, double scale, +// double fx, double fy, +// double cx, double cy, double skew, +// double k2, double k3, double k4, double k5, double k6, double k7) : +// AbstractCamera(width * scale , height * scale, scale), +// fx_(fx * scale), fy_(fy * scale), cx_(cx * scale), cy_(cy * scale), skew_(skew * scale), +// distortion_(fabs(k2) > 0.0000001) +// { +// cout << "scale: " << scale << endl; +// k2_ = k2; k3_ = k3; k4_ = k4; k5_ = k5; k6_ = k6; k7_ = k7; +// } + +PolynomialCamera:: +PolynomialCamera(double width, double height, // double scale, + double fx, double fy, + double cx, double cy, double skew, + double k2, double k3, double k4, double k5, double k6, double k7) : + AbstractCamera(width, height, 1.0), + fx_(fx), fy_(fy), cx_(cx), cy_(cy), skew_(skew), + distortion_(fabs(k2) > 0.0000001) +{ + // cout << "scale: " << scale << endl; + k2_ = k2; k3_ = k3; k4_ = k4; k5_ = k5; k6_ = k6; k7_ = k7; +} + +PolynomialCamera:: +~PolynomialCamera() +{} + +Vector3d PolynomialCamera:: +cam2world(const double& u, const double& v) const +{ + Vector3d xyz; + if(!distortion_) + { + // xyz[0] = (u - cx_)/fx_; + // xyz[1] = (v - cy_)/fy_; + // xyz[2] = 1.0; + xyz[1] = (v - cy_)/fy_; + xyz[0] = (u - cx_ - xyz[1]*skew_)/fx_; + xyz[2] = 1.0; + } + else + { + double y = (v - cy_)/fy_; + double x = (u - cx_ - y*skew_)/fx_; + + const double thetad = std::sqrt(x * x + y * y); + double theta = thetad; + for (int i = 0; i < 7; ++i) + { + const double theta2 = theta * theta; + const double theta3 = theta2 * theta; + const double theta4 = theta3 * theta; + const double theta5 = theta4 * theta; + const double theta6 = theta5 * theta; + theta = thetad / + (1.0 + k2_ * theta + k3_ * theta2 + k4_ * theta3 + k5_ * theta4 + k6_ * theta5 + k7_ * theta6); + } + const double scaling = std::tan(theta) / thetad; + x *= scaling; + y *= scaling; + xyz[0] = x; + xyz[1] = y; + xyz[2] = 1.0; + } + return xyz.normalized(); +} + +Vector3d PolynomialCamera:: +cam2world (const Vector2d& uv) const +{ + return cam2world(uv[0], uv[1]); +} + +Vector2d PolynomialCamera:: +world2cam(const Vector3d& xyz) const +{ + // return world2cam(project2d(xyz)); + Vector2d px; + if(!distortion_) + { + px[0] = fx_*xyz[0] + cx_; + px[1] = fy_*xyz[1] + cy_; + } + else + { + double xd, yd; + const double r = sqrt( xyz( 1 ) * xyz( 1 ) + xyz( 0 ) * xyz( 0 )); + // if (r < 1e-8) + // { + // return uv; + // } + const double theta = acos( xyz( 2 ) / xyz.norm( ) ); + const double thetad = thetad_from_theta(theta); + const double scaling = thetad / r; + xd = xyz[0] * scaling; + yd = xyz[1] * scaling; + px[0] = xd*fx_ + yd*skew_ + cx_; + px[1] = yd*fy_ + cy_; + } + return px; +} + +Vector2d PolynomialCamera:: +world2cam(const Vector2d& uv) const +{ + Vector2d px; + if(!distortion_) + { + px[0] = fx_*uv[0] + cx_; + px[1] = fy_*uv[1] + cy_; + } + else + { + double xd, yd; + const double r = uv.norm(); + if (r < 1e-8) + { + return uv; + } + const double theta = std::atan(r); + const double thetad = thetad_from_theta(theta); + const double scaling = thetad / r; + xd = uv[0] * scaling; + yd = uv[1] * scaling; + px[0] = xd*fx_ + yd*skew_ + cx_; + px[1] = yd*fy_ + cy_; + } + return px; +} + +} // end namespace vk diff --git a/src/rpg_vikit/vikit_common/src/robust_cost.cpp b/src/rpg_vikit/vikit_common/src/robust_cost.cpp new file mode 100644 index 0000000..e2bee84 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/robust_cost.cpp @@ -0,0 +1,162 @@ +/** +* This file is part of dvo. +* +* Copyright 2012 Christian Kerl (Technical University of Munich) +* For more information see . +* +* dvo 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 3 of the License, or +* (at your option) any later version. +* +* dvo 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 dvo. If not, see . +*/ + +#include +#include +#include +#include + +namespace vk { +namespace robust_cost { + +const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f; +const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f; + +TDistributionScaleEstimator:: +TDistributionScaleEstimator(const float dof) : + dof_(dof), + initial_sigma_(INITIAL_SIGMA) +{} + +float TDistributionScaleEstimator:: +compute(std::vector& errors) const +{ + float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_); + int num = 0; + float lambda = initial_lamda; + int iterations = 0; + do + { + ++iterations; + initial_lamda = lambda; + num = 0; + lambda = 0.0f; + + for(std::vector::iterator it=errors.begin(); it!=errors.end(); ++it) + { + if(std::isfinite(*it)) + { + ++num; + const float error2 = (*it)*(*it); + lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) ); + } + } + lambda = float(num) / lambda; + } while(std::abs(lambda - initial_lamda) > 1e-3); + + return std::sqrt(1.0f / lambda); +} + +const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745 + +float MADScaleEstimator:: +compute(std::vector& errors) const +{ + // error must be in absolute values! + return NORMALIZER * vk::getMedian(errors); +} + +float NormalDistributionScaleEstimator:: +compute(std::vector& errors) const +{ + const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size(); + float var = 0.0; + std::for_each(errors.begin(), errors.end(), [&](const float d) { + var += (d - mean) * (d - mean); + }); + return std::sqrt(var); // return standard deviation +} + +const float TukeyWeightFunction::DEFAULT_B = 4.6851f; + +TukeyWeightFunction::TukeyWeightFunction(const float b) +{ + configure(b); +} + +float TukeyWeightFunction::value(const float& x) const +{ + const float x_square = x * x; + if(x_square <= b_square) + { + const float tmp = 1.0f - x_square / b_square; + return tmp * tmp; + } + else + { + return 0.0f; + } +} + +void TukeyWeightFunction:: +configure(const float& param) +{ + b_square = param * param; +} + +const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f; + +TDistributionWeightFunction:: +TDistributionWeightFunction(const float dof) +{ + configure(dof); +} + +float TDistributionWeightFunction:: +value(const float & x) const +{ + return ((dof_ + 1.0f) / (dof_ + (x * x))); +} + +void TDistributionWeightFunction:: +configure(const float& param) +{ + dof_ = param; + normalizer_ = dof_ / (dof_ + 1.0f); +} + +const float HuberWeightFunction::DEFAULT_K = 1.345f; + +HuberWeightFunction:: +HuberWeightFunction(const float k) +{ + configure(k); +} + +void HuberWeightFunction:: +configure(const float& param) +{ + k = param; +} + +float HuberWeightFunction:: +value(const float& t) const +{ + const float t_abs = std::abs(t); + if(t_abs < k) + return 1.0f; + else + return k / t_abs; +} + +} // namespace robust_cost +} // namespace vk + + diff --git a/src/rpg_vikit/vikit_common/src/user_input_thread.cpp b/src/rpg_vikit/vikit_common/src/user_input_thread.cpp new file mode 100644 index 0000000..39a1924 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/user_input_thread.cpp @@ -0,0 +1,67 @@ +/* + * user_input_thread.cpp + * + * Created on: Jun 12, 2013 + * Author: pizzoli, cforster + */ + +#include +#include +#include + +namespace vk { + +UserInputThread::UserInputThread() : + stop_(false), + input_( (char) 0) +{ + tcgetattr(0, &original_terminal_settings_); // save old terminal i/o settings + new_terminal_settings_ = original_terminal_settings_; // make new settings same as old settings + new_terminal_settings_.c_lflag &= ~ICANON; // disable buffered i/o + new_terminal_settings_.c_lflag &= ~ECHO; // set echo mode + new_terminal_settings_.c_cc[VMIN] = 1; //minimum of number input read. + tcsetattr(0, TCSANOW, &new_terminal_settings_); // use these new terminal i/o settings now + + user_input_thread_ = new std::thread(&UserInputThread::acquireUserInput, this); +} + +UserInputThread::~UserInputThread() +{ + tcsetattr(0, TCSANOW, &original_terminal_settings_); + user_input_thread_->join(); + printf("UserInputThread destructed.\n"); +} + +char UserInputThread::getInput() +{ + char tmp = input_; + input_ = (char) 0; + return tmp; +} + +void UserInputThread::stop() +{ + stop_ = true; +} + +void UserInputThread::acquireUserInput() +{ + int c = 0; + while(!stop_) + { + c = getchar(); // TODO: this is blocking, so the interruption point is not reached... + if ((char)c == ' ') + printf("USER INPUT: SPACE\n"); + else + printf("USER INPUT: %c\n", (char) c); + input_ = (char) c; + c = 0; + + // interruption point: + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } +} + +} // end namespace vk + + diff --git a/src/rpg_vikit/vikit_common/src/vision.cpp b/src/rpg_vikit/vikit_common/src/vision.cpp new file mode 100644 index 0000000..b6d5f58 --- /dev/null +++ b/src/rpg_vikit/vikit_common/src/vision.cpp @@ -0,0 +1,295 @@ +/* + * vision.cpp + * + * Created on: May 14, 2013 + * Author: cforster + */ + +#include + +#if __SSE2__ +# include +#elif __ARM_NEON__ +# include +#endif + +namespace vk { + +#ifdef __SSE2__ +void halfSampleSSE2(const unsigned char* in, unsigned char* out, int w, int h) +{ + const unsigned long long mask[2] = {0x00FF00FF00FF00FFull, 0x00FF00FF00FF00FFull}; + const unsigned char* nextRow = in + w; + __m128i m = _mm_loadu_si128((const __m128i*)mask); + int sw = w >> 4; + int sh = h >> 1; + for (int i=0; i> 1)*out.cols; + for( int x = in.cols; x > 0 ; x-=16, in_top += 16, in_bottom += 16, out_data += 8) + { + uint8x8x2_t top = vld2_u8( (const uint8_t *)in_top ); + uint8x8x2_t bottom = vld2_u8( (const uint8_t *)in_bottom ); + uint16x8_t sum = vaddl_u8( top.val[0], top.val[1] ); + sum = vaddw_u8( sum, bottom.val[0] ); + sum = vaddw_u8( sum, bottom.val[1] ); + uint8x8_t final_sum = vshrn_n_u16(sum, 2); + vst1_u8(out_data, final_sum); + } + } +} +#endif + + +void +halfSample(const cv::Mat& in, cv::Mat& out) +{ + assert( in.rows/2==out.rows && in.cols/2==out.cols); + assert( in.type()==CV_8U && out.type()==CV_8U); + +#ifdef __SSE2__ + if(aligned_mem::is_aligned16(in.data) && aligned_mem::is_aligned16(out.data) && ((in.cols % 16) == 0)) + { + halfSampleSSE2(in.data, out.data, in.cols, in.rows); + return; + } +#endif +#ifdef __ARM_NEON__ + if( (in.cols % 16) == 0 ) + { + halfSampleNEON(in, out); + return; + } +#endif + + const int stride = in.step.p[0]; + uint8_t* top = (uint8_t*) in.data; + uint8_t* bottom = top + stride; + uint8_t* end = top + stride*in.rows; + const int out_width = out.cols; + uint8_t* p = (uint8_t*) out.data; + while (bottom < end) + { + for (int j=0; j( (uint16_t (top[0]) + top[1] + bottom[0] + bottom[1])/4 ); + p++; + top += 2; + bottom += 2; + } + top += stride; + bottom += stride; + } +} + + +float +shiTomasiScore(const cv::Mat& img, int u, int v) +{ + assert(img.type() == CV_8UC1); + + float dXX = 0.0; + float dYY = 0.0; + float dXY = 0.0; + const int halfbox_size = 4; + const int box_size = 2*halfbox_size; + const int box_area = box_size*box_size; + const int x_min = u-halfbox_size; + const int x_max = u+halfbox_size; + const int y_min = v-halfbox_size; + const int y_max = v+halfbox_size; + + if(x_min < 1 || x_max >= img.cols-1 || y_min < 1 || y_max >= img.rows-1) + return 0.0; // patch is too close to the boundary + + const int stride = img.step.p[0]; + for( int y=y_min; y::depth, cn*2)); + + int x, y, delta = (int)alignSize((cols + 2)*cn, 16); + AutoBuffer _tempBuf(delta*2 + 64); + deriv_type *trow0 = alignPtr(_tempBuf + cn, 16), *trow1 = alignPtr(trow0 + delta, 16); + +#ifdef __SSE2__ + __m128i z = _mm_setzero_si128(), c3 = _mm_set1_epi16(3), c10 = _mm_set1_epi16(10); +#endif + + for( y = 0; y < rows; y++ ) + { + const uchar* srow0 = src.ptr(y > 0 ? y-1 : rows > 1 ? 1 : 0); + const uchar* srow1 = src.ptr(y); + const uchar* srow2 = src.ptr(y < rows-1 ? y+1 : rows > 1 ? rows-2 : 0); + deriv_type* drow = dst.ptr(y); + + // do vertical convolution + x = 0; +#ifdef __SSE2__ + for( ; x <= colsn - 8; x += 8 ) + { + __m128i s0 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow0 + x)), z); + __m128i s1 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow1 + x)), z); + __m128i s2 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow2 + x)), z); + __m128i t0 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s0, s2), c3), _mm_mullo_epi16(s1, c10)); + __m128i t1 = _mm_sub_epi16(s2, s0); + _mm_store_si128((__m128i*)(trow0 + x), t0); + _mm_store_si128((__m128i*)(trow1 + x), t1); + } +#endif + for( ; x < colsn; x++ ) + { + int t0 = (srow0[x] + srow2[x])*3 + srow1[x]*10; + int t1 = srow2[x] - srow0[x]; + trow0[x] = (deriv_type)t0; + trow1[x] = (deriv_type)t1; + } + + // make border + int x0 = (cols > 1 ? 1 : 0)*cn, x1 = (cols > 1 ? cols-2 : 0)*cn; + for( int k = 0; k < cn; k++ ) + { + trow0[-cn + k] = trow0[x0 + k]; trow0[colsn + k] = trow0[x1 + k]; + trow1[-cn + k] = trow1[x0 + k]; trow1[colsn + k] = trow1[x1 + k]; + } + + // do horizontal convolution, interleave the results and store them to dst + x = 0; +#ifdef __SSE2__ + for( ; x <= colsn - 8; x += 8 ) + { + __m128i s0 = _mm_loadu_si128((const __m128i*)(trow0 + x - cn)); + __m128i s1 = _mm_loadu_si128((const __m128i*)(trow0 + x + cn)); + __m128i s2 = _mm_loadu_si128((const __m128i*)(trow1 + x - cn)); + __m128i s3 = _mm_load_si128((const __m128i*)(trow1 + x)); + __m128i s4 = _mm_loadu_si128((const __m128i*)(trow1 + x + cn)); + + __m128i t0 = _mm_sub_epi16(s1, s0); + __m128i t1 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s2, s4), c3), _mm_mullo_epi16(s3, c10)); + __m128i t2 = _mm_unpacklo_epi16(t0, t1); + t0 = _mm_unpackhi_epi16(t0, t1); + // this can probably be replaced with aligned stores if we aligned dst properly. + _mm_storeu_si128((__m128i*)(drow + x*2), t2); + _mm_storeu_si128((__m128i*)(drow + x*2 + 8), t0); + } +#endif + for( ; x < colsn; x++ ) + { + deriv_type t0 = (deriv_type)(trow0[x+cn] - trow0[x-cn]); + deriv_type t1 = (deriv_type)((trow1[x+cn] + trow1[x-cn])*3 + trow1[x]*10); + drow[x*2] = t0; drow[x*2+1] = t1; + } + } + +// vector vec_mat; +// cv::split(dst, vec_mat); +// cv::namedWindow("deriv"); +// cv::imshow("deriv", vec_mat[0]); +// cv::namedWindow("derivy"); +// cv::imshow("derivy", vec_mat[1]); +// cv::waitKey(0); +} + +#ifdef __SSE2__ +void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale) +{ + depth_32f.create(depth_16u.rows, depth_16u.cols, CV_32FC1); + + const unsigned short* input_ptr = depth_16u.ptr(); + float* output_ptr = depth_32f.ptr(); + + __m128 _scale = _mm_set1_ps(scale); + __m128 _zero = _mm_setzero_ps(); + __m128 _nan = _mm_set1_ps(std::numeric_limits::quiet_NaN()); + + for(int idx = 0; idx < depth_16u.size().area(); idx += 8, input_ptr += 8, output_ptr += 8) + { + __m128 _input, mask; + __m128i _inputi = _mm_load_si128((__m128i*) input_ptr); + + // load low shorts and convert to float + _input = _mm_cvtepi32_ps(_mm_unpacklo_epi16(_inputi, _mm_setzero_si128())); + + mask = _mm_cmpeq_ps(_input, _zero); + + // zero to nan + _input = _mm_or_ps(_input, _mm_and_ps(mask, _nan)); + // scale + _input = _mm_mul_ps(_input, _scale); + // save + _mm_store_ps(output_ptr + 0, _input); + + // load high shorts and convert to float + _input = _mm_cvtepi32_ps(_mm_unpackhi_epi16(_inputi, _mm_setzero_si128())); + + mask = _mm_cmpeq_ps(_input, _zero); + + // zero to nan + _input = _mm_or_ps(_input, _mm_and_ps(mask, _nan)); + // scale + _input = _mm_mul_ps(_input, _scale); + // save + _mm_store_ps(output_ptr + 4, _input); + } +} +#endif + +} + + diff --git a/src/rpg_vikit/vikit_common/test/data/scene_000.png b/src/rpg_vikit/vikit_common/test/data/scene_000.png new file mode 100644 index 0000000..7fdc8f9 Binary files /dev/null and b/src/rpg_vikit/vikit_common/test/data/scene_000.png differ diff --git a/src/rpg_vikit/vikit_common/test/test_camera.cpp b/src/rpg_vikit/vikit_common/test/test_camera.cpp new file mode 100644 index 0000000..ec48953 --- /dev/null +++ b/src/rpg_vikit/vikit_common/test/test_camera.cpp @@ -0,0 +1,82 @@ +/* + * camera_pinhole_test.cpp + * + * Created on: Oct 26, 2012 + * Author: cforster + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace Eigen; + + +void testTiming(vk::AbstractCamera* cam) +{ + Vector3d xyz; + Vector2d px(320.64, 253.54); + vk::Timer t; + t.start(); + for(size_t i=0; i<1000; ++i) + { + xyz = cam->cam2world(px); + } + t.stop(); + cout << "Time unproject = " << t.getTime()*1000 << "ms" << endl; + + t.start(); + for(size_t i=0; i<1000; ++i) + { + px = cam->world2cam(xyz); + } + t.stop(); + cout << "Time project = " << t.getTime()*1000 << "ms" << endl; +} + +void testAccuracy(vk::AbstractCamera* cam) +{ + double error = 0.0; + vk::Timer t; + for(size_t i=0; i<1000; ++i) + { + Vector2d px(1.0/100.0 * vk::Sample::uniform(0, cam->width()*100), + 1.0/100.0 * vk::Sample::uniform(0, cam->height()*100)); + Vector3d xyz = cam->cam2world(px); + Vector2d px2 = cam->world2cam(xyz); + error += (px-px2).norm(); + } + cout << "Reprojection error = " << error << " (took " << t.stop()*1000 << "ms)" << endl; + +} + +int main(int argc, char **argv) +{ + vk::AbstractCamera* cam_pinhole = + new vk::PinholeCamera(640, 480, + 323.725240539365, 323.53310403533, + 336.407165453746, 235.018271952295, + -0.258617082313663, 0.0623042373522829, 0.000445967802619555, -0.000269839440982019); + + vk::AbstractCamera* cam_atan = + new vk::ATANCamera(752, 480, 0.511496, 0.802603, 0.530199, 0.496011, 0.934092); + + printf("\nPINHOLE CAMERA:\n"); + testTiming(cam_pinhole); + testAccuracy(cam_pinhole); + + printf("\nATAN CAMERA:\n"); + testTiming(cam_atan); + testAccuracy(cam_atan); + + return 0; +} \ No newline at end of file diff --git a/src/rpg_vikit/vikit_common/test/test_patch_score.cpp b/src/rpg_vikit/vikit_common/test/test_patch_score.cpp new file mode 100644 index 0000000..a1cfe2e --- /dev/null +++ b/src/rpg_vikit/vikit_common/test/test_patch_score.cpp @@ -0,0 +1,107 @@ +/* + * test_patch_score.cpp + * + * Created on: Dec 4, 2012 + * Author: cforster + */ + +#include +#include +#include +#include +#include + +namespace { + +const int g_halfpatch_size = 4; +const int g_patch_size = g_halfpatch_size*2; + +void copyPatch(const cv::Mat& img, int x, int y, uint8_t* patch_data) +{ + cv::Mat patch(g_patch_size, g_patch_size, CV_8U, patch_data); + img(cv::Range(y-g_halfpatch_size, y+g_halfpatch_size), + cv::Range(x-g_halfpatch_size, x+g_halfpatch_size)).copyTo(patch); +} + +void copyPatch2(cv::Mat& img, int x, int y, uint8_t* patch_data) +{ + for(int v=0; v PatchScore; + + // create patch + uint8_t* ref_patch = vk::aligned_mem::aligned_alloc(g_patch_size*g_patch_size, 16); + uint8_t* cur_patch = vk::aligned_mem::aligned_alloc(g_patch_size*g_patch_size, 16); + + vk::Timer t; + for(int i=0; i<1000000; ++i) + { + copyPatch(img, x+10, y+10, ref_patch); + copyPatch(img, x, y, cur_patch); + } + printf("Copy patch cost %f\n", t.stop()); + + t.start(); + for(int i=0; i<1000000; ++i) + { + copyPatch2(img, x+10, y+10, ref_patch); + copyPatch2(img, x, y, cur_patch); + } + printf("Copy patch cost %f\n", t.stop()); + + // compute patch score + t.start(); + int b=10; + for(int i=0; i<1000000; ++i) + { + PatchScore patch_score(ref_patch); + b += patch_score.computeScore(cur_patch); + } + printf("Compute cost cost %f, %i\n", t.stop(), b); + + // compute patch score + t.start(); + int c=10; + uint8_t* data_ptr = img.data + (y-g_halfpatch_size)*img.cols + (x-g_halfpatch_size); + for(int i=0; i<1000000; ++i) + { + PatchScore patch_score(ref_patch); + c += patch_score.computeScore(data_ptr, img.cols); + } + printf("Compute cost, stride %f, %i\n", t.stop(), c); + + // check results + { + PatchScore patch_score(ref_patch); + printf("Score = %i\n", patch_score.computeScore(cur_patch)); + } + + // check results + { + PatchScore patch_score(ref_patch); + printf("Score = %i\n", patch_score.computeScore(data_ptr, img.cols)); + } + +} + +} // namespace + + +int main(int argc, char **argv) +{ + std::string img_name("test/data/scene_000.png"); + cv::Mat img(cv::imread(img_name, 0)); + assert(!img.empty()); + + testZMSSD(img); + + return 0; +} diff --git a/src/rpg_vikit/vikit_common/test/test_triangulation.cpp b/src/rpg_vikit/vikit_common/test/test_triangulation.cpp new file mode 100644 index 0000000..37b82c4 --- /dev/null +++ b/src/rpg_vikit/vikit_common/test/test_triangulation.cpp @@ -0,0 +1,39 @@ +/* + * camera_pinhole_test.cpp + * + * Created on: Oct 26, 2012 + * Author: cforster + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace Eigen; + +int main(int argc, char **argv) +{ + vk::PinholeCamera cam(752, 480, 1.0, 414.5, 414.2, 348.8, 240.0); + Matrix4d T_ref_cur(Matrix4d::Identity()); + T_ref_cur(0,3) = 0.5; + Vector2d u_cur(200, 300); + Vector3d f_cur(cam.cam2world(u_cur)); + double depth_cur = 2.0; + Vector3d f_ref(vk::project3d(T_ref_cur*vk::unproject3d(f_cur*depth_cur))); + double depth_ref = f_ref.norm(); + Vector2d u_ref(cam.world2cam(f_ref)); + double z_ref, z_cur; + vk::depthFromTriangulationExact(T_ref_cur.topLeftCorner<3,3>(), T_ref_cur.topRightCorner<3,1>(), f_ref, f_cur, z_ref, z_cur); + printf("depth = %f, triangulated depth = %f\n", depth_cur, z_cur); + printf("depth = %f, triangulated depth = %f\n", depth_ref, z_ref); + + return 0; +} diff --git a/src/rpg_vikit/vikit_common/vikit_commonConfig.cmake.in b/src/rpg_vikit/vikit_common/vikit_commonConfig.cmake.in new file mode 100644 index 0000000..2d7af57 --- /dev/null +++ b/src/rpg_vikit/vikit_common/vikit_commonConfig.cmake.in @@ -0,0 +1,17 @@ +####################################################### +# vikit_common source dir +set( vikit_common_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") + +####################################################### +# vikit_common build dir +set( vikit_common_DIR "@CMAKE_CURRENT_BINARY_DIR@") + +####################################################### +set( vikit_common_INCLUDE_DIR "@vikit_common_INCLUDE_DIR@" ) +set( vikit_common_INCLUDE_DIRS "@vikit_common_INCLUDE_DIR@" ) + +set( vikit_common_LIBRARIES "@vikit_common_LIBRARIES@" ) +set( vikit_common_LIBRARY "@vikit_common_LIBRARIES@" ) + +set( vikit_common_LIBRARY_DIR "@vikit_common_LIBRARY_DIR@" ) +set( vikit_common_LIBRARY_DIRS "@vikit_common_LIBRARY_DIR@" ) diff --git a/src/rpg_vikit/vikit_py/CMakeLists.txt b/src/rpg_vikit/vikit_py/CMakeLists.txt new file mode 100644 index 0000000..8a5912d --- /dev/null +++ b/src/rpg_vikit/vikit_py/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.5) +project(vikit_py) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) + +# set dependencies +set(dependencies +rclpy +) + +#install python modules +# ament_python_install_package(${PROJECT_NAME}) + +#install python executables +install(PROGRAMS + src/vikit_py/align_trajectory.py + src/vikit_py/cpu_info.py + src/vikit_py/depthmap_utils.py + src/vikit_py/math_utils.py + src/vikit_py/ros_node.py + src/vikit_py/transformations.py + DESTINATION lib/${PROJECT_NAME} +) + +#-------------------------------------------------------------------- +# export dependencies +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/src/rpg_vikit/vikit_py/package.xml b/src/rpg_vikit/vikit_py/package.xml new file mode 100644 index 0000000..050d928 --- /dev/null +++ b/src/rpg_vikit/vikit_py/package.xml @@ -0,0 +1,22 @@ + + + + vikit_py + 0.0.0 + The vikit_py package + Christian Forster + TODO: License declaration + + ament_cmake + + rclcpp + rclpy + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/rpg_vikit/vikit_py/setup.py b/src/rpg_vikit/vikit_py/setup.py new file mode 100644 index 0000000..5aec0e5 --- /dev/null +++ b/src/rpg_vikit/vikit_py/setup.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['vikit_py'], + package_dir={'': 'src'}, + install_requires=['rclpy', 'yaml'], + ) + +setup(**d) \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/.gitignore b/src/rpg_vikit/vikit_py/src/vikit_py/.gitignore new file mode 100644 index 0000000..7e99e36 --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/.gitignore @@ -0,0 +1 @@ +*.pyc \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/__init__.py b/src/rpg_vikit/vikit_py/src/vikit_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/align_trajectory.py b/src/rpg_vikit/vikit_py/src/vikit_py/align_trajectory.py new file mode 100644 index 0000000..f4201f8 --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/align_trajectory.py @@ -0,0 +1,129 @@ +#!/usr/bin/python + +import numpy as np +import vikit_py.transformations as transformations + +def align_sim3(model, data): + """Implementation of the paper: S. Umeyama, Least-Squares Estimation + of Transformation Parameters Between Two Point Patterns, + IEEE Trans. Pattern Anal. Mach. Intell., vol. 13, no. 4, 1991. + + Input: + model -- first trajectory (3xn) + data -- second trajectory (3xn) + + Output: + s -- scale factor (scalar) + R -- rotation matrix (3x3) + t -- translation vector (3x1) + t_error -- translational error per point (1xn) + + """ + + # substract mean + mu_M = model.mean(0).reshape(model.shape[0],1) + mu_D = data.mean(0).reshape(data.shape[0],1) + model_zerocentered = model - mu_M + data_zerocentered = data - mu_D + n = np.shape(model)[0] + + # correlation + C = 1.0/n*np.dot(model_zerocentered.transpose(), data_zerocentered) + sigma2 = 1.0/n*np.multiply(data_zerocentered,data_zerocentered).sum() + U_svd,D_svd,V_svd = np.linalg.linalg.svd(C) + D_svd = np.diag(D_svd) + V_svd = np.transpose(V_svd) + S = np.eye(3) + + if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0): + S[2,2] = -1 + + R = np.dot(U_svd, np.dot(S, np.transpose(V_svd))) + s = 1.0/sigma2*np.trace(np.dot(D_svd, S)) + t = mu_M-s*np.dot(R,mu_D) + + # TODO: + # model_aligned = s * R * model + t + # alignment_error = model_aligned - data + # t_error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),0)).A[0] + + return s, R, t #, t_error + +def align_se3(model,data, precision = False): + """Align two trajectories using the method of Horn (closed-form). + + Input: + model -- first trajectory (3xn) + data -- second trajectory (3xn) + + Output: + R -- rotation matrix (3x3) + t -- translation vector (3x1) + t_error -- translational error per point (1xn) + + """ + if not precision: + np.set_printoptions(precision=3,suppress=True) + model_zerocentered = model - model.mean(1).reshape(model.shape[0],1) + data_zerocentered = data - data.mean(1).reshape(data.shape[0],1) + + W = np.zeros( (3,3) ) + for column in range(model.shape[1]): + W += np.outer(model_zerocentered[:,column],data_zerocentered[:,column]) + U,d,Vh = np.linalg.linalg.svd(W.transpose()) + S = np.matrix(np.identity( 3 )) + if(np.linalg.det(U) * np.linalg.det(Vh)<0): + S[2,2] = -1 + R = U*S*Vh + t = data.mean(1).reshape(data.shape[0],1) - R * model.mean(1).reshape(model.shape[0],1) + + model_aligned = R * model + t + alignment_error = model_aligned - data + t_error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),0)).A[0] + + return R, t, t_error + +def _matrix_log(A): + theta = np.arccos((np.trace(A)-1.0)/2.0) + log_theta = 0.5*theta/np.sin(theta) * (A - A.transpose()) + x = np.array([log_theta[2,1], log_theta[0,2], log_theta[1,0]]) + return x + +def hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta=10, verbose=True): + """Implementation of the least squares solution described in the paper: + Robot Sensor Calibration: Solving AX=XB on the Euclidean Group + by Frank C. Park and Bryan J. Martin + """ + n = np.shape(I)[0] + M = np.zeros([3,3]) + C = np.zeros([3*n, 3]) + b_A = np.zeros([3*n,1]) + b_B = np.zeros([3*n,1]) + for ix, i in enumerate(I): + A1 = transformations.quaternion_matrix(q_es[i,:])[:3,:3] + A2 = transformations.quaternion_matrix(q_es[i+delta,:])[:3,:3] + A = np.dot(A1.transpose(), A2) + B1 = transformations.quaternion_matrix(q_gt[i,:])[:3,:3] + B2 = transformations.quaternion_matrix(q_gt[i+delta,:])[:3,:3] + B = np.dot(B1.transpose(), B2) + alpha = _matrix_log(A) + beta = _matrix_log(B) + M = M + np.dot(np.matrix(beta).transpose(), np.matrix(alpha)) + C[3*ix:3*ix+3,:] = np.eye(3) - A + b_A[3*ix:3*ix+3,0] = np.dot(np.transpose(A1), p_es[i+delta,:]-p_es[i,:]) + b_B[3*ix:3*ix+3,0] = np.dot(np.transpose(B1), p_gt[i+delta,:]-p_gt[i,:]) + + # compute rotation + D,V = np.linalg.linalg.eig(np.dot(M.transpose(), M)) + Lambda = np.diag([np.sqrt(1.0/D[0]), np.sqrt(1.0/D[1]), np.sqrt(1.0/D[2])]) + Vinv = np.linalg.linalg.inv(V) + X = np.dot(V, np.dot(Lambda, np.dot(Vinv, M.transpose()))) + + # compute translation + d = np.zeros([3*n,1]) + for i in range(n): + d[3*i:3*i+3,:] = b_A[3*i:3*i+3,:] - np.dot(X, b_B[3*i:3*i+3,:]) + + b = np.dot(np.linalg.inv(np.dot(np.transpose(C),C)), np.dot(np.transpose(C),d)) + + return np.array(X),b diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/cpu_info.py b/src/rpg_vikit/vikit_py/src/vikit_py/cpu_info.py new file mode 100644 index 0000000..d5a629b --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/cpu_info.py @@ -0,0 +1,11 @@ +#!/usr/bin/python + +import subprocess, re + +def get_cpu_info(): + command = "cat /proc/cpuinfo" + all_info = subprocess.check_output(command, shell=True).strip() + for line in all_info.split("\n"): + if "model name" in line: + model_name = re.sub(".*model name.*:", "", line,1).strip() + return model_name.replace("(R)","").replace("(TM)", "") \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/depthmap_utils.py b/src/rpg_vikit/vikit_py/src/vikit_py/depthmap_utils.py new file mode 100644 index 0000000..7eb639a --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/depthmap_utils.py @@ -0,0 +1,50 @@ +# -*- coding: utf-8 -*- + +import numpy as np +import os +import matplotlib.pyplot as plt +from mpl_toolkits.axes_grid1 import make_axes_locatable + +def load_depthmap(depthmap_full_file_path, depthmap_rows, depthmap_cols, + fileformat=np.float32, is_megapov_depthmap = 0): + depth_array = [] + if depthmap_full_file_path.endswith('.bin'): + try: + depth_array = np.fromfile(depthmap_full_file_path, fileformat, -1, '') # the separator character '' specifies a binary file + except IOError: + print 'Could not open file ' + depthmap_full_file_path + ' for reading binary data.' + raise + else: + if depthmap_full_file_path.endswith('.depth'): + try: + if is_megapov_depthmap: + depth_array = np.fromfile(depthmap_full_file_path, dtype='>d') + else: + depth_array = np.fromfile(depthmap_full_file_path, np.float32, -1, ' ') # the separator character ' ' specifies a text file + except IOError: + print 'Could not open file ' + depthmap_full_file_path + ' for reading text data.' + raise + else: + raise MapIOError('Depthmap filename suffix is not correct.') + if (len(depth_array) != (depthmap_rows * depthmap_cols)): + raise MapIOError('Read data do not match the provided size.') + return depth_array + +def load_povray_depthmap(depthmap_full_file_path, rows, cols, + scale_factor = 1.0, is_megapov_depthmap = 0): + depth_array = [] + try: + depth_array = load_depthmap(depthmap_full_file_path, rows, cols, is_megapov_depthmap) + except IOError: + raise + except MapIOError: + raise + return depth_array * scale_factor + +def show_depthmap(ax, depth_array, rows, cols, min_value = None, max_value = None): + image = np.reshape(depth_array, [rows, cols], 'C') + im = ax.imshow(image, vmin = min_value, vmax = max_value) + divider = make_axes_locatable(ax) + cax = divider.append_axes("right", size="5%", pad=0.05) + plt.colorbar(im, cax=cax) + \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/math_utils.py b/src/rpg_vikit/vikit_py/src/vikit_py/math_utils.py new file mode 100644 index 0000000..1d35ac8 --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/math_utils.py @@ -0,0 +1,22 @@ +# -*- coding: utf-8 -*- +""" +Created on Wed Aug 7 22:13:06 2013 + +@author: cforster +""" + +import numpy as np + +def unproject(a): + """Makes a vector homogeneous""" + return np.append(a, 1) + +def project(a): + """De-homogenises a vector""" + return a[:-1]/float(a[-1]) + +def skew(v): + """Returns the skew-symmetric matrix of a vector""" + return np.matrix([[0, -v[2], v[1]], + [v[2], 0, -v[0]], + [-v[1], v[0], 0]], dtype=np.float32) \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/ros_node.py b/src/rpg_vikit/vikit_py/src/vikit_py/ros_node.py new file mode 100644 index 0000000..7bd05cb --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/ros_node.py @@ -0,0 +1,23 @@ +#!/usr/bin/python + +import os + +class RosNode: + def __init__(self, package, executable): + self._package = package + self._executable = executable + self._param_string = '' + + def add_parameters(self, namespace, parameter_dictionary): + for key in parameter_dictionary.keys(): + if type(parameter_dictionary[key]) is dict: + self.add_parameters(namespace+key+'/', parameter_dictionary[key]) + else: + self._param_string += ' _'+namespace+key+':='+str(parameter_dictionary[key]) + + def run(self, parameter_dictionary, namespace=''): + self.add_parameters(namespace, parameter_dictionary) + print('Starting ROS node with parameters: '+self._param_string) + + os.system('ros2 run ' + self._package + ' ' + self._executable + ' ' + self._param_string) + print('ROS node finished processing.') \ No newline at end of file diff --git a/src/rpg_vikit/vikit_py/src/vikit_py/transformations.py b/src/rpg_vikit/vikit_py/src/vikit_py/transformations.py new file mode 100644 index 0000000..d954cb2 --- /dev/null +++ b/src/rpg_vikit/vikit_py/src/vikit_py/transformations.py @@ -0,0 +1,1709 @@ +# -*- coding: utf-8 -*- +# transformations.py + +# Copyright (c) 2006, Christoph Gohlke +# Copyright (c) 2006-2009, The Regents of the University of California +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the copyright holders nor the names of any +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Homogeneous Transformation Matrices and Quaternions. + +A library for calculating 4x4 matrices for translating, rotating, reflecting, +scaling, shearing, projecting, orthogonalizing, and superimposing arrays of +3D homogeneous coordinates as well as for converting between rotation matrices, +Euler angles, and quaternions. Also includes an Arcball control object and +functions to decompose transformation matrices. + +:Authors: + `Christoph Gohlke `__, + Laboratory for Fluorescence Dynamics, University of California, Irvine + +:Version: 20090418 + +Requirements +------------ + +* `Python 2.6 `__ +* `Numpy 1.3 `__ +* `transformations.c 20090418 `__ + (optional implementation of some functions in C) + +Notes +----- + +Matrices (M) can be inverted using numpy.linalg.inv(M), concatenated using +numpy.dot(M0, M1), or used to transform homogeneous coordinates (v) using +numpy.dot(M, v) for shape (4, \*) "point of arrays", respectively +numpy.dot(v, M.T) for shape (\*, 4) "array of points". + +Calculations are carried out with numpy.float64 precision. + +This Python implementation is not optimized for speed. + +Vector, point, quaternion, and matrix function arguments are expected to be +"array like", i.e. tuple, list, or numpy arrays. + +Return types are numpy arrays unless specified otherwise. + +Angles are in radians unless specified otherwise. + +Quaternions ix+jy+kz+w are represented as [x, y, z, w]. + +Use the transpose of transformation matrices for OpenGL glMultMatrixd(). + +A triple of Euler angles can be applied/interpreted in 24 ways, which can +be specified using a 4 character string or encoded 4-tuple: + + *Axes 4-string*: e.g. 'sxyz' or 'ryxy' + + - first character : rotations are applied to 's'tatic or 'r'otating frame + - remaining characters : successive rotation axis 'x', 'y', or 'z' + + *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) + + - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. + - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed + by 'z', or 'z' is followed by 'x'. Otherwise odd (1). + - repetition : first and last axis are same (1) or different (0). + - frame : rotations are applied to static (0) or rotating (1) frame. + +References +---------- + +(1) Matrices and transformations. Ronald Goldman. + In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. +(2) More matrices and transformations: shear and pseudo-perspective. + Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. +(3) Decomposing a matrix into simple transformations. Spencer Thomas. + In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. +(4) Recovering the data from the transformation matrix. Ronald Goldman. + In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. +(5) Euler angle conversion. Ken Shoemake. + In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. +(6) Arcball rotation control. Ken Shoemake. + In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. +(7) Representing attitude: Euler angles, unit quaternions, and rotation + vectors. James Diebel. 2006. +(8) A discussion of the solution for the best rotation to relate two sets + of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. +(9) Closed-form solution of absolute orientation using unit quaternions. + BKP Horn. J Opt Soc Am A. 1987. 4(4), 629-642. +(10) Quaternions. Ken Shoemake. + http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf +(11) From quaternion to matrix and back. JMP van Waveren. 2005. + http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm +(12) Uniform random rotations. Ken Shoemake. + In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. + + +Examples +-------- + +>>> alpha, beta, gamma = 0.123, -1.234, 2.345 +>>> origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1) +>>> I = identity_matrix() +>>> Rx = rotation_matrix(alpha, xaxis) +>>> Ry = rotation_matrix(beta, yaxis) +>>> Rz = rotation_matrix(gamma, zaxis) +>>> R = concatenate_matrices(Rx, Ry, Rz) +>>> euler = euler_from_matrix(R, 'rxyz') +>>> numpy.allclose([alpha, beta, gamma], euler) +True +>>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') +>>> is_same_transform(R, Re) +True +>>> al, be, ga = euler_from_matrix(Re, 'rxyz') +>>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) +True +>>> qx = quaternion_about_axis(alpha, xaxis) +>>> qy = quaternion_about_axis(beta, yaxis) +>>> qz = quaternion_about_axis(gamma, zaxis) +>>> q = quaternion_multiply(qx, qy) +>>> q = quaternion_multiply(q, qz) +>>> Rq = quaternion_matrix(q) +>>> is_same_transform(R, Rq) +True +>>> S = scale_matrix(1.23, origin) +>>> T = translation_matrix((1, 2, 3)) +>>> Z = shear_matrix(beta, xaxis, origin, zaxis) +>>> R = random_rotation_matrix(numpy.random.rand(3)) +>>> M = concatenate_matrices(T, R, Z, S) +>>> scale, shear, angles, trans, persp = decompose_matrix(M) +>>> numpy.allclose(scale, 1.23) +True +>>> numpy.allclose(trans, (1, 2, 3)) +True +>>> numpy.allclose(shear, (0, math.tan(beta), 0)) +True +>>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) +True +>>> M1 = compose_matrix(scale, shear, angles, trans, persp) +>>> is_same_transform(M, M1) +True + +""" + +from __future__ import division + +import warnings +import math + +import numpy + +# Documentation in HTML format can be generated with Epydoc +__docformat__ = "restructuredtext en" + + +def identity_matrix(): + """Return 4x4 identity/unit matrix. + + >>> I = identity_matrix() + >>> numpy.allclose(I, numpy.dot(I, I)) + True + >>> numpy.sum(I), numpy.trace(I) + (4.0, 4.0) + >>> numpy.allclose(I, numpy.identity(4, dtype=numpy.float64)) + True + + """ + return numpy.identity(4, dtype=numpy.float64) + + +def translation_matrix(direction): + """Return matrix to translate by direction vector. + + >>> v = numpy.random.random(3) - 0.5 + >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) + True + + """ + M = numpy.identity(4) + M[:3, 3] = direction[:3] + return M + + +def translation_from_matrix(matrix): + """Return translation vector from translation matrix. + + >>> v0 = numpy.random.random(3) - 0.5 + >>> v1 = translation_from_matrix(translation_matrix(v0)) + >>> numpy.allclose(v0, v1) + True + + """ + return numpy.array(matrix, copy=False)[:3, 3].copy() + +def convert_3x3_to_4x4(matrix_3x3): + M = numpy.identity(4) + M[:3,:3] = matrix_3x3 + return M + +def reflection_matrix(point, normal): + """Return matrix to mirror at plane defined by point and normal vector. + + >>> v0 = numpy.random.random(4) - 0.5 + >>> v0[3] = 1.0 + >>> v1 = numpy.random.random(3) - 0.5 + >>> R = reflection_matrix(v0, v1) + >>> numpy.allclose(2., numpy.trace(R)) + True + >>> numpy.allclose(v0, numpy.dot(R, v0)) + True + >>> v2 = v0.copy() + >>> v2[:3] += v1 + >>> v3 = v0.copy() + >>> v2[:3] -= v1 + >>> numpy.allclose(v2, numpy.dot(R, v3)) + True + + """ + normal = unit_vector(normal[:3]) + M = numpy.identity(4) + M[:3, :3] -= 2.0 * numpy.outer(normal, normal) + M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal + return M + + +def reflection_from_matrix(matrix): + """Return mirror plane point and normal vector from reflection matrix. + + >>> v0 = numpy.random.random(3) - 0.5 + >>> v1 = numpy.random.random(3) - 0.5 + >>> M0 = reflection_matrix(v0, v1) + >>> point, normal = reflection_from_matrix(M0) + >>> M1 = reflection_matrix(point, normal) + >>> is_same_transform(M0, M1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + # normal: unit eigenvector corresponding to eigenvalue -1 + l, V = numpy.linalg.eig(M[:3, :3]) + i = numpy.where(abs(numpy.real(l) + 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue -1") + normal = numpy.real(V[:, i[0]]).squeeze() + # point: any unit eigenvector corresponding to eigenvalue 1 + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + return point, normal + + +def rotation_matrix(angle, direction, point=None): + """Return matrix to rotate about axis defined by point and direction. + + >>> angle = (random.random() - 0.5) * (2*math.pi) + >>> direc = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) + >>> is_same_transform(R0, R1) + True + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(-angle, -direc, point) + >>> is_same_transform(R0, R1) + True + >>> I = numpy.identity(4, numpy.float64) + >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) + True + >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2, + ... direc, point))) + True + + """ + sina = math.sin(angle) + cosa = math.cos(angle) + direction = unit_vector(direction[:3]) + # rotation matrix around unit vector + R = numpy.array(((cosa, 0.0, 0.0), + (0.0, cosa, 0.0), + (0.0, 0.0, cosa)), dtype=numpy.float64) + R += numpy.outer(direction, direction) * (1.0 - cosa) + direction *= sina + R += numpy.array((( 0.0, -direction[2], direction[1]), + ( direction[2], 0.0, -direction[0]), + (-direction[1], direction[0], 0.0)), + dtype=numpy.float64) + M = numpy.identity(4) + M[:3, :3] = R + if point is not None: + # rotation not around origin + point = numpy.array(point[:3], dtype=numpy.float64, copy=False) + M[:3, 3] = point - numpy.dot(R, point) + return M + + +def rotation_from_matrix(matrix): + """Return rotation angle and axis from rotation matrix. + + >>> angle = (random.random() - 0.5) * (2*math.pi) + >>> direc = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> R0 = rotation_matrix(angle, direc, point) + >>> angle, direc, point = rotation_from_matrix(R0) + >>> R1 = rotation_matrix(angle, direc, point) + >>> is_same_transform(R0, R1) + True + + """ + R = numpy.array(matrix, dtype=numpy.float64, copy=False) + R33 = R[:3, :3] + # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 + l, W = numpy.linalg.eig(R33.T) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + direction = numpy.real(W[:, i[-1]]).squeeze() + # point: unit eigenvector of R33 corresponding to eigenvalue of 1 + l, Q = numpy.linalg.eig(R) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no unit eigenvector corresponding to eigenvalue 1") + point = numpy.real(Q[:, i[-1]]).squeeze() + point /= point[3] + # rotation angle depending on direction + cosa = (numpy.trace(R33) - 1.0) / 2.0 + if abs(direction[2]) > 1e-8: + sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] + elif abs(direction[1]) > 1e-8: + sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] + else: + sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] + angle = math.atan2(sina, cosa) + return angle, direction, point + + +def scale_matrix(factor, origin=None, direction=None): + """Return matrix to scale by factor around origin in direction. + + Use factor -1 for point symmetry. + + >>> v = (numpy.random.rand(4, 5) - 0.5) * 20.0 + >>> v[3] = 1.0 + >>> S = scale_matrix(-1.234) + >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) + True + >>> factor = random.random() * 10 - 5 + >>> origin = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> S = scale_matrix(factor, origin) + >>> S = scale_matrix(factor, origin, direct) + + """ + if direction is None: + # uniform scaling + M = numpy.array(((factor, 0.0, 0.0, 0.0), + (0.0, factor, 0.0, 0.0), + (0.0, 0.0, factor, 0.0), + (0.0, 0.0, 0.0, 1.0)), dtype=numpy.float64) + if origin is not None: + M[:3, 3] = origin[:3] + M[:3, 3] *= 1.0 - factor + else: + # nonuniform scaling + direction = unit_vector(direction[:3]) + factor = 1.0 - factor + M = numpy.identity(4) + M[:3, :3] -= factor * numpy.outer(direction, direction) + if origin is not None: + M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction + return M + + +def scale_from_matrix(matrix): + """Return scaling factor, origin and direction from scaling matrix. + + >>> factor = random.random() * 10 - 5 + >>> origin = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> S0 = scale_matrix(factor, origin) + >>> factor, origin, direction = scale_from_matrix(S0) + >>> S1 = scale_matrix(factor, origin, direction) + >>> is_same_transform(S0, S1) + True + >>> S0 = scale_matrix(factor, origin, direct) + >>> factor, origin, direction = scale_from_matrix(S0) + >>> S1 = scale_matrix(factor, origin, direction) + >>> is_same_transform(S0, S1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + factor = numpy.trace(M33) - 2.0 + try: + # direction: unit eigenvector corresponding to eigenvalue factor + l, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(l) - factor) < 1e-8)[0][0] + direction = numpy.real(V[:, i]).squeeze() + direction /= vector_norm(direction) + except IndexError: + # uniform scaling + factor = (factor + 2.0) / 3.0 + direction = None + # origin: any eigenvector corresponding to eigenvalue 1 + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 1") + origin = numpy.real(V[:, i[-1]]).squeeze() + origin /= origin[3] + return factor, origin, direction + + +def projection_matrix(point, normal, direction=None, + perspective=None, pseudo=False): + """Return matrix to project onto plane defined by point and normal. + + Using either perspective point, projection direction, or none of both. + + If pseudo is True, perspective projections will preserve relative depth + such that Perspective = dot(Orthogonal, PseudoPerspective). + + >>> P = projection_matrix((0, 0, 0), (1, 0, 0)) + >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) + True + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(3) - 0.5 + >>> P0 = projection_matrix(point, normal) + >>> P1 = projection_matrix(point, normal, direction=direct) + >>> P2 = projection_matrix(point, normal, perspective=persp) + >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) + >>> is_same_transform(P2, numpy.dot(P0, P3)) + True + >>> P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0)) + >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20.0 + >>> v0[3] = 1.0 + >>> v1 = numpy.dot(P, v0) + >>> numpy.allclose(v1[1], v0[1]) + True + >>> numpy.allclose(v1[0], 3.0-v1[1]) + True + + """ + M = numpy.identity(4) + point = numpy.array(point[:3], dtype=numpy.float64, copy=False) + normal = unit_vector(normal[:3]) + if perspective is not None: + # perspective projection + perspective = numpy.array(perspective[:3], dtype=numpy.float64, + copy=False) + M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) + M[:3, :3] -= numpy.outer(perspective, normal) + if pseudo: + # preserve relative depth + M[:3, :3] -= numpy.outer(normal, normal) + M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) + else: + M[:3, 3] = numpy.dot(point, normal) * perspective + M[3, :3] = -normal + M[3, 3] = numpy.dot(perspective, normal) + elif direction is not None: + # parallel projection + direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) + scale = numpy.dot(direction, normal) + M[:3, :3] -= numpy.outer(direction, normal) / scale + M[:3, 3] = direction * (numpy.dot(point, normal) / scale) + else: + # orthogonal projection + M[:3, :3] -= numpy.outer(normal, normal) + M[:3, 3] = numpy.dot(point, normal) * normal + return M + + +def projection_from_matrix(matrix, pseudo=False): + """Return projection plane and perspective point from projection matrix. + + Return values are same as arguments for projection_matrix function: + point, normal, direction, perspective, and pseudo. + + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.random.random(3) - 0.5 + >>> direct = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(3) - 0.5 + >>> P0 = projection_matrix(point, normal) + >>> result = projection_from_matrix(P0) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, direct) + >>> result = projection_from_matrix(P0) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) + >>> result = projection_from_matrix(P0, pseudo=False) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) + >>> result = projection_from_matrix(P0, pseudo=True) + >>> P1 = projection_matrix(*result) + >>> is_same_transform(P0, P1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not pseudo and len(i): + # point: any eigenvector corresponding to eigenvalue 1 + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + # direction: unit eigenvector corresponding to eigenvalue 0 + l, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 0") + direction = numpy.real(V[:, i[0]]).squeeze() + direction /= vector_norm(direction) + # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 + l, V = numpy.linalg.eig(M33.T) + i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] + if len(i): + # parallel projection + normal = numpy.real(V[:, i[0]]).squeeze() + normal /= vector_norm(normal) + return point, normal, direction, None, False + else: + # orthogonal projection, where normal equals direction vector + return point, direction, None, None, False + else: + # perspective projection + i = numpy.where(abs(numpy.real(l)) > 1e-8)[0] + if not len(i): + raise ValueError( + "no eigenvector not corresponding to eigenvalue 0") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + normal = - M[3, :3] + perspective = M[:3, 3] / numpy.dot(point[:3], normal) + if pseudo: + perspective -= normal + return point, normal, None, perspective, pseudo + + +def clip_matrix(left, right, bottom, top, near, far, perspective=False): + """Return matrix to obtain normalized device coordinates from frustrum. + + The frustrum bounds are axis-aligned along x (left, right), + y (bottom, top) and z (near, far). + + Normalized device coordinates are in range [-1, 1] if coordinates are + inside the frustrum. + + If perspective is True the frustrum is a truncated pyramid with the + perspective point at origin and direction along z axis, otherwise an + orthographic canonical view volume (a box). + + Homogeneous coordinates transformed by the perspective clip matrix + need to be dehomogenized (devided by w coordinate). + + >>> frustrum = numpy.random.rand(6) + >>> frustrum[1] += frustrum[0] + >>> frustrum[3] += frustrum[2] + >>> frustrum[5] += frustrum[4] + >>> M = clip_matrix(*frustrum, perspective=False) + >>> numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) + array([-1., -1., -1., 1.]) + >>> numpy.dot(M, [frustrum[1], frustrum[3], frustrum[5], 1.0]) + array([ 1., 1., 1., 1.]) + >>> M = clip_matrix(*frustrum, perspective=True) + >>> v = numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) + >>> v / v[3] + array([-1., -1., -1., 1.]) + >>> v = numpy.dot(M, [frustrum[1], frustrum[3], frustrum[4], 1.0]) + >>> v / v[3] + array([ 1., 1., -1., 1.]) + + """ + if left >= right or bottom >= top or near >= far: + raise ValueError("invalid frustrum") + if perspective: + if near <= _EPS: + raise ValueError("invalid frustrum: near <= 0") + t = 2.0 * near + M = ((-t/(right-left), 0.0, (right+left)/(right-left), 0.0), + (0.0, -t/(top-bottom), (top+bottom)/(top-bottom), 0.0), + (0.0, 0.0, -(far+near)/(far-near), t*far/(far-near)), + (0.0, 0.0, -1.0, 0.0)) + else: + M = ((2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)), + (0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)), + (0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)), + (0.0, 0.0, 0.0, 1.0)) + return numpy.array(M, dtype=numpy.float64) + + +def shear_matrix(angle, direction, point, normal): + """Return matrix to shear by angle along direction vector on shear plane. + + The shear plane is defined by a point and normal vector. The direction + vector must be orthogonal to the plane's normal vector. + + A point P is transformed by the shear matrix into P" such that + the vector P-P" is parallel to the direction vector and its extent is + given by the angle of P-P'-P", where P' is the orthogonal projection + of P onto the shear plane. + + >>> angle = (random.random() - 0.5) * 4*math.pi + >>> direct = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.cross(direct, numpy.random.random(3)) + >>> S = shear_matrix(angle, direct, point, normal) + >>> numpy.allclose(1.0, numpy.linalg.det(S)) + True + + """ + normal = unit_vector(normal[:3]) + direction = unit_vector(direction[:3]) + if abs(numpy.dot(normal, direction)) > 1e-6: + raise ValueError("direction and normal vectors are not orthogonal") + angle = math.tan(angle) + M = numpy.identity(4) + M[:3, :3] += angle * numpy.outer(direction, normal) + M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction + return M + + +def shear_from_matrix(matrix): + """Return shear angle, direction and plane from shear matrix. + + >>> angle = (random.random() - 0.5) * 4*math.pi + >>> direct = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> normal = numpy.cross(direct, numpy.random.random(3)) + >>> S0 = shear_matrix(angle, direct, point, normal) + >>> angle, direct, point, normal = shear_from_matrix(S0) + >>> S1 = shear_matrix(angle, direct, point, normal) + >>> is_same_transform(S0, S1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=False) + M33 = M[:3, :3] + # normal: cross independent eigenvectors corresponding to the eigenvalue 1 + l, V = numpy.linalg.eig(M33) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-4)[0] + if len(i) < 2: + raise ValueError("No two linear independent eigenvectors found %s" % l) + V = numpy.real(V[:, i]).squeeze().T + lenorm = -1.0 + for i0, i1 in ((0, 1), (0, 2), (1, 2)): + n = numpy.cross(V[i0], V[i1]) + l = vector_norm(n) + if l > lenorm: + lenorm = l + normal = n + normal /= lenorm + # direction and angle + direction = numpy.dot(M33 - numpy.identity(3), normal) + angle = vector_norm(direction) + direction /= angle + angle = math.atan(angle) + # point: eigenvector corresponding to eigenvalue 1 + l, V = numpy.linalg.eig(M) + i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] + if not len(i): + raise ValueError("no eigenvector corresponding to eigenvalue 1") + point = numpy.real(V[:, i[-1]]).squeeze() + point /= point[3] + return angle, direction, point, normal + + +def decompose_matrix(matrix): + """Return sequence of transformations from transformation matrix. + + matrix : array_like + Non-degenerative homogeneous transformation matrix + + Return tuple of: + scale : vector of 3 scaling factors + shear : list of shear factors for x-y, x-z, y-z axes + angles : list of Euler angles about static x, y, z axes + translate : translation vector along x, y, z axes + perspective : perspective partition of matrix + + Raise ValueError if matrix is of wrong type or degenerative. + + >>> T0 = translation_matrix((1, 2, 3)) + >>> scale, shear, angles, trans, persp = decompose_matrix(T0) + >>> T1 = translation_matrix(trans) + >>> numpy.allclose(T0, T1) + True + >>> S = scale_matrix(0.123) + >>> scale, shear, angles, trans, persp = decompose_matrix(S) + >>> scale[0] + 0.123 + >>> R0 = euler_matrix(1, 2, 3) + >>> scale, shear, angles, trans, persp = decompose_matrix(R0) + >>> R1 = euler_matrix(*angles) + >>> numpy.allclose(R0, R1) + True + + """ + M = numpy.array(matrix, dtype=numpy.float64, copy=True).T + if abs(M[3, 3]) < _EPS: + raise ValueError("M[3, 3] is zero") + M /= M[3, 3] + P = M.copy() + P[:, 3] = 0, 0, 0, 1 + if not numpy.linalg.det(P): + raise ValueError("Matrix is singular") + + scale = numpy.zeros((3, ), dtype=numpy.float64) + shear = [0, 0, 0] + angles = [0, 0, 0] + + if any(abs(M[:3, 3]) > _EPS): + perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) + M[:, 3] = 0, 0, 0, 1 + else: + perspective = numpy.array((0, 0, 0, 1), dtype=numpy.float64) + + translate = M[3, :3].copy() + M[3, :3] = 0 + + row = M[:3, :3].copy() + scale[0] = vector_norm(row[0]) + row[0] /= scale[0] + shear[0] = numpy.dot(row[0], row[1]) + row[1] -= row[0] * shear[0] + scale[1] = vector_norm(row[1]) + row[1] /= scale[1] + shear[0] /= scale[1] + shear[1] = numpy.dot(row[0], row[2]) + row[2] -= row[0] * shear[1] + shear[2] = numpy.dot(row[1], row[2]) + row[2] -= row[1] * shear[2] + scale[2] = vector_norm(row[2]) + row[2] /= scale[2] + shear[1:] /= scale[2] + + if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: + scale *= -1 + row *= -1 + + angles[1] = math.asin(-row[0, 2]) + if math.cos(angles[1]): + angles[0] = math.atan2(row[1, 2], row[2, 2]) + angles[2] = math.atan2(row[0, 1], row[0, 0]) + else: + #angles[0] = math.atan2(row[1, 0], row[1, 1]) + angles[0] = math.atan2(-row[2, 1], row[1, 1]) + angles[2] = 0.0 + + return scale, shear, angles, translate, perspective + + +def compose_matrix(scale=None, shear=None, angles=None, translate=None, + perspective=None): + """Return transformation matrix from sequence of transformations. + + This is the inverse of the decompose_matrix function. + + Sequence of transformations: + scale : vector of 3 scaling factors + shear : list of shear factors for x-y, x-z, y-z axes + angles : list of Euler angles about static x, y, z axes + translate : translation vector along x, y, z axes + perspective : perspective partition of matrix + + >>> scale = numpy.random.random(3) - 0.5 + >>> shear = numpy.random.random(3) - 0.5 + >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) + >>> trans = numpy.random.random(3) - 0.5 + >>> persp = numpy.random.random(4) - 0.5 + >>> M0 = compose_matrix(scale, shear, angles, trans, persp) + >>> result = decompose_matrix(M0) + >>> M1 = compose_matrix(*result) + >>> is_same_transform(M0, M1) + True + + """ + M = numpy.identity(4) + if perspective is not None: + P = numpy.identity(4) + P[3, :] = perspective[:4] + M = numpy.dot(M, P) + if translate is not None: + T = numpy.identity(4) + T[:3, 3] = translate[:3] + M = numpy.dot(M, T) + if angles is not None: + R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') + M = numpy.dot(M, R) + if shear is not None: + Z = numpy.identity(4) + Z[1, 2] = shear[2] + Z[0, 2] = shear[1] + Z[0, 1] = shear[0] + M = numpy.dot(M, Z) + if scale is not None: + S = numpy.identity(4) + S[0, 0] = scale[0] + S[1, 1] = scale[1] + S[2, 2] = scale[2] + M = numpy.dot(M, S) + M /= M[3, 3] + return M + + +def orthogonalization_matrix(lengths, angles): + """Return orthogonalization matrix for crystallographic cell coordinates. + + Angles are expected in degrees. + + The de-orthogonalization matrix is the inverse. + + >>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.)) + >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) + True + >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) + >>> numpy.allclose(numpy.sum(O), 43.063229) + True + + """ + a, b, c = lengths + angles = numpy.radians(angles) + sina, sinb, _ = numpy.sin(angles) + cosa, cosb, cosg = numpy.cos(angles) + co = (cosa * cosb - cosg) / (sina * sinb) + return numpy.array(( + ( a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0), + (-a*sinb*co, b*sina, 0.0, 0.0), + ( a*cosb, b*cosa, c, 0.0), + ( 0.0, 0.0, 0.0, 1.0)), + dtype=numpy.float64) + + +def superimposition_matrix(v0, v1, scaling=False, usesvd=True): + """Return matrix to transform given vector set into second vector set. + + v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 vectors. + + If usesvd is True, the weighted sum of squared deviations (RMSD) is + minimized according to the algorithm by W. Kabsch [8]. Otherwise the + quaternion based algorithm by B. Horn [9] is used (slower when using + this Python implementation). + + The returned matrix performs rotation, translation and uniform scaling + (if specified). + + >>> v0 = numpy.random.rand(3, 10) + >>> M = superimposition_matrix(v0, v0) + >>> numpy.allclose(M, numpy.identity(4)) + True + >>> R = random_rotation_matrix(numpy.random.random(3)) + >>> v0 = ((1,0,0), (0,1,0), (0,0,1), (1,1,1)) + >>> v1 = numpy.dot(R, v0) + >>> M = superimposition_matrix(v0, v1) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20.0 + >>> v0[3] = 1.0 + >>> v1 = numpy.dot(R, v0) + >>> M = superimposition_matrix(v0, v1) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> S = scale_matrix(random.random()) + >>> T = translation_matrix(numpy.random.random(3)-0.5) + >>> M = concatenate_matrices(T, R, S) + >>> v1 = numpy.dot(M, v0) + >>> v0[:3] += numpy.random.normal(0.0, 1e-9, 300).reshape(3, -1) + >>> M = superimposition_matrix(v0, v1, scaling=True) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) + >>> numpy.allclose(v1, numpy.dot(M, v0)) + True + >>> v = numpy.empty((4, 100, 3), dtype=numpy.float64) + >>> v[:, :, 0] = v0 + >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) + >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) + True + + """ + v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] + v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] + + if v0.shape != v1.shape or v0.shape[1] < 3: + raise ValueError("Vector sets are of wrong shape or type.") + + # move centroids to origin + t0 = numpy.mean(v0, axis=1) + t1 = numpy.mean(v1, axis=1) + v0 = v0 - t0.reshape(3, 1) + v1 = v1 - t1.reshape(3, 1) + + if usesvd: + # Singular Value Decomposition of covariance matrix + u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) + # rotation matrix from SVD orthonormal bases + R = numpy.dot(u, vh) + if numpy.linalg.det(R) < 0.0: + # R does not constitute right handed system + R -= numpy.outer(u[:, 2], vh[2, :]*2.0) + s[-1] *= -1.0 + # homogeneous transformation matrix + M = numpy.identity(4) + M[:3, :3] = R + else: + # compute symmetric matrix N + xx, yy, zz = numpy.sum(v0 * v1, axis=1) + xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) + xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) + N = ((xx+yy+zz, yz-zy, zx-xz, xy-yx), + (yz-zy, xx-yy-zz, xy+yx, zx+xz), + (zx-xz, xy+yx, -xx+yy-zz, yz+zy), + (xy-yx, zx+xz, yz+zy, -xx-yy+zz)) + # quaternion: eigenvector corresponding to most positive eigenvalue + l, V = numpy.linalg.eig(N) + q = V[:, numpy.argmax(l)] + q /= vector_norm(q) # unit quaternion + q = numpy.roll(q, -1) # move w component to end + # homogeneous transformation matrix + M = quaternion_matrix(q) + + # scale: ratio of rms deviations from centroid + if scaling: + v0 *= v0 + v1 *= v1 + M[:3, :3] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) + + # translation + M[:3, 3] = t1 + T = numpy.identity(4) + T[:3, 3] = -t0 + M = numpy.dot(M, T) + return M + + +def euler_matrix(ai, aj, ak, axes='sxyz'): + """Return homogeneous rotation matrix from Euler angles and axis sequence. + + ai, aj, ak : Euler's roll, pitch and yaw angles + axes : One of 24 axis sequences as string or encoded tuple + + >>> R = euler_matrix(1, 2, 3, 'syxz') + >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) + True + >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) + >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) + True + >>> ai, aj, ak = (4.0*math.pi) * (numpy.random.random(3) - 0.5) + >>> for axes in _AXES2TUPLE.keys(): + ... R = euler_matrix(ai, aj, ak, axes) + >>> for axes in _TUPLE2AXES.keys(): + ... R = euler_matrix(ai, aj, ak, axes) + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] + except (AttributeError, KeyError): + _ = _TUPLE2AXES[axes] + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + if frame: + ai, ak = ak, ai + if parity: + ai, aj, ak = -ai, -aj, -ak + + si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) + ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) + cc, cs = ci*ck, ci*sk + sc, ss = si*ck, si*sk + + M = numpy.identity(4) + if repetition: + M[i, i] = cj + M[i, j] = sj*si + M[i, k] = sj*ci + M[j, i] = sj*sk + M[j, j] = -cj*ss+cc + M[j, k] = -cj*cs-sc + M[k, i] = -sj*ck + M[k, j] = cj*sc+cs + M[k, k] = cj*cc-ss + else: + M[i, i] = cj*ck + M[i, j] = sj*sc-cs + M[i, k] = sj*cc+ss + M[j, i] = cj*sk + M[j, j] = sj*ss+cc + M[j, k] = sj*cs-sc + M[k, i] = -sj + M[k, j] = cj*si + M[k, k] = cj*ci + return M + + +def euler_from_matrix(matrix, axes='sxyz'): + """Return Euler angles from rotation matrix for specified axis sequence. + + axes : One of 24 axis sequences as string or encoded tuple + + Note that many Euler angle triplets can describe one matrix. + + >>> R0 = euler_matrix(1, 2, 3, 'syxz') + >>> al, be, ga = euler_from_matrix(R0, 'syxz') + >>> R1 = euler_matrix(al, be, ga, 'syxz') + >>> numpy.allclose(R0, R1) + True + >>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5) + >>> for axes in _AXES2TUPLE.keys(): + ... R0 = euler_matrix(axes=axes, *angles) + ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) + ... if not numpy.allclose(R0, R1): print axes, "failed" + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] + except (AttributeError, KeyError): + _ = _TUPLE2AXES[axes] + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] + if repetition: + sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) + if sy > _EPS: + ax = math.atan2( M[i, j], M[i, k]) + ay = math.atan2( sy, M[i, i]) + az = math.atan2( M[j, i], -M[k, i]) + else: + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2( sy, M[i, i]) + az = 0.0 + else: + cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) + if cy > _EPS: + ax = math.atan2( M[k, j], M[k, k]) + ay = math.atan2(-M[k, i], cy) + az = math.atan2( M[j, i], M[i, i]) + else: + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2(-M[k, i], cy) + az = 0.0 + + if parity: + ax, ay, az = -ax, -ay, -az + if frame: + ax, az = az, ax + return ax, ay, az + + +def euler_from_quaternion(quaternion, axes='sxyz'): + """Return Euler angles from quaternion for specified axis sequence. + + >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947]) + >>> numpy.allclose(angles, [0.123, 0, 0]) + True + + """ + return euler_from_matrix(quaternion_matrix(quaternion), axes) + + +def quaternion_from_euler(ai, aj, ak, axes='sxyz'): + """Return quaternion from Euler angles and axis sequence. + + ai, aj, ak : Euler's roll, pitch and yaw angles + axes : One of 24 axis sequences as string or encoded tuple + + >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') + >>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953]) + True + + """ + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] + except (AttributeError, KeyError): + _ = _TUPLE2AXES[axes] + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + if frame: + ai, ak = ak, ai + if parity: + aj = -aj + + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + quaternion = numpy.empty((4, ), dtype=numpy.float64) + if repetition: + quaternion[i] = cj*(cs + sc) + quaternion[j] = sj*(cc + ss) + quaternion[k] = sj*(cs - sc) + quaternion[3] = cj*(cc - ss) + else: + quaternion[i] = cj*sc - sj*cs + quaternion[j] = cj*ss + sj*cc + quaternion[k] = cj*cs - sj*sc + quaternion[3] = cj*cc + sj*ss + if parity: + quaternion[j] *= -1 + + return quaternion + + +def quaternion_about_axis(angle, axis): + """Return quaternion for rotation about axis. + + >>> q = quaternion_about_axis(0.123, (1, 0, 0)) + >>> numpy.allclose(q, [0.06146124, 0, 0, 0.99810947]) + True + + """ + quaternion = numpy.zeros((4, ), dtype=numpy.float64) + quaternion[:3] = axis[:3] + qlen = vector_norm(quaternion) + if qlen > _EPS: + quaternion *= math.sin(angle/2.0) / qlen + quaternion[3] = math.cos(angle/2.0) + return quaternion + + +def quaternion_matrix(quaternion): + """Return homogeneous rotation matrix from quaternion. + + >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947]) + >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0))) + True + + """ + q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True) + nq = numpy.dot(q, q) + if nq < _EPS: + return numpy.identity(4) + q *= math.sqrt(2.0 / nq) + q = numpy.outer(q, q) + return numpy.array(( + (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], 0.0), + ( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], 0.0), + ( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0), + ( 0.0, 0.0, 0.0, 1.0) + ), dtype=numpy.float64) + + +def quaternion_from_matrix(matrix): + """Return quaternion from rotation matrix. + + >>> R = rotation_matrix(0.123, (1, 2, 3)) + >>> q = quaternion_from_matrix(R) + >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095]) + True + + """ + q = numpy.empty((4, ), dtype=numpy.float64) + M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] + t = numpy.trace(M) + if t > M[3, 3]: + q[3] = t + q[2] = M[1, 0] - M[0, 1] + q[1] = M[0, 2] - M[2, 0] + q[0] = M[2, 1] - M[1, 2] + else: + i, j, k = 0, 1, 2 + if M[1, 1] > M[0, 0]: + i, j, k = 1, 2, 0 + if M[2, 2] > M[i, i]: + i, j, k = 2, 0, 1 + t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] + q[i] = t + q[j] = M[i, j] + M[j, i] + q[k] = M[k, i] + M[i, k] + q[3] = M[k, j] - M[j, k] + q *= 0.5 / math.sqrt(t * M[3, 3]) + return q + + +def quaternion_multiply(quaternion1, quaternion0): + """Return multiplication of two quaternions. + + >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) + >>> numpy.allclose(q, [-44, -14, 48, 28]) + True + + """ + x0, y0, z0, w0 = quaternion0 + x1, y1, z1, w1 = quaternion1 + return numpy.array(( + x1*w0 + y1*z0 - z1*y0 + w1*x0, + -x1*z0 + y1*w0 + z1*x0 + w1*y0, + x1*y0 - y1*x0 + z1*w0 + w1*z0, + -x1*x0 - y1*y0 - z1*z0 + w1*w0), dtype=numpy.float64) + + +def quaternion_conjugate(quaternion): + """Return conjugate of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_conjugate(q0) + >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3]) + True + + """ + return numpy.array((-quaternion[0], -quaternion[1], + -quaternion[2], quaternion[3]), dtype=numpy.float64) + + +def quaternion_inverse(quaternion): + """Return inverse of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_inverse(q0) + >>> numpy.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1]) + True + + """ + return quaternion_conjugate(quaternion) / numpy.dot(quaternion, quaternion) + + +def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): + """Return spherical linear interpolation between two quaternions. + + >>> q0 = random_quaternion() + >>> q1 = random_quaternion() + >>> q = quaternion_slerp(q0, q1, 0.0) + >>> numpy.allclose(q, q0) + True + >>> q = quaternion_slerp(q0, q1, 1.0, 1) + >>> numpy.allclose(q, q1) + True + >>> q = quaternion_slerp(q0, q1, 0.5) + >>> angle = math.acos(numpy.dot(q0, q)) + >>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \ + numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle) + True + + """ + q0 = unit_vector(quat0[:4]) + q1 = unit_vector(quat1[:4]) + if fraction == 0.0: + return q0 + elif fraction == 1.0: + return q1 + d = numpy.dot(q0, q1) + if abs(abs(d) - 1.0) < _EPS: + return q0 + if shortestpath and d < 0.0: + # invert rotation + d = -d + q1 *= -1.0 + angle = math.acos(d) + spin * math.pi + if abs(angle) < _EPS: + return q0 + isin = 1.0 / math.sin(angle) + q0 *= math.sin((1.0 - fraction) * angle) * isin + q1 *= math.sin(fraction * angle) * isin + q0 += q1 + return q0 + + +def random_quaternion(rand=None): + """Return uniform random unit quaternion. + + rand: array like or None + Three independent random variables that are uniformly distributed + between 0 and 1. + + >>> q = random_quaternion() + >>> numpy.allclose(1.0, vector_norm(q)) + True + >>> q = random_quaternion(numpy.random.random(3)) + >>> q.shape + (4,) + + """ + if rand is None: + rand = numpy.random.rand(3) + else: + assert len(rand) == 3 + r1 = numpy.sqrt(1.0 - rand[0]) + r2 = numpy.sqrt(rand[0]) + pi2 = math.pi * 2.0 + t1 = pi2 * rand[1] + t2 = pi2 * rand[2] + return numpy.array((numpy.sin(t1)*r1, + numpy.cos(t1)*r1, + numpy.sin(t2)*r2, + numpy.cos(t2)*r2), dtype=numpy.float64) + + +def random_rotation_matrix(rand=None): + """Return uniform random rotation matrix. + + rnd: array like + Three independent random variables that are uniformly distributed + between 0 and 1 for each returned quaternion. + + >>> R = random_rotation_matrix() + >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) + True + + """ + return quaternion_matrix(random_quaternion(rand)) + + +class Arcball(object): + """Virtual Trackball Control. + + >>> ball = Arcball() + >>> ball = Arcball(initial=numpy.identity(4)) + >>> ball.place([320, 320], 320) + >>> ball.down([500, 250]) + >>> ball.drag([475, 275]) + >>> R = ball.matrix() + >>> numpy.allclose(numpy.sum(R), 3.90583455) + True + >>> ball = Arcball(initial=[0, 0, 0, 1]) + >>> ball.place([320, 320], 320) + >>> ball.setaxes([1,1,0], [-1, 1, 0]) + >>> ball.setconstrain(True) + >>> ball.down([400, 200]) + >>> ball.drag([200, 400]) + >>> R = ball.matrix() + >>> numpy.allclose(numpy.sum(R), 0.2055924) + True + >>> ball.next() + + """ + + def __init__(self, initial=None): + """Initialize virtual trackball control. + + initial : quaternion or rotation matrix + + """ + self._axis = None + self._axes = None + self._radius = 1.0 + self._center = [0.0, 0.0] + self._vdown = numpy.array([0, 0, 1], dtype=numpy.float64) + self._constrain = False + + if initial is None: + self._qdown = numpy.array([0, 0, 0, 1], dtype=numpy.float64) + else: + initial = numpy.array(initial, dtype=numpy.float64) + if initial.shape == (4, 4): + self._qdown = quaternion_from_matrix(initial) + elif initial.shape == (4, ): + initial /= vector_norm(initial) + self._qdown = initial + else: + raise ValueError("initial not a quaternion or matrix.") + + self._qnow = self._qpre = self._qdown + + def place(self, center, radius): + """Place Arcball, e.g. when window size changes. + + center : sequence[2] + Window coordinates of trackball center. + radius : float + Radius of trackball in window coordinates. + + """ + self._radius = float(radius) + self._center[0] = center[0] + self._center[1] = center[1] + + def setaxes(self, *axes): + """Set axes to constrain rotations.""" + if axes is None: + self._axes = None + else: + self._axes = [unit_vector(axis) for axis in axes] + + def setconstrain(self, constrain): + """Set state of constrain to axis mode.""" + self._constrain = constrain == True + + def getconstrain(self): + """Return state of constrain to axis mode.""" + return self._constrain + + def down(self, point): + """Set initial cursor window coordinates and pick constrain-axis.""" + self._vdown = arcball_map_to_sphere(point, self._center, self._radius) + self._qdown = self._qpre = self._qnow + + if self._constrain and self._axes is not None: + self._axis = arcball_nearest_axis(self._vdown, self._axes) + self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) + else: + self._axis = None + + def drag(self, point): + """Update current cursor window coordinates.""" + vnow = arcball_map_to_sphere(point, self._center, self._radius) + + if self._axis is not None: + vnow = arcball_constrain_to_axis(vnow, self._axis) + + self._qpre = self._qnow + + t = numpy.cross(self._vdown, vnow) + if numpy.dot(t, t) < _EPS: + self._qnow = self._qdown + else: + q = [t[0], t[1], t[2], numpy.dot(self._vdown, vnow)] + self._qnow = quaternion_multiply(q, self._qdown) + + def next(self, acceleration=0.0): + """Continue rotation in direction of last drag.""" + q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) + self._qpre, self._qnow = self._qnow, q + + def matrix(self): + """Return homogeneous rotation matrix.""" + return quaternion_matrix(self._qnow) + + +def arcball_map_to_sphere(point, center, radius): + """Return unit sphere coordinates from window coordinates.""" + v = numpy.array(((point[0] - center[0]) / radius, + (center[1] - point[1]) / radius, + 0.0), dtype=numpy.float64) + n = v[0]*v[0] + v[1]*v[1] + if n > 1.0: + v /= math.sqrt(n) # position outside of sphere + else: + v[2] = math.sqrt(1.0 - n) + return v + + +def arcball_constrain_to_axis(point, axis): + """Return sphere point perpendicular to axis.""" + v = numpy.array(point, dtype=numpy.float64, copy=True) + a = numpy.array(axis, dtype=numpy.float64, copy=True) + v -= a * numpy.dot(a, v) # on plane + n = vector_norm(v) + if n > _EPS: + if v[2] < 0.0: + v *= -1.0 + v /= n + return v + if a[2] == 1.0: + return numpy.array([1, 0, 0], dtype=numpy.float64) + return unit_vector([-a[1], a[0], 0]) + + +def arcball_nearest_axis(point, axes): + """Return axis, which arc is nearest to point.""" + point = numpy.array(point, dtype=numpy.float64, copy=False) + nearest = None + mx = -1.0 + for axis in axes: + t = numpy.dot(arcball_constrain_to_axis(point, axis), point) + if t > mx: + nearest = axis + mx = t + return nearest + + +# epsilon for testing whether a number is close to zero +_EPS = numpy.finfo(float).eps * 4.0 + +# axis sequences for Euler angles +_NEXT_AXIS = [1, 2, 0, 1] + +# map axes strings to/from tuples of inner axis, parity, repetition, frame +_AXES2TUPLE = { + 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), + 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), + 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), + 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), + 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), + 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), + 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), + 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} + +_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) + +# helper functions + +def vector_norm(data, axis=None, out=None): + """Return length, i.e. eucledian norm, of ndarray along axis. + + >>> v = numpy.random.random(3) + >>> n = vector_norm(v) + >>> numpy.allclose(n, numpy.linalg.norm(v)) + True + >>> v = numpy.random.rand(6, 5, 3) + >>> n = vector_norm(v, axis=-1) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) + True + >>> n = vector_norm(v, axis=1) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) + True + >>> v = numpy.random.rand(5, 4, 3) + >>> n = numpy.empty((5, 3), dtype=numpy.float64) + >>> vector_norm(v, axis=1, out=n) + >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) + True + >>> vector_norm([]) + 0.0 + >>> vector_norm([1.0]) + 1.0 + + """ + data = numpy.array(data, dtype=numpy.float64, copy=True) + if out is None: + if data.ndim == 1: + return math.sqrt(numpy.dot(data, data)) + data *= data + out = numpy.atleast_1d(numpy.sum(data, axis=axis)) + numpy.sqrt(out, out) + return out + else: + data *= data + numpy.sum(data, axis=axis, out=out) + numpy.sqrt(out, out) + + +def unit_vector(data, axis=None, out=None): + """Return ndarray normalized by length, i.e. eucledian norm, along axis. + + >>> v0 = numpy.random.random(3) + >>> v1 = unit_vector(v0) + >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) + True + >>> v0 = numpy.random.rand(5, 4, 3) + >>> v1 = unit_vector(v0, axis=-1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) + >>> numpy.allclose(v1, v2) + True + >>> v1 = unit_vector(v0, axis=1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) + >>> numpy.allclose(v1, v2) + True + >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float64) + >>> unit_vector(v0, axis=1, out=v1) + >>> numpy.allclose(v1, v2) + True + >>> list(unit_vector([])) + [] + >>> list(unit_vector([1.0])) + [1.0] + + """ + if out is None: + data = numpy.array(data, dtype=numpy.float64, copy=True) + if data.ndim == 1: + data /= math.sqrt(numpy.dot(data, data)) + return data + else: + if out is not data: + out[:] = numpy.array(data, copy=False) + data = out + length = numpy.atleast_1d(numpy.sum(data*data, axis)) + numpy.sqrt(length, length) + if axis is not None: + length = numpy.expand_dims(length, axis) + data /= length + if out is None: + return data + + +def random_vector(size): + """Return array of random doubles in the half-open interval [0.0, 1.0). + + >>> v = random_vector(10000) + >>> numpy.all(v >= 0.0) and numpy.all(v < 1.0) + True + >>> v0 = random_vector(10) + >>> v1 = random_vector(10) + >>> numpy.any(v0 == v1) + False + + """ + return numpy.random.random(size) + + +def inverse_matrix(matrix): + """Return inverse of square transformation matrix. + + >>> M0 = random_rotation_matrix() + >>> M1 = inverse_matrix(M0.T) + >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) + True + >>> for size in range(1, 7): + ... M0 = numpy.random.rand(size, size) + ... M1 = inverse_matrix(M0) + ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print size + + """ + return numpy.linalg.inv(matrix) + + +def concatenate_matrices(*matrices): + """Return concatenation of series of transformation matrices. + + >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 + >>> numpy.allclose(M, concatenate_matrices(M)) + True + >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) + True + + """ + M = numpy.identity(4) + for i in matrices: + M = numpy.dot(M, i) + return M + + +def is_same_transform(matrix0, matrix1): + """Return True if two matrices perform same transformation. + + >>> is_same_transform(numpy.identity(4), numpy.identity(4)) + True + >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) + False + + """ + matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) + matrix0 /= matrix0[3, 3] + matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) + matrix1 /= matrix1[3, 3] + return numpy.allclose(matrix0, matrix1) + + +def _import_module(module_name, warn=True, prefix='_py_', ignore='_'): + """Try import all public attributes from module into global namespace. + + Existing attributes with name clashes are renamed with prefix. + Attributes starting with underscore are ignored by default. + + Return True on successful import. + + """ + try: + module = __import__(module_name) + except ImportError: + if warn: + warnings.warn("Failed to import module " + module_name) + else: + for attr in dir(module): + if ignore and attr.startswith(ignore): + continue + if prefix: + if attr in globals(): + globals()[prefix + attr] = globals()[attr] + elif warn: + warnings.warn("No Python implementation of " + attr) + globals()[attr] = getattr(module, attr) + return True diff --git a/src/rpg_vikit/vikit_ros/CMakeLists.txt b/src/rpg_vikit/vikit_ros/CMakeLists.txt new file mode 100644 index 0000000..cd903fd --- /dev/null +++ b/src/rpg_vikit/vikit_ros/CMakeLists.txt @@ -0,0 +1,118 @@ +cmake_minimum_required(VERSION 3.5) +project(vikit_ros) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +add_compile_options(-std=c++17) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3") +add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -pthread -fexceptions") +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +message("Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}") +if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)") + include(ProcessorCount) + ProcessorCount(N) + message("Processor number: ${N}") + if(N GREATER 4) + add_definitions(-DMP_EN) + add_definitions(-DMP_PROC_NUM=3) + message("Cores for MP: 3") + elseif(N GREATER 3) + add_definitions(-DMP_EN) + add_definitions(-DMP_PROC_NUM=2) + message("Cores for MP: 2") + else() + add_definitions(-DMP_PROC_NUM=1) + endif() +else() + add_definitions(-DMP_PROC_NUM=1) +endif() + +find_package(OpenMP QUIET) +if(OpenMP_FOUND) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +endif() + +# Find packages +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(vikit_common REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(OpenCV REQUIRED) +find_package(Eigen3 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(tf2_geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +set(dependencies + rclcpp + visualization_msgs + tf2_ros + tf2 + Eigen3 + tf2_geometry_msgs + vikit_common +) + +ament_export_dependencies(rosidl_default_runtime) +ament_export_dependencies(${dependencies}) + +# Link libraries +list(APPEND SOURCEFILES + src/output_helper.cpp + src/camera_loader.cpp +) + +add_library(${PROJECT_NAME} SHARED ${SOURCEFILES}) +# Modify target_include_directories to include the project directory directly +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${CMAKE_CURRENT_SOURCE_DIR} # Add this line to include the project root directory + ${Sophus_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +target_link_libraries(${PROJECT_NAME} + ${cpp_typesupport_target} +) + +ament_target_dependencies(${PROJECT_NAME} ${dependencies}) + +# Install headers directly to the include directory without the project name prefix +install(DIRECTORY include/ + DESTINATION include + COMPONENT ${PROJECT_NAME} +) + +# Install library +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +# Export the include directory for the project +set_target_properties(${PROJECT_NAME} PROPERTIES + PUBLIC_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/include" +) + +# Ensure the include directory is correctly set for downstream packages +ament_export_include_directories(include) + +ament_package() \ No newline at end of file diff --git a/src/rpg_vikit/vikit_ros/include/vikit/camera_loader.h b/src/rpg_vikit/vikit_ros/include/vikit/camera_loader.h new file mode 100644 index 0000000..dafe81a --- /dev/null +++ b/src/rpg_vikit/vikit_ros/include/vikit/camera_loader.h @@ -0,0 +1,32 @@ +/* + * camera_loader.h + * + * Created on: Feb 11, 2014 + * Author: cforster + * Update on: Feb 01, 2025 + * Author: StrangeFly + */ + +#ifndef VIKIT_CAMERA_LOADER_H_ +#define VIKIT_CAMERA_LOADER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { +namespace camera_loader { + +/// Load from ROS Namespace +bool loadFromRosNs(const rclcpp::Node::SharedPtr & nh, const std::string& ns, vk::AbstractCamera*& cam); +bool loadFromRosNs(const rclcpp::Node::SharedPtr & nh, const std::string& ns, std::vector& cam_list); + +} // namespace camera_loader +} // namespace vk + +#endif // VIKIT_CAMERA_LOADER_H_ diff --git a/src/rpg_vikit/vikit_ros/include/vikit/output_helper.h b/src/rpg_vikit/vikit_ros/include/vikit/output_helper.h new file mode 100644 index 0000000..582c142 --- /dev/null +++ b/src/rpg_vikit/vikit_ros/include/vikit/output_helper.h @@ -0,0 +1,102 @@ +/* + * output_helper.h + * + * Created on: Jan 20, 2013 + * Author: cforster + * Update on: Feb 01, 2025 + * Author: StrangeFly + */ + +#ifndef VIKIT_OUTPUT_HELPER_H_ +#define VIKIT_OUTPUT_HELPER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { +namespace output_helper { + +using namespace std; +using namespace Eigen; + +void +publishTfTransform (const Sophus::SE3& T, const rclcpp::Time& stamp, + const string& frame_id, const string& child_frame_id, + tf2_ros::TransformBroadcaster& br); + +void +publishPointMarker (const rclcpp::Publisher::SharedPtr pub, + const Vector3d& pos, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + rclcpp::Duration lifetime = rclcpp::Duration(0,0)); + +void +publishLineMarker (const rclcpp::Publisher::SharedPtr pub, + const Vector3d& start, + const Vector3d& end, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + rclcpp::Duration lifetime = rclcpp::Duration(0,0)); + +void +publishArrowMarker (const rclcpp::Publisher::SharedPtr pub, + const Vector3d& pos, + const Vector3d& dir, + double scale, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color); + +void +publishHexacopterMarker (const rclcpp::Publisher::SharedPtr pub, + const string& frame_id, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color); + +void +publishCameraMarker(const rclcpp::Publisher::SharedPtr pub, + const string& frame_id, + const string& ns, + const rclcpp::Time& timestamp, + int id, + double marker_scale, + const Vector3d& color); +void +publishFrameMarker (const rclcpp::Publisher::SharedPtr pub, + const Matrix3d& rot, + const Vector3d& pos, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + rclcpp::Duration lifetime = rclcpp::Duration(0,0)); + + +} // namespace output_helper +} // namespace vk + + +#endif /* VIKIT_OUTPUT_HELPER_H_ */ diff --git a/src/rpg_vikit/vikit_ros/include/vikit/params_helper.h b/src/rpg_vikit/vikit_ros/include/vikit/params_helper.h new file mode 100644 index 0000000..9e12931 --- /dev/null +++ b/src/rpg_vikit/vikit_ros/include/vikit/params_helper.h @@ -0,0 +1,148 @@ +/* + * ros_params_helper.h + * + * Created on: Feb 22, 2013 + * Author: cforster + * Update on: Feb 01, 2025 + * Author: StrangeFly + * + * from libpointmatcher_ros + */ + +#ifndef ROS_PARAMS_HELPER_H_ +#define ROS_PARAMS_HELPER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +template +T getParam(const std::string& node, const std::string& name, const T& defaultValue) { + // Keep popen as a absolute fallback if no Node handle is available + // But this is very slow! + try { + std::string full_node = node; + if (!full_node.empty() && full_node[0] != '/') full_node = "/" + full_node; + + std::string command = "ros2 param get " + full_node + " " + name + " 2>/dev/null"; + std::unique_ptr pipe(popen(command.c_str(), "r"), pclose); + if (!pipe) { + return defaultValue; + } + std::ostringstream resultStream; + char buffer[128]; + while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) { + resultStream << buffer; + } + std::string result = resultStream.str(); + + size_t pos = result.find(": "); + if (pos != std::string::npos) { + std::string valueStr = result.substr(pos + 2); + if (!valueStr.empty() && valueStr.back() == '\n') valueStr.pop_back(); + + if (!valueStr.empty()) { + std::stringstream ss(valueStr); + T value; + if (ss >> value) { + return value; + } + } + } + return defaultValue; + } catch (...) { + return defaultValue; + } +} + +template +T getParam(const std::string& node, const std::string& name) { + // override function could has no default value + if constexpr (std::is_same_v) { + return getParam(node, name, ""); // if std::string, default value is "" + } else if constexpr (std::is_integral_v) { + return getParam(node, name, 0); // if int. defalt 0 + } else if constexpr (std::is_floating_point_v) { + return getParam(node, name, 0.0); // if float，default 0.0 + } else { + throw std::runtime_error("Unsupported type for getParam without default value."); + } +} + +inline +bool hasParam(const rclcpp::Node::SharedPtr &nh, const std::string& name) +{ + return nh->has_parameter(name); +} + +template +T getParam(const rclcpp::Node::SharedPtr &nh, const std::string& name, const T& defaultValue) +{ + T v; + if(nh->get_parameter(name, v)) + { + return v; + } + + // If not found locally, it might be a prefix for a remote node or a local param not yet declared + if (!nh->has_parameter(name)) { + nh->declare_parameter(name, defaultValue); + if (nh->get_parameter(name, v)) return v; + } + + return defaultValue; +} + +// New function for cross-node parameter access +template +T getRemoteParam(const rclcpp::Node::SharedPtr &nh, const std::string& remote_node_name, const std::string& param_name, const T& defaultValue) +{ + // Try local first just in case + std::string full_name = remote_node_name + "." + param_name; // common convention + T v; + if (nh->get_parameter(full_name, v)) return v; + if (nh->get_parameter(param_name, v)) return v; + + // Use SyncParametersClient for high performance cross-node access + try { + auto parameters_client = std::make_shared(nh, remote_node_name); + // Wait briefly for the service to be available + if (parameters_client->wait_for_service(std::chrono::milliseconds(100))) { + auto values = parameters_client->get_parameters({param_name}); + if (!values.empty() && values[0].get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { + return values[0].get_value(); + } + } + } catch (...) { + // Fallback to popen if client fails + return getParam(remote_node_name, param_name, defaultValue); + } + + return defaultValue; +} + +template +T getParam(const rclcpp::Node::SharedPtr &nh, const std::string& name) +{ + T v; + if (nh->get_parameter(name, v)) { + RCLCPP_INFO_STREAM(nh->get_logger(), "Found parameter: " << name << ", value: " << v); + return v; + } + + // If not found, try to declare it (this might be useful if the parameter is expected to be there) + // or just return a default constructed T. + RCLCPP_ERROR_STREAM(nh->get_logger(), "Cannot find value for parameter: " << name << ". Returning default."); + return T(); +} + +} // namespace vk + +#endif // ROS_PARAMS_HELPER_H_ diff --git a/src/rpg_vikit/vikit_ros/package.xml b/src/rpg_vikit/vikit_ros/package.xml new file mode 100644 index 0000000..7e1cdff --- /dev/null +++ b/src/rpg_vikit/vikit_ros/package.xml @@ -0,0 +1,38 @@ + + + vikit_ros + 0.0.0 + + The vikit_ros package + + + cforster + + + GPLv3 + + + + + + + + ament_cmake + rosidl_default_generators + rclcpp + cmake_modules + vikit_common + tf2 + visualization_msgs + + rosidl_default_runtime + rosidl_interface_packages + + + + + + + ament_cmake + + \ No newline at end of file diff --git a/src/rpg_vikit/vikit_ros/src/camera_loader.cpp b/src/rpg_vikit/vikit_ros/src/camera_loader.cpp new file mode 100644 index 0000000..f6fbf71 --- /dev/null +++ b/src/rpg_vikit/vikit_ros/src/camera_loader.cpp @@ -0,0 +1,144 @@ +/* + * camera_loader.h + * + * Created on: Feb 11, 2014 + * Author: cforster + * Update on: Feb 01, 2025 + * Author: StrangeFly + */ + +#include + +namespace vk { +namespace camera_loader { + +/// Load from ROS Namespace +bool loadFromRosNs(const rclcpp::Node::SharedPtr & nh, const std::string& ns, vk::AbstractCamera*& cam) +{ + bool res = true; + std::string cam_model(getRemoteParam(nh, ns, "cam_model", "")); + if(cam_model == "Ocam") + { + cam = new vk::OmniCamera(getRemoteParam(nh, ns, "cam_calib_file", "")); + } + else if(cam_model == "Pinhole") + { + cam = new vk::PinholeCamera( + getRemoteParam(nh, ns, "cam_width", 0), + getRemoteParam(nh, ns, "cam_height", 0), + getRemoteParam(nh, ns, "scale", 1.0), + getRemoteParam(nh, ns, "cam_fx", 0.0), + getRemoteParam(nh, ns, "cam_fy", 0.0), + getRemoteParam(nh, ns, "cam_cx", 0.0), + getRemoteParam(nh, ns, "cam_cy", 0.0), + getRemoteParam(nh, ns, "cam_d0", 0.0), + getRemoteParam(nh, ns, "cam_d1", 0.0), + getRemoteParam(nh, ns, "cam_d2", 0.0), + getRemoteParam(nh, ns, "cam_d3", 0.0)); + } + else if(cam_model == "EquidistantCamera") + { + cam = new vk::EquidistantCamera( + getParam(nh, ns+"/cam_width"), + getParam(nh, ns+"/cam_height"), + getParam(nh, ns+"/scale", 1.0), + getParam(nh, ns+"/cam_fx"), + getParam(nh, ns+"/cam_fy"), + getParam(nh, ns+"/cam_cx"), + getParam(nh, ns+"/cam_cy"), + getParam(nh, ns+"/k1", 0.0), + getParam(nh, ns+"/k2", 0.0), + getParam(nh, ns+"/k3", 0.0), + getParam(nh, ns+"/k4", 0.0)); + } + else if(cam_model == "PolynomialCamera") + { + cam = new vk::PolynomialCamera( + getParam(nh, ns+"/cam_width"), + getParam(nh, ns+"/cam_height"), + // getParam(nh, ns+"/scale", 1.0), + getParam(nh, ns+"/cam_fx"), + getParam(nh, ns+"/cam_fy"), + getParam(nh, ns+"/cam_cx"), + getParam(nh, ns+"/cam_cy"), + getParam(nh, ns+"/cam_skew"), + getParam(nh, ns+"/k2", 0.0), + getParam(nh, ns+"/k3", 0.0), + getParam(nh, ns+"/k4", 0.0), + getParam(nh, ns+"/k5", 0.0), + getParam(nh, ns+"/k6", 0.0), + getParam(nh, ns+"/k7", 0.0)); + } + else if(cam_model == "ATAN") + { + cam = new vk::ATANCamera( + getParam(nh, ns+"/cam_width"), + getParam(nh, ns+"/cam_height"), + getParam(nh, ns+"/cam_fx"), + getParam(nh, ns+"/cam_fy"), + getParam(nh, ns+"/cam_cx"), + getParam(nh, ns+"/cam_cy"), + getParam(nh, ns+"/cam_d0")); + } + else + { + cam = NULL; + res = false; + } + return res; +} + +bool loadFromRosNs(const rclcpp::Node::SharedPtr & nh, const std::string& ns, std::vector& cam_list) +{ + bool res = true; + std::string cam_model(getParam(nh, ns+"/cam_model", "Pinhole")); + int cam_num = getParam(nh, ns+"/cam_num"); + for (int i = 0; i < cam_num; i ++) + { + std::string cam_ns = ns + "/cam_" + std::to_string(i); + std::string cam_model(getParam(nh, cam_ns+"/cam_model")); + if(cam_model == "FishPoly") + { + cam_list.push_back(new vk::PolynomialCamera( + getParam(nh, cam_ns+"/image_width"), + getParam(nh, cam_ns+"/image_height"), + // getParam(nh, cam_ns+"/scale", 1.0), + getParam(nh, cam_ns+"/A11"), // cam_fx + getParam(nh, cam_ns+"/A22"), // cam_fy + getParam(nh, cam_ns+"/u0"), // cam_cx + getParam(nh, cam_ns+"/v0"), // cam_cy + getParam(nh, cam_ns+"/A12"), // cam_skew + getParam(nh, cam_ns+"/k2", 0.0), + getParam(nh, cam_ns+"/k3", 0.0), + getParam(nh, cam_ns+"/k4", 0.0), + getParam(nh, cam_ns+"/k5", 0.0), + getParam(nh, cam_ns+"/k6", 0.0), + getParam(nh, cam_ns+"/k7", 0.0))); + } + else if(cam_model == "Pinhole") + { + cam_list.push_back(new vk::PinholeCamera( + getParam(nh, ns+"/cam_width"), + getParam(nh, ns+"/cam_height"), + getParam(nh, ns+"/scale", 1.0), + getParam(nh, ns+"/cam_fx"), + getParam(nh, ns+"/cam_fy"), + getParam(nh, ns+"/cam_cx"), + getParam(nh, ns+"/cam_cy"), + getParam(nh, ns+"/cam_d0", 0.0), + getParam(nh, ns+"/cam_d1", 0.0), + getParam(nh, ns+"/cam_d2", 0.0), + getParam(nh, ns+"/cam_d3", 0.0))); + } + else + { + // cam_list.clear(); + res = false; + } + } + + return res; +} + +} // namespace camera_loader +} // namespace vk diff --git a/src/rpg_vikit/vikit_ros/src/output_helper.cpp b/src/rpg_vikit/vikit_ros/src/output_helper.cpp new file mode 100644 index 0000000..7210cd1 --- /dev/null +++ b/src/rpg_vikit/vikit_ros/src/output_helper.cpp @@ -0,0 +1,424 @@ +/* + * output_helper.cpp + * + * Created on: Jan 20, 2013 + * Author: chrigi + * Update on: Feb 01, 2025 + * Author: StrangeFly + */ + +#include +#include + +namespace vk { +namespace output_helper { + +void +publishTfTransform(const Sophus::SE3& T, const rclcpp::Time& stamp, + const std::string& frame_id, const std::string& child_frame_id, + tf2_ros::TransformBroadcaster& br) +{ + geometry_msgs::msg::TransformStamped transform_msg; + + transform_msg.header.stamp = stamp; + transform_msg.header.frame_id = frame_id; + transform_msg.child_frame_id = child_frame_id; + + transform_msg.transform.translation.x = T.translation().x(); + transform_msg.transform.translation.y = T.translation().y(); + transform_msg.transform.translation.z = T.translation().z(); + + Eigen::Matrix3d rotationMatrix = T.rotationMatrix(); + Eigen::Quaterniond q(rotationMatrix); + transform_msg.transform.rotation.x = q.x(); + transform_msg.transform.rotation.y = q.y(); + transform_msg.transform.rotation.z = q.z(); + transform_msg.transform.rotation.w = q.w(); + + br.sendTransform(transform_msg); +} + +void +publishPointMarker(const rclcpp::Publisher::SharedPtr pub, + const Vector3d& pos, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + rclcpp::Duration lifetime) +{ + visualization_msgs::msg::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::msg::Marker::CUBE; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.scale.y = marker_scale; + msg.scale.z = marker_scale; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.lifetime = lifetime; + msg.pose.position.x = pos[0]; + msg.pose.position.y = pos[1]; + msg.pose.position.z = pos[2]; + pub->publish(msg); +} + +void +publishLineMarker(const rclcpp::Publisher::SharedPtr pub, + const Vector3d& start, + const Vector3d& end, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color, + rclcpp::Duration lifetime) +{ + visualization_msgs::msg::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::msg::Marker::LINE_STRIP; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.points.resize(2); + msg.lifetime = lifetime; + msg.points[0].x = start[0]; + msg.points[0].y = start[1]; + msg.points[0].z = start[2]; + msg.points[1].x = end[0]; + msg.points[1].y = end[1]; + msg.points[1].z = end[2]; + pub->publish(msg); +} + + +void +publishArrowMarker(const rclcpp::Publisher::SharedPtr pub, + const Vector3d& pos, + const Vector3d& dir, + double scale, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color) +{ + visualization_msgs::msg::Marker msg; + msg.header.frame_id = "/world"; + msg.header.stamp = timestamp; + msg.ns = ns; + msg.id = id; + msg.type = visualization_msgs::msg::Marker::ARROW; + msg.action = action; // 0 = add/modify + msg.scale.x = marker_scale; + msg.scale.y = marker_scale*0.35; + msg.scale.z = 0.0; + msg.color.a = 1.0; + msg.color.r = color[0]; + msg.color.g = color[1]; + msg.color.b = color[2]; + msg.points.resize(2); + msg.points[0].x = pos[0]; + msg.points[0].y = pos[1]; + msg.points[0].z = pos[2]; + msg.points[1].x = pos[0] + scale*dir[0]; + msg.points[1].y = pos[1] + scale*dir[1]; + msg.points[1].z = pos[2] + scale*dir[2]; + pub->publish(msg); +} + +void +publishHexacopterMarker(const rclcpp::Publisher::SharedPtr pub, + const string& frame_id, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + const Vector3d& color) +{ + /* + * Function by Markus Achtelik from libsfly_viz. + * Thank you. + */ + const double sqrt2_2 = sqrt(2) / 2; + + visualization_msgs::msg::Marker marker; + + // the marker will be displayed in frame_id + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.action = 0; + marker.id = id; + + // make rotors + marker.type = visualization_msgs::msg::Marker::CYLINDER; + marker.scale.x = 0.2*marker_scale; + marker.scale.y = 0.2*marker_scale; + marker.scale.z = 0.01*marker_scale; + marker.color.r = 0.4; + marker.color.g = 0.4; + marker.color.b = 0.4; + marker.color.a = 0.8; + marker.pose.position.z = 0; + + // front left/right + marker.pose.position.x = 0.19*marker_scale; + marker.pose.position.y = 0.11*marker_scale; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = 0.19*marker_scale; + marker.pose.position.y = -0.11*marker_scale; + marker.id--; + pub->publish(marker); + + // left/right + marker.pose.position.x = 0; + marker.pose.position.y = 0.22*marker_scale; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = 0; + marker.pose.position.y = -0.22*marker_scale; + marker.id--; + pub->publish(marker); + + // back left/right + marker.pose.position.x = -0.19*marker_scale; + marker.pose.position.y = 0.11*marker_scale; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = -0.19*marker_scale; + marker.pose.position.y = -0.11*marker_scale; + marker.id--; + pub->publish(marker); + + // make arms + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale.x = 0.44*marker_scale; + marker.scale.y = 0.02*marker_scale; + marker.scale.z = 0.01*marker_scale; + marker.color.r = color[0]; + marker.color.g = color[1]; + marker.color.b = color[2]; + marker.color.a = 1; + + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = -0.015*marker_scale; + marker.pose.orientation.x = 0; + marker.pose.orientation.y = 0; + + marker.pose.orientation.w = sqrt2_2; + marker.pose.orientation.z = sqrt2_2; + marker.id--; + pub->publish(marker); + + // 30 deg rotation 0.9659 0 0 0.2588 + marker.pose.orientation.w = 0.9659; + marker.pose.orientation.z = 0.2588; + marker.id--; + pub->publish(marker); + + marker.pose.orientation.w = 0.9659; + marker.pose.orientation.z = -0.2588; + marker.id--; + pub->publish(marker); +} + +void +publishCameraMarker(const rclcpp::Publisher::SharedPtr pub, + const string& frame_id, + const string& ns, + const rclcpp::Time& timestamp, + int id, + double marker_scale, + const Vector3d& color) +{ + /* + * draw a pyramid as the camera marker + */ + const double sqrt2_2 = sqrt(2) / 2; + + visualization_msgs::msg::Marker marker; + + // the marker will be displayed in frame_id + marker.header.frame_id = frame_id; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.action = 0; + marker.id = id; + + // make rectangles as frame + double r_w = 1.0; + double z_plane = (r_w / 2.0)*marker_scale; + marker.pose.position.x = 0; + marker.pose.position.y = (r_w / 4.0) *marker_scale; + marker.pose.position.z = z_plane; + + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.scale.x = r_w*marker_scale; + marker.scale.y = 0.04*marker_scale; + marker.scale.z = 0.04*marker_scale; + marker.color.r = color[0]; + marker.color.g = color[1]; + marker.color.b = color[2]; + marker.color.a = 1; + + marker.pose.orientation.x = 0; + marker.pose.orientation.y = 0; + marker.pose.orientation.z = 0; + marker.pose.orientation.w = 1; + marker.id--; + pub->publish(marker); + marker.pose.position.y = -(r_w/ 4.0)*marker_scale; + marker.id--; + pub->publish(marker); + + marker.scale.x = (r_w/2.0)*marker_scale; + marker.pose.position.x = (r_w / 2.0) *marker_scale; + marker.pose.position.y = 0; + marker.pose.orientation.w = sqrt2_2; + marker.pose.orientation.z = sqrt2_2; + marker.id--; + pub->publish(marker); + marker.pose.position.x = -(r_w / 2.0) *marker_scale; + marker.id--; + pub->publish(marker); + + // make pyramid edges + marker.scale.x = (3.0*r_w/4.0)*marker_scale; + marker.pose.position.z = 0.5*z_plane; + + marker.pose.position.x = (r_w / 4.0) *marker_scale; + marker.pose.position.y = (r_w / 8.0) *marker_scale; + // 0.08198092, -0.34727674, 0.21462883, 0.9091823 + marker.pose.orientation.x = 0.08198092; + marker.pose.orientation.y = -0.34727674; + marker.pose.orientation.z = 0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = -(r_w / 4.0) *marker_scale; + marker.pose.position.y = (r_w / 8.0) *marker_scale; + // -0.27395078, -0.22863284, 0.9091823 , 0.21462883 + marker.pose.orientation.x = 0.08198092; + marker.pose.orientation.y = 0.34727674; + marker.pose.orientation.z = -0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = -(r_w / 4.0) *marker_scale; + marker.pose.position.y = -(r_w / 8.0) *marker_scale; + // -0.08198092, 0.34727674, 0.21462883, 0.9091823 + marker.pose.orientation.x = -0.08198092; + marker.pose.orientation.y = 0.34727674; + marker.pose.orientation.z = 0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub->publish(marker); + + marker.pose.position.x = (r_w / 4.0) *marker_scale; + marker.pose.position.y = -(r_w / 8.0) *marker_scale; + // -0.08198092, -0.34727674, -0.21462883, 0.9091823 + marker.pose.orientation.x = -0.08198092; + marker.pose.orientation.y = -0.34727674; + marker.pose.orientation.z = -0.21462883; + marker.pose.orientation.w = 0.9091823; + marker.id--; + pub->publish(marker); +} + +void publishFrameMarker(const rclcpp::Publisher::SharedPtr pub, + const Matrix3d& rot, + const Vector3d& pos, + const string& ns, + const rclcpp::Time& timestamp, + int id, + int action, + double marker_scale, + rclcpp::Duration lifetime) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "/world"; + marker.header.stamp = timestamp; + marker.ns = ns; + marker.id = id++; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = action; // 0 = add/modify + marker.points.reserve(2); + geometry_msgs::msg::Point point; + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 2)); // Draw arrow in z-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 2)); // Draw arrow in z-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 2)); // Draw arrow in z-direction + marker.points.push_back(point); + marker.scale.x = 0.5*marker_scale; + marker.scale.y = 0.5*marker_scale; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.lifetime = lifetime; + pub->publish(marker); + + marker.id = id++; + marker.points.clear(); + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 0)); // Draw arrow in x-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 0)); // Draw arrow in x-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 0)); // Draw arrow in x-direction + marker.points.push_back(point); + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.lifetime = lifetime; + pub->publish(marker); + + marker.id = id++; + marker.points.clear(); + point.x = static_cast(pos.x()); + point.y = static_cast(pos.y()); + point.z = static_cast(pos.z()); + marker.points.push_back(point); + point.x = static_cast(pos.x() + marker_scale*rot(0, 1)); // Draw arrow in y-direction + point.y = static_cast(pos.y() + marker_scale*rot(1, 1)); // Draw arrow in y-direction + point.z = static_cast(pos.z() + marker_scale*rot(2, 1)); // Draw arrow in y-direction + marker.points.push_back(point); + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.lifetime = lifetime; + pub->publish(marker); +} + +} // namespace output_helper +} // namespace vk + +