robofish/RC2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

RC2025启动！

Browse Source
This commit is contained in:
robofish 2025-07-07 02:34:34 +08:00
commit d3c6cf5c0a
550 changed files with 1035984 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
.gitignore vendored Normal file
View File

@ -0,0 +1,65 @@
# ---> C++
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
# ---> ROS2
install/
log/
build/
# Ignore generated docs
*.dox
*.wikidoc
# eclipse stuff
.project
.cproject
# qcreator stuff
CMakeLists.txt.user
srv/_*.py
*.pcd
*.pyc
qtcreator-*
*.user
*~
# Emacs
.#*
# Colcon custom files
COLCON_IGNORE
AMENT_IGNORE

0
README.md Normal file
View File

1
build.sh Normal file
View File

@ -0,0 +1 @@
colcon build

19
nav.sh Normal file
View File

@ -0,0 +1,19 @@
source install/setup.bash
commands=(
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_aim.py"
"ros2 launch rm_nav_bringup bringup_real.launch.py \
world:=RC2025 \
mode:=nav \
lio:=fastlio \
localization:=icp \
lio_rviz:=false \
nav_rviz:=true"
"ros2 launch rm_simpal_move simple_move.launch.py"
"ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.56, y: 3.960, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
)
for cmd in "${commands[@]}"; do
gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
sleep 0.5
done

10
src/rm_driver/livox_ros_driver2/.gitignore vendored Normal file
View File

@ -0,0 +1,10 @@
devel/
build/
install/
log/
.vscode/
__pycache__/
.catkin_workspace
*.gv
*.pdf

21
src/rm_driver/livox_ros_driver2/LICENSE Normal file
View File

@ -0,0 +1,21 @@
MIT License
Copyright (c) 2023 ziknagXie
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.

329
src/rm_driver/livox_ros_driver2/README.md Normal file
View File

@ -0,0 +1,329 @@
# 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).
## ATTENTION
本仓库为深圳北理莫斯科大学北极熊战队内部修改版，非原版 livox_ros_driver2 。
相较于原版，修改了消息发布机制和 ```MID360_config.json``` 的 ip
```ros2 launch msg_MID360_launch.py``` 时会同时发布 ```CustomMsg``` 和 ```PointCloud2``` 两种类型消息。
**Topic name** | **Type** | **Note**
:------------------------:|:-------------------------------:|:--------------:
/livox/lidar | livox_ros_driver2/msg/CustomMsg | mid360 自定义消息类型
/livox/lidar/pointcloud | sensor_msgs/msg/PointCloud2 | ROS2 点云消息格式
/livox/imu | sensor_msgs/msg/Imu | mid360 机内 imu
## 1. Preparation
### 1.1 OS requirements
* 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 ROS2 Humble
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://gitee.com/SMBU-POLARBEAR/livox_ros_driver2_humble.git
```
**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 ROS2 Humble:
```shell
colcon build --symlink-install
```
### 2.4 Run Livox ROS Driver 2:
```shell
source install/setup.sh
ros2 launch livox_ros_driver2 [launch file]
```
in which,
* **[launch file]** : is the ROS2 launch file you want to use; the 'launch_ROS2' folder contains several launch samples for your reference.
A rviz launch example for HAP LiDAR would be:
```shell
ros2 launch livox_ros_driver2 rviz_HAP_launch.py
```
## 3. Launch file and livox_ros_driver2 internal parameter configuration instructions
### 3.1 Launch file configuration instructions
Launch files of ROS are in the "ws_livox/src/livox_ros_driver2/launch_ROS1" directory and launch files of ROS2 are in the "ws_livox/src/livox_ros_driver2/launch_ROS2" directory. Different launch files have different configuration parameter values and are used in different scenarios:
| launch file name | Description |
| ------------------------- | ------------------------------------------------------------ |
| rviz_HAP.launch | Connect to HAP LiDAR device<br>Publish pointcloud2 format data<br>Autoload rviz |
| msg_HAP.launch | Connect to HAP LiDAR device<br>Publish livox customized pointcloud data|
| rviz_MID360.launch | Connect to MID360 LiDAR device<br>Publish pointcloud2 format data <br>Autoload rviz|
| msg_MID360.launch | Connect to MID360 LiDAR device<br>Publish livox customized pointcloud data |
| rviz_mixed.launch | Connect to HAP and MID360 LiDAR device<br>Publish pointcloud2 format data <br>Autoload rviz|
| msg_mixed.launch | Connect to HAP and MID360 LiDAR device<br>Publish livox customized pointcloud data |
### 3.2 Livox ros driver 2 internal main parameter configuration instructions
All internal parameters of Livox_ros_driver2 are in the launch file. Below are detailed descriptions of the three commonly used parameters :
| Parameter | Detailed description | Default |
| ------------ | ------------------------------------------------------------ | ------- |
| publish_freq | Set the frequency of point cloud publish <br>Floating-point data type, recommended values 5.0, 10.0, 20.0, 50.0, etc. The maximum publish frequency is 100.0 Hz.| 10.0 |
| multi_topic | If the LiDAR device has an independent topic to publish pointcloud data<br>0 -- All LiDAR devices use the same topic to publish pointcloud data<br>1 -- Each LiDAR device has its own topic to publish point cloud data | 0 |
| xfer_format | Set pointcloud format<br>0 -- Livox pointcloud2(PointXYZRTLT) pointcloud format<br>1 -- Livox customized pointcloud format<br>2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS) | 0 |
**Note :**
Other parameters not mentioned in this table are not suggested to be changed unless fully understood.
&ensp;&ensp;&ensp;&ensp;***Livox_ros_driver2 pointcloud data detailed description :***
1. Livox pointcloud2 (PointXYZRTLT) point cloud format, as follows :
```c
float32 x # X axis, unit:m
float32 y # Y axis, unit:m
float32 z # Z axis, unit:m
float32 intensity # the value is reflectivity, 0.0~255.0
uint8 tag # livox tag
uint8 line # laser number in lidar
float64 timestamp # Timestamp of point
```
**Note :**
The number of points in the frame may be different, but each point provides a timestamp.
2. Livox customized data package format, as follows :
```c
std_msgs/Header header # ROS standard message header
uint64 timebase # The time of first point
uint32 point_num # Total number of pointclouds
uint8 lidar_id # Lidar device id number
uint8[3] rsvd # Reserved use
CustomPoint[] points # Pointcloud data
```
&ensp;&ensp;&ensp;&ensp;Customized Point Cloud (CustomPoint) format in the above customized data package :
```c
uint32 offset_time # offset time relative to the base time
float32 x # X axis, unit:m
float32 y # Y axis, unit:m
float32 z # Z axis, unit:m
uint8 reflectivity # reflectivity, 0~255
uint8 tag # livox tag
uint8 line # laser number in lidar
```
3. The standard pointcloud2 (pcl :: PointXYZI) format in the PCL library (only ROS can publish):
&ensp;&ensp;&ensp;&ensp;Please refer to the pcl :: PointXYZI data structure in the point_types.hpp file of the PCL library.
## 4. LiDAR config
LiDAR Configurations (such as ip, port, data type... etc.) can be set via a json-style config file. Config files for single HAP, Mid360 and mixed-LiDARs are in the "config" folder. The parameter naming *'user_config_path'* in launch files indicates such json file path.
1. Follow is a configuration example for HAP LiDAR (located in config/HAP_config.json):
```json
{
"lidar_summary_info" : {
"lidar_type": 8 # protocol type index, please don't revise this value
},
"HAP": {
"device_type" : "HAP",
"lidar_ipaddr": "",
"lidar_net_info" : {
"cmd_data_port": 56000, # command port
"push_msg_port": 0,
"point_data_port": 57000,
"imu_data_port": 58000,
"log_data_port": 59000
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5", # host ip (it can be revised)
"cmd_data_port": 56000,
"push_msg_ip": "",
"push_msg_port": 0,
"point_data_ip": "192.168.1.5", # host ip
"point_data_port": 57000,
"imu_data_ip" : "192.168.1.5", # host ip
"imu_data_port": 58000,
"log_data_ip" : "",
"log_data_port": 59000
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.100", # ip of the LiDAR you want to config
"pcl_data_type" : 1,
"pattern_mode" : 0,
"blind_spot_set" : 50,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
```
The parameter attributes in the above json file are described in the following table :
**LiDAR configuration parameter**
| Parameter | Type | Description | Default |
| :------------------------- | ------- | ------------------------------------------------------------ | --------------- |
| ip | String | Ip of the LiDAR you want to config | 192.168.1.100 |
| pcl_data_type | Int | Choose the resolution of the point cloud data to send<br>1 -- Cartesian coordinate data (32 bits)<br>2 -- Cartesian coordinate data (16 bits) <br>3 --Spherical coordinate data| 1 |
| pattern_mode | Int | Space scan pattern<br>0 -- non-repeating scanning pattern mode<br>1 -- repeating scanning pattern mode <br>2 -- repeating scanning pattern mode (low scanning rate) | 0 |
| blind_spot_set (Only for HAP LiDAR) | Int | Set blind spot<br>Range from 50 cm to 200 cm | 50 |
| extrinsic_parameter | | Set extrinsic parameter<br> The data types of "roll" "picth" "yaw" are float <br> The data types of "x" "y" "z" are int<br> |
For more infomation about the HAP config, please refer to:
[HAP Config File Description](https://github.com/Livox-SDK/Livox-SDK2/wiki/hap-config-file-description)
2. When connecting multiple LiDARs, add objects corresponding to different LiDARs to the "lidar_configs" array. Examples of mixed-LiDARs config file contents are as follows :
```json
{
"lidar_summary_info" : {
"lidar_type": 8 # protocol type index, please don't revise this value
},
"HAP": {
"lidar_net_info" : { # HAP ports, please don't revise these values
"cmd_data_port": 56000, # HAP command port
"push_msg_port": 0,
"point_data_port": 57000,
"imu_data_port": 58000,
"log_data_port": 59000
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5", # host ip
"cmd_data_port": 56000,
"push_msg_ip": "",
"push_msg_port": 0,
"point_data_ip": "192.168.1.5", # host ip
"point_data_port": 57000,
"imu_data_ip" : "192.168.1.5", # host ip
"imu_data_port": 58000,
"log_data_ip" : "",
"log_data_port": 59000
}
},
"MID360": {
"lidar_net_info" : { # Mid360 ports, please don't revise these values
"cmd_data_port": 56100, # Mid360 command port
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5", # host ip
"cmd_data_port": 56101,
"push_msg_ip": "192.168.1.5", # host ip
"push_msg_port": 56201,
"point_data_ip": "192.168.1.5", # host ip
"point_data_port": 56301,
"imu_data_ip" : "192.168.1.5", # host ip
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.100", # ip of the HAP you want to config
"pcl_data_type" : 1,
"pattern_mode" : 0,
"blind_spot_set" : 50,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
},
{
"ip" : "192.168.1.12", # ip of the Mid360 you want to config
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
```
## 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
```

308
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/allocators.h vendored Normal file
View File

@ -0,0 +1,308 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ALLOCATORS_H_
#define RAPIDJSON_ALLOCATORS_H_
#include "rapidjson.h"
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// Allocator
/*! \class rapidjson::Allocator
\brief Concept for allocating, resizing and freeing memory block.
Note that Malloc() and Realloc() are non-static but Free() is static.
So if an allocator need to support Free(), it needs to put its pointer in
the header of memory block.
\code
concept Allocator {
static const bool kNeedFree; //!< Whether this allocator needs to call
Free().
// Allocate a memory block.
// \param size of the memory block in bytes.
// \returns pointer to the memory block.
void* Malloc(size_t size);
// Resize a memory block.
// \param originalPtr The pointer to current memory block. Null pointer is
permitted.
// \param originalSize The current size in bytes. (Design issue: since some
allocator may not book-keep this, explicitly pass to it can save memory.)
// \param newSize the new size in bytes.
void* Realloc(void* originalPtr, size_t originalSize, size_t newSize);
// Free a memory block.
// \param pointer to the memory block. Null pointer is permitted.
static void Free(void *ptr);
};
\endcode
*/
/*! \def RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
\ingroup RAPIDJSON_CONFIG
\brief User-defined kDefaultChunkCapacity definition.
User can define this as any \c size that is a power of 2.
*/
#ifndef RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
#define RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY (64 * 1024)
#endif
///////////////////////////////////////////////////////////////////////////////
// CrtAllocator
//! C-runtime library allocator.
/*! This class is just wrapper for standard C library memory routines.
\note implements Allocator concept
*/
class CrtAllocator {
public:
static const bool kNeedFree = true;
void *Malloc(size_t size) {
if (size) // behavior of malloc(0) is implementation defined.
return std::malloc(size);
else
return NULL; // standardize to returning NULL.
}
void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
(void)originalSize;
if (newSize == 0) {
std::free(originalPtr);
return NULL;
}
return std::realloc(originalPtr, newSize);
}
static void Free(void *ptr) { std::free(ptr); }
};
///////////////////////////////////////////////////////////////////////////////
// MemoryPoolAllocator
//! Default memory allocator used by the parser and DOM.
/*! This allocator allocate memory blocks from pre-allocated memory chunks.
It does not free memory blocks. And Realloc() only allocate new memory.
The memory chunks are allocated by BaseAllocator, which is CrtAllocator by
default.
User may also supply a buffer as the first chunk.
If the user-buffer is full then additional chunks are allocated by
BaseAllocator.
The user-buffer is not deallocated by this allocator.
\tparam BaseAllocator the allocator type for allocating memory chunks.
Default is CrtAllocator. \note implements Allocator concept
*/
template <typename BaseAllocator = CrtAllocator>
class MemoryPoolAllocator {
public:
static const bool kNeedFree =
false; //!< Tell users that no need to call Free() with this allocator.
//!< (concept Allocator)
//! Constructor with chunkSize.
/*! \param chunkSize The size of memory chunk. The default is
kDefaultChunkSize. \param baseAllocator The allocator for allocating memory
chunks.
*/
MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity,
BaseAllocator *baseAllocator = 0)
: chunkHead_(0),
chunk_capacity_(chunkSize),
userBuffer_(0),
baseAllocator_(baseAllocator),
ownBaseAllocator_(0) {}
//! Constructor with user-supplied buffer.
/*! The user buffer will be used firstly. When it is full, memory pool
allocates new chunk with chunk size.
The user buffer will not be deallocated when this allocator is destructed.
\param buffer User supplied buffer.
\param size Size of the buffer in bytes. It must at least larger than
sizeof(ChunkHeader). \param chunkSize The size of memory chunk. The default
is kDefaultChunkSize. \param baseAllocator The allocator for allocating
memory chunks.
*/
MemoryPoolAllocator(void *buffer, size_t size,
size_t chunkSize = kDefaultChunkCapacity,
BaseAllocator *baseAllocator = 0)
: chunkHead_(0),
chunk_capacity_(chunkSize),
userBuffer_(buffer),
baseAllocator_(baseAllocator),
ownBaseAllocator_(0) {
RAPIDJSON_ASSERT(buffer != 0);
RAPIDJSON_ASSERT(size > sizeof(ChunkHeader));
chunkHead_ = reinterpret_cast<ChunkHeader *>(buffer);
chunkHead_->capacity = size - sizeof(ChunkHeader);
chunkHead_->size = 0;
chunkHead_->next = 0;
}
//! Destructor.
/*! This deallocates all memory chunks, excluding the user-supplied buffer.
*/
~MemoryPoolAllocator() {
Clear();
RAPIDJSON_DELETE(ownBaseAllocator_);
}
//! Deallocates all memory chunks, excluding the user-supplied buffer.
void Clear() {
while (chunkHead_ && chunkHead_ != userBuffer_) {
ChunkHeader *next = chunkHead_->next;
baseAllocator_->Free(chunkHead_);
chunkHead_ = next;
}
if (chunkHead_ && chunkHead_ == userBuffer_)
chunkHead_->size = 0; // Clear user buffer
}
//! Computes the total capacity of allocated memory chunks.
/*! \return total capacity in bytes.
*/
size_t Capacity() const {
size_t capacity = 0;
for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
capacity += c->capacity;
return capacity;
}
//! Computes the memory blocks allocated.
/*! \return total used bytes.
*/
size_t Size() const {
size_t size = 0;
for (ChunkHeader *c = chunkHead_; c != 0; c = c->next) size += c->size;
return size;
}
//! Allocates a memory block. (concept Allocator)
void *Malloc(size_t size) {
if (!size) return NULL;
size = RAPIDJSON_ALIGN(size);
if (chunkHead_ == 0 || chunkHead_->size + size > chunkHead_->capacity)
if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
return NULL;
void *buffer = reinterpret_cast<char *>(chunkHead_) +
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
chunkHead_->size += size;
return buffer;
}
//! Resizes a memory block (concept Allocator)
void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
if (originalPtr == 0) return Malloc(newSize);
if (newSize == 0) return NULL;
originalSize = RAPIDJSON_ALIGN(originalSize);
newSize = RAPIDJSON_ALIGN(newSize);
// Do not shrink if new size is smaller than original
if (originalSize >= newSize) return originalPtr;
// Simply expand it if it is the last allocation and there is sufficient
// space
if (originalPtr ==
reinterpret_cast<char *>(chunkHead_) +
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size -
originalSize) {
size_t increment = static_cast<size_t>(newSize - originalSize);
if (chunkHead_->size + increment <= chunkHead_->capacity) {
chunkHead_->size += increment;
return originalPtr;
}
}
// Realloc process: allocate and copy memory, do not free original buffer.
if (void *newBuffer = Malloc(newSize)) {
if (originalSize) std::memcpy(newBuffer, originalPtr, originalSize);
return newBuffer;
} else
return NULL;
}
//! Frees a memory block (concept Allocator)
static void Free(void *ptr) { (void)ptr; } // Do nothing
private:
//! Copy constructor is not permitted.
MemoryPoolAllocator(const MemoryPoolAllocator &rhs) /* = delete */;
//! Copy assignment operator is not permitted.
MemoryPoolAllocator &operator=(const MemoryPoolAllocator &rhs) /* = delete */;
//! Creates a new chunk.
/*! \param capacity Capacity of the chunk in bytes.
\return true if success.
*/
bool AddChunk(size_t capacity) {
if (!baseAllocator_)
ownBaseAllocator_ = baseAllocator_ = RAPIDJSON_NEW(BaseAllocator)();
if (ChunkHeader *chunk =
reinterpret_cast<ChunkHeader *>(baseAllocator_->Malloc(
RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) {
chunk->capacity = capacity;
chunk->size = 0;
chunk->next = chunkHead_;
chunkHead_ = chunk;
return true;
} else
return false;
}
static const int kDefaultChunkCapacity =
RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY; //!< Default chunk capacity.
//! Chunk header for perpending to each chunk.
/*! Chunks are stored as a singly linked list.
*/
struct ChunkHeader {
size_t capacity; //!< Capacity of the chunk in bytes (excluding the header
//!< itself).
size_t size; //!< Current size of allocated memory in bytes.
ChunkHeader *next; //!< Next chunk in the linked list.
};
ChunkHeader *chunkHead_; //!< Head of the chunk linked-list. Only the head
//!< chunk serves allocation.
size_t chunk_capacity_; //!< The minimum capacity of chunk when they are
//!< allocated.
void *userBuffer_; //!< User supplied buffer.
BaseAllocator
*baseAllocator_; //!< base allocator for allocating memory chunks.
BaseAllocator *ownBaseAllocator_; //!< base allocator created by this object.
};
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_ENCODINGS_H_

81
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/cursorstreamwrapper.h vendored Normal file
View File

@ -0,0 +1,81 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_
#define RAPIDJSON_CURSORSTREAMWRAPPER_H_
#include "stream.h"
#if defined(__GNUC__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
#endif
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Cursor stream wrapper for counting line and column number if error exists.
/*!
\tparam InputStream Any stream that implements Stream Concept
*/
template <typename InputStream, typename Encoding = UTF8<>>
class CursorStreamWrapper : public GenericStreamWrapper<InputStream, Encoding> {
public:
typedef typename Encoding::Ch Ch;
CursorStreamWrapper(InputStream &is)
: GenericStreamWrapper<InputStream, Encoding>(is), line_(1), col_(0) {}
// counting line and column number
Ch Take() {
Ch ch = this->is_.Take();
if (ch == '\n') {
line_++;
col_ = 0;
} else {
col_++;
}
return ch;
}
//! Get the error line number, if error exists.
size_t GetLine() const { return line_; }
//! Get the error column number, if error exists.
size_t GetColumn() const { return col_; }
private:
size_t line_; //!< Current Line
size_t col_; //!< Current Column
};
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_POP
#endif
#if defined(__GNUC__)
RAPIDJSON_DIAG_POP
#endif
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_CURSORSTREAMWRAPPER_H_

3309
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/document.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

407
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/encodedstream.h vendored Normal file
View File

@ -0,0 +1,407 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ENCODEDSTREAM_H_
#define RAPIDJSON_ENCODEDSTREAM_H_
#include "memorystream.h"
#include "stream.h"
#ifdef __GNUC__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
#endif
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Input byte stream wrapper with a statically bound encoding.
/*!
\tparam Encoding The interpretation of encoding of the stream. Either UTF8,
UTF16LE, UTF16BE, UTF32LE, UTF32BE. \tparam InputByteStream Type of input
byte stream. For example, FileReadStream.
*/
template <typename Encoding, typename InputByteStream>
class EncodedInputStream {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
public:
typedef typename Encoding::Ch Ch;
EncodedInputStream(InputByteStream &is) : is_(is) {
current_ = Encoding::TakeBOM(is_);
}
Ch Peek() const { return current_; }
Ch Take() {
Ch c = current_;
current_ = Encoding::Take(is_);
return c;
}
size_t Tell() const { return is_.Tell(); }
// Not implemented
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
EncodedInputStream(const EncodedInputStream &);
EncodedInputStream &operator=(const EncodedInputStream &);
InputByteStream &is_;
Ch current_;
};
//! Specialized for UTF8 MemoryStream.
template <>
class EncodedInputStream<UTF8<>, MemoryStream> {
public:
typedef UTF8<>::Ch Ch;
EncodedInputStream(MemoryStream &is) : is_(is) {
if (static_cast<unsigned char>(is_.Peek()) == 0xEFu) is_.Take();
if (static_cast<unsigned char>(is_.Peek()) == 0xBBu) is_.Take();
if (static_cast<unsigned char>(is_.Peek()) == 0xBFu) is_.Take();
}
Ch Peek() const { return is_.Peek(); }
Ch Take() { return is_.Take(); }
size_t Tell() const { return is_.Tell(); }
// Not implemented
void Put(Ch) {}
void Flush() {}
Ch *PutBegin() { return 0; }
size_t PutEnd(Ch *) { return 0; }
MemoryStream &is_;
private:
EncodedInputStream(const EncodedInputStream &);
EncodedInputStream &operator=(const EncodedInputStream &);
};
//! Output byte stream wrapper with statically bound encoding.
/*!
\tparam Encoding The interpretation of encoding of the stream. Either UTF8,
UTF16LE, UTF16BE, UTF32LE, UTF32BE. \tparam OutputByteStream Type of input
byte stream. For example, FileWriteStream.
*/
template <typename Encoding, typename OutputByteStream>
class EncodedOutputStream {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
public:
typedef typename Encoding::Ch Ch;
EncodedOutputStream(OutputByteStream &os, bool putBOM = true) : os_(os) {
if (putBOM) Encoding::PutBOM(os_);
}
void Put(Ch c) { Encoding::Put(os_, c); }
void Flush() { os_.Flush(); }
// Not implemented
Ch Peek() const {
RAPIDJSON_ASSERT(false);
return 0;
}
Ch Take() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t Tell() const {
RAPIDJSON_ASSERT(false);
return 0;
}
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
EncodedOutputStream(const EncodedOutputStream &);
EncodedOutputStream &operator=(const EncodedOutputStream &);
OutputByteStream &os_;
};
#define RAPIDJSON_ENCODINGS_FUNC(x) \
UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
//! Input stream wrapper with dynamically bound encoding and automatic encoding
//! detection.
/*!
\tparam CharType Type of character for reading.
\tparam InputByteStream type of input byte stream to be wrapped.
*/
template <typename CharType, typename InputByteStream>
class AutoUTFInputStream {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
public:
typedef CharType Ch;
//! Constructor.
/*!
\param is input stream to be wrapped.
\param type UTF encoding type if it is not detected from the stream.
*/
AutoUTFInputStream(InputByteStream &is, UTFType type = kUTF8)
: is_(&is), type_(type), hasBOM_(false) {
RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
DetectType();
static const TakeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Take)};
takeFunc_ = f[type_];
current_ = takeFunc_(*is_);
}
UTFType GetType() const { return type_; }
bool HasBOM() const { return hasBOM_; }
Ch Peek() const { return current_; }
Ch Take() {
Ch c = current_;
current_ = takeFunc_(*is_);
return c;
}
size_t Tell() const { return is_->Tell(); }
// Not implemented
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
AutoUTFInputStream(const AutoUTFInputStream &);
AutoUTFInputStream &operator=(const AutoUTFInputStream &);
// Detect encoding type with BOM or RFC 4627
void DetectType() {
// BOM (Byte Order Mark):
// 00 00 FE FF UTF-32BE
// FF FE 00 00 UTF-32LE
// FE FF UTF-16BE
// FF FE UTF-16LE
// EF BB BF UTF-8
const unsigned char *c =
reinterpret_cast<const unsigned char *>(is_->Peek4());
if (!c) return;
unsigned bom =
static_cast<unsigned>(c[0] | (c[1] << 8) | (c[2] << 16) | (c[3] << 24));
hasBOM_ = false;
if (bom == 0xFFFE0000) {
type_ = kUTF32BE;
hasBOM_ = true;
is_->Take();
is_->Take();
is_->Take();
is_->Take();
} else if (bom == 0x0000FEFF) {
type_ = kUTF32LE;
hasBOM_ = true;
is_->Take();
is_->Take();
is_->Take();
is_->Take();
} else if ((bom & 0xFFFF) == 0xFFFE) {
type_ = kUTF16BE;
hasBOM_ = true;
is_->Take();
is_->Take();
} else if ((bom & 0xFFFF) == 0xFEFF) {
type_ = kUTF16LE;
hasBOM_ = true;
is_->Take();
is_->Take();
} else if ((bom & 0xFFFFFF) == 0xBFBBEF) {
type_ = kUTF8;
hasBOM_ = true;
is_->Take();
is_->Take();
is_->Take();
}
// RFC 4627: Section 3
// "Since the first two characters of a JSON text will always be ASCII
// characters [RFC0020], it is possible to determine whether an octet
// stream is UTF-8, UTF-16 (BE or LE), or UTF-32 (BE or LE) by looking
// at the pattern of nulls in the first four octets."
// 00 00 00 xx UTF-32BE
// 00 xx 00 xx UTF-16BE
// xx 00 00 00 UTF-32LE
// xx 00 xx 00 UTF-16LE
// xx xx xx xx UTF-8
if (!hasBOM_) {
int pattern =
(c[0] ? 1 : 0) | (c[1] ? 2 : 0) | (c[2] ? 4 : 0) | (c[3] ? 8 : 0);
switch (pattern) {
case 0x08:
type_ = kUTF32BE;
break;
case 0x0A:
type_ = kUTF16BE;
break;
case 0x01:
type_ = kUTF32LE;
break;
case 0x05:
type_ = kUTF16LE;
break;
case 0x0F:
type_ = kUTF8;
break;
default:
break; // Use type defined by user.
}
}
// Runtime check whether the size of character type is sufficient. It only
// perform checks with assertion.
if (type_ == kUTF16LE || type_ == kUTF16BE)
RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
if (type_ == kUTF32LE || type_ == kUTF32BE)
RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
}
typedef Ch (*TakeFunc)(InputByteStream &is);
InputByteStream *is_;
UTFType type_;
Ch current_;
TakeFunc takeFunc_;
bool hasBOM_;
};
//! Output stream wrapper with dynamically bound encoding and automatic encoding
//! detection.
/*!
\tparam CharType Type of character for writing.
\tparam OutputByteStream type of output byte stream to be wrapped.
*/
template <typename CharType, typename OutputByteStream>
class AutoUTFOutputStream {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
public:
typedef CharType Ch;
//! Constructor.
/*!
\param os output stream to be wrapped.
\param type UTF encoding type.
\param putBOM Whether to write BOM at the beginning of the stream.
*/
AutoUTFOutputStream(OutputByteStream &os, UTFType type, bool putBOM)
: os_(&os), type_(type) {
RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
// Runtime check whether the size of character type is sufficient. It only
// perform checks with assertion.
if (type_ == kUTF16LE || type_ == kUTF16BE)
RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
if (type_ == kUTF32LE || type_ == kUTF32BE)
RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
static const PutFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Put)};
putFunc_ = f[type_];
if (putBOM) PutBOM();
}
UTFType GetType() const { return type_; }
void Put(Ch c) { putFunc_(*os_, c); }
void Flush() { os_->Flush(); }
// Not implemented
Ch Peek() const {
RAPIDJSON_ASSERT(false);
return 0;
}
Ch Take() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t Tell() const {
RAPIDJSON_ASSERT(false);
return 0;
}
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
AutoUTFOutputStream(const AutoUTFOutputStream &);
AutoUTFOutputStream &operator=(const AutoUTFOutputStream &);
void PutBOM() {
typedef void (*PutBOMFunc)(OutputByteStream &);
static const PutBOMFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(PutBOM)};
f[type_](*os_);
}
typedef void (*PutFunc)(OutputByteStream &, Ch);
OutputByteStream *os_;
UTFType type_;
PutFunc putFunc_;
};
#undef RAPIDJSON_ENCODINGS_FUNC
RAPIDJSON_NAMESPACE_END
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
#ifdef __GNUC__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_FILESTREAM_H_

816
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/encodings.h vendored Normal file
View File

@ -0,0 +1,816 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ENCODINGS_H_
#define RAPIDJSON_ENCODINGS_H_
#include "rapidjson.h"
#if defined(_MSC_VER) && !defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(
4244) // conversion from 'type1' to 'type2', possible loss of data
RAPIDJSON_DIAG_OFF(4702) // unreachable code
#elif defined(__GNUC__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
RAPIDJSON_DIAG_OFF(overflow)
#endif
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// Encoding
/*! \class rapidjson::Encoding
\brief Concept for encoding of Unicode characters.
\code
concept Encoding {
typename Ch; //! Type of character. A "character" is actually a code unit
in unicode's definition.
enum { supportUnicode = 1 }; // or 0 if not supporting unicode
//! \brief Encode a Unicode codepoint to an output stream.
//! \param os Output stream.
//! \param codepoint An unicode codepoint, ranging from 0x0 to 0x10FFFF
inclusively. template<typename OutputStream> static void Encode(OutputStream&
os, unsigned codepoint);
//! \brief Decode a Unicode codepoint from an input stream.
//! \param is Input stream.
//! \param codepoint Output of the unicode codepoint.
//! \return true if a valid codepoint can be decoded from the stream.
template <typename InputStream>
static bool Decode(InputStream& is, unsigned* codepoint);
//! \brief Validate one Unicode codepoint from an encoded stream.
//! \param is Input stream to obtain codepoint.
//! \param os Output for copying one codepoint.
//! \return true if it is valid.
//! \note This function just validating and copying the codepoint without
actually decode it. template <typename InputStream, typename OutputStream>
static bool Validate(InputStream& is, OutputStream& os);
// The following functions are deal with byte streams.
//! Take a character from input byte stream, skip BOM if exist.
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream& is);
//! Take a character from input byte stream.
template <typename InputByteStream>
static Ch Take(InputByteStream& is);
//! Put BOM to output byte stream.
template <typename OutputByteStream>
static void PutBOM(OutputByteStream& os);
//! Put a character to output byte stream.
template <typename OutputByteStream>
static void Put(OutputByteStream& os, Ch c);
};
\endcode
*/
///////////////////////////////////////////////////////////////////////////////
// UTF8
//! UTF-8 encoding.
/*! http://en.wikipedia.org/wiki/UTF-8
http://tools.ietf.org/html/rfc3629
\tparam CharType Code unit for storing 8-bit UTF-8 data. Default is char.
\note implements Encoding concept
*/
template <typename CharType = char>
struct UTF8 {
typedef CharType Ch;
enum { supportUnicode = 1 };
template <typename OutputStream>
static void Encode(OutputStream &os, unsigned codepoint) {
if (codepoint <= 0x7F)
os.Put(static_cast<Ch>(codepoint & 0xFF));
else if (codepoint <= 0x7FF) {
os.Put(static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
} else if (codepoint <= 0xFFFF) {
os.Put(static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
} else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
os.Put(static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
os.Put(static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
}
}
template <typename OutputStream>
static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
if (codepoint <= 0x7F)
PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
else if (codepoint <= 0x7FF) {
PutUnsafe(os, static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
} else if (codepoint <= 0xFFFF) {
PutUnsafe(os, static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
} else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
PutUnsafe(os, static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
}
}
template <typename InputStream>
static bool Decode(InputStream &is, unsigned *codepoint) {
#define RAPIDJSON_COPY() \
c = is.Take(); \
*codepoint = (*codepoint << 6) | (static_cast<unsigned char>(c) & 0x3Fu)
#define RAPIDJSON_TRANS(mask) \
result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() \
RAPIDJSON_COPY(); \
RAPIDJSON_TRANS(0x70)
typename InputStream::Ch c = is.Take();
if (!(c & 0x80)) {
*codepoint = static_cast<unsigned char>(c);
return true;
}
unsigned char type = GetRange(static_cast<unsigned char>(c));
if (type >= 32) {
*codepoint = 0;
} else {
*codepoint = (0xFFu >> type) & static_cast<unsigned char>(c);
}
bool result = true;
switch (type) {
case 2:
RAPIDJSON_TAIL();
return result;
case 3:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 4:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x50);
RAPIDJSON_TAIL();
return result;
case 5:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x10);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 6:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 10:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x20);
RAPIDJSON_TAIL();
return result;
case 11:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x60);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
default:
return false;
}
#undef RAPIDJSON_COPY
#undef RAPIDJSON_TRANS
#undef RAPIDJSON_TAIL
}
template <typename InputStream, typename OutputStream>
static bool Validate(InputStream &is, OutputStream &os) {
#define RAPIDJSON_COPY() os.Put(c = is.Take())
#define RAPIDJSON_TRANS(mask) \
result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
#define RAPIDJSON_TAIL() \
RAPIDJSON_COPY(); \
RAPIDJSON_TRANS(0x70)
Ch c;
RAPIDJSON_COPY();
if (!(c & 0x80)) return true;
bool result = true;
switch (GetRange(static_cast<unsigned char>(c))) {
case 2:
RAPIDJSON_TAIL();
return result;
case 3:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 4:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x50);
RAPIDJSON_TAIL();
return result;
case 5:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x10);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 6:
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
case 10:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x20);
RAPIDJSON_TAIL();
return result;
case 11:
RAPIDJSON_COPY();
RAPIDJSON_TRANS(0x60);
RAPIDJSON_TAIL();
RAPIDJSON_TAIL();
return result;
default:
return false;
}
#undef RAPIDJSON_COPY
#undef RAPIDJSON_TRANS
#undef RAPIDJSON_TAIL
}
static unsigned char GetRange(unsigned char c) {
// Referring to DFA of http://bjoern.hoehrmann.de/utf-8/decoder/dfa/
// With new mapping 1 -> 0x10, 7 -> 0x20, 9 -> 0x40, such that AND operation
// can test multiple types.
static const unsigned char type[] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0x10, 0x10, 0x10, 0x10,
0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10,
0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40,
0x40, 0x40, 0x40, 0x40, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
8, 8, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
2, 2, 2, 2, 2, 2, 2, 2, 10, 3, 3, 3,
3, 3, 3, 3, 3, 3, 3, 3, 3, 4, 3, 3,
11, 6, 6, 6, 5, 8, 8, 8, 8, 8, 8, 8,
8, 8, 8, 8,
};
return type[c];
}
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
typename InputByteStream::Ch c = Take(is);
if (static_cast<unsigned char>(c) != 0xEFu) return c;
c = is.Take();
if (static_cast<unsigned char>(c) != 0xBBu) return c;
c = is.Take();
if (static_cast<unsigned char>(c) != 0xBFu) return c;
c = is.Take();
return c;
}
template <typename InputByteStream>
static Ch Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
return static_cast<Ch>(is.Take());
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xEFu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xBBu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xBFu));
}
template <typename OutputByteStream>
static void Put(OutputByteStream &os, Ch c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(c));
}
};
///////////////////////////////////////////////////////////////////////////////
// UTF16
//! UTF-16 encoding.
/*! http://en.wikipedia.org/wiki/UTF-16
http://tools.ietf.org/html/rfc2781
\tparam CharType Type for storing 16-bit UTF-16 data. Default is wchar_t.
C++11 may use char16_t instead. \note implements Encoding concept
\note For in-memory access, no need to concern endianness. The code units
and code points are represented by CPU's endianness. For streaming, use
UTF16LE and UTF16BE, which handle endianness.
*/
template <typename CharType = wchar_t>
struct UTF16 {
typedef CharType Ch;
RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 2);
enum { supportUnicode = 1 };
template <typename OutputStream>
static void Encode(OutputStream &os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
if (codepoint <= 0xFFFF) {
RAPIDJSON_ASSERT(
codepoint < 0xD800 ||
codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
os.Put(static_cast<typename OutputStream::Ch>(codepoint));
} else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
unsigned v = codepoint - 0x10000;
os.Put(static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
os.Put(static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
}
}
template <typename OutputStream>
static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
if (codepoint <= 0xFFFF) {
RAPIDJSON_ASSERT(
codepoint < 0xD800 ||
codepoint > 0xDFFF); // Code point itself cannot be surrogate pair
PutUnsafe(os, static_cast<typename OutputStream::Ch>(codepoint));
} else {
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
unsigned v = codepoint - 0x10000;
PutUnsafe(os, static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
PutUnsafe(os,
static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
}
}
template <typename InputStream>
static bool Decode(InputStream &is, unsigned *codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
typename InputStream::Ch c = is.Take();
if (c < 0xD800 || c > 0xDFFF) {
*codepoint = static_cast<unsigned>(c);
return true;
} else if (c <= 0xDBFF) {
*codepoint = (static_cast<unsigned>(c) & 0x3FF) << 10;
c = is.Take();
*codepoint |= (static_cast<unsigned>(c) & 0x3FF);
*codepoint += 0x10000;
return c >= 0xDC00 && c <= 0xDFFF;
}
return false;
}
template <typename InputStream, typename OutputStream>
static bool Validate(InputStream &is, OutputStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
typename InputStream::Ch c;
os.Put(static_cast<typename OutputStream::Ch>(c = is.Take()));
if (c < 0xD800 || c > 0xDFFF)
return true;
else if (c <= 0xDBFF) {
os.Put(c = is.Take());
return c >= 0xDC00 && c <= 0xDFFF;
}
return false;
}
};
//! UTF-16 little endian encoding.
template <typename CharType = wchar_t>
struct UTF16LE : UTF16<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is);
return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
}
template <typename InputByteStream>
static CharType Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<uint8_t>(is.Take());
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
return static_cast<CharType>(c);
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
}
template <typename OutputByteStream>
static void Put(OutputByteStream &os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(
(static_cast<unsigned>(c) >> 8) & 0xFFu));
}
};
//! UTF-16 big endian encoding.
template <typename CharType = wchar_t>
struct UTF16BE : UTF16<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is);
return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
}
template <typename InputByteStream>
static CharType Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take()));
return static_cast<CharType>(c);
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
}
template <typename OutputByteStream>
static void Put(OutputByteStream &os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(
(static_cast<unsigned>(c) >> 8) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
0xFFu));
}
};
///////////////////////////////////////////////////////////////////////////////
// UTF32
//! UTF-32 encoding.
/*! http://en.wikipedia.org/wiki/UTF-32
\tparam CharType Type for storing 32-bit UTF-32 data. Default is unsigned.
C++11 may use char32_t instead. \note implements Encoding concept
\note For in-memory access, no need to concern endianness. The code units
and code points are represented by CPU's endianness. For streaming, use
UTF32LE and UTF32BE, which handle endianness.
*/
template <typename CharType = unsigned>
struct UTF32 {
typedef CharType Ch;
RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 4);
enum { supportUnicode = 1 };
template <typename OutputStream>
static void Encode(OutputStream &os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
os.Put(codepoint);
}
template <typename OutputStream>
static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
PutUnsafe(os, codepoint);
}
template <typename InputStream>
static bool Decode(InputStream &is, unsigned *codepoint) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
Ch c = is.Take();
*codepoint = c;
return c <= 0x10FFFF;
}
template <typename InputStream, typename OutputStream>
static bool Validate(InputStream &is, OutputStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
Ch c;
os.Put(c = is.Take());
return c <= 0x10FFFF;
}
};
//! UTF-32 little endian enocoding.
template <typename CharType = unsigned>
struct UTF32LE : UTF32<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is);
return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
}
template <typename InputByteStream>
static CharType Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<uint8_t>(is.Take());
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24;
return static_cast<CharType>(c);
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
}
template <typename OutputByteStream>
static void Put(OutputByteStream &os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu));
}
};
//! UTF-32 big endian encoding.
template <typename CharType = unsigned>
struct UTF32BE : UTF32<CharType> {
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
CharType c = Take(is);
return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
}
template <typename InputByteStream>
static CharType Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take()));
return static_cast<CharType>(c);
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
}
template <typename OutputByteStream>
static void Put(OutputByteStream &os, CharType c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu));
os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu));
}
};
///////////////////////////////////////////////////////////////////////////////
// ASCII
//! ASCII encoding.
/*! http://en.wikipedia.org/wiki/ASCII
\tparam CharType Code unit for storing 7-bit ASCII data. Default is char.
\note implements Encoding concept
*/
template <typename CharType = char>
struct ASCII {
typedef CharType Ch;
enum { supportUnicode = 0 };
template <typename OutputStream>
static void Encode(OutputStream &os, unsigned codepoint) {
RAPIDJSON_ASSERT(codepoint <= 0x7F);
os.Put(static_cast<Ch>(codepoint & 0xFF));
}
template <typename OutputStream>
static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
RAPIDJSON_ASSERT(codepoint <= 0x7F);
PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
}
template <typename InputStream>
static bool Decode(InputStream &is, unsigned *codepoint) {
uint8_t c = static_cast<uint8_t>(is.Take());
*codepoint = c;
return c <= 0X7F;
}
template <typename InputStream, typename OutputStream>
static bool Validate(InputStream &is, OutputStream &os) {
uint8_t c = static_cast<uint8_t>(is.Take());
os.Put(static_cast<typename OutputStream::Ch>(c));
return c <= 0x7F;
}
template <typename InputByteStream>
static CharType TakeBOM(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
uint8_t c = static_cast<uint8_t>(Take(is));
return static_cast<Ch>(c);
}
template <typename InputByteStream>
static Ch Take(InputByteStream &is) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
return static_cast<Ch>(is.Take());
}
template <typename OutputByteStream>
static void PutBOM(OutputByteStream &os) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
(void)os;
}
template <typename OutputByteStream>
static void Put(OutputByteStream &os, Ch c) {
RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
os.Put(static_cast<typename OutputByteStream::Ch>(c));
}
};
///////////////////////////////////////////////////////////////////////////////
// AutoUTF
//! Runtime-specified UTF encoding type of a stream.
enum UTFType {
kUTF8 = 0, //!< UTF-8.
kUTF16LE = 1, //!< UTF-16 little endian.
kUTF16BE = 2, //!< UTF-16 big endian.
kUTF32LE = 3, //!< UTF-32 little endian.
kUTF32BE = 4 //!< UTF-32 big endian.
};
//! Dynamically select encoding according to stream's runtime-specified UTF
//! encoding type.
/*! \note This class can be used with AutoUTFInputtStream and
* AutoUTFOutputStream, which provides GetType().
*/
template <typename CharType>
struct AutoUTF {
typedef CharType Ch;
enum { supportUnicode = 1 };
#define RAPIDJSON_ENCODINGS_FUNC(x) \
UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
template <typename OutputStream>
static RAPIDJSON_FORCEINLINE void Encode(OutputStream &os,
unsigned codepoint) {
typedef void (*EncodeFunc)(OutputStream &, unsigned);
static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Encode)};
(*f[os.GetType()])(os, codepoint);
}
template <typename OutputStream>
static RAPIDJSON_FORCEINLINE void EncodeUnsafe(OutputStream &os,
unsigned codepoint) {
typedef void (*EncodeFunc)(OutputStream &, unsigned);
static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(EncodeUnsafe)};
(*f[os.GetType()])(os, codepoint);
}
template <typename InputStream>
static RAPIDJSON_FORCEINLINE bool Decode(InputStream &is,
unsigned *codepoint) {
typedef bool (*DecodeFunc)(InputStream &, unsigned *);
static const DecodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Decode)};
return (*f[is.GetType()])(is, codepoint);
}
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
OutputStream &os) {
typedef bool (*ValidateFunc)(InputStream &, OutputStream &);
static const ValidateFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Validate)};
return (*f[is.GetType()])(is, os);
}
#undef RAPIDJSON_ENCODINGS_FUNC
};
///////////////////////////////////////////////////////////////////////////////
// Transcoder
//! Encoding conversion.
template <typename SourceEncoding, typename TargetEncoding>
struct Transcoder {
//! Take one Unicode codepoint from source encoding, convert it to target
//! encoding and put it to the output stream.
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
OutputStream &os) {
unsigned codepoint;
if (!SourceEncoding::Decode(is, &codepoint)) return false;
TargetEncoding::Encode(os, codepoint);
return true;
}
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
OutputStream &os) {
unsigned codepoint;
if (!SourceEncoding::Decode(is, &codepoint)) return false;
TargetEncoding::EncodeUnsafe(os, codepoint);
return true;
}
//! Validate one Unicode codepoint from an encoded stream.
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
OutputStream &os) {
return Transcode(
is, os); // Since source/target encoding is different, must transcode.
}
};
// Forward declaration.
template <typename Stream>
inline void PutUnsafe(Stream &stream, typename Stream::Ch c);
//! Specialization of Transcoder with same source and target encoding.
template <typename Encoding>
struct Transcoder<Encoding, Encoding> {
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
OutputStream &os) {
os.Put(is.Take()); // Just copy one code unit. This semantic is different
// from primary template class.
return true;
}
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
OutputStream &os) {
PutUnsafe(os, is.Take()); // Just copy one code unit. This semantic is
// different from primary template class.
return true;
}
template <typename InputStream, typename OutputStream>
static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
OutputStream &os) {
return Encoding::Validate(is, os); // source/target encoding are the same
}
};
RAPIDJSON_NAMESPACE_END
#if defined(__GNUC__) || (defined(_MSC_VER) && !defined(__clang__))
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_ENCODINGS_H_

104
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/error/en.h vendored Normal file
View File

@ -0,0 +1,104 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ERROR_EN_H_
#define RAPIDJSON_ERROR_EN_H_
#include "error.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(switch - enum)
RAPIDJSON_DIAG_OFF(covered - switch - default)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Maps error code of parsing into error message.
/*!
\ingroup RAPIDJSON_ERRORS
\param parseErrorCode Error code obtained in parsing.
\return the error message.
\note User can make a copy of this function for localization.
Using switch-case is safer for future modification of error codes.
*/
inline const RAPIDJSON_ERROR_CHARTYPE* GetParseError_En(
ParseErrorCode parseErrorCode) {
switch (parseErrorCode) {
case kParseErrorNone:
return RAPIDJSON_ERROR_STRING("No error.");
case kParseErrorDocumentEmpty:
return RAPIDJSON_ERROR_STRING("The document is empty.");
case kParseErrorDocumentRootNotSingular:
return RAPIDJSON_ERROR_STRING(
"The document root must not be followed by other values.");
case kParseErrorValueInvalid:
return RAPIDJSON_ERROR_STRING("Invalid value.");
case kParseErrorObjectMissName:
return RAPIDJSON_ERROR_STRING("Missing a name for object member.");
case kParseErrorObjectMissColon:
return RAPIDJSON_ERROR_STRING(
"Missing a colon after a name of object member.");
case kParseErrorObjectMissCommaOrCurlyBracket:
return RAPIDJSON_ERROR_STRING(
"Missing a comma or '}' after an object member.");
case kParseErrorArrayMissCommaOrSquareBracket:
return RAPIDJSON_ERROR_STRING(
"Missing a comma or ']' after an array element.");
case kParseErrorStringUnicodeEscapeInvalidHex:
return RAPIDJSON_ERROR_STRING(
"Incorrect hex digit after \\u escape in string.");
case kParseErrorStringUnicodeSurrogateInvalid:
return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid.");
case kParseErrorStringEscapeInvalid:
return RAPIDJSON_ERROR_STRING("Invalid escape character in string.");
case kParseErrorStringMissQuotationMark:
return RAPIDJSON_ERROR_STRING(
"Missing a closing quotation mark in string.");
case kParseErrorStringInvalidEncoding:
return RAPIDJSON_ERROR_STRING("Invalid encoding in string.");
case kParseErrorNumberTooBig:
return RAPIDJSON_ERROR_STRING("Number too big to be stored in double.");
case kParseErrorNumberMissFraction:
return RAPIDJSON_ERROR_STRING("Miss fraction part in number.");
case kParseErrorNumberMissExponent:
return RAPIDJSON_ERROR_STRING("Miss exponent in number.");
case kParseErrorTermination:
return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error.");
case kParseErrorUnspecificSyntaxError:
return RAPIDJSON_ERROR_STRING("Unspecific syntax error.");
default:
return RAPIDJSON_ERROR_STRING("Unknown error.");
}
}
RAPIDJSON_NAMESPACE_END
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_ERROR_EN_H_

186
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/error/error.h vendored Normal file
View File

@ -0,0 +1,186 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ERROR_ERROR_H_
#define RAPIDJSON_ERROR_ERROR_H_
#include "../rapidjson.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
#endif
/*! \file error.h */
/*! \defgroup RAPIDJSON_ERRORS RapidJSON error handling */
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ERROR_CHARTYPE
//! Character type of error messages.
/*! \ingroup RAPIDJSON_ERRORS
The default character type is \c char.
On Windows, user can define this macro as \c TCHAR for supporting both
unicode/non-unicode settings.
*/
#ifndef RAPIDJSON_ERROR_CHARTYPE
#define RAPIDJSON_ERROR_CHARTYPE char
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ERROR_STRING
//! Macro for converting string literial to \ref RAPIDJSON_ERROR_CHARTYPE[].
/*! \ingroup RAPIDJSON_ERRORS
By default this conversion macro does nothing.
On Windows, user can define this macro as \c _T(x) for supporting both
unicode/non-unicode settings.
*/
#ifndef RAPIDJSON_ERROR_STRING
#define RAPIDJSON_ERROR_STRING(x) x
#endif
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// ParseErrorCode
//! Error code of parsing.
/*! \ingroup RAPIDJSON_ERRORS
\see GenericReader::Parse, GenericReader::GetParseErrorCode
*/
enum ParseErrorCode {
kParseErrorNone = 0, //!< No error.
kParseErrorDocumentEmpty, //!< The document is empty.
kParseErrorDocumentRootNotSingular, //!< The document root must not follow by
//!< other values.
kParseErrorValueInvalid, //!< Invalid value.
kParseErrorObjectMissName, //!< Missing a name for object member.
kParseErrorObjectMissColon, //!< Missing a colon after a name of object
//!< member.
kParseErrorObjectMissCommaOrCurlyBracket, //!< Missing a comma or '}' after
//!an
//!< object member.
kParseErrorArrayMissCommaOrSquareBracket, //!< Missing a comma or ']' after
//!an
//!< array element.
kParseErrorStringUnicodeEscapeInvalidHex, //!< Incorrect hex digit after \\u
//!< escape in string.
kParseErrorStringUnicodeSurrogateInvalid, //!< The surrogate pair in string
//!is
//!< invalid.
kParseErrorStringEscapeInvalid, //!< Invalid escape character in string.
kParseErrorStringMissQuotationMark, //!< Missing a closing quotation mark in
//!< string.
kParseErrorStringInvalidEncoding, //!< Invalid encoding in string.
kParseErrorNumberTooBig, //!< Number too big to be stored in double.
kParseErrorNumberMissFraction, //!< Miss fraction part in number.
kParseErrorNumberMissExponent, //!< Miss exponent in number.
kParseErrorTermination, //!< Parsing was terminated.
kParseErrorUnspecificSyntaxError //!< Unspecific syntax error.
};
//! Result of parsing (wraps ParseErrorCode)
/*!
\ingroup RAPIDJSON_ERRORS
\code
Document doc;
ParseResult ok = doc.Parse("[42]");
if (!ok) {
fprintf(stderr, "JSON parse error: %s (%u)",
GetParseError_En(ok.Code()), ok.Offset());
exit(EXIT_FAILURE);
}
\endcode
\see GenericReader::Parse, GenericDocument::Parse
*/
struct ParseResult {
//!! Unspecified boolean type
typedef bool (ParseResult::*BooleanType)() const;
public:
//! Default constructor, no error.
ParseResult() : code_(kParseErrorNone), offset_(0) {}
//! Constructor to set an error.
ParseResult(ParseErrorCode code, size_t offset)
: code_(code), offset_(offset) {}
//! Get the error code.
ParseErrorCode Code() const { return code_; }
//! Get the error offset, if \ref IsError(), 0 otherwise.
size_t Offset() const { return offset_; }
//! Explicit conversion to \c bool, returns \c true, iff !\ref IsError().
operator BooleanType() const {
return !IsError() ? &ParseResult::IsError : NULL;
}
//! Whether the result is an error.
bool IsError() const { return code_ != kParseErrorNone; }
bool operator==(const ParseResult &that) const { return code_ == that.code_; }
bool operator==(ParseErrorCode code) const { return code_ == code; }
friend bool operator==(ParseErrorCode code, const ParseResult &err) {
return code == err.code_;
}
bool operator!=(const ParseResult &that) const { return !(*this == that); }
bool operator!=(ParseErrorCode code) const { return !(*this == code); }
friend bool operator!=(ParseErrorCode code, const ParseResult &err) {
return err != code;
}
//! Reset error code.
void Clear() { Set(kParseErrorNone); }
//! Update error code and offset.
void Set(ParseErrorCode code, size_t offset = 0) {
code_ = code;
offset_ = offset;
}
private:
ParseErrorCode code_;
size_t offset_;
};
//! Function pointer type of GetParseError().
/*! \ingroup RAPIDJSON_ERRORS
This is the prototype for \c GetParseError_X(), where \c X is a locale.
User can dynamically change locale in runtime, e.g.:
\code
GetParseErrorFunc GetParseError = GetParseError_En; // or whatever
const RAPIDJSON_ERROR_CHARTYPE* s =
GetParseError(document.GetParseErrorCode()); \endcode
*/
typedef const RAPIDJSON_ERROR_CHARTYPE *(*GetParseErrorFunc)(ParseErrorCode);
RAPIDJSON_NAMESPACE_END
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_ERROR_ERROR_H_

123
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/filereadstream.h vendored Normal file
View File

@ -0,0 +1,123 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_FILEREADSTREAM_H_
#define RAPIDJSON_FILEREADSTREAM_H_
#include <cstdio>
#include "stream.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
RAPIDJSON_DIAG_OFF(unreachable - code)
RAPIDJSON_DIAG_OFF(missing - noreturn)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! File byte stream for input using fread().
/*!
\note implements Stream concept
*/
class FileReadStream {
public:
typedef char Ch; //!< Character type (byte).
//! Constructor.
/*!
\param fp File pointer opened for read.
\param buffer user-supplied buffer.
\param bufferSize size of buffer in bytes. Must >=4 bytes.
*/
FileReadStream(std::FILE *fp, char *buffer, size_t bufferSize)
: fp_(fp),
buffer_(buffer),
bufferSize_(bufferSize),
bufferLast_(0),
current_(buffer_),
readCount_(0),
count_(0),
eof_(false) {
RAPIDJSON_ASSERT(fp_ != 0);
RAPIDJSON_ASSERT(bufferSize >= 4);
Read();
}
Ch Peek() const { return *current_; }
Ch Take() {
Ch c = *current_;
Read();
return c;
}
size_t Tell() const {
return count_ + static_cast<size_t>(current_ - buffer_);
}
// Not implemented
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
// For encoding detection only.
const Ch *Peek4() const {
return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
}
private:
void Read() {
if (current_ < bufferLast_)
++current_;
else if (!eof_) {
count_ += readCount_;
readCount_ = std::fread(buffer_, 1, bufferSize_, fp_);
bufferLast_ = buffer_ + readCount_ - 1;
current_ = buffer_;
if (readCount_ < bufferSize_) {
buffer_[readCount_] = '\0';
++bufferLast_;
eof_ = true;
}
}
}
std::FILE *fp_;
Ch *buffer_;
size_t bufferSize_;
Ch *bufferLast_;
Ch *current_;
size_t readCount_;
size_t count_; //!< Number of characters read
bool eof_;
};
RAPIDJSON_NAMESPACE_END
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_FILESTREAM_H_

128
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/filewritestream.h vendored Normal file
View File

@ -0,0 +1,128 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_FILEWRITESTREAM_H_
#define RAPIDJSON_FILEWRITESTREAM_H_
#include <cstdio>
#include "stream.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(unreachable - code)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Wrapper of C file stream for output using fwrite().
/*!
\note implements Stream concept
*/
class FileWriteStream {
public:
typedef char Ch; //!< Character type. Only support char.
FileWriteStream(std::FILE *fp, char *buffer, size_t bufferSize)
: fp_(fp),
buffer_(buffer),
bufferEnd_(buffer + bufferSize),
current_(buffer_) {
RAPIDJSON_ASSERT(fp_ != 0);
}
void Put(char c) {
if (current_ >= bufferEnd_) Flush();
*current_++ = c;
}
void PutN(char c, size_t n) {
size_t avail = static_cast<size_t>(bufferEnd_ - current_);
while (n > avail) {
std::memset(current_, c, avail);
current_ += avail;
Flush();
n -= avail;
avail = static_cast<size_t>(bufferEnd_ - current_);
}
if (n > 0) {
std::memset(current_, c, n);
current_ += n;
}
}
void Flush() {
if (current_ != buffer_) {
size_t result =
std::fwrite(buffer_, 1, static_cast<size_t>(current_ - buffer_), fp_);
if (result < static_cast<size_t>(current_ - buffer_)) {
// failure deliberately ignored at this time
// added to avoid warn_unused_result build errors
}
current_ = buffer_;
}
}
// Not implemented
char Peek() const {
RAPIDJSON_ASSERT(false);
return 0;
}
char Take() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t Tell() const {
RAPIDJSON_ASSERT(false);
return 0;
}
char *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(char *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
// Prohibit copy constructor & assignment operator.
FileWriteStream(const FileWriteStream &);
FileWriteStream &operator=(const FileWriteStream &);
std::FILE *fp_;
char *buffer_;
char *bufferEnd_;
char *current_;
};
//! Implement specialized version of PutN() with memset() for better
//! performance.
template <>
inline void PutN(FileWriteStream &stream, char c, size_t n) {
stream.PutN(c, n);
}
RAPIDJSON_NAMESPACE_END
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_FILESTREAM_H_

170
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/fwd.h vendored Normal file
View File

@ -0,0 +1,170 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_FWD_H_
#define RAPIDJSON_FWD_H_
#include "rapidjson.h"
RAPIDJSON_NAMESPACE_BEGIN
// encodings.h
template <typename CharType>
struct UTF8;
template <typename CharType>
struct UTF16;
template <typename CharType>
struct UTF16BE;
template <typename CharType>
struct UTF16LE;
template <typename CharType>
struct UTF32;
template <typename CharType>
struct UTF32BE;
template <typename CharType>
struct UTF32LE;
template <typename CharType>
struct ASCII;
template <typename CharType>
struct AutoUTF;
template <typename SourceEncoding, typename TargetEncoding>
struct Transcoder;
// allocators.h
class CrtAllocator;
template <typename BaseAllocator>
class MemoryPoolAllocator;
// stream.h
template <typename Encoding>
struct GenericStringStream;
typedef GenericStringStream<UTF8<char>> StringStream;
template <typename Encoding>
struct GenericInsituStringStream;
typedef GenericInsituStringStream<UTF8<char>> InsituStringStream;
// stringbuffer.h
template <typename Encoding, typename Allocator>
class GenericStringBuffer;
typedef GenericStringBuffer<UTF8<char>, CrtAllocator> StringBuffer;
// filereadstream.h
class FileReadStream;
// filewritestream.h
class FileWriteStream;
// memorybuffer.h
template <typename Allocator>
struct GenericMemoryBuffer;
typedef GenericMemoryBuffer<CrtAllocator> MemoryBuffer;
// memorystream.h
struct MemoryStream;
// reader.h
template <typename Encoding, typename Derived>
struct BaseReaderHandler;
template <typename SourceEncoding, typename TargetEncoding,
typename StackAllocator>
class GenericReader;
typedef GenericReader<UTF8<char>, UTF8<char>, CrtAllocator> Reader;
// writer.h
template <typename OutputStream, typename SourceEncoding,
typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
class Writer;
// prettywriter.h
template <typename OutputStream, typename SourceEncoding,
typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
class PrettyWriter;
// document.h
template <typename Encoding, typename Allocator>
class GenericMember;
template <bool Const, typename Encoding, typename Allocator>
class GenericMemberIterator;
template <typename CharType>
struct GenericStringRef;
template <typename Encoding, typename Allocator>
class GenericValue;
typedef GenericValue<UTF8<char>, MemoryPoolAllocator<CrtAllocator>> Value;
template <typename Encoding, typename Allocator, typename StackAllocator>
class GenericDocument;
typedef GenericDocument<UTF8<char>, MemoryPoolAllocator<CrtAllocator>,
CrtAllocator>
Document;
// pointer.h
template <typename ValueType, typename Allocator>
class GenericPointer;
typedef GenericPointer<Value, CrtAllocator> Pointer;
// schema.h
template <typename SchemaDocumentType>
class IGenericRemoteSchemaDocumentProvider;
template <typename ValueT, typename Allocator>
class GenericSchemaDocument;
typedef GenericSchemaDocument<Value, CrtAllocator> SchemaDocument;
typedef IGenericRemoteSchemaDocumentProvider<SchemaDocument>
IRemoteSchemaDocumentProvider;
template <typename SchemaDocumentType, typename OutputHandler,
typename StateAllocator>
class GenericSchemaValidator;
typedef GenericSchemaValidator<
SchemaDocument, BaseReaderHandler<UTF8<char>, void>, CrtAllocator>
SchemaValidator;
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_RAPIDJSONFWD_H_

295
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/biginteger.h vendored Normal file
View File

@ -0,0 +1,295 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_BIGINTEGER_H_
#define RAPIDJSON_BIGINTEGER_H_
#include "../rapidjson.h"
#if defined(_MSC_VER) && !__INTEL_COMPILER && defined(_M_AMD64)
#include <intrin.h> // for _umul128
#pragma intrinsic(_umul128)
#endif
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
class BigInteger {
public:
typedef uint64_t Type;
BigInteger(const BigInteger &rhs) : count_(rhs.count_) {
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
}
explicit BigInteger(uint64_t u) : count_(1) { digits_[0] = u; }
BigInteger(const char *decimals, size_t length) : count_(1) {
RAPIDJSON_ASSERT(length > 0);
digits_[0] = 0;
size_t i = 0;
const size_t kMaxDigitPerIteration =
19; // 2^64 = 18446744073709551616 > 10^19
while (length >= kMaxDigitPerIteration) {
AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
length -= kMaxDigitPerIteration;
i += kMaxDigitPerIteration;
}
if (length > 0) AppendDecimal64(decimals + i, decimals + i + length);
}
BigInteger &operator=(const BigInteger &rhs) {
if (this != &rhs) {
count_ = rhs.count_;
std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
}
return *this;
}
BigInteger &operator=(uint64_t u) {
digits_[0] = u;
count_ = 1;
return *this;
}
BigInteger &operator+=(uint64_t u) {
Type backup = digits_[0];
digits_[0] += u;
for (size_t i = 0; i < count_ - 1; i++) {
if (digits_[i] >= backup) return *this; // no carry
backup = digits_[i + 1];
digits_[i + 1] += 1;
}
// Last carry
if (digits_[count_ - 1] < backup) PushBack(1);
return *this;
}
BigInteger &operator*=(uint64_t u) {
if (u == 0) return *this = 0;
if (u == 1) return *this;
if (*this == 1) return *this = u;
uint64_t k = 0;
for (size_t i = 0; i < count_; i++) {
uint64_t hi;
digits_[i] = MulAdd64(digits_[i], u, k, &hi);
k = hi;
}
if (k > 0) PushBack(k);
return *this;
}
BigInteger &operator*=(uint32_t u) {
if (u == 0) return *this = 0;
if (u == 1) return *this;
if (*this == 1) return *this = u;
uint64_t k = 0;
for (size_t i = 0; i < count_; i++) {
const uint64_t c = digits_[i] >> 32;
const uint64_t d = digits_[i] & 0xFFFFFFFF;
const uint64_t uc = u * c;
const uint64_t ud = u * d;
const uint64_t p0 = ud + k;
const uint64_t p1 = uc + (p0 >> 32);
digits_[i] = (p0 & 0xFFFFFFFF) | (p1 << 32);
k = p1 >> 32;
}
if (k > 0) PushBack(k);
return *this;
}
BigInteger &operator<<=(size_t shift) {
if (IsZero() || shift == 0) return *this;
size_t offset = shift / kTypeBit;
size_t interShift = shift % kTypeBit;
RAPIDJSON_ASSERT(count_ + offset <= kCapacity);
if (interShift == 0) {
std::memmove(digits_ + offset, digits_, count_ * sizeof(Type));
count_ += offset;
} else {
digits_[count_] = 0;
for (size_t i = count_; i > 0; i--)
digits_[i + offset] = (digits_[i] << interShift) |
(digits_[i - 1] >> (kTypeBit - interShift));
digits_[offset] = digits_[0] << interShift;
count_ += offset;
if (digits_[count_]) count_++;
}
std::memset(digits_, 0, offset * sizeof(Type));
return *this;
}
bool operator==(const BigInteger &rhs) const {
return count_ == rhs.count_ &&
std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0;
}
bool operator==(const Type rhs) const {
return count_ == 1 && digits_[0] == rhs;
}
BigInteger &MultiplyPow5(unsigned exp) {
static const uint32_t kPow5[12] = {
5,
5 * 5,
5 * 5 * 5,
5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5};
if (exp == 0) return *this;
for (; exp >= 27; exp -= 27)
*this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D); // 5^27
for (; exp >= 13; exp -= 13)
*this *= static_cast<uint32_t>(1220703125u); // 5^13
if (exp > 0) *this *= kPow5[exp - 1];
return *this;
}
// Compute absolute difference of this and rhs.
// Assume this != rhs
bool Difference(const BigInteger &rhs, BigInteger *out) const {
int cmp = Compare(rhs);
RAPIDJSON_ASSERT(cmp != 0);
const BigInteger *a, *b; // Makes a > b
bool ret;
if (cmp < 0) {
a = &rhs;
b = this;
ret = true;
} else {
a = this;
b = &rhs;
ret = false;
}
Type borrow = 0;
for (size_t i = 0; i < a->count_; i++) {
Type d = a->digits_[i] - borrow;
if (i < b->count_) d -= b->digits_[i];
borrow = (d > a->digits_[i]) ? 1 : 0;
out->digits_[i] = d;
if (d != 0) out->count_ = i + 1;
}
return ret;
}
int Compare(const BigInteger &rhs) const {
if (count_ != rhs.count_) return count_ < rhs.count_ ? -1 : 1;
for (size_t i = count_; i-- > 0;)
if (digits_[i] != rhs.digits_[i])
return digits_[i] < rhs.digits_[i] ? -1 : 1;
return 0;
}
size_t GetCount() const { return count_; }
Type GetDigit(size_t index) const {
RAPIDJSON_ASSERT(index < count_);
return digits_[index];
}
bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
private:
void AppendDecimal64(const char *begin, const char *end) {
uint64_t u = ParseUint64(begin, end);
if (IsZero())
*this = u;
else {
unsigned exp = static_cast<unsigned>(end - begin);
(MultiplyPow5(exp) <<= exp) += u; // *this = *this * 10^exp + u
}
}
void PushBack(Type digit) {
RAPIDJSON_ASSERT(count_ < kCapacity);
digits_[count_++] = digit;
}
static uint64_t ParseUint64(const char *begin, const char *end) {
uint64_t r = 0;
for (const char *p = begin; p != end; ++p) {
RAPIDJSON_ASSERT(*p >= '0' && *p <= '9');
r = r * 10u + static_cast<unsigned>(*p - '0');
}
return r;
}
// Assume a * b + k < 2^128
static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k,
uint64_t *outHigh) {
#if defined(_MSC_VER) && defined(_M_AMD64)
uint64_t low = _umul128(a, b, outHigh) + k;
if (low < k) (*outHigh)++;
return low;
#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
defined(__x86_64__)
__extension__ typedef unsigned __int128 uint128;
uint128 p = static_cast<uint128>(a) * static_cast<uint128>(b);
p += k;
*outHigh = static_cast<uint64_t>(p >> 64);
return static_cast<uint64_t>(p);
#else
const uint64_t a0 = a & 0xFFFFFFFF, a1 = a >> 32, b0 = b & 0xFFFFFFFF,
b1 = b >> 32;
uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1;
x1 += (x0 >> 32); // can't give carry
x1 += x2;
if (x1 < x2) x3 += (static_cast<uint64_t>(1) << 32);
uint64_t lo = (x1 << 32) + (x0 & 0xFFFFFFFF);
uint64_t hi = x3 + (x1 >> 32);
lo += k;
if (lo < k) hi++;
*outHigh = hi;
return lo;
#endif
}
static const size_t kBitCount = 3328; // 64bit * 54 > 10^1000
static const size_t kCapacity = kBitCount / sizeof(Type);
static const size_t kTypeBit = sizeof(Type) * 8;
Type digits_[kCapacity];
size_t count_;
};
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_BIGINTEGER_H_

77
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/clzll.h vendored Normal file
View File

@ -0,0 +1,77 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_CLZLL_H_
#define RAPIDJSON_CLZLL_H_
#include "../rapidjson.h"
#if defined(_MSC_VER)
#include <intrin.h>
#if defined(_WIN64)
#pragma intrinsic(_BitScanReverse64)
#else
#pragma intrinsic(_BitScanReverse)
#endif
#endif
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
#if (defined(__GNUC__) && __GNUC__ >= 4) || \
RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
#define RAPIDJSON_CLZLL __builtin_clzll
#else
inline uint32_t clzll(uint64_t x) {
// Passing 0 to __builtin_clzll is UB in GCC and results in an
// infinite loop in the software implementation.
RAPIDJSON_ASSERT(x != 0);
#if defined(_MSC_VER)
unsigned long r = 0;
#if defined(_WIN64)
_BitScanReverse64(&r, x);
#else
// Scan the high 32 bits.
if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32))) return 63 - (r + 32);
// Scan the low 32 bits.
_BitScanReverse(&r, static_cast<uint32_t>(x & 0xFFFFFFFF));
#endif // _WIN64
return 63 - r;
#else
uint32_t r;
while (!(x & (static_cast<uint64_t>(1) << 63))) {
x <<= 1;
++r;
}
return r;
#endif // _MSC_VER
}
#define RAPIDJSON_CLZLL RAPIDJSON_NAMESPACE::internal::clzll
#endif // (defined(__GNUC__) && __GNUC__ >= 4) ||
// RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_CLZLL_H_

305
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/diyfp.h vendored Normal file
View File

@ -0,0 +1,305 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
// This is a C++ header-only implementation of Grisu2 algorithm from the
// publication: Loitsch, Florian. "Printing floating-point numbers quickly and
// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243.
#ifndef RAPIDJSON_DIYFP_H_
#define RAPIDJSON_DIYFP_H_
#include <limits>
#include "../rapidjson.h"
#include "clzll.h"
#if defined(_MSC_VER) && defined(_M_AMD64) && !defined(__INTEL_COMPILER)
#include <intrin.h>
#pragma intrinsic(_umul128)
#endif
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
#ifdef __GNUC__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
#endif
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
#endif
struct DiyFp {
DiyFp() : f(), e() {}
DiyFp(uint64_t fp, int exp) : f(fp), e(exp) {}
explicit DiyFp(double d) {
union {
double d;
uint64_t u64;
} u = {d};
int biased_e =
static_cast<int>((u.u64 & kDpExponentMask) >> kDpSignificandSize);
uint64_t significand = (u.u64 & kDpSignificandMask);
if (biased_e != 0) {
f = significand + kDpHiddenBit;
e = biased_e - kDpExponentBias;
} else {
f = significand;
e = kDpMinExponent + 1;
}
}
DiyFp operator-(const DiyFp &rhs) const { return DiyFp(f - rhs.f, e); }
DiyFp operator*(const DiyFp &rhs) const {
#if defined(_MSC_VER) && defined(_M_AMD64)
uint64_t h;
uint64_t l = _umul128(f, rhs.f, &h);
if (l & (uint64_t(1) << 63)) // rounding
h++;
return DiyFp(h, e + rhs.e + 64);
#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
defined(__x86_64__)
__extension__ typedef unsigned __int128 uint128;
uint128 p = static_cast<uint128>(f) * static_cast<uint128>(rhs.f);
uint64_t h = static_cast<uint64_t>(p >> 64);
uint64_t l = static_cast<uint64_t>(p);
if (l & (uint64_t(1) << 63)) // rounding
h++;
return DiyFp(h, e + rhs.e + 64);
#else
const uint64_t M32 = 0xFFFFFFFF;
const uint64_t a = f >> 32;
const uint64_t b = f & M32;
const uint64_t c = rhs.f >> 32;
const uint64_t d = rhs.f & M32;
const uint64_t ac = a * c;
const uint64_t bc = b * c;
const uint64_t ad = a * d;
const uint64_t bd = b * d;
uint64_t tmp = (bd >> 32) + (ad & M32) + (bc & M32);
tmp += 1U << 31; /// mult_round
return DiyFp(ac + (ad >> 32) + (bc >> 32) + (tmp >> 32), e + rhs.e + 64);
#endif
}
DiyFp Normalize() const {
int s = static_cast<int>(RAPIDJSON_CLZLL(f));
return DiyFp(f << s, e - s);
}
DiyFp NormalizeBoundary() const {
DiyFp res = *this;
while (!(res.f & (kDpHiddenBit << 1))) {
res.f <<= 1;
res.e--;
}
res.f <<= (kDiySignificandSize - kDpSignificandSize - 2);
res.e = res.e - (kDiySignificandSize - kDpSignificandSize - 2);
return res;
}
void NormalizedBoundaries(DiyFp *minus, DiyFp *plus) const {
DiyFp pl = DiyFp((f << 1) + 1, e - 1).NormalizeBoundary();
DiyFp mi = (f == kDpHiddenBit) ? DiyFp((f << 2) - 1, e - 2)
: DiyFp((f << 1) - 1, e - 1);
mi.f <<= mi.e - pl.e;
mi.e = pl.e;
*plus = pl;
*minus = mi;
}
double ToDouble() const {
union {
double d;
uint64_t u64;
} u;
RAPIDJSON_ASSERT(f <= kDpHiddenBit + kDpSignificandMask);
if (e < kDpDenormalExponent) {
// Underflow.
return 0.0;
}
if (e >= kDpMaxExponent) {
// Overflow.
return std::numeric_limits<double>::infinity();
}
const uint64_t be = (e == kDpDenormalExponent && (f & kDpHiddenBit) == 0)
? 0
: static_cast<uint64_t>(e + kDpExponentBias);
u.u64 = (f & kDpSignificandMask) | (be << kDpSignificandSize);
return u.d;
}
static const int kDiySignificandSize = 64;
static const int kDpSignificandSize = 52;
static const int kDpExponentBias = 0x3FF + kDpSignificandSize;
static const int kDpMaxExponent = 0x7FF - kDpExponentBias;
static const int kDpMinExponent = -kDpExponentBias;
static const int kDpDenormalExponent = -kDpExponentBias + 1;
static const uint64_t kDpExponentMask =
RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
static const uint64_t kDpSignificandMask =
RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
static const uint64_t kDpHiddenBit =
RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
uint64_t f;
int e;
};
inline DiyFp GetCachedPowerByIndex(size_t index) {
// 10^-348, 10^-340, ..., 10^340
static const uint64_t kCachedPowers_F[] = {
RAPIDJSON_UINT64_C2(0xfa8fd5a0, 0x081c0288),
RAPIDJSON_UINT64_C2(0xbaaee17f, 0xa23ebf76),
RAPIDJSON_UINT64_C2(0x8b16fb20, 0x3055ac76),
RAPIDJSON_UINT64_C2(0xcf42894a, 0x5dce35ea),
RAPIDJSON_UINT64_C2(0x9a6bb0aa, 0x55653b2d),
RAPIDJSON_UINT64_C2(0xe61acf03, 0x3d1a45df),
RAPIDJSON_UINT64_C2(0xab70fe17, 0xc79ac6ca),
RAPIDJSON_UINT64_C2(0xff77b1fc, 0xbebcdc4f),
RAPIDJSON_UINT64_C2(0xbe5691ef, 0x416bd60c),
RAPIDJSON_UINT64_C2(0x8dd01fad, 0x907ffc3c),
RAPIDJSON_UINT64_C2(0xd3515c28, 0x31559a83),
RAPIDJSON_UINT64_C2(0x9d71ac8f, 0xada6c9b5),
RAPIDJSON_UINT64_C2(0xea9c2277, 0x23ee8bcb),
RAPIDJSON_UINT64_C2(0xaecc4991, 0x4078536d),
RAPIDJSON_UINT64_C2(0x823c1279, 0x5db6ce57),
RAPIDJSON_UINT64_C2(0xc2109436, 0x4dfb5637),
RAPIDJSON_UINT64_C2(0x9096ea6f, 0x3848984f),
RAPIDJSON_UINT64_C2(0xd77485cb, 0x25823ac7),
RAPIDJSON_UINT64_C2(0xa086cfcd, 0x97bf97f4),
RAPIDJSON_UINT64_C2(0xef340a98, 0x172aace5),
RAPIDJSON_UINT64_C2(0xb23867fb, 0x2a35b28e),
RAPIDJSON_UINT64_C2(0x84c8d4df, 0xd2c63f3b),
RAPIDJSON_UINT64_C2(0xc5dd4427, 0x1ad3cdba),
RAPIDJSON_UINT64_C2(0x936b9fce, 0xbb25c996),
RAPIDJSON_UINT64_C2(0xdbac6c24, 0x7d62a584),
RAPIDJSON_UINT64_C2(0xa3ab6658, 0x0d5fdaf6),
RAPIDJSON_UINT64_C2(0xf3e2f893, 0xdec3f126),
RAPIDJSON_UINT64_C2(0xb5b5ada8, 0xaaff80b8),
RAPIDJSON_UINT64_C2(0x87625f05, 0x6c7c4a8b),
RAPIDJSON_UINT64_C2(0xc9bcff60, 0x34c13053),
RAPIDJSON_UINT64_C2(0x964e858c, 0x91ba2655),
RAPIDJSON_UINT64_C2(0xdff97724, 0x70297ebd),
RAPIDJSON_UINT64_C2(0xa6dfbd9f, 0xb8e5b88f),
RAPIDJSON_UINT64_C2(0xf8a95fcf, 0x88747d94),
RAPIDJSON_UINT64_C2(0xb9447093, 0x8fa89bcf),
RAPIDJSON_UINT64_C2(0x8a08f0f8, 0xbf0f156b),
RAPIDJSON_UINT64_C2(0xcdb02555, 0x653131b6),
RAPIDJSON_UINT64_C2(0x993fe2c6, 0xd07b7fac),
RAPIDJSON_UINT64_C2(0xe45c10c4, 0x2a2b3b06),
RAPIDJSON_UINT64_C2(0xaa242499, 0x697392d3),
RAPIDJSON_UINT64_C2(0xfd87b5f2, 0x8300ca0e),
RAPIDJSON_UINT64_C2(0xbce50864, 0x92111aeb),
RAPIDJSON_UINT64_C2(0x8cbccc09, 0x6f5088cc),
RAPIDJSON_UINT64_C2(0xd1b71758, 0xe219652c),
RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000),
RAPIDJSON_UINT64_C2(0xe8d4a510, 0x00000000),
RAPIDJSON_UINT64_C2(0xad78ebc5, 0xac620000),
RAPIDJSON_UINT64_C2(0x813f3978, 0xf8940984),
RAPIDJSON_UINT64_C2(0xc097ce7b, 0xc90715b3),
RAPIDJSON_UINT64_C2(0x8f7e32ce, 0x7bea5c70),
RAPIDJSON_UINT64_C2(0xd5d238a4, 0xabe98068),
RAPIDJSON_UINT64_C2(0x9f4f2726, 0x179a2245),
RAPIDJSON_UINT64_C2(0xed63a231, 0xd4c4fb27),
RAPIDJSON_UINT64_C2(0xb0de6538, 0x8cc8ada8),
RAPIDJSON_UINT64_C2(0x83c7088e, 0x1aab65db),
RAPIDJSON_UINT64_C2(0xc45d1df9, 0x42711d9a),
RAPIDJSON_UINT64_C2(0x924d692c, 0xa61be758),
RAPIDJSON_UINT64_C2(0xda01ee64, 0x1a708dea),
RAPIDJSON_UINT64_C2(0xa26da399, 0x9aef774a),
RAPIDJSON_UINT64_C2(0xf209787b, 0xb47d6b85),
RAPIDJSON_UINT64_C2(0xb454e4a1, 0x79dd1877),
RAPIDJSON_UINT64_C2(0x865b8692, 0x5b9bc5c2),
RAPIDJSON_UINT64_C2(0xc83553c5, 0xc8965d3d),
RAPIDJSON_UINT64_C2(0x952ab45c, 0xfa97a0b3),
RAPIDJSON_UINT64_C2(0xde469fbd, 0x99a05fe3),
RAPIDJSON_UINT64_C2(0xa59bc234, 0xdb398c25),
RAPIDJSON_UINT64_C2(0xf6c69a72, 0xa3989f5c),
RAPIDJSON_UINT64_C2(0xb7dcbf53, 0x54e9bece),
RAPIDJSON_UINT64_C2(0x88fcf317, 0xf22241e2),
RAPIDJSON_UINT64_C2(0xcc20ce9b, 0xd35c78a5),
RAPIDJSON_UINT64_C2(0x98165af3, 0x7b2153df),
RAPIDJSON_UINT64_C2(0xe2a0b5dc, 0x971f303a),
RAPIDJSON_UINT64_C2(0xa8d9d153, 0x5ce3b396),
RAPIDJSON_UINT64_C2(0xfb9b7cd9, 0xa4a7443c),
RAPIDJSON_UINT64_C2(0xbb764c4c, 0xa7a44410),
RAPIDJSON_UINT64_C2(0x8bab8eef, 0xb6409c1a),
RAPIDJSON_UINT64_C2(0xd01fef10, 0xa657842c),
RAPIDJSON_UINT64_C2(0x9b10a4e5, 0xe9913129),
RAPIDJSON_UINT64_C2(0xe7109bfb, 0xa19c0c9d),
RAPIDJSON_UINT64_C2(0xac2820d9, 0x623bf429),
RAPIDJSON_UINT64_C2(0x80444b5e, 0x7aa7cf85),
RAPIDJSON_UINT64_C2(0xbf21e440, 0x03acdd2d),
RAPIDJSON_UINT64_C2(0x8e679c2f, 0x5e44ff8f),
RAPIDJSON_UINT64_C2(0xd433179d, 0x9c8cb841),
RAPIDJSON_UINT64_C2(0x9e19db92, 0xb4e31ba9),
RAPIDJSON_UINT64_C2(0xeb96bf6e, 0xbadf77d9),
RAPIDJSON_UINT64_C2(0xaf87023b, 0x9bf0ee6b)};
static const int16_t kCachedPowers_E[] = {
-1220, -1193, -1166, -1140, -1113, -1087, -1060, -1034, -1007, -980, -954,
-927, -901, -874, -847, -821, -794, -768, -741, -715, -688, -661,
-635, -608, -582, -555, -529, -502, -475, -449, -422, -396, -369,
-343, -316, -289, -263, -236, -210, -183, -157, -130, -103, -77,
-50, -24, 3, 30, 56, 83, 109, 136, 162, 189, 216,
242, 269, 295, 322, 348, 375, 402, 428, 455, 481, 508,
534, 561, 588, 614, 641, 667, 694, 720, 747, 774, 800,
827, 853, 880, 907, 933, 960, 986, 1013, 1039, 1066};
RAPIDJSON_ASSERT(index < 87);
return DiyFp(kCachedPowers_F[index], kCachedPowers_E[index]);
}
inline DiyFp GetCachedPower(int e, int *K) {
// int k = static_cast<int>(ceil((-61 - e) * 0.30102999566398114)) + 374;
double dk = (-61 - e) * 0.30102999566398114 +
347; // dk must be positive, so can do ceiling in positive
int k = static_cast<int>(dk);
if (dk - k > 0.0) k++;
unsigned index = static_cast<unsigned>((k >> 3) + 1);
*K = -(-348 + static_cast<int>(
index << 3)); // decimal exponent no need lookup table
return GetCachedPowerByIndex(index);
}
inline DiyFp GetCachedPower10(int exp, int *outExp) {
RAPIDJSON_ASSERT(exp >= -348);
unsigned index = static_cast<unsigned>(exp + 348) / 8u;
*outExp = -348 + static_cast<int>(index) * 8;
return GetCachedPowerByIndex(index);
}
#ifdef __GNUC__
RAPIDJSON_DIAG_POP
#endif
#ifdef __clang__
RAPIDJSON_DIAG_POP
RAPIDJSON_DIAG_OFF(padded)
#endif
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_DIYFP_H_

269
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/dtoa.h vendored Normal file
View File

@ -0,0 +1,269 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
// This is a C++ header-only implementation of Grisu2 algorithm from the
// publication: Loitsch, Florian. "Printing floating-point numbers quickly and
// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243.
#ifndef RAPIDJSON_DTOA_
#define RAPIDJSON_DTOA_
#include "diyfp.h"
#include "ieee754.h"
#include "itoa.h" // GetDigitsLut()
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
#ifdef __GNUC__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
RAPIDJSON_DIAG_OFF(array - bounds) // some gcc versions generate wrong warnings
// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59124
#endif
inline void GrisuRound(char *buffer, int len, uint64_t delta, uint64_t rest,
uint64_t ten_kappa, uint64_t wp_w) {
while (rest < wp_w && delta - rest >= ten_kappa &&
(rest + ten_kappa < wp_w || /// closer
wp_w - rest > rest + ten_kappa - wp_w)) {
buffer[len - 1]--;
rest += ten_kappa;
}
}
inline int CountDecimalDigit32(uint32_t n) {
// Simple pure C++ implementation was faster than __builtin_clz version in
// this situation.
if (n < 10) return 1;
if (n < 100) return 2;
if (n < 1000) return 3;
if (n < 10000) return 4;
if (n < 100000) return 5;
if (n < 1000000) return 6;
if (n < 10000000) return 7;
if (n < 100000000) return 8;
// Will not reach 10 digits in DigitGen()
// if (n < 1000000000) return 9;
// return 10;
return 9;
}
inline void DigitGen(const DiyFp &W, const DiyFp &Mp, uint64_t delta,
char *buffer, int *len, int *K) {
static const uint32_t kPow10[] = {1, 10, 100, 1000,
10000, 100000, 1000000, 10000000,
100000000, 1000000000};
const DiyFp one(uint64_t(1) << -Mp.e, Mp.e);
const DiyFp wp_w = Mp - W;
uint32_t p1 = static_cast<uint32_t>(Mp.f >> -one.e);
uint64_t p2 = Mp.f & (one.f - 1);
int kappa = CountDecimalDigit32(p1); // kappa in [0, 9]
*len = 0;
while (kappa > 0) {
uint32_t d = 0;
switch (kappa) {
case 9:
d = p1 / 100000000;
p1 %= 100000000;
break;
case 8:
d = p1 / 10000000;
p1 %= 10000000;
break;
case 7:
d = p1 / 1000000;
p1 %= 1000000;
break;
case 6:
d = p1 / 100000;
p1 %= 100000;
break;
case 5:
d = p1 / 10000;
p1 %= 10000;
break;
case 4:
d = p1 / 1000;
p1 %= 1000;
break;
case 3:
d = p1 / 100;
p1 %= 100;
break;
case 2:
d = p1 / 10;
p1 %= 10;
break;
case 1:
d = p1;
p1 = 0;
break;
default:;
}
if (d || *len)
buffer[(*len)++] = static_cast<char>('0' + static_cast<char>(d));
kappa--;
uint64_t tmp = (static_cast<uint64_t>(p1) << -one.e) + p2;
if (tmp <= delta) {
*K += kappa;
GrisuRound(buffer, *len, delta, tmp,
static_cast<uint64_t>(kPow10[kappa]) << -one.e, wp_w.f);
return;
}
}
// kappa = 0
for (;;) {
p2 *= 10;
delta *= 10;
char d = static_cast<char>(p2 >> -one.e);
if (d || *len) buffer[(*len)++] = static_cast<char>('0' + d);
p2 &= one.f - 1;
kappa--;
if (p2 < delta) {
*K += kappa;
int index = -kappa;
GrisuRound(buffer, *len, delta, p2, one.f,
wp_w.f * (index < 9 ? kPow10[index] : 0));
return;
}
}
}
inline void Grisu2(double value, char *buffer, int *length, int *K) {
const DiyFp v(value);
DiyFp w_m, w_p;
v.NormalizedBoundaries(&w_m, &w_p);
const DiyFp c_mk = GetCachedPower(w_p.e, K);
const DiyFp W = v.Normalize() * c_mk;
DiyFp Wp = w_p * c_mk;
DiyFp Wm = w_m * c_mk;
Wm.f++;
Wp.f--;
DigitGen(W, Wp, Wp.f - Wm.f, buffer, length, K);
}
inline char *WriteExponent(int K, char *buffer) {
if (K < 0) {
*buffer++ = '-';
K = -K;
}
if (K >= 100) {
*buffer++ = static_cast<char>('0' + static_cast<char>(K / 100));
K %= 100;
const char *d = GetDigitsLut() + K * 2;
*buffer++ = d[0];
*buffer++ = d[1];
} else if (K >= 10) {
const char *d = GetDigitsLut() + K * 2;
*buffer++ = d[0];
*buffer++ = d[1];
} else
*buffer++ = static_cast<char>('0' + static_cast<char>(K));
return buffer;
}
inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
const int kk = length + k; // 10^(kk-1) <= v < 10^kk
if (0 <= k && kk <= 21) {
// 1234e7 -> 12340000000
for (int i = length; i < kk; i++) buffer[i] = '0';
buffer[kk] = '.';
buffer[kk + 1] = '0';
return &buffer[kk + 2];
} else if (0 < kk && kk <= 21) {
// 1234e-2 -> 12.34
std::memmove(&buffer[kk + 1], &buffer[kk],
static_cast<size_t>(length - kk));
buffer[kk] = '.';
if (0 > k + maxDecimalPlaces) {
// When maxDecimalPlaces = 2, 1.2345 -> 1.23, 1.102 -> 1.1
// Remove extra trailing zeros (at least one) after truncation.
for (int i = kk + maxDecimalPlaces; i > kk + 1; i--)
if (buffer[i] != '0') return &buffer[i + 1];
return &buffer[kk + 2]; // Reserve one zero
} else
return &buffer[length + 1];
} else if (-6 < kk && kk <= 0) {
// 1234e-6 -> 0.001234
const int offset = 2 - kk;
std::memmove(&buffer[offset], &buffer[0], static_cast<size_t>(length));
buffer[0] = '0';
buffer[1] = '.';
for (int i = 2; i < offset; i++) buffer[i] = '0';
if (length - kk > maxDecimalPlaces) {
// When maxDecimalPlaces = 2, 0.123 -> 0.12, 0.102 -> 0.1
// Remove extra trailing zeros (at least one) after truncation.
for (int i = maxDecimalPlaces + 1; i > 2; i--)
if (buffer[i] != '0') return &buffer[i + 1];
return &buffer[3]; // Reserve one zero
} else
return &buffer[length + offset];
} else if (kk < -maxDecimalPlaces) {
// Truncate to zero
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0';
return &buffer[3];
} else if (length == 1) {
// 1e30
buffer[1] = 'e';
return WriteExponent(kk - 1, &buffer[2]);
} else {
// 1234e30 -> 1.234e33
std::memmove(&buffer[2], &buffer[1], static_cast<size_t>(length - 1));
buffer[1] = '.';
buffer[length + 1] = 'e';
return WriteExponent(kk - 1, &buffer[0 + length + 2]);
}
}
inline char *dtoa(double value, char *buffer, int maxDecimalPlaces = 324) {
RAPIDJSON_ASSERT(maxDecimalPlaces >= 1);
Double d(value);
if (d.IsZero()) {
if (d.Sign()) *buffer++ = '-'; // -0.0, Issue #289
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0';
return &buffer[3];
} else {
if (value < 0) {
*buffer++ = '-';
value = -value;
}
int length, K;
Grisu2(value, buffer, &length, &K);
return Prettify(buffer, length, K, maxDecimalPlaces);
}
}
#ifdef __GNUC__
RAPIDJSON_DIAG_POP
#endif
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_DTOA_

100
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/ieee754.h vendored Normal file
View File

@ -0,0 +1,100 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_IEEE754_
#define RAPIDJSON_IEEE754_
#include "../rapidjson.h"
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
class Double {
public:
Double() {}
Double(double d) : d_(d) {}
Double(uint64_t u) : u_(u) {}
double Value() const { return d_; }
uint64_t Uint64Value() const { return u_; }
double NextPositiveDouble() const {
RAPIDJSON_ASSERT(!Sign());
return Double(u_ + 1).Value();
}
bool Sign() const { return (u_ & kSignMask) != 0; }
uint64_t Significand() const { return u_ & kSignificandMask; }
int Exponent() const {
return static_cast<int>(((u_ & kExponentMask) >> kSignificandSize) -
kExponentBias);
}
bool IsNan() const {
return (u_ & kExponentMask) == kExponentMask && Significand() != 0;
}
bool IsInf() const {
return (u_ & kExponentMask) == kExponentMask && Significand() == 0;
}
bool IsNanOrInf() const { return (u_ & kExponentMask) == kExponentMask; }
bool IsNormal() const {
return (u_ & kExponentMask) != 0 || Significand() == 0;
}
bool IsZero() const { return (u_ & (kExponentMask | kSignificandMask)) == 0; }
uint64_t IntegerSignificand() const {
return IsNormal() ? Significand() | kHiddenBit : Significand();
}
int IntegerExponent() const {
return (IsNormal() ? Exponent() : kDenormalExponent) - kSignificandSize;
}
uint64_t ToBias() const {
return (u_ & kSignMask) ? ~u_ + 1 : u_ | kSignMask;
}
static int EffectiveSignificandSize(int order) {
if (order >= -1021)
return 53;
else if (order <= -1074)
return 0;
else
return order + 1074;
}
private:
static const int kSignificandSize = 52;
static const int kExponentBias = 0x3FF;
static const int kDenormalExponent = 1 - kExponentBias;
static const uint64_t kSignMask = RAPIDJSON_UINT64_C2(0x80000000, 0x00000000);
static const uint64_t kExponentMask =
RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
static const uint64_t kSignificandMask =
RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
static const uint64_t kHiddenBit =
RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
union {
double d_;
uint64_t u_;
};
};
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_IEEE754_

288
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/itoa.h vendored Normal file
View File

@ -0,0 +1,288 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ITOA_
#define RAPIDJSON_ITOA_
#include "../rapidjson.h"
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
inline const char *GetDigitsLut() {
static const char cDigitsLut[200] = {
'0', '0', '0', '1', '0', '2', '0', '3', '0', '4', '0', '5', '0', '6', '0',
'7', '0', '8', '0', '9', '1', '0', '1', '1', '1', '2', '1', '3', '1', '4',
'1', '5', '1', '6', '1', '7', '1', '8', '1', '9', '2', '0', '2', '1', '2',
'2', '2', '3', '2', '4', '2', '5', '2', '6', '2', '7', '2', '8', '2', '9',
'3', '0', '3', '1', '3', '2', '3', '3', '3', '4', '3', '5', '3', '6', '3',
'7', '3', '8', '3', '9', '4', '0', '4', '1', '4', '2', '4', '3', '4', '4',
'4', '5', '4', '6', '4', '7', '4', '8', '4', '9', '5', '0', '5', '1', '5',
'2', '5', '3', '5', '4', '5', '5', '5', '6', '5', '7', '5', '8', '5', '9',
'6', '0', '6', '1', '6', '2', '6', '3', '6', '4', '6', '5', '6', '6', '6',
'7', '6', '8', '6', '9', '7', '0', '7', '1', '7', '2', '7', '3', '7', '4',
'7', '5', '7', '6', '7', '7', '7', '8', '7', '9', '8', '0', '8', '1', '8',
'2', '8', '3', '8', '4', '8', '5', '8', '6', '8', '7', '8', '8', '8', '9',
'9', '0', '9', '1', '9', '2', '9', '3', '9', '4', '9', '5', '9', '6', '9',
'7', '9', '8', '9', '9'};
return cDigitsLut;
}
inline char *u32toa(uint32_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0);
const char *cDigitsLut = GetDigitsLut();
if (value < 10000) {
const uint32_t d1 = (value / 100) << 1;
const uint32_t d2 = (value % 100) << 1;
if (value >= 1000) *buffer++ = cDigitsLut[d1];
if (value >= 100) *buffer++ = cDigitsLut[d1 + 1];
if (value >= 10) *buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
} else if (value < 100000000) {
// value = bbbbcccc
const uint32_t b = value / 10000;
const uint32_t c = value % 10000;
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
if (value >= 10000000) *buffer++ = cDigitsLut[d1];
if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
if (value >= 100000) *buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
} else {
// value = aabbbbcccc in decimal
const uint32_t a = value / 100000000; // 1 to 42
value %= 100000000;
if (a >= 10) {
const unsigned i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
} else
*buffer++ = static_cast<char>('0' + static_cast<char>(a));
const uint32_t b = value / 10000; // 0 to 9999
const uint32_t c = value % 10000; // 0 to 9999
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
*buffer++ = cDigitsLut[d1];
*buffer++ = cDigitsLut[d1 + 1];
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
return buffer;
}
inline char *i32toa(int32_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0);
uint32_t u = static_cast<uint32_t>(value);
if (value < 0) {
*buffer++ = '-';
u = ~u + 1;
}
return u32toa(u, buffer);
}
inline char *u64toa(uint64_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0);
const char *cDigitsLut = GetDigitsLut();
const uint64_t kTen8 = 100000000;
const uint64_t kTen9 = kTen8 * 10;
const uint64_t kTen10 = kTen8 * 100;
const uint64_t kTen11 = kTen8 * 1000;
const uint64_t kTen12 = kTen8 * 10000;
const uint64_t kTen13 = kTen8 * 100000;
const uint64_t kTen14 = kTen8 * 1000000;
const uint64_t kTen15 = kTen8 * 10000000;
const uint64_t kTen16 = kTen8 * kTen8;
if (value < kTen8) {
uint32_t v = static_cast<uint32_t>(value);
if (v < 10000) {
const uint32_t d1 = (v / 100) << 1;
const uint32_t d2 = (v % 100) << 1;
if (v >= 1000) *buffer++ = cDigitsLut[d1];
if (v >= 100) *buffer++ = cDigitsLut[d1 + 1];
if (v >= 10) *buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
} else {
// value = bbbbcccc
const uint32_t b = v / 10000;
const uint32_t c = v % 10000;
const uint32_t d1 = (b / 100) << 1;
const uint32_t d2 = (b % 100) << 1;
const uint32_t d3 = (c / 100) << 1;
const uint32_t d4 = (c % 100) << 1;
if (value >= 10000000) *buffer++ = cDigitsLut[d1];
if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
if (value >= 100000) *buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
}
} else if (value < kTen16) {
const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
const uint32_t b0 = v0 / 10000;
const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
if (value >= kTen15) *buffer++ = cDigitsLut[d1];
if (value >= kTen14) *buffer++ = cDigitsLut[d1 + 1];
if (value >= kTen13) *buffer++ = cDigitsLut[d2];
if (value >= kTen12) *buffer++ = cDigitsLut[d2 + 1];
if (value >= kTen11) *buffer++ = cDigitsLut[d3];
if (value >= kTen10) *buffer++ = cDigitsLut[d3 + 1];
if (value >= kTen9) *buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5];
*buffer++ = cDigitsLut[d5 + 1];
*buffer++ = cDigitsLut[d6];
*buffer++ = cDigitsLut[d6 + 1];
*buffer++ = cDigitsLut[d7];
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
} else {
const uint32_t a = static_cast<uint32_t>(value / kTen16); // 1 to 1844
value %= kTen16;
if (a < 10)
*buffer++ = static_cast<char>('0' + static_cast<char>(a));
else if (a < 100) {
const uint32_t i = a << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
} else if (a < 1000) {
*buffer++ = static_cast<char>('0' + static_cast<char>(a / 100));
const uint32_t i = (a % 100) << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
} else {
const uint32_t i = (a / 100) << 1;
const uint32_t j = (a % 100) << 1;
*buffer++ = cDigitsLut[i];
*buffer++ = cDigitsLut[i + 1];
*buffer++ = cDigitsLut[j];
*buffer++ = cDigitsLut[j + 1];
}
const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
const uint32_t b0 = v0 / 10000;
const uint32_t c0 = v0 % 10000;
const uint32_t d1 = (b0 / 100) << 1;
const uint32_t d2 = (b0 % 100) << 1;
const uint32_t d3 = (c0 / 100) << 1;
const uint32_t d4 = (c0 % 100) << 1;
const uint32_t b1 = v1 / 10000;
const uint32_t c1 = v1 % 10000;
const uint32_t d5 = (b1 / 100) << 1;
const uint32_t d6 = (b1 % 100) << 1;
const uint32_t d7 = (c1 / 100) << 1;
const uint32_t d8 = (c1 % 100) << 1;
*buffer++ = cDigitsLut[d1];
*buffer++ = cDigitsLut[d1 + 1];
*buffer++ = cDigitsLut[d2];
*buffer++ = cDigitsLut[d2 + 1];
*buffer++ = cDigitsLut[d3];
*buffer++ = cDigitsLut[d3 + 1];
*buffer++ = cDigitsLut[d4];
*buffer++ = cDigitsLut[d4 + 1];
*buffer++ = cDigitsLut[d5];
*buffer++ = cDigitsLut[d5 + 1];
*buffer++ = cDigitsLut[d6];
*buffer++ = cDigitsLut[d6 + 1];
*buffer++ = cDigitsLut[d7];
*buffer++ = cDigitsLut[d7 + 1];
*buffer++ = cDigitsLut[d8];
*buffer++ = cDigitsLut[d8 + 1];
}
return buffer;
}
inline char *i64toa(int64_t value, char *buffer) {
RAPIDJSON_ASSERT(buffer != 0);
uint64_t u = static_cast<uint64_t>(value);
if (value < 0) {
*buffer++ = '-';
u = ~u + 1;
}
return u64toa(u, buffer);
}
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_ITOA_

243
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/meta.h vendored Normal file
View File

@ -0,0 +1,243 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_META_H_
#define RAPIDJSON_INTERNAL_META_H_
#include "../rapidjson.h"
#ifdef __GNUC__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
#endif
#if defined(_MSC_VER) && !defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(6334)
#endif
#if RAPIDJSON_HAS_CXX11_TYPETRAITS
#include <type_traits>
#endif
//@cond RAPIDJSON_INTERNAL
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
// Helper to wrap/convert arbitrary types to void, useful for arbitrary type
// matching
template <typename T>
struct Void {
typedef void Type;
};
///////////////////////////////////////////////////////////////////////////////
// BoolType, TrueType, FalseType
//
template <bool Cond>
struct BoolType {
static const bool Value = Cond;
typedef BoolType Type;
};
typedef BoolType<true> TrueType;
typedef BoolType<false> FalseType;
///////////////////////////////////////////////////////////////////////////////
// SelectIf, BoolExpr, NotExpr, AndExpr, OrExpr
//
template <bool C>
struct SelectIfImpl {
template <typename T1, typename T2>
struct Apply {
typedef T1 Type;
};
};
template <>
struct SelectIfImpl<false> {
template <typename T1, typename T2>
struct Apply {
typedef T2 Type;
};
};
template <bool C, typename T1, typename T2>
struct SelectIfCond : SelectIfImpl<C>::template Apply<T1, T2> {};
template <typename C, typename T1, typename T2>
struct SelectIf : SelectIfCond<C::Value, T1, T2> {};
template <bool Cond1, bool Cond2>
struct AndExprCond : FalseType {};
template <>
struct AndExprCond<true, true> : TrueType {};
template <bool Cond1, bool Cond2>
struct OrExprCond : TrueType {};
template <>
struct OrExprCond<false, false> : FalseType {};
template <typename C>
struct BoolExpr : SelectIf<C, TrueType, FalseType>::Type {};
template <typename C>
struct NotExpr : SelectIf<C, FalseType, TrueType>::Type {};
template <typename C1, typename C2>
struct AndExpr : AndExprCond<C1::Value, C2::Value>::Type {};
template <typename C1, typename C2>
struct OrExpr : OrExprCond<C1::Value, C2::Value>::Type {};
///////////////////////////////////////////////////////////////////////////////
// AddConst, MaybeAddConst, RemoveConst
template <typename T>
struct AddConst {
typedef const T Type;
};
template <bool Constify, typename T>
struct MaybeAddConst : SelectIfCond<Constify, const T, T> {};
template <typename T>
struct RemoveConst {
typedef T Type;
};
template <typename T>
struct RemoveConst<const T> {
typedef T Type;
};
///////////////////////////////////////////////////////////////////////////////
// IsSame, IsConst, IsMoreConst, IsPointer
//
template <typename T, typename U>
struct IsSame : FalseType {};
template <typename T>
struct IsSame<T, T> : TrueType {};
template <typename T>
struct IsConst : FalseType {};
template <typename T>
struct IsConst<const T> : TrueType {};
template <typename CT, typename T>
struct IsMoreConst
: AndExpr<
IsSame<typename RemoveConst<CT>::Type, typename RemoveConst<T>::Type>,
BoolType<IsConst<CT>::Value >= IsConst<T>::Value>>::Type {};
template <typename T>
struct IsPointer : FalseType {};
template <typename T>
struct IsPointer<T *> : TrueType {};
///////////////////////////////////////////////////////////////////////////////
// IsBaseOf
//
#if RAPIDJSON_HAS_CXX11_TYPETRAITS
template <typename B, typename D>
struct IsBaseOf : BoolType<::std::is_base_of<B, D>::value> {};
#else // simplified version adopted from Boost
template <typename B, typename D>
struct IsBaseOfImpl {
RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0);
RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0);
typedef char (&Yes)[1];
typedef char (&No)[2];
template <typename T>
static Yes Check(const D *, T);
static No Check(const B *, int);
struct Host {
operator const B *() const;
operator const D *();
};
enum { Value = (sizeof(Check(Host(), 0)) == sizeof(Yes)) };
};
template <typename B, typename D>
struct IsBaseOf : OrExpr<IsSame<B, D>, BoolExpr<IsBaseOfImpl<B, D>>>::Type {};
#endif // RAPIDJSON_HAS_CXX11_TYPETRAITS
//////////////////////////////////////////////////////////////////////////
// EnableIf / DisableIf
//
template <bool Condition, typename T = void>
struct EnableIfCond {
typedef T Type;
};
template <typename T>
struct EnableIfCond<false, T> { /* empty */
};
template <bool Condition, typename T = void>
struct DisableIfCond {
typedef T Type;
};
template <typename T>
struct DisableIfCond<true, T> { /* empty */
};
template <typename Condition, typename T = void>
struct EnableIf : EnableIfCond<Condition::Value, T> {};
template <typename Condition, typename T = void>
struct DisableIf : DisableIfCond<Condition::Value, T> {};
// SFINAE helpers
struct SfinaeTag {};
template <typename T>
struct RemoveSfinaeTag;
template <typename T>
struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
typedef T Type;
};
#define RAPIDJSON_REMOVEFPTR_(type) \
typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag< \
::RAPIDJSON_NAMESPACE::internal::SfinaeTag &(*)type>::Type
#define RAPIDJSON_ENABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf<RAPIDJSON_REMOVEFPTR_( \
cond)>::Type * = NULL
#define RAPIDJSON_DISABLEIF(cond) \
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf<RAPIDJSON_REMOVEFPTR_( \
cond)>::Type * = NULL
#define RAPIDJSON_ENABLEIF_RETURN(cond, returntype) \
typename ::RAPIDJSON_NAMESPACE::internal::EnableIf< \
RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
#define RAPIDJSON_DISABLEIF_RETURN(cond, returntype) \
typename ::RAPIDJSON_NAMESPACE::internal::DisableIf< \
RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
} // namespace internal
RAPIDJSON_NAMESPACE_END
//@endcond
#if defined(_MSC_VER) && !defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#ifdef __GNUC__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_INTERNAL_META_H_

77
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/pow10.h vendored Normal file
View File

@ -0,0 +1,77 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_POW10_
#define RAPIDJSON_POW10_
#include "../rapidjson.h"
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
//! Computes integer powers of 10 in double (10.0^n).
/*! This function uses lookup table for fast and accurate results.
\param n non-negative exponent. Must <= 308.
\return 10.0^n
*/
inline double Pow10(int n) {
static const double e[] = {
// 1e-0...1e308: 309 * 8 bytes = 2472 bytes
1e+0, 1e+1, 1e+2, 1e+3, 1e+4, 1e+5, 1e+6, 1e+7, 1e+8,
1e+9, 1e+10, 1e+11, 1e+12, 1e+13, 1e+14, 1e+15, 1e+16, 1e+17,
1e+18, 1e+19, 1e+20, 1e+21, 1e+22, 1e+23, 1e+24, 1e+25, 1e+26,
1e+27, 1e+28, 1e+29, 1e+30, 1e+31, 1e+32, 1e+33, 1e+34, 1e+35,
1e+36, 1e+37, 1e+38, 1e+39, 1e+40, 1e+41, 1e+42, 1e+43, 1e+44,
1e+45, 1e+46, 1e+47, 1e+48, 1e+49, 1e+50, 1e+51, 1e+52, 1e+53,
1e+54, 1e+55, 1e+56, 1e+57, 1e+58, 1e+59, 1e+60, 1e+61, 1e+62,
1e+63, 1e+64, 1e+65, 1e+66, 1e+67, 1e+68, 1e+69, 1e+70, 1e+71,
1e+72, 1e+73, 1e+74, 1e+75, 1e+76, 1e+77, 1e+78, 1e+79, 1e+80,
1e+81, 1e+82, 1e+83, 1e+84, 1e+85, 1e+86, 1e+87, 1e+88, 1e+89,
1e+90, 1e+91, 1e+92, 1e+93, 1e+94, 1e+95, 1e+96, 1e+97, 1e+98,
1e+99, 1e+100, 1e+101, 1e+102, 1e+103, 1e+104, 1e+105, 1e+106, 1e+107,
1e+108, 1e+109, 1e+110, 1e+111, 1e+112, 1e+113, 1e+114, 1e+115, 1e+116,
1e+117, 1e+118, 1e+119, 1e+120, 1e+121, 1e+122, 1e+123, 1e+124, 1e+125,
1e+126, 1e+127, 1e+128, 1e+129, 1e+130, 1e+131, 1e+132, 1e+133, 1e+134,
1e+135, 1e+136, 1e+137, 1e+138, 1e+139, 1e+140, 1e+141, 1e+142, 1e+143,
1e+144, 1e+145, 1e+146, 1e+147, 1e+148, 1e+149, 1e+150, 1e+151, 1e+152,
1e+153, 1e+154, 1e+155, 1e+156, 1e+157, 1e+158, 1e+159, 1e+160, 1e+161,
1e+162, 1e+163, 1e+164, 1e+165, 1e+166, 1e+167, 1e+168, 1e+169, 1e+170,
1e+171, 1e+172, 1e+173, 1e+174, 1e+175, 1e+176, 1e+177, 1e+178, 1e+179,
1e+180, 1e+181, 1e+182, 1e+183, 1e+184, 1e+185, 1e+186, 1e+187, 1e+188,
1e+189, 1e+190, 1e+191, 1e+192, 1e+193, 1e+194, 1e+195, 1e+196, 1e+197,
1e+198, 1e+199, 1e+200, 1e+201, 1e+202, 1e+203, 1e+204, 1e+205, 1e+206,
1e+207, 1e+208, 1e+209, 1e+210, 1e+211, 1e+212, 1e+213, 1e+214, 1e+215,
1e+216, 1e+217, 1e+218, 1e+219, 1e+220, 1e+221, 1e+222, 1e+223, 1e+224,
1e+225, 1e+226, 1e+227, 1e+228, 1e+229, 1e+230, 1e+231, 1e+232, 1e+233,
1e+234, 1e+235, 1e+236, 1e+237, 1e+238, 1e+239, 1e+240, 1e+241, 1e+242,
1e+243, 1e+244, 1e+245, 1e+246, 1e+247, 1e+248, 1e+249, 1e+250, 1e+251,
1e+252, 1e+253, 1e+254, 1e+255, 1e+256, 1e+257, 1e+258, 1e+259, 1e+260,
1e+261, 1e+262, 1e+263, 1e+264, 1e+265, 1e+266, 1e+267, 1e+268, 1e+269,
1e+270, 1e+271, 1e+272, 1e+273, 1e+274, 1e+275, 1e+276, 1e+277, 1e+278,
1e+279, 1e+280, 1e+281, 1e+282, 1e+283, 1e+284, 1e+285, 1e+286, 1e+287,
1e+288, 1e+289, 1e+290, 1e+291, 1e+292, 1e+293, 1e+294, 1e+295, 1e+296,
1e+297, 1e+298, 1e+299, 1e+300, 1e+301, 1e+302, 1e+303, 1e+304, 1e+305,
1e+306, 1e+307, 1e+308};
RAPIDJSON_ASSERT(n >= 0 && n <= 308);
return e[n];
}
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_POW10_

753
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/regex.h vendored Normal file
View File

@ -0,0 +1,753 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_REGEX_H_
#define RAPIDJSON_INTERNAL_REGEX_H_
#include "../allocators.h"
#include "../stream.h"
#include "stack.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
RAPIDJSON_DIAG_OFF(switch - enum)
#elif defined(_MSC_VER)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif
#ifdef __GNUC__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
#endif
#ifndef RAPIDJSON_REGEX_VERBOSE
#define RAPIDJSON_REGEX_VERBOSE 0
#endif
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
///////////////////////////////////////////////////////////////////////////////
// DecodedStream
template <typename SourceStream, typename Encoding>
class DecodedStream {
public:
DecodedStream(SourceStream &ss) : ss_(ss), codepoint_() { Decode(); }
unsigned Peek() { return codepoint_; }
unsigned Take() {
unsigned c = codepoint_;
if (c) // No further decoding when '\0'
Decode();
return c;
}
private:
void Decode() {
if (!Encoding::Decode(ss_, &codepoint_)) codepoint_ = 0;
}
SourceStream &ss_;
unsigned codepoint_;
};
///////////////////////////////////////////////////////////////////////////////
// GenericRegex
static const SizeType kRegexInvalidState = ~SizeType(
0); //!< Represents an invalid index in GenericRegex::State::out, out1
static const SizeType kRegexInvalidRange = ~SizeType(0);
template <typename Encoding, typename Allocator>
class GenericRegexSearch;
//! Regular expression engine with subset of ECMAscript grammar.
/*!
Supported regular expression syntax:
- \c ab Concatenation
- \c a|b Alternation
- \c a? Zero or one
- \c a* Zero or more
- \c a+ One or more
- \c a{3} Exactly 3 times
- \c a{3,} At least 3 times
- \c a{3,5} 3 to 5 times
- \c (ab) Grouping
- \c ^a At the beginning
- \c a$ At the end
- \c . Any character
- \c [abc] Character classes
- \c [a-c] Character class range
- \c [a-z0-9_] Character class combination
- \c [^abc] Negated character classes
- \c [^a-c] Negated character class range
- \c [\b] Backspace (U+0008)
- \c \\| \\\\ ... Escape characters
- \c \\f Form feed (U+000C)
- \c \\n Line feed (U+000A)
- \c \\r Carriage return (U+000D)
- \c \\t Tab (U+0009)
- \c \\v Vertical tab (U+000B)
\note This is a Thompson NFA engine, implemented with reference to
Cox, Russ. "Regular Expression Matching Can Be Simple And Fast (but is
slow in Java, Perl, PHP, Python, Ruby,...).",
https://swtch.com/~rsc/regexp/regexp1.html
*/
template <typename Encoding, typename Allocator = CrtAllocator>
class GenericRegex {
public:
typedef Encoding EncodingType;
typedef typename Encoding::Ch Ch;
template <typename, typename>
friend class GenericRegexSearch;
GenericRegex(const Ch *source, Allocator *allocator = 0)
: ownAllocator_(allocator ? 0 : RAPIDJSON_NEW(Allocator)()),
allocator_(allocator ? allocator : ownAllocator_),
states_(allocator_, 256),
ranges_(allocator_, 256),
root_(kRegexInvalidState),
stateCount_(),
rangeCount_(),
anchorBegin_(),
anchorEnd_() {
GenericStringStream<Encoding> ss(source);
DecodedStream<GenericStringStream<Encoding>, Encoding> ds(ss);
Parse(ds);
}
~GenericRegex() { RAPIDJSON_DELETE(ownAllocator_); }
bool IsValid() const { return root_ != kRegexInvalidState; }
private:
enum Operator {
kZeroOrOne,
kZeroOrMore,
kOneOrMore,
kConcatenation,
kAlternation,
kLeftParenthesis
};
static const unsigned kAnyCharacterClass = 0xFFFFFFFF; //!< For '.'
static const unsigned kRangeCharacterClass = 0xFFFFFFFE;
static const unsigned kRangeNegationFlag = 0x80000000;
struct Range {
unsigned start; //
unsigned end;
SizeType next;
};
struct State {
SizeType out; //!< Equals to kInvalid for matching state
SizeType out1; //!< Equals to non-kInvalid for split
SizeType rangeStart;
unsigned codepoint;
};
struct Frag {
Frag(SizeType s, SizeType o, SizeType m) : start(s), out(o), minIndex(m) {}
SizeType start;
SizeType out; //!< link-list of all output states
SizeType minIndex;
};
State &GetState(SizeType index) {
RAPIDJSON_ASSERT(index < stateCount_);
return states_.template Bottom<State>()[index];
}
const State &GetState(SizeType index) const {
RAPIDJSON_ASSERT(index < stateCount_);
return states_.template Bottom<State>()[index];
}
Range &GetRange(SizeType index) {
RAPIDJSON_ASSERT(index < rangeCount_);
return ranges_.template Bottom<Range>()[index];
}
const Range &GetRange(SizeType index) const {
RAPIDJSON_ASSERT(index < rangeCount_);
return ranges_.template Bottom<Range>()[index];
}
template <typename InputStream>
void Parse(DecodedStream<InputStream, Encoding> &ds) {
Stack<Allocator> operandStack(allocator_, 256); // Frag
Stack<Allocator> operatorStack(allocator_, 256); // Operator
Stack<Allocator> atomCountStack(allocator_,
256); // unsigned (Atom per parenthesis)
*atomCountStack.template Push<unsigned>() = 0;
unsigned codepoint;
while (ds.Peek() != 0) {
switch (codepoint = ds.Take()) {
case '^':
anchorBegin_ = true;
break;
case '$':
anchorEnd_ = true;
break;
case '|':
while (!operatorStack.Empty() &&
*operatorStack.template Top<Operator>() < kAlternation)
if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
return;
*operatorStack.template Push<Operator>() = kAlternation;
*atomCountStack.template Top<unsigned>() = 0;
break;
case '(':
*operatorStack.template Push<Operator>() = kLeftParenthesis;
*atomCountStack.template Push<unsigned>() = 0;
break;
case ')':
while (!operatorStack.Empty() &&
*operatorStack.template Top<Operator>() != kLeftParenthesis)
if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
return;
if (operatorStack.Empty()) return;
operatorStack.template Pop<Operator>(1);
atomCountStack.template Pop<unsigned>(1);
ImplicitConcatenation(atomCountStack, operatorStack);
break;
case '?':
if (!Eval(operandStack, kZeroOrOne)) return;
break;
case '*':
if (!Eval(operandStack, kZeroOrMore)) return;
break;
case '+':
if (!Eval(operandStack, kOneOrMore)) return;
break;
case '{': {
unsigned n, m;
if (!ParseUnsigned(ds, &n)) return;
if (ds.Peek() == ',') {
ds.Take();
if (ds.Peek() == '}')
m = kInfinityQuantifier;
else if (!ParseUnsigned(ds, &m) || m < n)
return;
} else
m = n;
if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}') return;
ds.Take();
} break;
case '.':
PushOperand(operandStack, kAnyCharacterClass);
ImplicitConcatenation(atomCountStack, operatorStack);
break;
case '[': {
SizeType range;
if (!ParseRange(ds, &range)) return;
SizeType s = NewState(kRegexInvalidState, kRegexInvalidState,
kRangeCharacterClass);
GetState(s).rangeStart = range;
*operandStack.template Push<Frag>() = Frag(s, s, s);
}
ImplicitConcatenation(atomCountStack, operatorStack);
break;
case '\\': // Escape character
if (!CharacterEscape(ds, &codepoint))
return; // Unsupported escape character
// fall through to default
RAPIDJSON_DELIBERATE_FALLTHROUGH;
default: // Pattern character
PushOperand(operandStack, codepoint);
ImplicitConcatenation(atomCountStack, operatorStack);
}
}
while (!operatorStack.Empty())
if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1))) return;
// Link the operand to matching state.
if (operandStack.GetSize() == sizeof(Frag)) {
Frag *e = operandStack.template Pop<Frag>(1);
Patch(e->out, NewState(kRegexInvalidState, kRegexInvalidState, 0));
root_ = e->start;
#if RAPIDJSON_REGEX_VERBOSE
printf("root: %d\n", root_);
for (SizeType i = 0; i < stateCount_; i++) {
State &s = GetState(i);
printf("[%2d] out: %2d out1: %2d c: '%c'\n", i, s.out, s.out1,
(char)s.codepoint);
}
printf("\n");
#endif
}
}
SizeType NewState(SizeType out, SizeType out1, unsigned codepoint) {
State *s = states_.template Push<State>();
s->out = out;
s->out1 = out1;
s->codepoint = codepoint;
s->rangeStart = kRegexInvalidRange;
return stateCount_++;
}
void PushOperand(Stack<Allocator> &operandStack, unsigned codepoint) {
SizeType s = NewState(kRegexInvalidState, kRegexInvalidState, codepoint);
*operandStack.template Push<Frag>() = Frag(s, s, s);
}
void ImplicitConcatenation(Stack<Allocator> &atomCountStack,
Stack<Allocator> &operatorStack) {
if (*atomCountStack.template Top<unsigned>())
*operatorStack.template Push<Operator>() = kConcatenation;
(*atomCountStack.template Top<unsigned>())++;
}
SizeType Append(SizeType l1, SizeType l2) {
SizeType old = l1;
while (GetState(l1).out != kRegexInvalidState) l1 = GetState(l1).out;
GetState(l1).out = l2;
return old;
}
void Patch(SizeType l, SizeType s) {
for (SizeType next; l != kRegexInvalidState; l = next) {
next = GetState(l).out;
GetState(l).out = s;
}
}
bool Eval(Stack<Allocator> &operandStack, Operator op) {
switch (op) {
case kConcatenation:
RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag) * 2);
{
Frag e2 = *operandStack.template Pop<Frag>(1);
Frag e1 = *operandStack.template Pop<Frag>(1);
Patch(e1.out, e2.start);
*operandStack.template Push<Frag>() =
Frag(e1.start, e2.out, Min(e1.minIndex, e2.minIndex));
}
return true;
case kAlternation:
if (operandStack.GetSize() >= sizeof(Frag) * 2) {
Frag e2 = *operandStack.template Pop<Frag>(1);
Frag e1 = *operandStack.template Pop<Frag>(1);
SizeType s = NewState(e1.start, e2.start, 0);
*operandStack.template Push<Frag>() =
Frag(s, Append(e1.out, e2.out), Min(e1.minIndex, e2.minIndex));
return true;
}
return false;
case kZeroOrOne:
if (operandStack.GetSize() >= sizeof(Frag)) {
Frag e = *operandStack.template Pop<Frag>(1);
SizeType s = NewState(kRegexInvalidState, e.start, 0);
*operandStack.template Push<Frag>() =
Frag(s, Append(e.out, s), e.minIndex);
return true;
}
return false;
case kZeroOrMore:
if (operandStack.GetSize() >= sizeof(Frag)) {
Frag e = *operandStack.template Pop<Frag>(1);
SizeType s = NewState(kRegexInvalidState, e.start, 0);
Patch(e.out, s);
*operandStack.template Push<Frag>() = Frag(s, s, e.minIndex);
return true;
}
return false;
case kOneOrMore:
if (operandStack.GetSize() >= sizeof(Frag)) {
Frag e = *operandStack.template Pop<Frag>(1);
SizeType s = NewState(kRegexInvalidState, e.start, 0);
Patch(e.out, s);
*operandStack.template Push<Frag>() = Frag(e.start, s, e.minIndex);
return true;
}
return false;
default:
// syntax error (e.g. unclosed kLeftParenthesis)
return false;
}
}
bool EvalQuantifier(Stack<Allocator> &operandStack, unsigned n, unsigned m) {
RAPIDJSON_ASSERT(n <= m);
RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag));
if (n == 0) {
if (m == 0) // a{0} not support
return false;
else if (m == kInfinityQuantifier)
Eval(operandStack, kZeroOrMore); // a{0,} -> a*
else {
Eval(operandStack, kZeroOrOne); // a{0,5} -> a?
for (unsigned i = 0; i < m - 1; i++)
CloneTopOperand(operandStack); // a{0,5} -> a? a? a? a? a?
for (unsigned i = 0; i < m - 1; i++)
Eval(operandStack, kConcatenation); // a{0,5} -> a?a?a?a?a?
}
return true;
}
for (unsigned i = 0; i < n - 1; i++) // a{3} -> a a a
CloneTopOperand(operandStack);
if (m == kInfinityQuantifier)
Eval(operandStack, kOneOrMore); // a{3,} -> a a a+
else if (m > n) {
CloneTopOperand(operandStack); // a{3,5} -> a a a a
Eval(operandStack, kZeroOrOne); // a{3,5} -> a a a a?
for (unsigned i = n; i < m - 1; i++)
CloneTopOperand(operandStack); // a{3,5} -> a a a a? a?
for (unsigned i = n; i < m; i++)
Eval(operandStack, kConcatenation); // a{3,5} -> a a aa?a?
}
for (unsigned i = 0; i < n - 1; i++)
Eval(operandStack,
kConcatenation); // a{3} -> aaa, a{3,} -> aaa+, a{3.5} -> aaaa?a?
return true;
}
static SizeType Min(SizeType a, SizeType b) { return a < b ? a : b; }
void CloneTopOperand(Stack<Allocator> &operandStack) {
const Frag src =
*operandStack
.template Top<Frag>(); // Copy constructor to prevent invalidation
SizeType count =
stateCount_ - src.minIndex; // Assumes top operand contains states in
// [src->minIndex, stateCount_)
State *s = states_.template Push<State>(count);
memcpy(s, &GetState(src.minIndex), count * sizeof(State));
for (SizeType j = 0; j < count; j++) {
if (s[j].out != kRegexInvalidState) s[j].out += count;
if (s[j].out1 != kRegexInvalidState) s[j].out1 += count;
}
*operandStack.template Push<Frag>() =
Frag(src.start + count, src.out + count, src.minIndex + count);
stateCount_ += count;
}
template <typename InputStream>
bool ParseUnsigned(DecodedStream<InputStream, Encoding> &ds, unsigned *u) {
unsigned r = 0;
if (ds.Peek() < '0' || ds.Peek() > '9') return false;
while (ds.Peek() >= '0' && ds.Peek() <= '9') {
if (r >= 429496729 && ds.Peek() > '5') // 2^32 - 1 = 4294967295
return false; // overflow
r = r * 10 + (ds.Take() - '0');
}
*u = r;
return true;
}
template <typename InputStream>
bool ParseRange(DecodedStream<InputStream, Encoding> &ds, SizeType *range) {
bool isBegin = true;
bool negate = false;
int step = 0;
SizeType start = kRegexInvalidRange;
SizeType current = kRegexInvalidRange;
unsigned codepoint;
while ((codepoint = ds.Take()) != 0) {
if (isBegin) {
isBegin = false;
if (codepoint == '^') {
negate = true;
continue;
}
}
switch (codepoint) {
case ']':
if (start == kRegexInvalidRange)
return false; // Error: nothing inside []
if (step == 2) { // Add trailing '-'
SizeType r = NewRange('-');
RAPIDJSON_ASSERT(current != kRegexInvalidRange);
GetRange(current).next = r;
}
if (negate) GetRange(start).start |= kRangeNegationFlag;
*range = start;
return true;
case '\\':
if (ds.Peek() == 'b') {
ds.Take();
codepoint = 0x0008; // Escape backspace character
} else if (!CharacterEscape(ds, &codepoint))
return false;
// fall through to default
RAPIDJSON_DELIBERATE_FALLTHROUGH;
default:
switch (step) {
case 1:
if (codepoint == '-') {
step++;
break;
}
// fall through to step 0 for other characters
RAPIDJSON_DELIBERATE_FALLTHROUGH;
case 0: {
SizeType r = NewRange(codepoint);
if (current != kRegexInvalidRange) GetRange(current).next = r;
if (start == kRegexInvalidRange) start = r;
current = r;
}
step = 1;
break;
default:
RAPIDJSON_ASSERT(step == 2);
GetRange(current).end = codepoint;
step = 0;
}
}
}
return false;
}
SizeType NewRange(unsigned codepoint) {
Range *r = ranges_.template Push<Range>();
r->start = r->end = codepoint;
r->next = kRegexInvalidRange;
return rangeCount_++;
}
template <typename InputStream>
bool CharacterEscape(DecodedStream<InputStream, Encoding> &ds,
unsigned *escapedCodepoint) {
unsigned codepoint;
switch (codepoint = ds.Take()) {
case '^':
case '$':
case '|':
case '(':
case ')':
case '?':
case '*':
case '+':
case '.':
case '[':
case ']':
case '{':
case '}':
case '\\':
*escapedCodepoint = codepoint;
return true;
case 'f':
*escapedCodepoint = 0x000C;
return true;
case 'n':
*escapedCodepoint = 0x000A;
return true;
case 'r':
*escapedCodepoint = 0x000D;
return true;
case 't':
*escapedCodepoint = 0x0009;
return true;
case 'v':
*escapedCodepoint = 0x000B;
return true;
default:
return false; // Unsupported escape character
}
}
Allocator *ownAllocator_;
Allocator *allocator_;
Stack<Allocator> states_;
Stack<Allocator> ranges_;
SizeType root_;
SizeType stateCount_;
SizeType rangeCount_;
static const unsigned kInfinityQuantifier = ~0u;
// For SearchWithAnchoring()
bool anchorBegin_;
bool anchorEnd_;
};
template <typename RegexType, typename Allocator = CrtAllocator>
class GenericRegexSearch {
public:
typedef typename RegexType::EncodingType Encoding;
typedef typename Encoding::Ch Ch;
GenericRegexSearch(const RegexType &regex, Allocator *allocator = 0)
: regex_(regex),
allocator_(allocator),
ownAllocator_(0),
state0_(allocator, 0),
state1_(allocator, 0),
stateSet_() {
RAPIDJSON_ASSERT(regex_.IsValid());
if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
stateSet_ = static_cast<unsigned *>(allocator_->Malloc(GetStateSetSize()));
state0_.template Reserve<SizeType>(regex_.stateCount_);
state1_.template Reserve<SizeType>(regex_.stateCount_);
}
~GenericRegexSearch() {
Allocator::Free(stateSet_);
RAPIDJSON_DELETE(ownAllocator_);
}
template <typename InputStream>
bool Match(InputStream &is) {
return SearchWithAnchoring(is, true, true);
}
bool Match(const Ch *s) {
GenericStringStream<Encoding> is(s);
return Match(is);
}
template <typename InputStream>
bool Search(InputStream &is) {
return SearchWithAnchoring(is, regex_.anchorBegin_, regex_.anchorEnd_);
}
bool Search(const Ch *s) {
GenericStringStream<Encoding> is(s);
return Search(is);
}
private:
typedef typename RegexType::State State;
typedef typename RegexType::Range Range;
template <typename InputStream>
bool SearchWithAnchoring(InputStream &is, bool anchorBegin, bool anchorEnd) {
DecodedStream<InputStream, Encoding> ds(is);
state0_.Clear();
Stack<Allocator> *current = &state0_, *next = &state1_;
const size_t stateSetSize = GetStateSetSize();
std::memset(stateSet_, 0, stateSetSize);
bool matched = AddState(*current, regex_.root_);
unsigned codepoint;
while (!current->Empty() && (codepoint = ds.Take()) != 0) {
std::memset(stateSet_, 0, stateSetSize);
next->Clear();
matched = false;
for (const SizeType *s = current->template Bottom<SizeType>();
s != current->template End<SizeType>(); ++s) {
const State &sr = regex_.GetState(*s);
if (sr.codepoint == codepoint ||
sr.codepoint == RegexType::kAnyCharacterClass ||
(sr.codepoint == RegexType::kRangeCharacterClass &&
MatchRange(sr.rangeStart, codepoint))) {
matched = AddState(*next, sr.out) || matched;
if (!anchorEnd && matched) return true;
}
if (!anchorBegin) AddState(*next, regex_.root_);
}
internal::Swap(current, next);
}
return matched;
}
size_t GetStateSetSize() const { return (regex_.stateCount_ + 31) / 32 * 4; }
// Return whether the added states is a match state
bool AddState(Stack<Allocator> &l, SizeType index) {
RAPIDJSON_ASSERT(index != kRegexInvalidState);
const State &s = regex_.GetState(index);
if (s.out1 != kRegexInvalidState) { // Split
bool matched = AddState(l, s.out);
return AddState(l, s.out1) || matched;
} else if (!(stateSet_[index >> 5] & (1u << (index & 31)))) {
stateSet_[index >> 5] |= (1u << (index & 31));
*l.template PushUnsafe<SizeType>() = index;
}
return s.out ==
kRegexInvalidState; // by using PushUnsafe() above, we can ensure s
// is not validated due to reallocation.
}
bool MatchRange(SizeType rangeIndex, unsigned codepoint) const {
bool yes = (regex_.GetRange(rangeIndex).start &
RegexType::kRangeNegationFlag) == 0;
while (rangeIndex != kRegexInvalidRange) {
const Range &r = regex_.GetRange(rangeIndex);
if (codepoint >= (r.start & ~RegexType::kRangeNegationFlag) &&
codepoint <= r.end)
return yes;
rangeIndex = r.next;
}
return !yes;
}
const RegexType &regex_;
Allocator *allocator_;
Allocator *ownAllocator_;
Stack<Allocator> state0_;
Stack<Allocator> state1_;
uint32_t *stateSet_;
};
typedef GenericRegex<UTF8<>> Regex;
typedef GenericRegexSearch<Regex> RegexSearch;
} // namespace internal
RAPIDJSON_NAMESPACE_END
#ifdef __GNUC__
RAPIDJSON_DIAG_POP
#endif
#if defined(__clang__) || defined(_MSC_VER)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_INTERNAL_REGEX_H_

245
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/stack.h vendored Normal file
View File

@ -0,0 +1,245 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_STACK_H_
#define RAPIDJSON_INTERNAL_STACK_H_
#include <cstddef>
#include "../allocators.h"
#include "swap.h"
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
///////////////////////////////////////////////////////////////////////////////
// Stack
//! A type-unsafe stack for storing different types of data.
/*! \tparam Allocator Allocator for allocating stack memory.
*/
template <typename Allocator>
class Stack {
public:
// Optimization note: Do not allocate memory for stack_ in constructor.
// Do it lazily when first Push() -> Expand() -> Resize().
Stack(Allocator *allocator, size_t stackCapacity)
: allocator_(allocator),
ownAllocator_(0),
stack_(0),
stackTop_(0),
stackEnd_(0),
initialCapacity_(stackCapacity) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
Stack(Stack &&rhs)
: allocator_(rhs.allocator_),
ownAllocator_(rhs.ownAllocator_),
stack_(rhs.stack_),
stackTop_(rhs.stackTop_),
stackEnd_(rhs.stackEnd_),
initialCapacity_(rhs.initialCapacity_) {
rhs.allocator_ = 0;
rhs.ownAllocator_ = 0;
rhs.stack_ = 0;
rhs.stackTop_ = 0;
rhs.stackEnd_ = 0;
rhs.initialCapacity_ = 0;
}
#endif
~Stack() { Destroy(); }
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
Stack &operator=(Stack &&rhs) {
if (&rhs != this) {
Destroy();
allocator_ = rhs.allocator_;
ownAllocator_ = rhs.ownAllocator_;
stack_ = rhs.stack_;
stackTop_ = rhs.stackTop_;
stackEnd_ = rhs.stackEnd_;
initialCapacity_ = rhs.initialCapacity_;
rhs.allocator_ = 0;
rhs.ownAllocator_ = 0;
rhs.stack_ = 0;
rhs.stackTop_ = 0;
rhs.stackEnd_ = 0;
rhs.initialCapacity_ = 0;
}
return *this;
}
#endif
void Swap(Stack &rhs) RAPIDJSON_NOEXCEPT {
internal::Swap(allocator_, rhs.allocator_);
internal::Swap(ownAllocator_, rhs.ownAllocator_);
internal::Swap(stack_, rhs.stack_);
internal::Swap(stackTop_, rhs.stackTop_);
internal::Swap(stackEnd_, rhs.stackEnd_);
internal::Swap(initialCapacity_, rhs.initialCapacity_);
}
void Clear() { stackTop_ = stack_; }
void ShrinkToFit() {
if (Empty()) {
// If the stack is empty, completely deallocate the memory.
Allocator::Free(stack_); // NOLINT (+clang-analyzer-unix.Malloc)
stack_ = 0;
stackTop_ = 0;
stackEnd_ = 0;
} else
Resize(GetSize());
}
// Optimization note: try to minimize the size of this function for force
// inline. Expansion is run very infrequently, so it is moved to another
// (probably non-inline) function.
template <typename T>
RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) {
// Expand the stack if needed
if (RAPIDJSON_UNLIKELY(static_cast<std::ptrdiff_t>(sizeof(T) * count) >
(stackEnd_ - stackTop_)))
Expand<T>(count);
}
template <typename T>
RAPIDJSON_FORCEINLINE T *Push(size_t count = 1) {
Reserve<T>(count);
return PushUnsafe<T>(count);
}
template <typename T>
RAPIDJSON_FORCEINLINE T *PushUnsafe(size_t count = 1) {
RAPIDJSON_ASSERT(stackTop_);
RAPIDJSON_ASSERT(static_cast<std::ptrdiff_t>(sizeof(T) * count) <=
(stackEnd_ - stackTop_));
T *ret = reinterpret_cast<T *>(stackTop_);
stackTop_ += sizeof(T) * count;
return ret;
}
template <typename T>
T *Pop(size_t count) {
RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T));
stackTop_ -= count * sizeof(T);
return reinterpret_cast<T *>(stackTop_);
}
template <typename T>
T *Top() {
RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
return reinterpret_cast<T *>(stackTop_ - sizeof(T));
}
template <typename T>
const T *Top() const {
RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
return reinterpret_cast<T *>(stackTop_ - sizeof(T));
}
template <typename T>
T *End() {
return reinterpret_cast<T *>(stackTop_);
}
template <typename T>
const T *End() const {
return reinterpret_cast<T *>(stackTop_);
}
template <typename T>
T *Bottom() {
return reinterpret_cast<T *>(stack_);
}
template <typename T>
const T *Bottom() const {
return reinterpret_cast<T *>(stack_);
}
bool HasAllocator() const { return allocator_ != 0; }
Allocator &GetAllocator() {
RAPIDJSON_ASSERT(allocator_);
return *allocator_;
}
bool Empty() const { return stackTop_ == stack_; }
size_t GetSize() const { return static_cast<size_t>(stackTop_ - stack_); }
size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); }
private:
template <typename T>
void Expand(size_t count) {
// Only expand the capacity if the current stack exists. Otherwise just
// create a stack with initial capacity.
size_t newCapacity;
if (stack_ == 0) {
if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
newCapacity = initialCapacity_;
} else {
newCapacity = GetCapacity();
newCapacity += (newCapacity + 1) / 2;
}
size_t newSize = GetSize() + sizeof(T) * count;
if (newCapacity < newSize) newCapacity = newSize;
Resize(newCapacity);
}
void Resize(size_t newCapacity) {
const size_t size = GetSize(); // Backup the current size
stack_ = static_cast<char *>(
allocator_->Realloc(stack_, GetCapacity(), newCapacity));
stackTop_ = stack_ + size;
stackEnd_ = stack_ + newCapacity;
}
void Destroy() {
Allocator::Free(stack_);
RAPIDJSON_DELETE(ownAllocator_); // Only delete if it is owned by the stack
}
// Prohibit copy constructor & assignment operator.
Stack(const Stack &);
Stack &operator=(const Stack &);
Allocator *allocator_;
Allocator *ownAllocator_;
char *stack_;
char *stackTop_;
char *stackEnd_;
size_t initialCapacity_;
};
} // namespace internal
RAPIDJSON_NAMESPACE_END
#if defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_STACK_H_

74
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/strfunc.h vendored Normal file
View File

@ -0,0 +1,74 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_STRFUNC_H_
#define RAPIDJSON_INTERNAL_STRFUNC_H_
#include <cwchar>
#include "../stream.h"
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
//! Custom strlen() which works on different character types.
/*! \tparam Ch Character type (e.g. char, wchar_t, short)
\param s Null-terminated input string.
\return Number of characters in the string.
\note This has the same semantics as strlen(), the return value is not
number of Unicode codepoints.
*/
template <typename Ch>
inline SizeType StrLen(const Ch *s) {
RAPIDJSON_ASSERT(s != 0);
const Ch *p = s;
while (*p) ++p;
return SizeType(p - s);
}
template <>
inline SizeType StrLen(const char *s) {
return SizeType(std::strlen(s));
}
template <>
inline SizeType StrLen(const wchar_t *s) {
return SizeType(std::wcslen(s));
}
//! Returns number of code points in a encoded string.
template <typename Encoding>
bool CountStringCodePoint(const typename Encoding::Ch *s, SizeType length,
SizeType *outCount) {
RAPIDJSON_ASSERT(s != 0);
RAPIDJSON_ASSERT(outCount != 0);
GenericStringStream<Encoding> is(s);
const typename Encoding::Ch *end = s + length;
SizeType count = 0;
while (is.src_ < end) {
unsigned codepoint;
if (!Encoding::Decode(is, &codepoint)) return false;
count++;
}
*outCount = count;
return true;
}
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_INTERNAL_STRFUNC_H_

303
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/strtod.h vendored Normal file
View File

@ -0,0 +1,303 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_STRTOD_
#define RAPIDJSON_STRTOD_
#include <climits>
#include <limits>
#include "biginteger.h"
#include "diyfp.h"
#include "ieee754.h"
#include "pow10.h"
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
inline double FastPath(double significand, int exp) {
if (exp < -308)
return 0.0;
else if (exp >= 0)
return significand * internal::Pow10(exp);
else
return significand / internal::Pow10(-exp);
}
inline double StrtodNormalPrecision(double d, int p) {
if (p < -308) {
// Prevent expSum < -308, making Pow10(p) = 0
d = FastPath(d, -308);
d = FastPath(d, p + 308);
} else
d = FastPath(d, p);
return d;
}
template <typename T>
inline T Min3(T a, T b, T c) {
T m = a;
if (m > b) m = b;
if (m > c) m = c;
return m;
}
inline int CheckWithinHalfULP(double b, const BigInteger &d, int dExp) {
const Double db(b);
const uint64_t bInt = db.IntegerSignificand();
const int bExp = db.IntegerExponent();
const int hExp = bExp - 1;
int dS_Exp2 = 0, dS_Exp5 = 0, bS_Exp2 = 0, bS_Exp5 = 0, hS_Exp2 = 0,
hS_Exp5 = 0;
// Adjust for decimal exponent
if (dExp >= 0) {
dS_Exp2 += dExp;
dS_Exp5 += dExp;
} else {
bS_Exp2 -= dExp;
bS_Exp5 -= dExp;
hS_Exp2 -= dExp;
hS_Exp5 -= dExp;
}
// Adjust for binary exponent
if (bExp >= 0)
bS_Exp2 += bExp;
else {
dS_Exp2 -= bExp;
hS_Exp2 -= bExp;
}
// Adjust for half ulp exponent
if (hExp >= 0)
hS_Exp2 += hExp;
else {
dS_Exp2 -= hExp;
bS_Exp2 -= hExp;
}
// Remove common power of two factor from all three scaled values
int common_Exp2 = Min3(dS_Exp2, bS_Exp2, hS_Exp2);
dS_Exp2 -= common_Exp2;
bS_Exp2 -= common_Exp2;
hS_Exp2 -= common_Exp2;
BigInteger dS = d;
dS.MultiplyPow5(static_cast<unsigned>(dS_Exp5)) <<=
static_cast<unsigned>(dS_Exp2);
BigInteger bS(bInt);
bS.MultiplyPow5(static_cast<unsigned>(bS_Exp5)) <<=
static_cast<unsigned>(bS_Exp2);
BigInteger hS(1);
hS.MultiplyPow5(static_cast<unsigned>(hS_Exp5)) <<=
static_cast<unsigned>(hS_Exp2);
BigInteger delta(0);
dS.Difference(bS, &delta);
return delta.Compare(hS);
}
inline bool StrtodFast(double d, int p, double *result) {
// Use fast path for string-to-double conversion if possible
// see
// http://www.exploringbinary.com/fast-path-decimal-to-floating-point-conversion/
if (p > 22 && p < 22 + 16) {
// Fast Path Cases In Disguise
d *= internal::Pow10(p - 22);
p = 22;
}
if (p >= -22 && p <= 22 && d <= 9007199254740991.0) { // 2^53 - 1
*result = FastPath(d, p);
return true;
} else
return false;
}
// Compute an approximation and see if it is within 1/2 ULP
inline bool StrtodDiyFp(const char *decimals, int dLen, int dExp,
double *result) {
uint64_t significand = 0;
int i = 0; // 2^64 - 1 = 18446744073709551615, 1844674407370955161 =
// 0x1999999999999999
for (; i < dLen; i++) {
if (significand > RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) ||
(significand == RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) &&
decimals[i] > '5'))
break;
significand = significand * 10u + static_cast<unsigned>(decimals[i] - '0');
}
if (i < dLen && decimals[i] >= '5') // Rounding
significand++;
int remaining = dLen - i;
const int kUlpShift = 3;
const int kUlp = 1 << kUlpShift;
int64_t error = (remaining == 0) ? 0 : kUlp / 2;
DiyFp v(significand, 0);
v = v.Normalize();
error <<= -v.e;
dExp += remaining;
int actualExp;
DiyFp cachedPower = GetCachedPower10(dExp, &actualExp);
if (actualExp != dExp) {
static const DiyFp kPow10[] = {
DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60), // 10^1
DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57), // 10^2
DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54), // 10^3
DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50), // 10^4
DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47), // 10^5
DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44), // 10^6
DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40) // 10^7
};
int adjustment = dExp - actualExp;
RAPIDJSON_ASSERT(adjustment >= 1 && adjustment < 8);
v = v * kPow10[adjustment - 1];
if (dLen + adjustment >
19) // has more digits than decimal digits in 64-bit
error += kUlp / 2;
}
v = v * cachedPower;
error += kUlp + (error == 0 ? 0 : 1);
const int oldExp = v.e;
v = v.Normalize();
error <<= oldExp - v.e;
const int effectiveSignificandSize =
Double::EffectiveSignificandSize(64 + v.e);
int precisionSize = 64 - effectiveSignificandSize;
if (precisionSize + kUlpShift >= 64) {
int scaleExp = (precisionSize + kUlpShift) - 63;
v.f >>= scaleExp;
v.e += scaleExp;
error = (error >> scaleExp) + 1 + kUlp;
precisionSize -= scaleExp;
}
DiyFp rounded(v.f >> precisionSize, v.e + precisionSize);
const uint64_t precisionBits =
(v.f & ((uint64_t(1) << precisionSize) - 1)) * kUlp;
const uint64_t halfWay = (uint64_t(1) << (precisionSize - 1)) * kUlp;
if (precisionBits >= halfWay + static_cast<unsigned>(error)) {
rounded.f++;
if (rounded.f & (DiyFp::kDpHiddenBit
<< 1)) { // rounding overflows mantissa (issue #340)
rounded.f >>= 1;
rounded.e++;
}
}
*result = rounded.ToDouble();
return halfWay - static_cast<unsigned>(error) >= precisionBits ||
precisionBits >= halfWay + static_cast<unsigned>(error);
}
inline double StrtodBigInteger(double approx, const char *decimals, int dLen,
int dExp) {
RAPIDJSON_ASSERT(dLen >= 0);
const BigInteger dInt(decimals, static_cast<unsigned>(dLen));
Double a(approx);
int cmp = CheckWithinHalfULP(a.Value(), dInt, dExp);
if (cmp < 0)
return a.Value(); // within half ULP
else if (cmp == 0) {
// Round towards even
if (a.Significand() & 1)
return a.NextPositiveDouble();
else
return a.Value();
} else // adjustment
return a.NextPositiveDouble();
}
inline double StrtodFullPrecision(double d, int p, const char *decimals,
size_t length, size_t decimalPosition,
int exp) {
RAPIDJSON_ASSERT(d >= 0.0);
RAPIDJSON_ASSERT(length >= 1);
double result = 0.0;
if (StrtodFast(d, p, &result)) return result;
RAPIDJSON_ASSERT(length <= INT_MAX);
int dLen = static_cast<int>(length);
RAPIDJSON_ASSERT(length >= decimalPosition);
RAPIDJSON_ASSERT(length - decimalPosition <= INT_MAX);
int dExpAdjust = static_cast<int>(length - decimalPosition);
RAPIDJSON_ASSERT(exp >= INT_MIN + dExpAdjust);
int dExp = exp - dExpAdjust;
// Make sure length+dExp does not overflow
RAPIDJSON_ASSERT(dExp <= INT_MAX - dLen);
// Trim leading zeros
while (dLen > 0 && *decimals == '0') {
dLen--;
decimals++;
}
// Trim trailing zeros
while (dLen > 0 && decimals[dLen - 1] == '0') {
dLen--;
dExp++;
}
if (dLen == 0) { // Buffer only contains zeros.
return 0.0;
}
// Trim right-most digits
const int kMaxDecimalDigit = 767 + 1;
if (dLen > kMaxDecimalDigit) {
dExp += dLen - kMaxDecimalDigit;
dLen = kMaxDecimalDigit;
}
// If too small, underflow to zero.
// Any x <= 10^-324 is interpreted as zero.
if (dLen + dExp <= -324) return 0.0;
// If too large, overflow to infinity.
// Any x >= 10^309 is interpreted as +infinity.
if (dLen + dExp > 309) return std::numeric_limits<double>::infinity();
if (StrtodDiyFp(decimals, dLen, dExp, &result)) return result;
// Use approximation from StrtodDiyFp and make adjustment with BigInteger
// comparison
return StrtodBigInteger(result, decimals, dLen, dExp);
}
} // namespace internal
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_STRTOD_

50
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/swap.h vendored Normal file
View File

@ -0,0 +1,50 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_INTERNAL_SWAP_H_
#define RAPIDJSON_INTERNAL_SWAP_H_
#include "../rapidjson.h"
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif
RAPIDJSON_NAMESPACE_BEGIN
namespace internal {
//! Custom swap() to avoid dependency on C++ <algorithm> header
/*! \tparam T Type of the arguments to swap, should be instantiated with
primitive C++ types only. \note This has the same semantics as std::swap().
*/
template <typename T>
inline void Swap(T &a, T &b) RAPIDJSON_NOEXCEPT {
T tmp = a;
a = b;
b = tmp;
}
} // namespace internal
RAPIDJSON_NAMESPACE_END
#if defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_INTERNAL_SWAP_H_

161
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/istreamwrapper.h vendored Normal file
View File

@ -0,0 +1,161 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_ISTREAMWRAPPER_H_
#define RAPIDJSON_ISTREAMWRAPPER_H_
#include <ios>
#include <iosfwd>
#include "stream.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
#elif defined(_MSC_VER)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4351) // new behavior: elements of array 'array' will be
// default initialized
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Wrapper of \c std::basic_istream into RapidJSON's Stream concept.
/*!
The classes can be wrapped including but not limited to:
- \c std::istringstream
- \c std::stringstream
- \c std::wistringstream
- \c std::wstringstream
- \c std::ifstream
- \c std::fstream
- \c std::wifstream
- \c std::wfstream
\tparam StreamType Class derived from \c std::basic_istream.
*/
template <typename StreamType>
class BasicIStreamWrapper {
public:
typedef typename StreamType::char_type Ch;
//! Constructor.
/*!
\param stream stream opened for read.
*/
BasicIStreamWrapper(StreamType &stream)
: stream_(stream),
buffer_(peekBuffer_),
bufferSize_(4),
bufferLast_(0),
current_(buffer_),
readCount_(0),
count_(0),
eof_(false) {
Read();
}
//! Constructor.
/*!
\param stream stream opened for read.
\param buffer user-supplied buffer.
\param bufferSize size of buffer in bytes. Must >=4 bytes.
*/
BasicIStreamWrapper(StreamType &stream, char *buffer, size_t bufferSize)
: stream_(stream),
buffer_(buffer),
bufferSize_(bufferSize),
bufferLast_(0),
current_(buffer_),
readCount_(0),
count_(0),
eof_(false) {
RAPIDJSON_ASSERT(bufferSize >= 4);
Read();
}
Ch Peek() const { return *current_; }
Ch Take() {
Ch c = *current_;
Read();
return c;
}
size_t Tell() const {
return count_ + static_cast<size_t>(current_ - buffer_);
}
// Not implemented
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
// For encoding detection only.
const Ch *Peek4() const {
return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
}
private:
BasicIStreamWrapper();
BasicIStreamWrapper(const BasicIStreamWrapper &);
BasicIStreamWrapper &operator=(const BasicIStreamWrapper &);
void Read() {
if (current_ < bufferLast_)
++current_;
else if (!eof_) {
count_ += readCount_;
readCount_ = bufferSize_;
bufferLast_ = buffer_ + readCount_ - 1;
current_ = buffer_;
if (!stream_.read(buffer_, static_cast<std::streamsize>(bufferSize_))) {
readCount_ = static_cast<size_t>(stream_.gcount());
*(bufferLast_ = buffer_ + readCount_) = '\0';
eof_ = true;
}
}
}
StreamType &stream_;
Ch peekBuffer_[4], *buffer_;
size_t bufferSize_;
Ch *bufferLast_;
Ch *current_;
size_t readCount_;
size_t count_; //!< Number of characters read
bool eof_;
};
typedef BasicIStreamWrapper<std::istream> IStreamWrapper;
typedef BasicIStreamWrapper<std::wistream> WIStreamWrapper;
#if defined(__clang__) || defined(_MSC_VER)
RAPIDJSON_DIAG_POP
#endif
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_ISTREAMWRAPPER_H_

57
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/license.txt vendored Normal file
View File

@ -0,0 +1,57 @@
Tencent is pleased to support the open source community by making RapidJSON available.
Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All rights reserved.
If you have downloaded a copy of the RapidJSON binary from Tencent, please note that the RapidJSON binary is licensed under the MIT License.
If you have downloaded a copy of the RapidJSON source code from Tencent, please note that RapidJSON source code is licensed under the MIT License, except for the third-party components listed below which are subject to different license terms. Your integration of RapidJSON into your own projects may require compliance with the MIT License, as well as the other licenses applicable to the third-party components included within RapidJSON. To avoid the problematic JSON license in your own projects, it's sufficient to exclude the bin/jsonchecker/ directory, as it's the only code under the JSON license.
A copy of the MIT License is included in this file.
Other dependencies and licenses:
Open Source Software Licensed Under the BSD License:
--------------------------------------------------------------------
The msinttypes r29
Copyright (c) 2006-2013 Alexander Chemeris
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
* Neither the name of copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Open Source Software Licensed Under the JSON License:
--------------------------------------------------------------------
json.org
Copyright (c) 2002 JSON.org
All Rights Reserved.
JSON_checker
Copyright (c) 2002 JSON.org
All Rights Reserved.
Terms of the JSON License:
---------------------------------------------------
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
The Software shall be used for Good, not Evil.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
Terms of the MIT License:
--------------------------------------------------------------------
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

78
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/memorybuffer.h vendored Normal file
View File

@ -0,0 +1,78 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_MEMORYBUFFER_H_
#define RAPIDJSON_MEMORYBUFFER_H_
#include "internal/stack.h"
#include "stream.h"
RAPIDJSON_NAMESPACE_BEGIN
//! Represents an in-memory output byte stream.
/*!
This class is mainly for being wrapped by EncodedOutputStream or
AutoUTFOutputStream.
It is similar to FileWriteBuffer but the destination is an in-memory buffer
instead of a file.
Differences between MemoryBuffer and StringBuffer:
1. StringBuffer has Encoding but MemoryBuffer is only a byte buffer.
2. StringBuffer::GetString() returns a null-terminated string.
MemoryBuffer::GetBuffer() returns a buffer without terminator.
\tparam Allocator type for allocating memory buffer.
\note implements Stream concept
*/
template <typename Allocator = CrtAllocator>
struct GenericMemoryBuffer {
typedef char Ch; // byte
GenericMemoryBuffer(Allocator *allocator = 0,
size_t capacity = kDefaultCapacity)
: stack_(allocator, capacity) {}
void Put(Ch c) { *stack_.template Push<Ch>() = c; }
void Flush() {}
void Clear() { stack_.Clear(); }
void ShrinkToFit() { stack_.ShrinkToFit(); }
Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
void Pop(size_t count) { stack_.template Pop<Ch>(count); }
const Ch *GetBuffer() const { return stack_.template Bottom<Ch>(); }
size_t GetSize() const { return stack_.GetSize(); }
static const size_t kDefaultCapacity = 256;
mutable internal::Stack<Allocator> stack_;
};
typedef GenericMemoryBuffer<> MemoryBuffer;
//! Implement specialized version of PutN() with memset() for better
//! performance.
template <>
inline void PutN(MemoryBuffer &memoryBuffer, char c, size_t n) {
std::memset(memoryBuffer.stack_.Push<char>(n), c, n * sizeof(c));
}
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_MEMORYBUFFER_H_

84
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/memorystream.h vendored Normal file
View File

@ -0,0 +1,84 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_MEMORYSTREAM_H_
#define RAPIDJSON_MEMORYSTREAM_H_
#include "stream.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(unreachable - code)
RAPIDJSON_DIAG_OFF(missing - noreturn)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Represents an in-memory input byte stream.
/*!
This class is mainly for being wrapped by EncodedInputStream or
AutoUTFInputStream.
It is similar to FileReadBuffer but the source is an in-memory buffer
instead of a file.
Differences between MemoryStream and StringStream:
1. StringStream has encoding but MemoryStream is a byte stream.
2. MemoryStream needs size of the source buffer and the buffer don't need to
be null terminated. StringStream assume null-terminated string as source.
3. MemoryStream supports Peek4() for encoding detection. StringStream is
specified with an encoding so it should not have Peek4(). \note implements
Stream concept
*/
struct MemoryStream {
typedef char Ch; // byte
MemoryStream(const Ch *src, size_t size)
: src_(src), begin_(src), end_(src + size), size_(size) {}
Ch Peek() const { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_; }
Ch Take() { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_++; }
size_t Tell() const { return static_cast<size_t>(src_ - begin_); }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
// For encoding detection only.
const Ch *Peek4() const { return Tell() + 4 <= size_ ? src_ : 0; }
const Ch *src_; //!< Current read position.
const Ch *begin_; //!< Original head of the string.
const Ch *end_; //!< End of stream.
size_t size_; //!< Size of the stream.
};
RAPIDJSON_NAMESPACE_END
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_MEMORYBUFFER_H_

316
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/msinttypes/inttypes.h vendored Normal file
View File

@ -0,0 +1,316 @@
// ISO C9x compliant inttypes.h for Microsoft Visual Studio
// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
//
// Copyright (c) 2006-2013 Alexander Chemeris
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the product nor the names of its contributors may
// be used to endorse or promote products derived from this software
// without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////////
// The above software in this distribution may have been modified by
// THL A29 Limited ("Tencent Modifications").
// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
#ifndef _MSC_VER // [
#error "Use this header only with Microsoft Visual C++ compilers!"
#endif // _MSC_VER ]
#ifndef _MSC_INTTYPES_H_ // [
#define _MSC_INTTYPES_H_
#if _MSC_VER > 1000
#pragma once
#endif
#include "stdint.h"
// miloyip: VC supports inttypes.h since VC2013
#if _MSC_VER >= 1800
#include <inttypes.h>
#else
// 7.8 Format conversion of integer types
typedef struct {
intmax_t quot;
intmax_t rem;
} imaxdiv_t;
// 7.8.1 Macros for format specifiers
#if !defined(__cplusplus) || \
defined(__STDC_FORMAT_MACROS) // [ See footnote 185 at page 198
// The fprintf macros for signed integers are:
#define PRId8 "d"
#define PRIi8 "i"
#define PRIdLEAST8 "d"
#define PRIiLEAST8 "i"
#define PRIdFAST8 "d"
#define PRIiFAST8 "i"
#define PRId16 "hd"
#define PRIi16 "hi"
#define PRIdLEAST16 "hd"
#define PRIiLEAST16 "hi"
#define PRIdFAST16 "hd"
#define PRIiFAST16 "hi"
#define PRId32 "I32d"
#define PRIi32 "I32i"
#define PRIdLEAST32 "I32d"
#define PRIiLEAST32 "I32i"
#define PRIdFAST32 "I32d"
#define PRIiFAST32 "I32i"
#define PRId64 "I64d"
#define PRIi64 "I64i"
#define PRIdLEAST64 "I64d"
#define PRIiLEAST64 "I64i"
#define PRIdFAST64 "I64d"
#define PRIiFAST64 "I64i"
#define PRIdMAX "I64d"
#define PRIiMAX "I64i"
#define PRIdPTR "Id"
#define PRIiPTR "Ii"
// The fprintf macros for unsigned integers are:
#define PRIo8 "o"
#define PRIu8 "u"
#define PRIx8 "x"
#define PRIX8 "X"
#define PRIoLEAST8 "o"
#define PRIuLEAST8 "u"
#define PRIxLEAST8 "x"
#define PRIXLEAST8 "X"
#define PRIoFAST8 "o"
#define PRIuFAST8 "u"
#define PRIxFAST8 "x"
#define PRIXFAST8 "X"
#define PRIo16 "ho"
#define PRIu16 "hu"
#define PRIx16 "hx"
#define PRIX16 "hX"
#define PRIoLEAST16 "ho"
#define PRIuLEAST16 "hu"
#define PRIxLEAST16 "hx"
#define PRIXLEAST16 "hX"
#define PRIoFAST16 "ho"
#define PRIuFAST16 "hu"
#define PRIxFAST16 "hx"
#define PRIXFAST16 "hX"
#define PRIo32 "I32o"
#define PRIu32 "I32u"
#define PRIx32 "I32x"
#define PRIX32 "I32X"
#define PRIoLEAST32 "I32o"
#define PRIuLEAST32 "I32u"
#define PRIxLEAST32 "I32x"
#define PRIXLEAST32 "I32X"
#define PRIoFAST32 "I32o"
#define PRIuFAST32 "I32u"
#define PRIxFAST32 "I32x"
#define PRIXFAST32 "I32X"
#define PRIo64 "I64o"
#define PRIu64 "I64u"
#define PRIx64 "I64x"
#define PRIX64 "I64X"
#define PRIoLEAST64 "I64o"
#define PRIuLEAST64 "I64u"
#define PRIxLEAST64 "I64x"
#define PRIXLEAST64 "I64X"
#define PRIoFAST64 "I64o"
#define PRIuFAST64 "I64u"
#define PRIxFAST64 "I64x"
#define PRIXFAST64 "I64X"
#define PRIoMAX "I64o"
#define PRIuMAX "I64u"
#define PRIxMAX "I64x"
#define PRIXMAX "I64X"
#define PRIoPTR "Io"
#define PRIuPTR "Iu"
#define PRIxPTR "Ix"
#define PRIXPTR "IX"
// The fscanf macros for signed integers are:
#define SCNd8 "d"
#define SCNi8 "i"
#define SCNdLEAST8 "d"
#define SCNiLEAST8 "i"
#define SCNdFAST8 "d"
#define SCNiFAST8 "i"
#define SCNd16 "hd"
#define SCNi16 "hi"
#define SCNdLEAST16 "hd"
#define SCNiLEAST16 "hi"
#define SCNdFAST16 "hd"
#define SCNiFAST16 "hi"
#define SCNd32 "ld"
#define SCNi32 "li"
#define SCNdLEAST32 "ld"
#define SCNiLEAST32 "li"
#define SCNdFAST32 "ld"
#define SCNiFAST32 "li"
#define SCNd64 "I64d"
#define SCNi64 "I64i"
#define SCNdLEAST64 "I64d"
#define SCNiLEAST64 "I64i"
#define SCNdFAST64 "I64d"
#define SCNiFAST64 "I64i"
#define SCNdMAX "I64d"
#define SCNiMAX "I64i"
#ifdef _WIN64 // [
#define SCNdPTR "I64d"
#define SCNiPTR "I64i"
#else // _WIN64 ][
#define SCNdPTR "ld"
#define SCNiPTR "li"
#endif // _WIN64 ]
// The fscanf macros for unsigned integers are:
#define SCNo8 "o"
#define SCNu8 "u"
#define SCNx8 "x"
#define SCNX8 "X"
#define SCNoLEAST8 "o"
#define SCNuLEAST8 "u"
#define SCNxLEAST8 "x"
#define SCNXLEAST8 "X"
#define SCNoFAST8 "o"
#define SCNuFAST8 "u"
#define SCNxFAST8 "x"
#define SCNXFAST8 "X"
#define SCNo16 "ho"
#define SCNu16 "hu"
#define SCNx16 "hx"
#define SCNX16 "hX"
#define SCNoLEAST16 "ho"
#define SCNuLEAST16 "hu"
#define SCNxLEAST16 "hx"
#define SCNXLEAST16 "hX"
#define SCNoFAST16 "ho"
#define SCNuFAST16 "hu"
#define SCNxFAST16 "hx"
#define SCNXFAST16 "hX"
#define SCNo32 "lo"
#define SCNu32 "lu"
#define SCNx32 "lx"
#define SCNX32 "lX"
#define SCNoLEAST32 "lo"
#define SCNuLEAST32 "lu"
#define SCNxLEAST32 "lx"
#define SCNXLEAST32 "lX"
#define SCNoFAST32 "lo"
#define SCNuFAST32 "lu"
#define SCNxFAST32 "lx"
#define SCNXFAST32 "lX"
#define SCNo64 "I64o"
#define SCNu64 "I64u"
#define SCNx64 "I64x"
#define SCNX64 "I64X"
#define SCNoLEAST64 "I64o"
#define SCNuLEAST64 "I64u"
#define SCNxLEAST64 "I64x"
#define SCNXLEAST64 "I64X"
#define SCNoFAST64 "I64o"
#define SCNuFAST64 "I64u"
#define SCNxFAST64 "I64x"
#define SCNXFAST64 "I64X"
#define SCNoMAX "I64o"
#define SCNuMAX "I64u"
#define SCNxMAX "I64x"
#define SCNXMAX "I64X"
#ifdef _WIN64 // [
#define SCNoPTR "I64o"
#define SCNuPTR "I64u"
#define SCNxPTR "I64x"
#define SCNXPTR "I64X"
#else // _WIN64 ][
#define SCNoPTR "lo"
#define SCNuPTR "lu"
#define SCNxPTR "lx"
#define SCNXPTR "lX"
#endif // _WIN64 ]
#endif // __STDC_FORMAT_MACROS ]
// 7.8.2 Functions for greatest-width integer types
// 7.8.2.1 The imaxabs function
#define imaxabs _abs64
// 7.8.2.2 The imaxdiv function
// This is modified version of div() function from Microsoft's div.c found
// in %MSVC.NET%\crt\src\div.c
#ifdef STATIC_IMAXDIV // [
static
#else // STATIC_IMAXDIV ][
_inline
#endif // STATIC_IMAXDIV ]
imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) {
imaxdiv_t result;
result.quot = numer / denom;
result.rem = numer % denom;
if (numer < 0 && result.rem > 0) {
// did division wrong; must fix up
++result.quot;
result.rem -= denom;
}
return result;
}
// 7.8.2.3 The strtoimax and strtoumax functions
#define strtoimax _strtoi64
#define strtoumax _strtoui64
// 7.8.2.4 The wcstoimax and wcstoumax functions
#define wcstoimax _wcstoi64
#define wcstoumax _wcstoui64
#endif // _MSC_VER >= 1800
#endif // _MSC_INTTYPES_H_ ]

301
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/msinttypes/stdint.h vendored Normal file
View File

@ -0,0 +1,301 @@
// ISO C9x compliant stdint.h for Microsoft Visual Studio
// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
//
// Copyright (c) 2006-2013 Alexander Chemeris
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the product nor the names of its contributors may
// be used to endorse or promote products derived from this software
// without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
// EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////////
// The above software in this distribution may have been modified by
// THL A29 Limited ("Tencent Modifications").
// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
#ifndef _MSC_VER // [
#error "Use this header only with Microsoft Visual C++ compilers!"
#endif // _MSC_VER ]
#ifndef _MSC_STDINT_H_ // [
#define _MSC_STDINT_H_
#if _MSC_VER > 1000
#pragma once
#endif
// miloyip: Originally Visual Studio 2010 uses its own stdint.h. However it
// generates warning with INT64_C(), so change to use this file for vs2010.
#if _MSC_VER >= 1600 // [
#include <stdint.h>
#if !defined(__cplusplus) || \
defined(__STDC_CONSTANT_MACROS) // [ See footnote 224 at page 260
#undef INT8_C
#undef INT16_C
#undef INT32_C
#undef INT64_C
#undef UINT8_C
#undef UINT16_C
#undef UINT32_C
#undef UINT64_C
// 7.18.4.1 Macros for minimum-width integer constants
#define INT8_C(val) val##i8
#define INT16_C(val) val##i16
#define INT32_C(val) val##i32
#define INT64_C(val) val##i64
#define UINT8_C(val) val##ui8
#define UINT16_C(val) val##ui16
#define UINT32_C(val) val##ui32
#define UINT64_C(val) val##ui64
// 7.18.4.2 Macros for greatest-width integer constants
// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
// Check out Issue 9 for the details.
#ifndef INTMAX_C // [
#define INTMAX_C INT64_C
#endif // INTMAX_C ]
#ifndef UINTMAX_C // [
#define UINTMAX_C UINT64_C
#endif // UINTMAX_C ]
#endif // __STDC_CONSTANT_MACROS ]
#else // ] _MSC_VER >= 1700 [
#include <limits.h>
// For Visual Studio 6 in C++ mode and for many Visual Studio versions when
// compiling for ARM we have to wrap <wchar.h> include with 'extern "C++" {}'
// or compiler would give many errors like this:
// error C2733: second C linkage of overloaded function 'wmemchr' not allowed
#if defined(__cplusplus) && !defined(_M_ARM)
extern "C" {
#endif
#include <wchar.h>
#if defined(__cplusplus) && !defined(_M_ARM)
}
#endif
// Define _W64 macros to mark types changing their size, like intptr_t.
#ifndef _W64
#if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300
#define _W64 __w64
#else
#define _W64
#endif
#endif
// 7.18.1 Integer types
// 7.18.1.1 Exact-width integer types
// Visual Studio 6 and Embedded Visual C++ 4 doesn't
// realize that, e.g. char has the same size as __int8
// so we give up on __intX for them.
#if (_MSC_VER < 1300)
typedef signed char int8_t;
typedef signed short int16_t;
typedef signed int int32_t;
typedef unsigned char uint8_t;
typedef unsigned short uint16_t;
typedef unsigned int uint32_t;
#else
typedef signed __int8 int8_t;
typedef signed __int16 int16_t;
typedef signed __int32 int32_t;
typedef unsigned __int8 uint8_t;
typedef unsigned __int16 uint16_t;
typedef unsigned __int32 uint32_t;
#endif
typedef signed __int64 int64_t;
typedef unsigned __int64 uint64_t;
// 7.18.1.2 Minimum-width integer types
typedef int8_t int_least8_t;
typedef int16_t int_least16_t;
typedef int32_t int_least32_t;
typedef int64_t int_least64_t;
typedef uint8_t uint_least8_t;
typedef uint16_t uint_least16_t;
typedef uint32_t uint_least32_t;
typedef uint64_t uint_least64_t;
// 7.18.1.3 Fastest minimum-width integer types
typedef int8_t int_fast8_t;
typedef int16_t int_fast16_t;
typedef int32_t int_fast32_t;
typedef int64_t int_fast64_t;
typedef uint8_t uint_fast8_t;
typedef uint16_t uint_fast16_t;
typedef uint32_t uint_fast32_t;
typedef uint64_t uint_fast64_t;
// 7.18.1.4 Integer types capable of holding object pointers
#ifdef _WIN64 // [
typedef signed __int64 intptr_t;
typedef unsigned __int64 uintptr_t;
#else // _WIN64 ][
typedef _W64 signed int intptr_t;
typedef _W64 unsigned int uintptr_t;
#endif // _WIN64 ]
// 7.18.1.5 Greatest-width integer types
typedef int64_t intmax_t;
typedef uint64_t uintmax_t;
// 7.18.2 Limits of specified-width integer types
#if !defined(__cplusplus) || \
defined(__STDC_LIMIT_MACROS) // [ See footnote 220 at page 257 and
// footnote 221 at page 259
// 7.18.2.1 Limits of exact-width integer types
#define INT8_MIN ((int8_t)_I8_MIN)
#define INT8_MAX _I8_MAX
#define INT16_MIN ((int16_t)_I16_MIN)
#define INT16_MAX _I16_MAX
#define INT32_MIN ((int32_t)_I32_MIN)
#define INT32_MAX _I32_MAX
#define INT64_MIN ((int64_t)_I64_MIN)
#define INT64_MAX _I64_MAX
#define UINT8_MAX _UI8_MAX
#define UINT16_MAX _UI16_MAX
#define UINT32_MAX _UI32_MAX
#define UINT64_MAX _UI64_MAX
// 7.18.2.2 Limits of minimum-width integer types
#define INT_LEAST8_MIN INT8_MIN
#define INT_LEAST8_MAX INT8_MAX
#define INT_LEAST16_MIN INT16_MIN
#define INT_LEAST16_MAX INT16_MAX
#define INT_LEAST32_MIN INT32_MIN
#define INT_LEAST32_MAX INT32_MAX
#define INT_LEAST64_MIN INT64_MIN
#define INT_LEAST64_MAX INT64_MAX
#define UINT_LEAST8_MAX UINT8_MAX
#define UINT_LEAST16_MAX UINT16_MAX
#define UINT_LEAST32_MAX UINT32_MAX
#define UINT_LEAST64_MAX UINT64_MAX
// 7.18.2.3 Limits of fastest minimum-width integer types
#define INT_FAST8_MIN INT8_MIN
#define INT_FAST8_MAX INT8_MAX
#define INT_FAST16_MIN INT16_MIN
#define INT_FAST16_MAX INT16_MAX
#define INT_FAST32_MIN INT32_MIN
#define INT_FAST32_MAX INT32_MAX
#define INT_FAST64_MIN INT64_MIN
#define INT_FAST64_MAX INT64_MAX
#define UINT_FAST8_MAX UINT8_MAX
#define UINT_FAST16_MAX UINT16_MAX
#define UINT_FAST32_MAX UINT32_MAX
#define UINT_FAST64_MAX UINT64_MAX
// 7.18.2.4 Limits of integer types capable of holding object pointers
#ifdef _WIN64 // [
#define INTPTR_MIN INT64_MIN
#define INTPTR_MAX INT64_MAX
#define UINTPTR_MAX UINT64_MAX
#else // _WIN64 ][
#define INTPTR_MIN INT32_MIN
#define INTPTR_MAX INT32_MAX
#define UINTPTR_MAX UINT32_MAX
#endif // _WIN64 ]
// 7.18.2.5 Limits of greatest-width integer types
#define INTMAX_MIN INT64_MIN
#define INTMAX_MAX INT64_MAX
#define UINTMAX_MAX UINT64_MAX
// 7.18.3 Limits of other integer types
#ifdef _WIN64 // [
#define PTRDIFF_MIN _I64_MIN
#define PTRDIFF_MAX _I64_MAX
#else // _WIN64 ][
#define PTRDIFF_MIN _I32_MIN
#define PTRDIFF_MAX _I32_MAX
#endif // _WIN64 ]
#define SIG_ATOMIC_MIN INT_MIN
#define SIG_ATOMIC_MAX INT_MAX
#ifndef SIZE_MAX // [
#ifdef _WIN64 // [
#define SIZE_MAX _UI64_MAX
#else // _WIN64 ][
#define SIZE_MAX _UI32_MAX
#endif // _WIN64 ]
#endif // SIZE_MAX ]
// WCHAR_MIN and WCHAR_MAX are also defined in <wchar.h>
#ifndef WCHAR_MIN // [
#define WCHAR_MIN 0
#endif // WCHAR_MIN ]
#ifndef WCHAR_MAX // [
#define WCHAR_MAX _UI16_MAX
#endif // WCHAR_MAX ]
#define WINT_MIN 0
#define WINT_MAX _UI16_MAX
#endif // __STDC_LIMIT_MACROS ]
// 7.18.4 Limits of other integer types
#if !defined(__cplusplus) || \
defined(__STDC_CONSTANT_MACROS) // [ See footnote 224 at page 260
// 7.18.4.1 Macros for minimum-width integer constants
#define INT8_C(val) val##i8
#define INT16_C(val) val##i16
#define INT32_C(val) val##i32
#define INT64_C(val) val##i64
#define UINT8_C(val) val##ui8
#define UINT16_C(val) val##ui16
#define UINT32_C(val) val##ui32
#define UINT64_C(val) val##ui64
// 7.18.4.2 Macros for greatest-width integer constants
// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
// Check out Issue 9 for the details.
#ifndef INTMAX_C // [
#define INTMAX_C INT64_C
#endif // INTMAX_C ]
#ifndef UINTMAX_C // [
#define UINTMAX_C UINT64_C
#endif // UINTMAX_C ]
#endif // __STDC_CONSTANT_MACROS ]
#endif // _MSC_VER >= 1600 ]
#endif // _MSC_STDINT_H_ ]

96
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/ostreamwrapper.h vendored Normal file
View File

@ -0,0 +1,96 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_OSTREAMWRAPPER_H_
#define RAPIDJSON_OSTREAMWRAPPER_H_
#include <iosfwd>
#include "stream.h"
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Wrapper of \c std::basic_ostream into RapidJSON's Stream concept.
/*!
The classes can be wrapped including but not limited to:
- \c std::ostringstream
- \c std::stringstream
- \c std::wpstringstream
- \c std::wstringstream
- \c std::ifstream
- \c std::fstream
- \c std::wofstream
- \c std::wfstream
\tparam StreamType Class derived from \c std::basic_ostream.
*/
template <typename StreamType>
class BasicOStreamWrapper {
public:
typedef typename StreamType::char_type Ch;
BasicOStreamWrapper(StreamType &stream) : stream_(stream) {}
void Put(Ch c) { stream_.put(c); }
void Flush() { stream_.flush(); }
// Not implemented
char Peek() const {
RAPIDJSON_ASSERT(false);
return 0;
}
char Take() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t Tell() const {
RAPIDJSON_ASSERT(false);
return 0;
}
char *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
size_t PutEnd(char *) {
RAPIDJSON_ASSERT(false);
return 0;
}
private:
BasicOStreamWrapper(const BasicOStreamWrapper &);
BasicOStreamWrapper &operator=(const BasicOStreamWrapper &);
StreamType &stream_;
};
typedef BasicOStreamWrapper<std::ostream> OStreamWrapper;
typedef BasicOStreamWrapper<std::wostream> WOStreamWrapper;
#ifdef __clang__
RAPIDJSON_DIAG_POP
#endif
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_OSTREAMWRAPPER_H_

1712
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/pointer.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

333
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/prettywriter.h vendored Normal file
View File

@ -0,0 +1,333 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_PRETTYWRITER_H_
#define RAPIDJSON_PRETTYWRITER_H_
#include "writer.h"
#ifdef __GNUC__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(effc++)
#endif
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Combination of PrettyWriter format flags.
/*! \see PrettyWriter::SetFormatOptions
*/
enum PrettyFormatOptions {
kFormatDefault = 0, //!< Default pretty formatting.
kFormatSingleLineArray = 1 //!< Format arrays on a single line.
};
//! Writer with indentation and spacing.
/*!
\tparam OutputStream Type of output os.
\tparam SourceEncoding Encoding of source string.
\tparam TargetEncoding Encoding of output stream.
\tparam StackAllocator Type of allocator for allocating memory of stack.
*/
template <typename OutputStream, typename SourceEncoding = UTF8<>,
typename TargetEncoding = UTF8<>,
typename StackAllocator = CrtAllocator,
unsigned writeFlags = kWriteDefaultFlags>
class PrettyWriter : public Writer<OutputStream, SourceEncoding, TargetEncoding,
StackAllocator, writeFlags> {
public:
typedef Writer<OutputStream, SourceEncoding, TargetEncoding, StackAllocator,
writeFlags>
Base;
typedef typename Base::Ch Ch;
//! Constructor
/*! \param os Output stream.
\param allocator User supplied allocator. If it is null, it will create a
private one. \param levelDepth Initial capacity of stack.
*/
explicit PrettyWriter(OutputStream &os, StackAllocator *allocator = 0,
size_t levelDepth = Base::kDefaultLevelDepth)
: Base(os, allocator, levelDepth),
indentChar_(' '),
indentCharCount_(4),
formatOptions_(kFormatDefault) {}
explicit PrettyWriter(StackAllocator *allocator = 0,
size_t levelDepth = Base::kDefaultLevelDepth)
: Base(allocator, levelDepth), indentChar_(' '), indentCharCount_(4) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
PrettyWriter(PrettyWriter &&rhs)
: Base(std::forward<PrettyWriter>(rhs)),
indentChar_(rhs.indentChar_),
indentCharCount_(rhs.indentCharCount_),
formatOptions_(rhs.formatOptions_) {}
#endif
//! Set custom indentation.
/*! \param indentChar Character for indentation. Must be whitespace
character (' ', '\\t', '\\n', '\\r'). \param indentCharCount Number of
indent characters for each indentation level. \note The default indentation
is 4 spaces.
*/
PrettyWriter &SetIndent(Ch indentChar, unsigned indentCharCount) {
RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' ||
indentChar == '\n' || indentChar == '\r');
indentChar_ = indentChar;
indentCharCount_ = indentCharCount;
return *this;
}
//! Set pretty writer formatting options.
/*! \param options Formatting options.
*/
PrettyWriter &SetFormatOptions(PrettyFormatOptions options) {
formatOptions_ = options;
return *this;
}
/*! @name Implementation of Handler
\see Handler
*/
//@{
bool Null() {
PrettyPrefix(kNullType);
return Base::EndValue(Base::WriteNull());
}
bool Bool(bool b) {
PrettyPrefix(b ? kTrueType : kFalseType);
return Base::EndValue(Base::WriteBool(b));
}
bool Int(int i) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteInt(i));
}
bool Uint(unsigned u) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteUint(u));
}
bool Int64(int64_t i64) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteInt64(i64));
}
bool Uint64(uint64_t u64) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteUint64(u64));
}
bool Double(double d) {
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteDouble(d));
}
bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
PrettyPrefix(kNumberType);
return Base::EndValue(Base::WriteString(str, length));
}
bool String(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
PrettyPrefix(kStringType);
return Base::EndValue(Base::WriteString(str, length));
}
#if RAPIDJSON_HAS_STDSTRING
bool String(const std::basic_string<Ch> &str) {
return String(str.data(), SizeType(str.size()));
}
#endif
bool StartObject() {
PrettyPrefix(kObjectType);
new (Base::level_stack_.template Push<typename Base::Level>())
typename Base::Level(false);
return Base::WriteStartObject();
}
bool Key(const Ch *str, SizeType length, bool copy = false) {
return String(str, length, copy);
}
#if RAPIDJSON_HAS_STDSTRING
bool Key(const std::basic_string<Ch> &str) {
return Key(str.data(), SizeType(str.size()));
}
#endif
bool EndObject(SizeType memberCount = 0) {
(void)memberCount;
RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
sizeof(typename Base::Level)); // not inside an Object
RAPIDJSON_ASSERT(!Base::level_stack_.template Top<typename Base::Level>()
->inArray); // currently inside an Array, not Object
RAPIDJSON_ASSERT(
0 ==
Base::level_stack_.template Top<typename Base::Level>()->valueCount %
2); // Object has a Key without a Value
bool empty =
Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
0;
if (!empty) {
Base::os_->Put('\n');
WriteIndent();
}
bool ret = Base::EndValue(Base::WriteEndObject());
(void)ret;
RAPIDJSON_ASSERT(ret == true);
if (Base::level_stack_.Empty()) // end of json text
Base::Flush();
return true;
}
bool StartArray() {
PrettyPrefix(kArrayType);
new (Base::level_stack_.template Push<typename Base::Level>())
typename Base::Level(true);
return Base::WriteStartArray();
}
bool EndArray(SizeType memberCount = 0) {
(void)memberCount;
RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
sizeof(typename Base::Level));
RAPIDJSON_ASSERT(
Base::level_stack_.template Top<typename Base::Level>()->inArray);
bool empty =
Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
0;
if (!empty && !(formatOptions_ & kFormatSingleLineArray)) {
Base::os_->Put('\n');
WriteIndent();
}
bool ret = Base::EndValue(Base::WriteEndArray());
(void)ret;
RAPIDJSON_ASSERT(ret == true);
if (Base::level_stack_.Empty()) // end of json text
Base::Flush();
return true;
}
//@}
/*! @name Convenience extensions */
//@{
//! Simpler but slower overload.
bool String(const Ch *str) { return String(str, internal::StrLen(str)); }
bool Key(const Ch *str) { return Key(str, internal::StrLen(str)); }
//@}
//! Write a raw JSON value.
/*!
For user to write a stringified JSON as a value.
\param json A well-formed JSON value. It should not contain null character
within [0, length - 1] range. \param length Length of the json. \param type
Type of the root of json. \note When using PrettyWriter::RawValue(), the
result json may not be indented correctly.
*/
bool RawValue(const Ch *json, size_t length, Type type) {
RAPIDJSON_ASSERT(json != 0);
PrettyPrefix(type);
return Base::EndValue(Base::WriteRawValue(json, length));
}
protected:
void PrettyPrefix(Type type) {
(void)type;
if (Base::level_stack_.GetSize() != 0) { // this value is not at root
typename Base::Level *level =
Base::level_stack_.template Top<typename Base::Level>();
if (level->inArray) {
if (level->valueCount > 0) {
Base::os_->Put(
','); // add comma if it is not the first element in array
if (formatOptions_ & kFormatSingleLineArray) Base::os_->Put(' ');
}
if (!(formatOptions_ & kFormatSingleLineArray)) {
Base::os_->Put('\n');
WriteIndent();
}
} else { // in object
if (level->valueCount > 0) {
if (level->valueCount % 2 == 0) {
Base::os_->Put(',');
Base::os_->Put('\n');
} else {
Base::os_->Put(':');
Base::os_->Put(' ');
}
} else
Base::os_->Put('\n');
if (level->valueCount % 2 == 0) WriteIndent();
}
if (!level->inArray && level->valueCount % 2 == 0)
RAPIDJSON_ASSERT(type == kStringType); // if it's in object, then even
// number should be a name
level->valueCount++;
} else {
RAPIDJSON_ASSERT(
!Base::hasRoot_); // Should only has one and only one root.
Base::hasRoot_ = true;
}
}
void WriteIndent() {
size_t count =
(Base::level_stack_.GetSize() / sizeof(typename Base::Level)) *
indentCharCount_;
PutN(*Base::os_, static_cast<typename OutputStream::Ch>(indentChar_),
count);
}
Ch indentChar_;
unsigned indentCharCount_;
PrettyFormatOptions formatOptions_;
private:
// Prohibit copy constructor & assignment operator.
PrettyWriter(const PrettyWriter &);
PrettyWriter &operator=(const PrettyWriter &);
};
RAPIDJSON_NAMESPACE_END
#if defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#ifdef __GNUC__
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_RAPIDJSON_H_

719
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/rapidjson.h vendored Normal file
View File

@ -0,0 +1,719 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_RAPIDJSON_H_
#define RAPIDJSON_RAPIDJSON_H_
/*!\file rapidjson.h
\brief common definitions and configuration
\see RAPIDJSON_CONFIG
*/
/*! \defgroup RAPIDJSON_CONFIG RapidJSON configuration
\brief Configuration macros for library features
Some RapidJSON features are configurable to adapt the library to a wide
variety of platforms, environments and usage scenarios. Most of the
features can be configured in terms of overridden or predefined
preprocessor macros at compile-time.
Some additional customization is available in the \ref RAPIDJSON_ERRORS
APIs.
\note These macros should be given on the compiler command-line
(where applicable) to avoid inconsistent values when compiling
different translation units of a single application.
*/
#include <cstdlib> // malloc(), realloc(), free(), size_t
#include <cstring> // memset(), memcpy(), memmove(), memcmp()
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_VERSION_STRING
//
// ALWAYS synchronize the following 3 macros with corresponding variables in
// /CMakeLists.txt.
//
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
// token stringification
#define RAPIDJSON_STRINGIFY(x) RAPIDJSON_DO_STRINGIFY(x)
#define RAPIDJSON_DO_STRINGIFY(x) #x
// token concatenation
#define RAPIDJSON_JOIN(X, Y) RAPIDJSON_DO_JOIN(X, Y)
#define RAPIDJSON_DO_JOIN(X, Y) RAPIDJSON_DO_JOIN2(X, Y)
#define RAPIDJSON_DO_JOIN2(X, Y) X##Y
//!@endcond
/*! \def RAPIDJSON_MAJOR_VERSION
\ingroup RAPIDJSON_CONFIG
\brief Major version of RapidJSON in integer.
*/
/*! \def RAPIDJSON_MINOR_VERSION
\ingroup RAPIDJSON_CONFIG
\brief Minor version of RapidJSON in integer.
*/
/*! \def RAPIDJSON_PATCH_VERSION
\ingroup RAPIDJSON_CONFIG
\brief Patch version of RapidJSON in integer.
*/
/*! \def RAPIDJSON_VERSION_STRING
\ingroup RAPIDJSON_CONFIG
\brief Version of RapidJSON in "<major>.<minor>.<patch>" string format.
*/
#define RAPIDJSON_MAJOR_VERSION 1
#define RAPIDJSON_MINOR_VERSION 1
#define RAPIDJSON_PATCH_VERSION 0
#define RAPIDJSON_VERSION_STRING \
RAPIDJSON_STRINGIFY( \
RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION)
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NAMESPACE_(BEGIN|END)
/*! \def RAPIDJSON_NAMESPACE
\ingroup RAPIDJSON_CONFIG
\brief provide custom rapidjson namespace
In order to avoid symbol clashes and/or "One Definition Rule" errors
between multiple inclusions of (different versions of) RapidJSON in
a single binary, users can customize the name of the main RapidJSON
namespace.
In case of a single nesting level, defining \c RAPIDJSON_NAMESPACE
to a custom name (e.g. \c MyRapidJSON) is sufficient. If multiple
levels are needed, both \ref RAPIDJSON_NAMESPACE_BEGIN and \ref
RAPIDJSON_NAMESPACE_END need to be defined as well:
\code
// in some .cpp file
#define RAPIDJSON_NAMESPACE my::rapidjson
#define RAPIDJSON_NAMESPACE_BEGIN namespace my { namespace rapidjson {
#define RAPIDJSON_NAMESPACE_END } }
#include "rapidjson/..."
\endcode
\see rapidjson
*/
/*! \def RAPIDJSON_NAMESPACE_BEGIN
\ingroup RAPIDJSON_CONFIG
\brief provide custom rapidjson namespace (opening expression)
\see RAPIDJSON_NAMESPACE
*/
/*! \def RAPIDJSON_NAMESPACE_END
\ingroup RAPIDJSON_CONFIG
\brief provide custom rapidjson namespace (closing expression)
\see RAPIDJSON_NAMESPACE
*/
#ifndef RAPIDJSON_NAMESPACE
#define RAPIDJSON_NAMESPACE rapidjson
#endif
#ifndef RAPIDJSON_NAMESPACE_BEGIN
#define RAPIDJSON_NAMESPACE_BEGIN namespace RAPIDJSON_NAMESPACE {
#endif
#ifndef RAPIDJSON_NAMESPACE_END
#define RAPIDJSON_NAMESPACE_END }
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_HAS_STDSTRING
#ifndef RAPIDJSON_HAS_STDSTRING
#ifdef RAPIDJSON_DOXYGEN_RUNNING
#define RAPIDJSON_HAS_STDSTRING 1 // force generation of documentation
#else
#define RAPIDJSON_HAS_STDSTRING 0 // no std::string support by default
#endif
/*! \def RAPIDJSON_HAS_STDSTRING
\ingroup RAPIDJSON_CONFIG
\brief Enable RapidJSON support for \c std::string
By defining this preprocessor symbol to \c 1, several convenience functions
for using \ref rapidjson::GenericValue with \c std::string are enabled,
especially for construction and comparison.
\hideinitializer
*/
#endif // !defined(RAPIDJSON_HAS_STDSTRING)
#if RAPIDJSON_HAS_STDSTRING
#include <string>
#endif // RAPIDJSON_HAS_STDSTRING
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NO_INT64DEFINE
/*! \def RAPIDJSON_NO_INT64DEFINE
\ingroup RAPIDJSON_CONFIG
\brief Use external 64-bit integer types.
RapidJSON requires the 64-bit integer types \c int64_t and \c uint64_t
types to be available at global scope.
If users have their own definition, define RAPIDJSON_NO_INT64DEFINE to
prevent RapidJSON from defining its own types.
*/
#ifndef RAPIDJSON_NO_INT64DEFINE
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#if defined(_MSC_VER) && (_MSC_VER < 1800) // Visual Studio 2013
#include "msinttypes/inttypes.h"
#include "msinttypes/stdint.h"
#else
// Other compilers should have this.
#include <inttypes.h>
#include <stdint.h>
#endif
//!@endcond
#ifdef RAPIDJSON_DOXYGEN_RUNNING
#define RAPIDJSON_NO_INT64DEFINE
#endif
#endif // RAPIDJSON_NO_INT64TYPEDEF
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_FORCEINLINE
#ifndef RAPIDJSON_FORCEINLINE
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#if defined(_MSC_VER) && defined(NDEBUG)
#define RAPIDJSON_FORCEINLINE __forceinline
#elif defined(__GNUC__) && __GNUC__ >= 4 && defined(NDEBUG)
#define RAPIDJSON_FORCEINLINE __attribute__((always_inline))
#else
#define RAPIDJSON_FORCEINLINE
#endif
//!@endcond
#endif // RAPIDJSON_FORCEINLINE
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ENDIAN
#define RAPIDJSON_LITTLEENDIAN 0 //!< Little endian machine
#define RAPIDJSON_BIGENDIAN 1 //!< Big endian machine
//! Endianness of the machine.
/*!
\def RAPIDJSON_ENDIAN
\ingroup RAPIDJSON_CONFIG
GCC 4.6 provided macro for detecting endianness of the target machine. But
other compilers may not have this. User can define RAPIDJSON_ENDIAN to either
\ref RAPIDJSON_LITTLEENDIAN or \ref RAPIDJSON_BIGENDIAN.
Default detection implemented with reference to
\li
https://gcc.gnu.org/onlinedocs/gcc-4.6.0/cpp/Common-Predefined-Macros.html
\li http://www.boost.org/doc/libs/1_42_0/boost/detail/endian.hpp
*/
#ifndef RAPIDJSON_ENDIAN
// Detect with GCC 4.6's macro
#ifdef __BYTE_ORDER__
#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
#else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
#endif // __BYTE_ORDER__
// Detect with GLIBC's endian.h
#elif defined(__GLIBC__)
#include <endian.h>
#if (__BYTE_ORDER == __LITTLE_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif (__BYTE_ORDER == __BIG_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
#else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
#endif // __GLIBC__
// Detect with _LITTLE_ENDIAN and _BIG_ENDIAN macro
#elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN)
#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
// Detect with architecture macros
#elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || \
defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || \
defined(__hppa) || defined(_MIPSEB) || defined(_POWER) || \
defined(__s390__)
#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
#elif defined(__i386__) || defined(__alpha__) || defined(__ia64) || \
defined(__ia64__) || defined(_M_IX86) || defined(_M_IA64) || \
defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) || \
defined(_M_AMD64) || defined(__x86_64) || defined(__x86_64__) || \
defined(_M_X64) || defined(__bfin__)
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64))
#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
#elif defined(RAPIDJSON_DOXYGEN_RUNNING)
#define RAPIDJSON_ENDIAN
#else
# error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
#endif
#endif // RAPIDJSON_ENDIAN
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_64BIT
//! Whether using 64-bit architecture
#ifndef RAPIDJSON_64BIT
#if defined(__LP64__) || (defined(__x86_64__) && defined(__ILP32__)) || \
defined(_WIN64) || defined(__EMSCRIPTEN__)
#define RAPIDJSON_64BIT 1
#else
#define RAPIDJSON_64BIT 0
#endif
#endif // RAPIDJSON_64BIT
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ALIGN
//! Data alignment of the machine.
/*! \ingroup RAPIDJSON_CONFIG
\param x pointer to align
Some machines require strict data alignment. The default is 8 bytes.
User can customize by defining the RAPIDJSON_ALIGN function macro.
*/
#ifndef RAPIDJSON_ALIGN
#define RAPIDJSON_ALIGN(x) \
(((x) + static_cast<size_t>(7u)) & ~static_cast<size_t>(7u))
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_UINT64_C2
//! Construct a 64-bit literal by a pair of 32-bit integer.
/*!
64-bit literal with or without ULL suffix is prone to compiler warnings.
UINT64_C() is C macro which cause compilation problems.
Use this macro to define 64-bit constants by a pair of 32-bit integer.
*/
#ifndef RAPIDJSON_UINT64_C2
#define RAPIDJSON_UINT64_C2(high32, low32) \
((static_cast<uint64_t>(high32) << 32) | static_cast<uint64_t>(low32))
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_48BITPOINTER_OPTIMIZATION
//! Use only lower 48-bit address for some pointers.
/*!
\ingroup RAPIDJSON_CONFIG
This optimization uses the fact that current X86-64 architecture only
implement lower 48-bit virtual address. The higher 16-bit can be used for
storing other data. \c GenericValue uses this optimization to reduce its size
form 24 bytes to 16 bytes in 64-bit architecture.
*/
#ifndef RAPIDJSON_48BITPOINTER_OPTIMIZATION
#if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || \
defined(__x86_64) || defined(_M_X64) || defined(_M_AMD64)
#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 1
#else
#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 0
#endif
#endif // RAPIDJSON_48BITPOINTER_OPTIMIZATION
#if RAPIDJSON_48BITPOINTER_OPTIMIZATION == 1
#if RAPIDJSON_64BIT != 1
#error RAPIDJSON_48BITPOINTER_OPTIMIZATION can only be set to 1 when RAPIDJSON_64BIT=1
#endif
#define RAPIDJSON_SETPOINTER(type, p, x) \
(p = reinterpret_cast<type *>( \
(reinterpret_cast<uintptr_t>(p) & \
static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0xFFFF0000, 0x00000000))) | \
reinterpret_cast<uintptr_t>(reinterpret_cast<const void *>(x))))
#define RAPIDJSON_GETPOINTER(type, p) \
(reinterpret_cast<type *>( \
reinterpret_cast<uintptr_t>(p) & \
static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0x0000FFFF, 0xFFFFFFFF))))
#else
#define RAPIDJSON_SETPOINTER(type, p, x) (p = (x))
#define RAPIDJSON_GETPOINTER(type, p) (p)
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_SSE2/RAPIDJSON_SSE42/RAPIDJSON_NEON/RAPIDJSON_SIMD
/*! \def RAPIDJSON_SIMD
\ingroup RAPIDJSON_CONFIG
\brief Enable SSE2/SSE4.2/Neon optimization.
RapidJSON supports optimized implementations for some parsing operations
based on the SSE2, SSE4.2 or NEon SIMD extensions on modern Intel
or ARM compatible processors.
To enable these optimizations, three different symbols can be defined;
\code
// Enable SSE2 optimization.
#define RAPIDJSON_SSE2
// Enable SSE4.2 optimization.
#define RAPIDJSON_SSE42
\endcode
// Enable ARM Neon optimization.
#define RAPIDJSON_NEON
\endcode
\c RAPIDJSON_SSE42 takes precedence over SSE2, if both are defined.
If any of these symbols is defined, RapidJSON defines the macro
\c RAPIDJSON_SIMD to indicate the availability of the optimized code.
*/
#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) || \
defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING)
#define RAPIDJSON_SIMD
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NO_SIZETYPEDEFINE
#ifndef RAPIDJSON_NO_SIZETYPEDEFINE
/*! \def RAPIDJSON_NO_SIZETYPEDEFINE
\ingroup RAPIDJSON_CONFIG
\brief User-provided \c SizeType definition.
In order to avoid using 32-bit size types for indexing strings and arrays,
define this preprocessor symbol and provide the type rapidjson::SizeType
before including RapidJSON:
\code
#define RAPIDJSON_NO_SIZETYPEDEFINE
namespace rapidjson { typedef ::std::size_t SizeType; }
#include "rapidjson/..."
\endcode
\see rapidjson::SizeType
*/
#ifdef RAPIDJSON_DOXYGEN_RUNNING
#define RAPIDJSON_NO_SIZETYPEDEFINE
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Size type (for string lengths, array sizes, etc.)
/*! RapidJSON uses 32-bit array/string indices even on 64-bit platforms,
instead of using \c size_t. Users may override the SizeType by defining
\ref RAPIDJSON_NO_SIZETYPEDEFINE.
*/
typedef unsigned SizeType;
RAPIDJSON_NAMESPACE_END
#endif
// always import std::size_t to rapidjson namespace
RAPIDJSON_NAMESPACE_BEGIN
using std::size_t;
RAPIDJSON_NAMESPACE_END
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_ASSERT
//! Assertion.
/*! \ingroup RAPIDJSON_CONFIG
By default, rapidjson uses C \c assert() for internal assertions.
User can override it by defining RAPIDJSON_ASSERT(x) macro.
\note Parsing errors are handled and can be customized by the
\ref RAPIDJSON_ERRORS APIs.
*/
#ifndef RAPIDJSON_ASSERT
#include <cassert>
#define RAPIDJSON_ASSERT(x) assert(x)
#endif // RAPIDJSON_ASSERT
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_STATIC_ASSERT
// Prefer C++11 static_assert, if available
#ifndef RAPIDJSON_STATIC_ASSERT
#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800)
#define RAPIDJSON_STATIC_ASSERT(x) static_assert(x, RAPIDJSON_STRINGIFY(x))
#endif // C++11
#endif // RAPIDJSON_STATIC_ASSERT
// Adopt C++03 implementation from boost
#ifndef RAPIDJSON_STATIC_ASSERT
#ifndef __clang__
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#endif
RAPIDJSON_NAMESPACE_BEGIN
template <bool x>
struct STATIC_ASSERTION_FAILURE;
template <>
struct STATIC_ASSERTION_FAILURE<true> {
enum { value = 1 };
};
template <size_t x>
struct StaticAssertTest {};
RAPIDJSON_NAMESPACE_END
#if defined(__GNUC__) || defined(__clang__)
#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE __attribute__((unused))
#else
#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#endif
#ifndef __clang__
//!@endcond
#endif
/*! \def RAPIDJSON_STATIC_ASSERT
\brief (Internal) macro to check for conditions at compile-time
\param x compile-time condition
\hideinitializer
*/
#define RAPIDJSON_STATIC_ASSERT(x) \
typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest<sizeof( \
::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x)>)> \
RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__) \
RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
#endif // RAPIDJSON_STATIC_ASSERT
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_LIKELY, RAPIDJSON_UNLIKELY
//! Compiler branching hint for expression with high probability to be true.
/*!
\ingroup RAPIDJSON_CONFIG
\param x Boolean expression likely to be true.
*/
#ifndef RAPIDJSON_LIKELY
#if defined(__GNUC__) || defined(__clang__)
#define RAPIDJSON_LIKELY(x) __builtin_expect(!!(x), 1)
#else
#define RAPIDJSON_LIKELY(x) (x)
#endif
#endif
//! Compiler branching hint for expression with low probability to be true.
/*!
\ingroup RAPIDJSON_CONFIG
\param x Boolean expression unlikely to be true.
*/
#ifndef RAPIDJSON_UNLIKELY
#if defined(__GNUC__) || defined(__clang__)
#define RAPIDJSON_UNLIKELY(x) __builtin_expect(!!(x), 0)
#else
#define RAPIDJSON_UNLIKELY(x) (x)
#endif
#endif
///////////////////////////////////////////////////////////////////////////////
// Helpers
//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
#define RAPIDJSON_MULTILINEMACRO_BEGIN do {
#define RAPIDJSON_MULTILINEMACRO_END \
} \
while ((void)0, 0)
// adopted from Boost
#define RAPIDJSON_VERSION_CODE(x, y, z) (((x)*100000) + ((y)*100) + (z))
#if defined(__has_builtin)
#define RAPIDJSON_HAS_BUILTIN(x) __has_builtin(x)
#else
#define RAPIDJSON_HAS_BUILTIN(x) 0
#endif
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_DIAG_PUSH/POP, RAPIDJSON_DIAG_OFF
#if defined(__GNUC__)
#define RAPIDJSON_GNUC \
RAPIDJSON_VERSION_CODE(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__)
#endif
#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 2, 0))
#define RAPIDJSON_PRAGMA(x) _Pragma(RAPIDJSON_STRINGIFY(x))
#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(GCC diagnostic x)
#define RAPIDJSON_DIAG_OFF(x) \
RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W, x)))
// push/pop support in Clang and GCC>=4.6
#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0))
#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
#else // GCC >= 4.2, < 4.6
#define RAPIDJSON_DIAG_PUSH /* ignored */
#define RAPIDJSON_DIAG_POP /* ignored */
#endif
#elif defined(_MSC_VER)
// pragma (MSVC specific)
#define RAPIDJSON_PRAGMA(x) __pragma(x)
#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(warning(x))
#define RAPIDJSON_DIAG_OFF(x) RAPIDJSON_DIAG_PRAGMA(disable : x)
#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
#else
#define RAPIDJSON_DIAG_OFF(x) /* ignored */
#define RAPIDJSON_DIAG_PUSH /* ignored */
#define RAPIDJSON_DIAG_POP /* ignored */
#endif // RAPIDJSON_DIAG_*
///////////////////////////////////////////////////////////////////////////////
// C++11 features
#ifndef RAPIDJSON_HAS_CXX11_RVALUE_REFS
#if defined(__clang__)
#if __has_feature(cxx_rvalue_references) && \
(defined(_MSC_VER) || defined(_LIBCPP_VERSION) || \
defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306)
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
#else
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
#endif
#elif (defined(RAPIDJSON_GNUC) && \
(RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 3, 0)) && \
defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1600) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
#else
#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
#endif
#endif // RAPIDJSON_HAS_CXX11_RVALUE_REFS
#ifndef RAPIDJSON_HAS_CXX11_NOEXCEPT
#if defined(__clang__)
#define RAPIDJSON_HAS_CXX11_NOEXCEPT __has_feature(cxx_noexcept)
#elif (defined(RAPIDJSON_GNUC) && \
(RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1900) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 1
#else
#define RAPIDJSON_HAS_CXX11_NOEXCEPT 0
#endif
#endif
#if RAPIDJSON_HAS_CXX11_NOEXCEPT
#define RAPIDJSON_NOEXCEPT noexcept
#else
#define RAPIDJSON_NOEXCEPT /* noexcept */
#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT
// no automatic detection, yet
#ifndef RAPIDJSON_HAS_CXX11_TYPETRAITS
#if (defined(_MSC_VER) && _MSC_VER >= 1700)
#define RAPIDJSON_HAS_CXX11_TYPETRAITS 1
#else
#define RAPIDJSON_HAS_CXX11_TYPETRAITS 0
#endif
#endif
#ifndef RAPIDJSON_HAS_CXX11_RANGE_FOR
#if defined(__clang__)
#define RAPIDJSON_HAS_CXX11_RANGE_FOR __has_feature(cxx_range_for)
#elif (defined(RAPIDJSON_GNUC) && \
(RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
defined(__GXX_EXPERIMENTAL_CXX0X__)) || \
(defined(_MSC_VER) && _MSC_VER >= 1700) || \
(defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 && \
defined(__GXX_EXPERIMENTAL_CXX0X__))
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 1
#else
#define RAPIDJSON_HAS_CXX11_RANGE_FOR 0
#endif
#endif // RAPIDJSON_HAS_CXX11_RANGE_FOR
///////////////////////////////////////////////////////////////////////////////
// C++17 features
#if defined(__has_cpp_attribute)
#if __has_cpp_attribute(fallthrough)
#define RAPIDJSON_DELIBERATE_FALLTHROUGH [[fallthrough]]
#else
#define RAPIDJSON_DELIBERATE_FALLTHROUGH
#endif
#else
#define RAPIDJSON_DELIBERATE_FALLTHROUGH
#endif
//!@endcond
//! Assertion (in non-throwing contexts).
/*! \ingroup RAPIDJSON_CONFIG
Some functions provide a \c noexcept guarantee, if the compiler supports it.
In these cases, the \ref RAPIDJSON_ASSERT macro cannot be overridden to
throw an exception. This macro adds a separate customization point for
such cases.
Defaults to C \c assert() (as \ref RAPIDJSON_ASSERT), if \c noexcept is
supported, and to \ref RAPIDJSON_ASSERT otherwise.
*/
///////////////////////////////////////////////////////////////////////////////
// RAPIDJSON_NOEXCEPT_ASSERT
#ifndef RAPIDJSON_NOEXCEPT_ASSERT
#ifdef RAPIDJSON_ASSERT_THROWS
#if RAPIDJSON_HAS_CXX11_NOEXCEPT
#define RAPIDJSON_NOEXCEPT_ASSERT(x)
#else
#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
#endif // RAPIDJSON_HAS_CXX11_NOEXCEPT
#else
#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
#endif // RAPIDJSON_ASSERT_THROWS
#endif // RAPIDJSON_NOEXCEPT_ASSERT
///////////////////////////////////////////////////////////////////////////////
// new/delete
#ifndef RAPIDJSON_NEW
///! customization point for global \c new
#define RAPIDJSON_NEW(TypeName) new TypeName
#endif
#ifndef RAPIDJSON_DELETE
///! customization point for global \c delete
#define RAPIDJSON_DELETE(x) delete x
#endif
///////////////////////////////////////////////////////////////////////////////
// Type
/*! \namespace rapidjson
\brief main RapidJSON namespace
\see RAPIDJSON_NAMESPACE
*/
RAPIDJSON_NAMESPACE_BEGIN
//! Type of JSON value
enum Type {
kNullType = 0, //!< null
kFalseType = 1, //!< false
kTrueType = 2, //!< true
kObjectType = 3, //!< object
kArrayType = 4, //!< array
kStringType = 5, //!< string
kNumberType = 6 //!< number
};
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_RAPIDJSON_H_

2458
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/reader.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

2743
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/schema.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

242
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/stream.h vendored Normal file
View File

@ -0,0 +1,242 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#include "rapidjson.h"
#ifndef RAPIDJSON_STREAM_H_
#define RAPIDJSON_STREAM_H_
#include "encodings.h"
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// Stream
/*! \class rapidjson::Stream
\brief Concept for reading and writing characters.
For read-only stream, no need to implement PutBegin(), Put(), Flush() and
PutEnd().
For write-only stream, only need to implement Put() and Flush().
\code
concept Stream {
typename Ch; //!< Character type of the stream.
//! Read the current character from stream without moving the read cursor.
Ch Peek() const;
//! Read the current character from stream and moving the read cursor to
next character. Ch Take();
//! Get the current read cursor.
//! \return Number of characters read from start.
size_t Tell();
//! Begin writing operation at the current read pointer.
//! \return The begin writer pointer.
Ch* PutBegin();
//! Write a character.
void Put(Ch c);
//! Flush the buffer.
void Flush();
//! End the writing operation.
//! \param begin The begin write pointer returned by PutBegin().
//! \return Number of characters written.
size_t PutEnd(Ch* begin);
}
\endcode
*/
//! Provides additional information for stream.
/*!
By using traits pattern, this type provides a default configuration for
stream. For custom stream, this type can be specialized for other
configuration. See TEST(Reader, CustomStringStream) in readertest.cpp for
example.
*/
template <typename Stream>
struct StreamTraits {
//! Whether to make local copy of stream for optimization during parsing.
/*!
By default, for safety, streams do not use local copy optimization.
Stream that can be copied fast should specialize this, like
StreamTraits<StringStream>.
*/
enum { copyOptimization = 0 };
};
//! Reserve n characters for writing to a stream.
template <typename Stream>
inline void PutReserve(Stream &stream, size_t count) {
(void)stream;
(void)count;
}
//! Write character to a stream, presuming buffer is reserved.
template <typename Stream>
inline void PutUnsafe(Stream &stream, typename Stream::Ch c) {
stream.Put(c);
}
//! Put N copies of a character to a stream.
template <typename Stream, typename Ch>
inline void PutN(Stream &stream, Ch c, size_t n) {
PutReserve(stream, n);
for (size_t i = 0; i < n; i++) PutUnsafe(stream, c);
}
///////////////////////////////////////////////////////////////////////////////
// GenericStreamWrapper
//! A Stream Wrapper
/*! \tThis string stream is a wrapper for any stream by just forwarding any
\treceived message to the origin stream.
\note implements Stream concept
*/
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4702) // unreachable code
RAPIDJSON_DIAG_OFF(4512) // assignment operator could not be generated
#endif
template <typename InputStream, typename Encoding = UTF8<>>
class GenericStreamWrapper {
public:
typedef typename Encoding::Ch Ch;
GenericStreamWrapper(InputStream &is) : is_(is) {}
Ch Peek() const { return is_.Peek(); }
Ch Take() { return is_.Take(); }
size_t Tell() { return is_.Tell(); }
Ch *PutBegin() { return is_.PutBegin(); }
void Put(Ch ch) { is_.Put(ch); }
void Flush() { is_.Flush(); }
size_t PutEnd(Ch *ch) { return is_.PutEnd(ch); }
// wrapper for MemoryStream
const Ch *Peek4() const { return is_.Peek4(); }
// wrapper for AutoUTFInputStream
UTFType GetType() const { return is_.GetType(); }
bool HasBOM() const { return is_.HasBOM(); }
protected:
InputStream &is_;
};
#if defined(_MSC_VER) && _MSC_VER <= 1800
RAPIDJSON_DIAG_POP
#endif
///////////////////////////////////////////////////////////////////////////////
// StringStream
//! Read-only string stream.
/*! \note implements Stream concept
*/
template <typename Encoding>
struct GenericStringStream {
typedef typename Encoding::Ch Ch;
GenericStringStream(const Ch *src) : src_(src), head_(src) {}
Ch Peek() const { return *src_; }
Ch Take() { return *src_++; }
size_t Tell() const { return static_cast<size_t>(src_ - head_); }
Ch *PutBegin() {
RAPIDJSON_ASSERT(false);
return 0;
}
void Put(Ch) { RAPIDJSON_ASSERT(false); }
void Flush() { RAPIDJSON_ASSERT(false); }
size_t PutEnd(Ch *) {
RAPIDJSON_ASSERT(false);
return 0;
}
const Ch *src_; //!< Current read position.
const Ch *head_; //!< Original head of the string.
};
template <typename Encoding>
struct StreamTraits<GenericStringStream<Encoding>> {
enum { copyOptimization = 1 };
};
//! String stream with UTF8 encoding.
typedef GenericStringStream<UTF8<>> StringStream;
///////////////////////////////////////////////////////////////////////////////
// InsituStringStream
//! A read-write string stream.
/*! This string stream is particularly designed for in-situ parsing.
\note implements Stream concept
*/
template <typename Encoding>
struct GenericInsituStringStream {
typedef typename Encoding::Ch Ch;
GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {}
// Read
Ch Peek() { return *src_; }
Ch Take() { return *src_++; }
size_t Tell() { return static_cast<size_t>(src_ - head_); }
// Write
void Put(Ch c) {
RAPIDJSON_ASSERT(dst_ != 0);
*dst_++ = c;
}
Ch *PutBegin() { return dst_ = src_; }
size_t PutEnd(Ch *begin) { return static_cast<size_t>(dst_ - begin); }
void Flush() {}
Ch *Push(size_t count) {
Ch *begin = dst_;
dst_ += count;
return begin;
}
void Pop(size_t count) { dst_ -= count; }
Ch *src_;
Ch *dst_;
Ch *head_;
};
template <typename Encoding>
struct StreamTraits<GenericInsituStringStream<Encoding>> {
enum { copyOptimization = 1 };
};
//! Insitu string stream with UTF8 encoding.
typedef GenericInsituStringStream<UTF8<>> InsituStringStream;
RAPIDJSON_NAMESPACE_END
#endif // RAPIDJSON_STREAM_H_

130
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/stringbuffer.h vendored Normal file
View File

@ -0,0 +1,130 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_STRINGBUFFER_H_
#define RAPIDJSON_STRINGBUFFER_H_
#include "internal/stack.h"
#include "stream.h"
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
#include <utility> // std::move
#endif
#include "internal/stack.h"
#if defined(__clang__)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#endif
RAPIDJSON_NAMESPACE_BEGIN
//! Represents an in-memory output stream.
/*!
\tparam Encoding Encoding of the stream.
\tparam Allocator type for allocating memory buffer.
\note implements Stream concept
*/
template <typename Encoding, typename Allocator = CrtAllocator>
class GenericStringBuffer {
public:
typedef typename Encoding::Ch Ch;
GenericStringBuffer(Allocator *allocator = 0,
size_t capacity = kDefaultCapacity)
: stack_(allocator, capacity) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
GenericStringBuffer(GenericStringBuffer &&rhs)
: stack_(std::move(rhs.stack_)) {}
GenericStringBuffer &operator=(GenericStringBuffer &&rhs) {
if (&rhs != this) stack_ = std::move(rhs.stack_);
return *this;
}
#endif
void Put(Ch c) { *stack_.template Push<Ch>() = c; }
void PutUnsafe(Ch c) { *stack_.template PushUnsafe<Ch>() = c; }
void Flush() {}
void Clear() { stack_.Clear(); }
void ShrinkToFit() {
// Push and pop a null terminator. This is safe.
*stack_.template Push<Ch>() = '\0';
stack_.ShrinkToFit();
stack_.template Pop<Ch>(1);
}
void Reserve(size_t count) { stack_.template Reserve<Ch>(count); }
Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
Ch *PushUnsafe(size_t count) { return stack_.template PushUnsafe<Ch>(count); }
void Pop(size_t count) { stack_.template Pop<Ch>(count); }
const Ch *GetString() const {
// Push and pop a null terminator. This is safe.
*stack_.template Push<Ch>() = '\0';
stack_.template Pop<Ch>(1);
return stack_.template Bottom<Ch>();
}
//! Get the size of string in bytes in the string buffer.
size_t GetSize() const { return stack_.GetSize(); }
//! Get the length of string in Ch in the string buffer.
size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); }
static const size_t kDefaultCapacity = 256;
mutable internal::Stack<Allocator> stack_;
private:
// Prohibit copy constructor & assignment operator.
GenericStringBuffer(const GenericStringBuffer &);
GenericStringBuffer &operator=(const GenericStringBuffer &);
};
//! String buffer with UTF8 encoding
typedef GenericStringBuffer<UTF8<>> StringBuffer;
template <typename Encoding, typename Allocator>
inline void PutReserve(GenericStringBuffer<Encoding, Allocator> &stream,
size_t count) {
stream.Reserve(count);
}
template <typename Encoding, typename Allocator>
inline void PutUnsafe(GenericStringBuffer<Encoding, Allocator> &stream,
typename Encoding::Ch c) {
stream.PutUnsafe(c);
}
//! Implement specialized version of PutN() with memset() for better
//! performance.
template <>
inline void PutN(GenericStringBuffer<UTF8<>> &stream, char c, size_t n) {
std::memset(stream.stack_.Push<char>(n), c, n * sizeof(c));
}
RAPIDJSON_NAMESPACE_END
#if defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_STRINGBUFFER_H_

811
src/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/writer.h vendored Normal file
View File

@ -0,0 +1,811 @@
// Tencent is pleased to support the open source community by making RapidJSON
// available.
//
// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
// rights reserved.
//
// Licensed under the MIT License (the "License"); you may not use this file
// except in compliance with the License. You may obtain a copy of the License
// at
//
// http://opensource.org/licenses/MIT
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations under
// the License.
#ifndef RAPIDJSON_WRITER_H_
#define RAPIDJSON_WRITER_H_
#include <new> // placement new
#include "internal/clzll.h"
#include "internal/dtoa.h"
#include "internal/itoa.h"
#include "internal/meta.h"
#include "internal/stack.h"
#include "internal/strfunc.h"
#include "stream.h"
#include "stringbuffer.h"
#if defined(RAPIDJSON_SIMD) && defined(_MSC_VER)
#include <intrin.h>
#pragma intrinsic(_BitScanForward)
#endif
#ifdef RAPIDJSON_SSE42
#include <nmmintrin.h>
#elif defined(RAPIDJSON_SSE2)
#include <emmintrin.h>
#elif defined(RAPIDJSON_NEON)
#include <arm_neon.h>
#endif
#ifdef __clang__
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(padded)
RAPIDJSON_DIAG_OFF(unreachable - code)
RAPIDJSON_DIAG_OFF(c++ 98 - compat)
#elif defined(_MSC_VER)
RAPIDJSON_DIAG_PUSH
RAPIDJSON_DIAG_OFF(4127) // conditional expression is constant
#endif
RAPIDJSON_NAMESPACE_BEGIN
///////////////////////////////////////////////////////////////////////////////
// WriteFlag
/*! \def RAPIDJSON_WRITE_DEFAULT_FLAGS
\ingroup RAPIDJSON_CONFIG
\brief User-defined kWriteDefaultFlags definition.
User can define this as any \c WriteFlag combinations.
*/
#ifndef RAPIDJSON_WRITE_DEFAULT_FLAGS
#define RAPIDJSON_WRITE_DEFAULT_FLAGS kWriteNoFlags
#endif
//! Combination of writeFlags
enum WriteFlag {
kWriteNoFlags = 0, //!< No flags are set.
kWriteValidateEncodingFlag = 1, //!< Validate encoding of JSON strings.
kWriteNanAndInfFlag = 2, //!< Allow writing of Infinity, -Infinity and NaN.
kWriteDefaultFlags =
RAPIDJSON_WRITE_DEFAULT_FLAGS //!< Default write flags. Can be customized
//!< by defining
//!< RAPIDJSON_WRITE_DEFAULT_FLAGS
};
//! JSON writer
/*! Writer implements the concept Handler.
It generates JSON text by events to an output os.
User may programmatically calls the functions of a writer to generate JSON
text.
On the other side, a writer can also be passed to objects that generates
events,
for example Reader::Parse() and Document::Accept().
\tparam OutputStream Type of output stream.
\tparam SourceEncoding Encoding of source string.
\tparam TargetEncoding Encoding of output stream.
\tparam StackAllocator Type of allocator for allocating memory of stack.
\note implements Handler concept
*/
template <typename OutputStream, typename SourceEncoding = UTF8<>,
typename TargetEncoding = UTF8<>,
typename StackAllocator = CrtAllocator,
unsigned writeFlags = kWriteDefaultFlags>
class Writer {
public:
typedef typename SourceEncoding::Ch Ch;
static const int kDefaultMaxDecimalPlaces = 324;
//! Constructor
/*! \param os Output stream.
\param stackAllocator User supplied allocator. If it is null, it will
create a private one. \param levelDepth Initial capacity of stack.
*/
explicit Writer(OutputStream &os, StackAllocator *stackAllocator = 0,
size_t levelDepth = kDefaultLevelDepth)
: os_(&os),
level_stack_(stackAllocator, levelDepth * sizeof(Level)),
maxDecimalPlaces_(kDefaultMaxDecimalPlaces),
hasRoot_(false) {}
explicit Writer(StackAllocator *allocator = 0,
size_t levelDepth = kDefaultLevelDepth)
: os_(0),
level_stack_(allocator, levelDepth * sizeof(Level)),
maxDecimalPlaces_(kDefaultMaxDecimalPlaces),
hasRoot_(false) {}
#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
Writer(Writer &&rhs)
: os_(rhs.os_),
level_stack_(std::move(rhs.level_stack_)),
maxDecimalPlaces_(rhs.maxDecimalPlaces_),
hasRoot_(rhs.hasRoot_) {
rhs.os_ = 0;
}
#endif
//! Reset the writer with a new stream.
/*!
This function reset the writer with a new stream and default settings,
in order to make a Writer object reusable for output multiple JSONs.
\param os New output stream.
\code
Writer<OutputStream> writer(os1);
writer.StartObject();
// ...
writer.EndObject();
writer.Reset(os2);
writer.StartObject();
// ...
writer.EndObject();
\endcode
*/
void Reset(OutputStream &os) {
os_ = &os;
hasRoot_ = false;
level_stack_.Clear();
}
//! Checks whether the output is a complete JSON.
/*!
A complete JSON has a complete root object or array.
*/
bool IsComplete() const { return hasRoot_ && level_stack_.Empty(); }
int GetMaxDecimalPlaces() const { return maxDecimalPlaces_; }
//! Sets the maximum number of decimal places for double output.
/*!
This setting truncates the output with specified number of decimal places.
For example,
\code
writer.SetMaxDecimalPlaces(3);
writer.StartArray();
writer.Double(0.12345); // "0.123"
writer.Double(0.0001); // "0.0"
writer.Double(1.234567890123456e30); // "1.234567890123456e30" (do not
truncate significand for positive exponent) writer.Double(1.23e-4); //
"0.0" (do truncate significand for negative exponent)
writer.EndArray();
\endcode
The default setting does not truncate any decimal places. You can restore
to this setting by calling \code
writer.SetMaxDecimalPlaces(Writer::kDefaultMaxDecimalPlaces);
\endcode
*/
void SetMaxDecimalPlaces(int maxDecimalPlaces) {
maxDecimalPlaces_ = maxDecimalPlaces;
}
/*!@name Implementation of Handler
\see Handler
*/
//@{
bool Null() {
Prefix(kNullType);
return EndValue(WriteNull());
}
bool Bool(bool b) {
Prefix(b ? kTrueType : kFalseType);
return EndValue(WriteBool(b));
}
bool Int(int i) {
Prefix(kNumberType);
return EndValue(WriteInt(i));
}
bool Uint(unsigned u) {
Prefix(kNumberType);
return EndValue(WriteUint(u));
}
bool Int64(int64_t i64) {
Prefix(kNumberType);
return EndValue(WriteInt64(i64));
}
bool Uint64(uint64_t u64) {
Prefix(kNumberType);
return EndValue(WriteUint64(u64));
}
//! Writes the given \c double value to the stream
/*!
\param d The value to be written.
\return Whether it is succeed.
*/
bool Double(double d) {
Prefix(kNumberType);
return EndValue(WriteDouble(d));
}
bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
Prefix(kNumberType);
return EndValue(WriteString(str, length));
}
bool String(const Ch *str, SizeType length, bool copy = false) {
RAPIDJSON_ASSERT(str != 0);
(void)copy;
Prefix(kStringType);
return EndValue(WriteString(str, length));
}
#if RAPIDJSON_HAS_STDSTRING
bool String(const std::basic_string<Ch> &str) {
return String(str.data(), SizeType(str.size()));
}
#endif
bool StartObject() {
Prefix(kObjectType);
new (level_stack_.template Push<Level>()) Level(false);
return WriteStartObject();
}
bool Key(const Ch *str, SizeType length, bool copy = false) {
return String(str, length, copy);
}
#if RAPIDJSON_HAS_STDSTRING
bool Key(const std::basic_string<Ch> &str) {
return Key(str.data(), SizeType(str.size()));
}
#endif
bool EndObject(SizeType memberCount = 0) {
(void)memberCount;
RAPIDJSON_ASSERT(level_stack_.GetSize() >=
sizeof(Level)); // not inside an Object
RAPIDJSON_ASSERT(!level_stack_.template Top<Level>()
->inArray); // currently inside an Array, not Object
RAPIDJSON_ASSERT(0 ==
level_stack_.template Top<Level>()->valueCount %
2); // Object has a Key without a Value
level_stack_.template Pop<Level>(1);
return EndValue(WriteEndObject());
}
bool StartArray() {
Prefix(kArrayType);
new (level_stack_.template Push<Level>()) Level(true);
return WriteStartArray();
}
bool EndArray(SizeType elementCount = 0) {
(void)elementCount;
RAPIDJSON_ASSERT(level_stack_.GetSize() >= sizeof(Level));
RAPIDJSON_ASSERT(level_stack_.template Top<Level>()->inArray);
level_stack_.template Pop<Level>(1);
return EndValue(WriteEndArray());
}
//@}
/*! @name Convenience extensions */
//@{
//! Simpler but slower overload.
bool String(const Ch *const &str) {
return String(str, internal::StrLen(str));
}
bool Key(const Ch *const &str) { return Key(str, internal::StrLen(str)); }
//@}
//! Write a raw JSON value.
/*!
For user to write a stringified JSON as a value.
\param json A well-formed JSON value. It should not contain null character
within [0, length - 1] range. \param length Length of the json. \param type
Type of the root of json.
*/
bool RawValue(const Ch *json, size_t length, Type type) {
RAPIDJSON_ASSERT(json != 0);
Prefix(type);
return EndValue(WriteRawValue(json, length));
}
//! Flush the output stream.
/*!
Allows the user to flush the output stream immediately.
*/
void Flush() { os_->Flush(); }
protected:
//! Information for each nested level
struct Level {
Level(bool inArray_) : valueCount(0), inArray(inArray_) {}
size_t valueCount; //!< number of values in this level
bool inArray; //!< true if in array, otherwise in object
};
static const size_t kDefaultLevelDepth = 32;
bool WriteNull() {
PutReserve(*os_, 4);
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'u');
PutUnsafe(*os_, 'l');
PutUnsafe(*os_, 'l');
return true;
}
bool WriteBool(bool b) {
if (b) {
PutReserve(*os_, 4);
PutUnsafe(*os_, 't');
PutUnsafe(*os_, 'r');
PutUnsafe(*os_, 'u');
PutUnsafe(*os_, 'e');
} else {
PutReserve(*os_, 5);
PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'a');
PutUnsafe(*os_, 'l');
PutUnsafe(*os_, 's');
PutUnsafe(*os_, 'e');
}
return true;
}
bool WriteInt(int i) {
char buffer[11];
const char *end = internal::i32toa(i, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (const char *p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteUint(unsigned u) {
char buffer[10];
const char *end = internal::u32toa(u, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (const char *p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteInt64(int64_t i64) {
char buffer[21];
const char *end = internal::i64toa(i64, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (const char *p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteUint64(uint64_t u64) {
char buffer[20];
char *end = internal::u64toa(u64, buffer);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (char *p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteDouble(double d) {
if (internal::Double(d).IsNanOrInf()) {
if (!(writeFlags & kWriteNanAndInfFlag)) return false;
if (internal::Double(d).IsNan()) {
PutReserve(*os_, 3);
PutUnsafe(*os_, 'N');
PutUnsafe(*os_, 'a');
PutUnsafe(*os_, 'N');
return true;
}
if (internal::Double(d).Sign()) {
PutReserve(*os_, 9);
PutUnsafe(*os_, '-');
} else
PutReserve(*os_, 8);
PutUnsafe(*os_, 'I');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 't');
PutUnsafe(*os_, 'y');
return true;
}
char buffer[25];
char *end = internal::dtoa(d, buffer, maxDecimalPlaces_);
PutReserve(*os_, static_cast<size_t>(end - buffer));
for (char *p = buffer; p != end; ++p)
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
return true;
}
bool WriteString(const Ch *str, SizeType length) {
static const typename OutputStream::Ch hexDigits[16] = {
'0', '1', '2', '3', '4', '5', '6', '7',
'8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
static const char escape[256] = {
#define Z16 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
// 0 1 2 3 4 5 6 7 8 9 A B C D E
// F
'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'b', 't',
'n', 'u', 'f', 'r', 'u', 'u', // 00
'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u', 'u',
'u', 'u', 'u', 'u', 'u', 'u', // 10
0, 0, '"', 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, // 20
Z16, Z16, // 30~4F
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, '\\', 0, 0, 0, // 50
Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16, Z16 // 60~FF
#undef Z16
};
if (TargetEncoding::supportUnicode)
PutReserve(*os_, 2 + length * 6); // "\uxxxx..."
else
PutReserve(*os_, 2 + length * 12); // "\uxxxx\uyyyy..."
PutUnsafe(*os_, '\"');
GenericStringStream<SourceEncoding> is(str);
while (ScanWriteUnescapedString(is, length)) {
const Ch c = is.Peek();
if (!TargetEncoding::supportUnicode && static_cast<unsigned>(c) >= 0x80) {
// Unicode escaping
unsigned codepoint;
if (RAPIDJSON_UNLIKELY(!SourceEncoding::Decode(is, &codepoint)))
return false;
PutUnsafe(*os_, '\\');
PutUnsafe(*os_, 'u');
if (codepoint <= 0xD7FF ||
(codepoint >= 0xE000 && codepoint <= 0xFFFF)) {
PutUnsafe(*os_, hexDigits[(codepoint >> 12) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint >> 8) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint >> 4) & 15]);
PutUnsafe(*os_, hexDigits[(codepoint)&15]);
} else {
RAPIDJSON_ASSERT(codepoint >= 0x010000 && codepoint <= 0x10FFFF);
// Surrogate pair
unsigned s = codepoint - 0x010000;
unsigned lead = (s >> 10) + 0xD800;
unsigned trail = (s & 0x3FF) + 0xDC00;
PutUnsafe(*os_, hexDigits[(lead >> 12) & 15]);
PutUnsafe(*os_, hexDigits[(lead >> 8) & 15]);
PutUnsafe(*os_, hexDigits[(lead >> 4) & 15]);
PutUnsafe(*os_, hexDigits[(lead)&15]);
PutUnsafe(*os_, '\\');
PutUnsafe(*os_, 'u');
PutUnsafe(*os_, hexDigits[(trail >> 12) & 15]);
PutUnsafe(*os_, hexDigits[(trail >> 8) & 15]);
PutUnsafe(*os_, hexDigits[(trail >> 4) & 15]);
PutUnsafe(*os_, hexDigits[(trail)&15]);
}
} else if ((sizeof(Ch) == 1 || static_cast<unsigned>(c) < 256) &&
RAPIDJSON_UNLIKELY(escape[static_cast<unsigned char>(c)])) {
is.Take();
PutUnsafe(*os_, '\\');
PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(
escape[static_cast<unsigned char>(c)]));
if (escape[static_cast<unsigned char>(c)] == 'u') {
PutUnsafe(*os_, '0');
PutUnsafe(*os_, '0');
PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) >> 4]);
PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) & 0xF]);
}
} else if (RAPIDJSON_UNLIKELY(!(
writeFlags & kWriteValidateEncodingFlag
? Transcoder<SourceEncoding, TargetEncoding>::Validate(
is, *os_)
: Transcoder<SourceEncoding,
TargetEncoding>::TranscodeUnsafe(is,
*os_))))
return false;
}
PutUnsafe(*os_, '\"');
return true;
}
bool ScanWriteUnescapedString(GenericStringStream<SourceEncoding> &is,
size_t length) {
return RAPIDJSON_LIKELY(is.Tell() < length);
}
bool WriteStartObject() {
os_->Put('{');
return true;
}
bool WriteEndObject() {
os_->Put('}');
return true;
}
bool WriteStartArray() {
os_->Put('[');
return true;
}
bool WriteEndArray() {
os_->Put(']');
return true;
}
bool WriteRawValue(const Ch *json, size_t length) {
PutReserve(*os_, length);
GenericStringStream<SourceEncoding> is(json);
while (RAPIDJSON_LIKELY(is.Tell() < length)) {
RAPIDJSON_ASSERT(is.Peek() != '\0');
if (RAPIDJSON_UNLIKELY(!(
writeFlags & kWriteValidateEncodingFlag
? Transcoder<SourceEncoding, TargetEncoding>::Validate(is,
*os_)
: Transcoder<SourceEncoding, TargetEncoding>::TranscodeUnsafe(
is, *os_))))
return false;
}
return true;
}
void Prefix(Type type) {
(void)type;
if (RAPIDJSON_LIKELY(level_stack_.GetSize() !=
0)) { // this value is not at root
Level *level = level_stack_.template Top<Level>();
if (level->valueCount > 0) {
if (level->inArray)
os_->Put(','); // add comma if it is not the first element in array
else // in object
os_->Put((level->valueCount % 2 == 0) ? ',' : ':');
}
if (!level->inArray && level->valueCount % 2 == 0)
RAPIDJSON_ASSERT(type == kStringType); // if it's in object, then even
// number should be a name
level->valueCount++;
} else {
RAPIDJSON_ASSERT(!hasRoot_); // Should only has one and only one root.
hasRoot_ = true;
}
}
// Flush the value if it is the top level one.
bool EndValue(bool ret) {
if (RAPIDJSON_UNLIKELY(level_stack_.Empty())) // end of json text
Flush();
return ret;
}
OutputStream *os_;
internal::Stack<StackAllocator> level_stack_;
int maxDecimalPlaces_;
bool hasRoot_;
private:
// Prohibit copy constructor & assignment operator.
Writer(const Writer &);
Writer &operator=(const Writer &);
};
// Full specialization for StringStream to prevent memory copying
template <>
inline bool Writer<StringBuffer>::WriteInt(int i) {
char *buffer = os_->Push(11);
const char *end = internal::i32toa(i, buffer);
os_->Pop(static_cast<size_t>(11 - (end - buffer)));
return true;
}
template <>
inline bool Writer<StringBuffer>::WriteUint(unsigned u) {
char *buffer = os_->Push(10);
const char *end = internal::u32toa(u, buffer);
os_->Pop(static_cast<size_t>(10 - (end - buffer)));
return true;
}
template <>
inline bool Writer<StringBuffer>::WriteInt64(int64_t i64) {
char *buffer = os_->Push(21);
const char *end = internal::i64toa(i64, buffer);
os_->Pop(static_cast<size_t>(21 - (end - buffer)));
return true;
}
template <>
inline bool Writer<StringBuffer>::WriteUint64(uint64_t u) {
char *buffer = os_->Push(20);
const char *end = internal::u64toa(u, buffer);
os_->Pop(static_cast<size_t>(20 - (end - buffer)));
return true;
}
template <>
inline bool Writer<StringBuffer>::WriteDouble(double d) {
if (internal::Double(d).IsNanOrInf()) {
// Note: This code path can only be reached if
// (RAPIDJSON_WRITE_DEFAULT_FLAGS & kWriteNanAndInfFlag).
if (!(kWriteDefaultFlags & kWriteNanAndInfFlag)) return false;
if (internal::Double(d).IsNan()) {
PutReserve(*os_, 3);
PutUnsafe(*os_, 'N');
PutUnsafe(*os_, 'a');
PutUnsafe(*os_, 'N');
return true;
}
if (internal::Double(d).Sign()) {
PutReserve(*os_, 9);
PutUnsafe(*os_, '-');
} else
PutReserve(*os_, 8);
PutUnsafe(*os_, 'I');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'f');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 'n');
PutUnsafe(*os_, 'i');
PutUnsafe(*os_, 't');
PutUnsafe(*os_, 'y');
return true;
}
char *buffer = os_->Push(25);
char *end = internal::dtoa(d, buffer, maxDecimalPlaces_);
os_->Pop(static_cast<size_t>(25 - (end - buffer)));
return true;
}
#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42)
template <>
inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
size_t length) {
if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
const char *p = is.src_;
const char *end = is.head_ + length;
const char *nextAligned = reinterpret_cast<const char *>(
(reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
const char *endAligned = reinterpret_cast<const char *>(
reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
if (nextAligned > end) return true;
while (p != nextAligned)
if (*p < 0x20 || *p == '\"' || *p == '\\') {
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
} else
os_->PutUnsafe(*p++);
// The rest of string using SIMD
static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"',
'\"', '\"', '\"', '\"', '\"', '\"',
'\"', '\"', '\"', '\"'};
static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\',
'\\', '\\', '\\', '\\', '\\', '\\',
'\\', '\\', '\\', '\\'};
static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
0x1F, 0x1F, 0x1F, 0x1F};
const __m128i dq =
_mm_loadu_si128(reinterpret_cast<const __m128i *>(&dquote[0]));
const __m128i bs =
_mm_loadu_si128(reinterpret_cast<const __m128i *>(&bslash[0]));
const __m128i sp =
_mm_loadu_si128(reinterpret_cast<const __m128i *>(&space[0]));
for (; p != endAligned; p += 16) {
const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
const __m128i t1 = _mm_cmpeq_epi8(s, dq);
const __m128i t2 = _mm_cmpeq_epi8(s, bs);
const __m128i t3 = _mm_cmpeq_epi8(_mm_max_epu8(s, sp),
sp); // s < 0x20 <=> max(s, 0x1F) == 0x1F
const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3);
unsigned short r = static_cast<unsigned short>(_mm_movemask_epi8(x));
if (RAPIDJSON_UNLIKELY(r != 0)) { // some of characters is escaped
SizeType len;
#ifdef _MSC_VER // Find the index of first escaped
unsigned long offset;
_BitScanForward(&offset, r);
len = offset;
#else
len = static_cast<SizeType>(__builtin_ffs(r) - 1);
#endif
char *q = reinterpret_cast<char *>(os_->PushUnsafe(len));
for (size_t i = 0; i < len; i++) q[i] = p[i];
p += len;
break;
}
_mm_storeu_si128(reinterpret_cast<__m128i *>(os_->PushUnsafe(16)), s);
}
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
}
#elif defined(RAPIDJSON_NEON)
template <>
inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
size_t length) {
if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
const char *p = is.src_;
const char *end = is.head_ + length;
const char *nextAligned = reinterpret_cast<const char *>(
(reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
const char *endAligned = reinterpret_cast<const char *>(
reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
if (nextAligned > end) return true;
while (p != nextAligned)
if (*p < 0x20 || *p == '\"' || *p == '\\') {
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
} else
os_->PutUnsafe(*p++);
// The rest of string using SIMD
const uint8x16_t s0 = vmovq_n_u8('"');
const uint8x16_t s1 = vmovq_n_u8('\\');
const uint8x16_t s2 = vmovq_n_u8('\b');
const uint8x16_t s3 = vmovq_n_u8(32);
for (; p != endAligned; p += 16) {
const uint8x16_t s = vld1q_u8(reinterpret_cast<const uint8_t *>(p));
uint8x16_t x = vceqq_u8(s, s0);
x = vorrq_u8(x, vceqq_u8(s, s1));
x = vorrq_u8(x, vceqq_u8(s, s2));
x = vorrq_u8(x, vcltq_u8(s, s3));
x = vrev64q_u8(x); // Rev in 64
uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0); // extract
uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1); // extract
SizeType len = 0;
bool escaped = false;
if (low == 0) {
if (high != 0) {
uint32_t lz = RAPIDJSON_CLZLL(high);
len = 8 + (lz >> 3);
escaped = true;
}
} else {
uint32_t lz = RAPIDJSON_CLZLL(low);
len = lz >> 3;
escaped = true;
}
if (RAPIDJSON_UNLIKELY(escaped)) { // some of characters is escaped
char *q = reinterpret_cast<char *>(os_->PushUnsafe(len));
for (size_t i = 0; i < len; i++) q[i] = p[i];
p += len;
break;
}
vst1q_u8(reinterpret_cast<uint8_t *>(os_->PushUnsafe(16)), s);
}
is.src_ = p;
return RAPIDJSON_LIKELY(is.Tell() < length);
}
#endif // RAPIDJSON_NEON
RAPIDJSON_NAMESPACE_END
#if defined(_MSC_VER) || defined(__clang__)
RAPIDJSON_DIAG_POP
#endif
#endif // RAPIDJSON_RAPIDJSON_H_

53
src/rm_driver/livox_ros_driver2/src/CHANGELOG.md Normal file
View File

@ -0,0 +1,53 @@
# Changelog
All notable changes to this project will be documented in this file.
## [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.

146
src/rm_driver/livox_ros_driver2/src/CMakeLists.txt Normal file
View File

@ -0,0 +1,146 @@
# Copyright(c) 2020 livoxtech limited.
cmake_minimum_required(VERSION 3.14)
project(livox_ros_driver2)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules")
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
endif()
# Printf version info
include(cmake/version.cmake)
project(${PROJECT_NAME} VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}")
#---------------------------------------------------------------------------------------
# Add ROS Version MACRO
#---------------------------------------------------------------------------------------
add_definitions(-DBUILDING_ROS2)
# find dependencies
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()
find_package(PCL REQUIRED QUIET)
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($ENV{ROS_DISTRO} STREQUAL "foxy")
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)
else()
rosidl_get_typesupport_target(cpp_typesupport_target
${LIVOX_INTERFACES} "rosidl_typesupport_cpp")
target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}")
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
)

412
src/rm_driver/livox_ros_driver2/src/LICENSE.txt Normal file
View File

@ -0,0 +1,412 @@
The following portions of the LIVOX’s Livox ROS Driver2 (“Software” referred to in the terms below) are made available to you under the terms of the MIT License provided below and is also available at https://opensource.org/licenses/MIT.
livox_ros_driver2
├── build.sh
├── cmake
│   └── version.cmake
├── CMakeLists.txt
├── config
│   ├── display_point_cloud_ROS1.rviz
│   ├── display_point_cloud_ROS2.rviz
│   ├── HAP_config.json
│   ├── MID360_config.json
│   └── mixed_HAP_MID360_config.json
├── launch_ROS1
│   ├── msg_HAP.launch
│   ├── msg_MID360.launch
│   ├── msg_mixed.launch
│   ├── rviz_HAP.launch
│   ├── rviz_MID360.launch
│   └── rviz_mixed.launch
├── launch_ROS2
│   ├── msg_HAP_launch.py
│   ├── msg_MID360_launch.py
│   ├── rviz_HAP_launch.py
│   ├── rviz_MID360_launch.py
│   └── rviz_mixed.py
├── msg
│   ├── CustomMsg.msg
│   └── CustomPoint.msg
├── package_ROS1.xml
├── package_ROS2.xml
├── package.xml
├── README.md
└── src
├── call_back
│   ├── lidar_common_callback.cpp
│   ├── lidar_common_callback.h
│   ├── livox_lidar_callback.cpp
│   └── livox_lidar_callback.h
├── comm
│   ├── cache_index.cpp
│   ├── cache_index.h
│   ├── comm.cpp
│   ├── comm.h
│   ├── ldq.cpp
│   ├── ldq.h
│   ├── lidar_imu_data_queue.cpp
│   ├── lidar_imu_data_queue.h
│   ├── pub_handler.cpp
│   ├── pub_handler.h
│   ├── semaphore.cpp
│   └── semaphore.h
├── driver_node.cpp
├── driver_node.h
├── include
│   ├── livox_ros_driver2.h
│   ├── ros1_headers.h
│   ├── ros2_headers.h
│   └── ros_headers.h
├── lddc.cpp
├── lddc.h
├── lds.cpp
├── lds.h
├── lds_lidar.cpp
├── lds_lidar.h
├── livox_ros_driver2.cpp
└── parse_cfg_file
├── parse_cfg_file.cpp
├── parse_cfg_file.h
├── parse_livox_lidar_cfg.cpp
└── parse_livox_lidar_cfg.h
---------------------------------
The MIT License (MIT)
Copyright (c) 2022 Livox. All rights reserved.
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
===============================================================
LIVOX’s Livox ROS Driver2 uses unmodified source code of RapidJSON (https://github.com/Tencent/rapidjson), which is also licensed under MIT license. A copy of the MIT license is provided below and is also available at https://opensource.org/licenses/MIT.
livox_ros_driver2
├── 3rdparty
│   └── rapidjson
│   ├── allocators.h
│   ├── cursorstreamwrapper.h
│   ├── document.h
│   ├── encodedstream.h
│   ├── encodings.h
│   ├── error
│   │   ├── en.h
│   │   └── error.h
│   ├── filereadstream.h
│   ├── filewritestream.h
│   ├── fwd.h
│   ├── internal
│   │   ├── biginteger.h
│   │   ├── clzll.h
│   │   ├── diyfp.h
│   │   ├── dtoa.h
│   │   ├── ieee754.h
│   │   ├── itoa.h
│   │   ├── meta.h
│   │   ├── pow10.h
│   │   ├── regex.h
│   │   ├── stack.h
│   │   ├── strfunc.h
│   │   ├── strtod.h
│   │   └── swap.h
│   ├── istreamwrapper.h
│   ├── memorybuffer.h
│   ├── memorystream.h
│   ├── msinttypes
│   │   ├── inttypes.h
│   │   └── stdint.h
│   ├── ostreamwrapper.h
│   ├── pointer.h
│   ├── prettywriter.h
│   ├── rapidjson.h
│   ├── reader.h
│   ├── schema.h
│   ├── stream.h
│   ├── stringbuffer.h
│   └── writer.h
-------------------------------------------------------------
Tencent is pleased to support the open source community by making RapidJSON
available.
Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
rights reserved.
Licensed under the MIT License (the "License"); you may not use this file
except in compliance with the License. You may obtain a copy of the License
at
http://opensource.org/licenses/MIT
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
License for the specific language governing permissions and limitations under
the License.
===============================================================
LIVOX’s Livox ROS Driver2 uses unmodified libraries and interfaces of ROS (https://www.ros.org/), which is also licensed under 3-Clause-BSD license. A copy of the 3-Clause-BSD license is provided below and is also available at https://opensource.org/licenses/BSD-3-Clause.
-------------------------------------------------------------
The 3-Clause BSD License
Copyright (c) 2001 - 2009, The Board of Trustees of the University of Illinois.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
* Redistributions of source code must retain the above
copyright notice, this list of conditions and the
following disclaimer.
* Redistributions in binary form must reproduce the
above copyright notice, this list of conditions
and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the University of Illinois
nor the names of its contributors may be used to
endorse or promote products derived from this
software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
===============================================================
LIVOX’s Livox ROS Driver2 uses unmodified libraries and interfaces of ROS2-rclcpp (https://github.com/ros2), which is also licensed under Apache License 2.0. A copy of the Apache License 2.0 is provided below and is also available at https://www.apache.org/licenses/LICENSE-2.0.
-------------------------------------------------------------
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

16
src/rm_driver/livox_ros_driver2/src/cmake/version.cmake Normal file
View File

@ -0,0 +1,16 @@
#---------------------------------------------------------------------------------------
# Get livox_ros_driver2 version from include/livox_ros_driver2.h
#---------------------------------------------------------------------------------------
file(READ "${CMAKE_CURRENT_LIST_DIR}/../src/include/livox_ros_driver2.h" LIVOX_ROS_DRIVER2_VERSION_FILE)
string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MAJOR ([0-9]+)" _ "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
set(ver_major ${CMAKE_MATCH_1})
string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MINOR ([0-9]+)" _ "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
set(ver_minor ${CMAKE_MATCH_1})
string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_PATCH ([0-9]+)" _ "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
set(ver_patch ${CMAKE_MATCH_1})
if (NOT DEFINED ver_major OR NOT DEFINED ver_minor OR NOT DEFINED ver_patch)
message(FATAL_ERROR "Could not extract valid version from include/livox_ros_driver2.h")
endif()
set (LIVOX_ROS_DRIVER2_VERSION "${ver_major}.${ver_minor}.${ver_patch}")

42
src/rm_driver/livox_ros_driver2/src/config/HAP_config.json Normal file
View File

@ -0,0 +1,42 @@
{
"lidar_summary_info" : {
"lidar_type": 8
},
"HAP": {
"lidar_net_info" : {
"cmd_data_port": 56000,
"push_msg_port": 0,
"point_data_port": 57000,
"imu_data_port": 58000,
"log_data_port": 59000
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5",
"cmd_data_port": 56000,
"push_msg_ip": "",
"push_msg_port": 0,
"point_data_ip": "192.168.1.5",
"point_data_port": 57000,
"imu_data_ip" : "192.168.1.5",
"imu_data_port": 58000,
"log_data_ip" : "",
"log_data_port": 59000
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.100",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}

41
src/rm_driver/livox_ros_driver2/src/config/MID360_config.json Normal file
View File

@ -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.50",
"cmd_data_port": 56101,
"push_msg_ip": "192.168.1.50",
"push_msg_port": 56201,
"point_data_ip": "192.168.1.50",
"point_data_port": 56301,
"imu_data_ip" : "192.168.1.50",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.137",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}

137
src/rm_driver/livox_ros_driver2/src/config/display_point_cloud.rviz Normal file
View File

@ -0,0 +1,137 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /PointCloud21
Splitter Ratio: 0.5
Tree Height: 796
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0.20000000298023224
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 150
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /livox/lidar
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: livox_frame
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic: /move_base_simple/goal
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 14.215983390808105
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.9503982067108154
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.6603963375091553
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1025
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000015f000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c3000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1853
X: 67
Y: 27

76
src/rm_driver/livox_ros_driver2/src/config/mixed_HAP_MID360_config.json Normal file
View File

@ -0,0 +1,76 @@
{
"lidar_summary_info" : {
"lidar_type": 8
},
"HAP": {
"lidar_net_info" : {
"cmd_data_port": 56000,
"push_msg_port": 0,
"point_data_port": 57000,
"imu_data_port": 58000,
"log_data_port": 59000
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5",
"cmd_data_port": 56000,
"push_msg_ip": "",
"push_msg_port": 0,
"point_data_ip": "192.168.1.5",
"point_data_port": 57000,
"imu_data_ip" : "192.168.1.5",
"imu_data_port": 58000,
"log_data_ip" : "",
"log_data_port": 59000
}
},
"MID360": {
"lidar_net_info" : {
"cmd_data_port": 56100,
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5",
"cmd_data_port": 56101,
"push_msg_ip": "192.168.1.5",
"push_msg_port": 56201,
"point_data_ip": "192.168.1.5",
"point_data_port": 56301,
"imu_data_ip" : "192.168.1.5",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.100",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
},
{
"ip" : "192.168.1.12",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}

55
src/rm_driver/livox_ros_driver2/src/launch/msg_HAP_launch.py Normal file
View File

@ -0,0 +1,55 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
import launch
################### user configure parameters for ros2 start ###################
xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic
data_src = 0 # 0-lidar, others-Invalid data src
publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
output_type = 0
frame_id = 'livox_frame'
lvx_file_path = '/home/livox/livox_test.lvx'
cmdline_bd_code = 'livox0000000001'
cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
cur_config_path = cur_path + '../config'
rviz_config_path = os.path.join(cur_config_path, 'livox_lidar.rviz')
user_config_path = os.path.join(cur_config_path, 'HAP_config.json')
################### user configure parameters for ros2 end #####################
livox_ros2_params = [
{"xfer_format": xfer_format},
{"multi_topic": multi_topic},
{"data_src": data_src},
{"publish_freq": publish_freq},
{"output_data_type": output_type},
{"frame_id": frame_id},
{"lvx_file_path": lvx_file_path},
{"user_config_path": user_config_path},
{"cmdline_input_bd_code": cmdline_bd_code}
]
def generate_launch_description():
livox_driver = Node(
package='livox_ros_driver2',
executable='livox_ros_driver2_node',
name='livox_lidar_publisher',
output='screen',
parameters=livox_ros2_params
)
return LaunchDescription([
livox_driver,
# launch.actions.RegisterEventHandler(
# event_handler=launch.event_handlers.OnProcessExit(
# target_action=livox_rviz,
# on_exit=[
# launch.actions.EmitEvent(event=launch.events.Shutdown()),
# ]
# )
# )
])

54
src/rm_driver/livox_ros_driver2/src/launch/msg_MID360_launch.py Normal file
View File

@ -0,0 +1,54 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
import launch
################### user configure parameters for ros2 start ###################
xfer_format = 4 # 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()),
# ]
# )
# )
])

63
src/rm_driver/livox_ros_driver2/src/launch/rviz_HAP_launch.py Normal file
View File

@ -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.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()),
# ]
# )
# )
])

63
src/rm_driver/livox_ros_driver2/src/launch/rviz_MID360_launch.py Normal file
View File

@ -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.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()),
# ]
# )
# )
])

63
src/rm_driver/livox_ros_driver2/src/launch/rviz_mixed.py Normal file
View File

@ -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.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()),
# ]
# )
# )
])

9
src/rm_driver/livox_ros_driver2/src/msg/CustomMsg.msg Normal file
View File

@ -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

10
src/rm_driver/livox_ros_driver2/src/msg/CustomPoint.msg Normal file
View File

@ -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

35
src/rm_driver/livox_ros_driver2/src/package.xml Normal file
View File

@ -0,0 +1,35 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>livox_ros_driver2</name>
<version>1.0.0</version>
<description>The ROS device driver for Livox 3D LiDARs, for ROS2</description>
<maintainer email="dev@livoxtech.com">feng</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>rcutils</depend>
<depend>pcl_conversions</depend>
<depend>rcl_interfaces</depend>
<depend>libpcl-all-dev</depend>
<exec_depend>rosbag2</exec_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<depend>git</depend>
<depend>apr</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

73
src/rm_driver/livox_ros_driver2/src/src/call_back/lidar_common_callback.cpp Normal file
View File

@ -0,0 +1,73 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "lidar_common_callback.h"
#include "../lds_lidar.h"
#include <string>
namespace livox_ros {
void LidarCommonCallback::OnLidarPointClounCb(PointFrame* frame, void* client_data) {
if (frame == nullptr) {
printf("LidarPointCloudCb frame is nullptr.\n");
return;
}
if (client_data == nullptr) {
printf("Lidar point cloud cb failed, client data is nullptr.\n");
return;
}
if (frame->lidar_num ==0) {
printf("LidarPointCloudCb lidar_num:%u.\n", frame->lidar_num);
return;
}
LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
//printf("Lidar point cloud, lidar_num:%u.\n", frame->lidar_num);
lds_lidar->StoragePointData(frame);
}
void LidarCommonCallback::LidarImuDataCallback(ImuData* imu_data, void *client_data) {
if (imu_data == nullptr) {
printf("Imu data is nullptr.\n");
return;
}
if (client_data == nullptr) {
printf("Lidar point cloud cb failed, client data is nullptr.\n");
return;
}
LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
lds_lidar->StorageImuData(imu_data);
}
} // namespace livox_ros

40
src/rm_driver/livox_ros_driver2/src/src/call_back/lidar_common_callback.h Normal file
View File

@ -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_

333
src/rm_driver/livox_ros_driver2/src/src/call_back/livox_lidar_callback.cpp Normal file
View File

@ -0,0 +1,333 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "livox_lidar_callback.h"
#include <string>
#include <thread>
#include <iostream>
namespace livox_ros {
void LivoxLidarCallback::LidarInfoChangeCallback(const uint32_t handle,
const LivoxLidarInfo* info,
void* client_data) {
if (client_data == nullptr) {
std::cout << "lidar info change callback failed, client data is nullptr" << std::endl;
return;
}
LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
LidarDevice* lidar_device = GetLidarDevice(handle, client_data);
if (lidar_device == nullptr) {
std::cout << "found lidar not defined in the user-defined config, ip: " << IpNumToString(handle) << std::endl;
// add lidar device
uint8_t index = 0;
int8_t ret = lds_lidar->cache_index_.GetFreeIndex(kLivoxLidarType, handle, index);
if (ret != 0) {
std::cout << "failed to add lidar device, lidar ip: " << IpNumToString(handle) << std::endl;
return;
}
LidarDevice *p_lidar = &(lds_lidar->lidars_[index]);
p_lidar->lidar_type = kLivoxLidarType;
} else {
// set the lidar according to the user-defined config
const UserLivoxLidarConfig& config = lidar_device->livox_config;
// lock for modify the lidar device set_bits
{
std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
if (config.pcl_data_type != -1 ) {
lidar_device->livox_config.set_bits |= kConfigDataType;
SetLivoxLidarPclDataType(handle, static_cast<LivoxLidarPointDataType>(config.pcl_data_type),
LivoxLidarCallback::SetDataTypeCallback, lds_lidar);
std::cout << "set pcl data type, handle: " << handle << ", data type: "
<< static_cast<int32_t>(config.pcl_data_type) << std::endl;
}
if (config.pattern_mode != -1) {
lidar_device->livox_config.set_bits |= kConfigScanPattern;
SetLivoxLidarScanPattern(handle, static_cast<LivoxLidarScanPattern>(config.pattern_mode),
LivoxLidarCallback::SetPatternModeCallback, lds_lidar);
std::cout << "set scan pattern, handle: " << handle << ", scan pattern: "
<< static_cast<int32_t>(config.pattern_mode) << std::endl;
}
if (config.blind_spot_set != -1) {
lidar_device->livox_config.set_bits |= kConfigBlindSpot;
SetLivoxLidarBlindSpot(handle, config.blind_spot_set,
LivoxLidarCallback::SetBlindSpotCallback, lds_lidar);
std::cout << "set blind spot, handle: " << handle << ", blind spot distance: "
<< config.blind_spot_set << std::endl;
}
if (config.dual_emit_en != -1) {
lidar_device->livox_config.set_bits |= kConfigDualEmit;
SetLivoxLidarDualEmit(handle, (config.dual_emit_en == 0 ? false : true),
LivoxLidarCallback::SetDualEmitCallback, lds_lidar);
std::cout << "set dual emit mode, handle: " << handle << ", enable dual emit: "
<< static_cast<int32_t>(config.dual_emit_en) << std::endl;
}
} // free lock for set_bits
// set extrinsic params into lidar
LivoxLidarInstallAttitude attitude {
config.extrinsic_param.roll,
config.extrinsic_param.pitch,
config.extrinsic_param.yaw,
config.extrinsic_param.x,
config.extrinsic_param.y,
config.extrinsic_param.z
};
SetLivoxLidarInstallAttitude(config.handle, &attitude,
LivoxLidarCallback::SetAttitudeCallback, lds_lidar);
}
std::cout << "begin to change work mode to 'Normal', handle: " << handle << std::endl;
SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr);
EnableLivoxLidarImuData(handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar);
return;
}
void LivoxLidarCallback::WorkModeChangedCallback(livox_status status,
uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data) {
if (status != kLivoxLidarStatusSuccess) {
std::cout << "failed to change work mode, handle: " << handle << ", try again..."<< std::endl;
std::this_thread::sleep_for(std::chrono::seconds(1));
SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr);
return;
}
std::cout << "successfully change work mode, handle: " << handle << std::endl;
return;
}
void LivoxLidarCallback::SetDataTypeCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data) {
LidarDevice* lidar_device = GetLidarDevice(handle, client_data);
if (lidar_device == nullptr) {
std::cout << "failed to set data type since no lidar device found, handle: "
<< handle << std::endl;
return;
}
LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
if (status == kLivoxLidarStatusSuccess) {
std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDataType));
if (!lidar_device->livox_config.set_bits) {
lidar_device->connect_state = kConnectStateSampling;
}
std::cout << "successfully set data type, handle: " << handle
<< ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
} else if (status == kLivoxLidarStatusTimeout) {
const UserLivoxLidarConfig& config = lidar_device->livox_config;
SetLivoxLidarPclDataType(handle, static_cast<LivoxLidarPointDataType>(config.pcl_data_type),
LivoxLidarCallback::SetDataTypeCallback, client_data);
std::cout << "set data type timeout, handle: " << handle
<< ", try again..." << std::endl;
} else {
std::cout << "failed to set data type, handle: " << handle
<< ", return code: " << response->ret_code
<< ", error key: " << response->error_key << std::endl;
}
return;
}
void LivoxLidarCallback::SetPatternModeCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data) {
LidarDevice* lidar_device = GetLidarDevice(handle, client_data);
if (lidar_device == nullptr) {
std::cout << "failed to set pattern mode since no lidar device found, handle: "
<< handle << std::endl;
return;
}
LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
if (status == kLivoxLidarStatusSuccess) {
std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigScanPattern));
if (!lidar_device->livox_config.set_bits) {
lidar_device->connect_state = kConnectStateSampling;
}
std::cout << "successfully set pattern mode, handle: " << handle
<< ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
} else if (status == kLivoxLidarStatusTimeout) {
const UserLivoxLidarConfig& config = lidar_device->livox_config;
SetLivoxLidarScanPattern(handle, static_cast<LivoxLidarScanPattern>(config.pattern_mode),
LivoxLidarCallback::SetPatternModeCallback, client_data);
std::cout << "set pattern mode timeout, handle: " << handle
<< ", try again..." << std::endl;
} else {
std::cout << "failed to set pattern mode, handle: " << handle
<< ", return code: " << response->ret_code
<< ", error key: " << response->error_key << std::endl;
}
return;
}
void LivoxLidarCallback::SetBlindSpotCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data) {
LidarDevice* lidar_device = GetLidarDevice(handle, client_data);
if (lidar_device == nullptr) {
std::cout << "failed to set blind spot since no lidar device found, handle: "
<< handle << std::endl;
return;
}
LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
if (status == kLivoxLidarStatusSuccess) {
std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigBlindSpot));
if (!lidar_device->livox_config.set_bits) {
lidar_device->connect_state = kConnectStateSampling;
}
std::cout << "successfully set blind spot, handle: " << handle
<< ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
} else if (status == kLivoxLidarStatusTimeout) {
const UserLivoxLidarConfig& config = lidar_device->livox_config;
SetLivoxLidarBlindSpot(handle, config.blind_spot_set,
LivoxLidarCallback::SetBlindSpotCallback, client_data);
std::cout << "set blind spot timeout, handle: " << handle
<< ", try again..." << std::endl;
} else {
std::cout << "failed to set blind spot, handle: " << handle
<< ", return code: " << response->ret_code
<< ", error key: " << response->error_key << std::endl;
}
return;
}
void LivoxLidarCallback::SetDualEmitCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data) {
LidarDevice* lidar_device = GetLidarDevice(handle, client_data);
if (lidar_device == nullptr) {
std::cout << "failed to set dual emit mode since no lidar device found, handle: "
<< handle << std::endl;
return;
}
LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
if (status == kLivoxLidarStatusSuccess) {
std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDualEmit));
if (!lidar_device->livox_config.set_bits) {
lidar_device->connect_state = kConnectStateSampling;
}
std::cout << "successfully set dual emit mode, handle: " << handle
<< ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
} else if (status == kLivoxLidarStatusTimeout) {
const UserLivoxLidarConfig& config = lidar_device->livox_config;
SetLivoxLidarDualEmit(handle, config.dual_emit_en,
LivoxLidarCallback::SetDualEmitCallback, client_data);
std::cout << "set dual emit mode timeout, handle: " << handle
<< ", try again..." << std::endl;
} else {
std::cout << "failed to set dual emit mode, handle: " << handle
<< ", return code: " << response->ret_code
<< ", error key: " << response->error_key << std::endl;
}
return;
}
void LivoxLidarCallback::SetAttitudeCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data) {
LidarDevice* lidar_device = GetLidarDevice(handle, client_data);
if (lidar_device == nullptr) {
std::cout << "failed to set dual emit mode since no lidar device found, handle: "
<< handle << std::endl;
return;
}
LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
if (status == kLivoxLidarStatusSuccess) {
std::cout << "successfully set lidar attitude, ip: " << IpNumToString(handle) << std::endl;
} else if (status == kLivoxLidarStatusTimeout) {
std::cout << "set lidar attitude timeout, ip: " << IpNumToString(handle)
<< ", try again..." << std::endl;
const UserLivoxLidarConfig& config = lidar_device->livox_config;
LivoxLidarInstallAttitude attitude {
config.extrinsic_param.roll,
config.extrinsic_param.pitch,
config.extrinsic_param.yaw,
config.extrinsic_param.x,
config.extrinsic_param.y,
config.extrinsic_param.z
};
SetLivoxLidarInstallAttitude(config.handle, &attitude,
LivoxLidarCallback::SetAttitudeCallback, lds_lidar);
} else {
std::cout << "failed to set lidar attitude, ip: " << IpNumToString(handle) << std::endl;
}
}
void LivoxLidarCallback::EnableLivoxLidarImuDataCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data) {
LidarDevice* lidar_device = GetLidarDevice(handle, client_data);
if (lidar_device == nullptr) {
std::cout << "failed to set pattern mode since no lidar device found, handle: "
<< handle << std::endl;
return;
}
LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
if (response == nullptr) {
std::cout << "failed to get response since no lidar IMU sensor found, handle: "
<< handle << std::endl;
return;
}
if (status == kLivoxLidarStatusSuccess) {
std::cout << "successfully enable Livox Lidar imu, ip: " << IpNumToString(handle) << std::endl;
} else if (status == kLivoxLidarStatusTimeout) {
std::cout << "enable Livox Lidar imu timeout, ip: " << IpNumToString(handle)
<< ", try again..." << std::endl;
EnableLivoxLidarImuData(handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar);
} else {
std::cout << "failed to enable Livox Lidar imu, ip: " << IpNumToString(handle) << std::endl;
}
}
LidarDevice* LivoxLidarCallback::GetLidarDevice(const uint32_t handle, void* client_data) {
if (client_data == nullptr) {
std::cout << "failed to get lidar device, client data is nullptr" << std::endl;
return nullptr;
}
LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
uint8_t index = 0;
int8_t ret = lds_lidar->cache_index_.GetIndex(kLivoxLidarType, handle, index);
if (ret != 0) {
return nullptr;
}
return &(lds_lidar->lidars_[index]);
}
} // namespace livox_ros

71
src/rm_driver/livox_ros_driver2/src/src/call_back/livox_lidar_callback.h Normal file
View File

@ -0,0 +1,71 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_
#define LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_
#include "../lds.h"
#include "../lds_lidar.h"
#include "../comm/comm.h"
#include "livox_lidar_api.h"
#include "livox_lidar_def.h"
namespace livox_ros {
class LivoxLidarCallback {
public:
static void LidarInfoChangeCallback(const uint32_t handle,
const LivoxLidarInfo* info,
void* client_data);
static void WorkModeChangedCallback(livox_status status,
uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data);
static void SetDataTypeCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data);
static void SetPatternModeCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data);
static void SetBlindSpotCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data);
static void SetDualEmitCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data);
static void SetAttitudeCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data);
static void EnableLivoxLidarImuDataCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response,
void *client_data);
private:
static LidarDevice* GetLidarDevice(const uint32_t handle, void* client_data);
};
} // namespace livox_ros
#endif // LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_

121
src/rm_driver/livox_ros_driver2/src/src/comm/cache_index.cpp Normal file
View File

@ -0,0 +1,121 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "cache_index.h"
#include "livox_lidar_def.h"
namespace livox_ros {
CacheIndex::CacheIndex() {
std::array<bool, kMaxLidarCount> index_cache = {0};
index_cache_.swap(index_cache);
}
int8_t CacheIndex::GetFreeIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) {
std::string key;
int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key);
if (ret != 0) {
return -1;
}
{
std::lock_guard<std::mutex> lock(index_mutex_);
if (map_index_.find(key) != map_index_.end()) {
index = map_index_[key];
return 0;
}
}
{
printf("GetFreeIndex key:%s.\n", key.c_str());
std::lock_guard<std::mutex> lock(index_mutex_);
for (size_t i = 0; i < kMaxSourceLidar; ++i) {
if (!index_cache_[i]) {
index_cache_[i] = 1;
map_index_[key] = static_cast<uint8_t>(i);
index = static_cast<uint8_t>(i);
return 0;
}
}
}
return -1;
}
int8_t CacheIndex::GenerateIndexKey(const uint8_t livox_lidar_type, const uint32_t handle, std::string& key) {
if (livox_lidar_type == kLivoxLidarType) {
key = "livox_lidar_" + std::to_string(handle);
} else {
printf("Can not generate index, the livox lidar type is unknown, the livox lidar type:%u\n", livox_lidar_type);
return -1;
}
return 0;
}
int8_t CacheIndex::GetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) {
std::string key;
int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key);
if (ret != 0) {
return -1;
}
if (map_index_.find(key) != map_index_.end()) {
std::lock_guard<std::mutex> lock(index_mutex_);
index = map_index_[key];
return 0;
}
printf("Can not get index, the livox lidar type:%u, handle:%u\n", livox_lidar_type, handle);
return -1;
}
int8_t CacheIndex::LvxGetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) {
std::string key;
int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key);
if (ret != 0) {
return -1;
}
if (map_index_.find(key) != map_index_.end()) {
index = map_index_[key];
return 0;
}
return GetFreeIndex(livox_lidar_type, handle, index);
}
void CacheIndex::ResetIndex(LidarDevice *lidar) {
std::string key;
int8_t ret = GenerateIndexKey(lidar->lidar_type, lidar->handle, key);
if (ret != 0) {
printf("Reset index failed, can not generate index key, lidar type:%u, handle:%u.\n", lidar->lidar_type, lidar->handle);
return;
}
if (map_index_.find(key) != map_index_.end()) {
uint8_t index = map_index_[key];
std::lock_guard<std::mutex> lock(index_mutex_);
map_index_.erase(key);
index_cache_[index] = 0;
}
}
} // namespace

54
src/rm_driver/livox_ros_driver2/src/src/comm/cache_index.h Normal file
View File

@ -0,0 +1,54 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_ROS_DRIVER_CACHE_INDEX_H_
#define LIVOX_ROS_DRIVER_CACHE_INDEX_H_
#include <mutex>
#include <array>
#include <map>
#include <string>
#include "comm/comm.h"
namespace livox_ros {
class CacheIndex {
public:
CacheIndex();
int8_t GetFreeIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index);
int8_t GetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index);
int8_t GenerateIndexKey(const uint8_t livox_lidar_type, const uint32_t handle, std::string& key);
int8_t LvxGetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index);
void ResetIndex(LidarDevice *lidar);
private:
std::mutex index_mutex_;
std::map<std::string, uint8_t> map_index_; /* key:handle/slot, val:index */
std::array<bool, kMaxSourceLidar> index_cache_;
};
} // namespace livox_ros
# endif // LIVOX_ROS_DRIVER_CACHE_INDEX_H_

70
src/rm_driver/livox_ros_driver2/src/src/comm/comm.cpp Normal file
View File

@ -0,0 +1,70 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "comm/comm.h"
#include <string.h>
#include <arpa/inet.h>
namespace livox_ros {
/** Common function --------------------------------------------------------- */
bool IsFilePathValid(const char *path_str) {
int str_len = strlen(path_str);
if ((str_len > kPathStrMinSize) && (str_len < kPathStrMaxSize)) {
return true;
} else {
return false;
}
}
uint32_t CalculatePacketQueueSize(const double publish_freq) {
uint32_t queue_size = 10;
if (publish_freq > 10.0) {
queue_size = static_cast<uint32_t>(publish_freq) + 1;
}
return queue_size;
}
std::string IpNumToString(uint32_t ip_num) {
struct in_addr ip;
ip.s_addr = ip_num;
return std::string(inet_ntoa(ip));
}
uint32_t IpStringToNum(std::string ip_string) {
return static_cast<uint32_t>(inet_addr(ip_string.c_str()));
}
std::string ReplacePeriodByUnderline(std::string str) {
std::size_t pos = str.find(".");
while (pos != std::string::npos) {
str.replace(pos, 1, "_");
pos = str.find(".");
}
return str;
}
} // namespace livox_ros

304
src/rm_driver/livox_ros_driver2/src/src/comm/comm.h Normal file
View File

@ -0,0 +1,304 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_ROS_DRIVER2_COMM_H_
#define LIVOX_ROS_DRIVER2_COMM_H_
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <string>
#include <vector>
#include <memory>
#include <map>
#include "lidar_imu_data_queue.h"
namespace livox_ros {
/** Max lidar data source num */
const uint8_t kMaxSourceLidar = 32;
/** Eth packet relative info parama */
const uint32_t kMaxPointPerEthPacket = 100;
const uint32_t kMinEthPacketQueueSize = 32; /**< must be 2^n */
const uint32_t kMaxEthPacketQueueSize = 131072; /**< must be 2^n */
const uint32_t kImuEthPacketQueueSize = 256;
/** Max packet length according to Ethernet MTU */
const uint32_t KEthPacketMaxLength = 1500;
const uint32_t KEthPacketHeaderLength = 18; /**< (sizeof(LivoxEthPacket) - 1) */
const uint32_t KCartesianPointSize = 13;
const uint32_t KSphericalPointSzie = 9;
const uint64_t kRosTimeMax = 4294967296000000000; /**< 2^32 * 1000000000ns */
const int64_t kPacketTimeGap = 1000000; /**< 1ms = 1000000ns */
/**< the threshold of packet continuous */
const int64_t kMaxPacketTimeGap = 1700000;
/**< the threshold of device disconect */
const int64_t kDeviceDisconnectThreshold = 1000000000;
const uint32_t kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */
const uint32_t kNsTolerantFrameTimeDeviation = 1000000; /**< 1ms = 1000000ns */
const uint32_t kRatioOfMsToNs = 1000000; /**< 1ms = 1000000ns */
const int kPathStrMinSize = 4; /**< Must more than 4 char */
const int kPathStrMaxSize = 256; /**< Must less than 256 char */
const int kBdCodeSize = 15;
const uint32_t kPointXYZRSize = 16;
const uint32_t kPointXYZRTRSize = 18;
const double PI = 3.14159265358979323846;
constexpr uint32_t kMaxBufferSize = 0x8000; // 32k bytes
/** Device Line Number **/
const uint8_t kLineNumberDefault = 1;
const uint8_t kLineNumberMid360 = 4;
const uint8_t kLineNumberHAP = 6;
// SDK related
typedef enum {
kIndustryLidarType = 1,
kVehicleLidarType = 2,
kDirectLidarType = 4,
kLivoxLidarType = 8
} LidarProtoType;
// SDK related
/** Timestamp sync mode define. */
typedef enum {
kTimestampTypeNoSync = 0, /**< No sync signal mode. */
kTimestampTypeGptpOrPtp = 1, /**< gPTP or PTP sync mode */
kTimestampTypeGps = 2 /**< GPS sync mode. */
} TimestampType;
/** Lidar connect state */
typedef enum {
kConnectStateOff = 0,
kConnectStateOn = 1,
kConnectStateConfig = 2,
kConnectStateSampling = 3,
} LidarConnectState;
/** Device data source type */
typedef enum {
kSourceRawLidar = 0, /**< Data from raw lidar. */
kSourceRawHub = 1, /**< Data from lidar hub. */
kSourceLvxFile, /**< Data from parse lvx file. */
kSourceUndef,
} LidarDataSourceType;
typedef enum { kCoordinateCartesian = 0, kCoordinateSpherical } CoordinateType;
typedef enum {
kConfigDataType = 1 << 0,
kConfigScanPattern = 1 << 1,
kConfigBlindSpot = 1 << 2,
kConfigDualEmit = 1 << 3,
kConfigUnknown
} LivoxLidarConfigCodeBit;
typedef enum {
kNoneExtrinsicParameter,
kExtrinsicParameterFromLidar,
kExtrinsicParameterFromXml
} ExtrinsicParameterType;
typedef struct {
uint8_t lidar_type {};
} LidarSummaryInfo;
/** 8bytes stamp to uint64_t stamp */
typedef union {
struct {
uint32_t low;
uint32_t high;
} stamp_word;
uint8_t stamp_bytes[8];
int64_t stamp;
} LdsStamp;
#pragma pack(1)
typedef struct {
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
float reflectivity; /**< Reflectivity */
uint8_t tag; /**< Livox point tag */
uint8_t line; /**< Laser line id */
double timestamp; /**< Timestamp of point*/
} LivoxPointXyzrtlt;
typedef struct {
float x;
float y;
float z;
float intensity;
uint8_t tag;
uint8_t line;
uint64_t offset_time;
} PointXyzlt;
typedef struct {
uint32_t handle;
uint8_t lidar_type; ////refer to LivoxLidarType
uint32_t points_num;
PointXyzlt* points;
} PointPacket;
typedef struct {
uint64_t base_time[kMaxSourceLidar] {};
uint8_t lidar_num {};
PointPacket lidar_point[kMaxSourceLidar] {};
} PointFrame;
#pragma pack()
typedef struct {
LidarProtoType lidar_type;
uint32_t handle;
uint64_t base_time;
uint32_t points_num;
std::vector<PointXyzlt> points;
} StoragePacket;
typedef struct {
LidarProtoType lidar_type;
uint32_t handle;
bool extrinsic_enable;
uint32_t point_num;
uint8_t data_type;
uint8_t line_num;
uint64_t time_stamp;
uint64_t point_interval;
std::vector<uint8_t> raw_data;
} RawPacket;
typedef struct {
StoragePacket *storage_packet;
volatile uint32_t rd_idx;
volatile uint32_t wr_idx;
uint32_t mask;
uint32_t size; /**< must be power of 2. */
} LidarDataQueue;
/*****************************/
/* About Extrinsic Parameter */
typedef struct {
float roll; /**< Roll angle, unit: degree. */
float pitch; /**< Pitch angle, unit: degree. */
float yaw; /**< Yaw angle, unit: degree. */
int32_t x; /**< X translation, unit: mm. */
int32_t y; /**< Y translation, unit: mm. */
int32_t z; /**< Z translation, unit: mm. */
} ExtParameter;
typedef float TranslationVector[3]; /**< x, y, z translation, unit: mm. */
typedef float RotationMatrix[3][3];
typedef struct {
TranslationVector trans;
RotationMatrix rotation;
} ExtParameterDetailed;
typedef struct {
LidarProtoType lidar_type;
uint32_t handle;
ExtParameter param;
} LidarExtParameter;
/** Configuration in json config file for livox lidar */
typedef struct {
char broadcast_code[16];
bool enable_connect;
bool enable_fan;
uint32_t return_mode;
uint32_t coordinate;
uint32_t imu_rate;
uint32_t extrinsic_parameter_source;
bool enable_high_sensitivity;
} UserRawConfig;
typedef struct {
bool enable_fan;
uint32_t return_mode;
uint32_t coordinate; /**< 0 for CartesianCoordinate; others for SphericalCoordinate. */
uint32_t imu_rate;
uint32_t extrinsic_parameter_source;
bool enable_high_sensitivity;
volatile uint32_t set_bits;
volatile uint32_t get_bits;
} UserConfig;
typedef struct {
uint32_t handle;
int8_t pcl_data_type;
int8_t pattern_mode;
int32_t blind_spot_set;
int8_t dual_emit_en;
ExtParameter extrinsic_param;
volatile uint32_t set_bits;
volatile uint32_t get_bits;
} UserLivoxLidarConfig;
/** Lidar data source info abstract */
typedef struct {
uint8_t lidar_type;
uint32_t handle;
// union {
// uint8_t slot : 4; //slot for LivoxLidarType::kVehicleLidarType
// uint8_t handle : 4; // handle for LivoxLidarType::kIndustryLidarType
// };
uint8_t data_src; /**< From raw lidar or livox file. */
volatile LidarConnectState connect_state;
// DeviceInfo info;
LidarDataQueue data;
LidarImuDataQueue imu_data;
uint32_t firmware_ver; /**< Firmware version of lidar */
UserLivoxLidarConfig livox_config;
} LidarDevice;
constexpr uint32_t kMaxProductType = 10;
constexpr uint32_t kDeviceTypeLidarMid70 = 6;
/***********************************/
/* Global function for general use */
bool IsFilePathValid(const char *path_str);
uint32_t CalculatePacketQueueSize(const double publish_freq);
std::string IpNumToString(uint32_t ip_num);
uint32_t IpStringToNum(std::string ip_string);
std::string ReplacePeriodByUnderline(std::string str);
} // namespace livox_ros
#endif // LIVOX_ROS_DRIVER2_COMM_H_

150
src/rm_driver/livox_ros_driver2/src/src/comm/ldq.cpp Normal file
View File

@ -0,0 +1,150 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <stdio.h>
#include <string.h>
#include "ldq.h"
namespace livox_ros {
/* for pointcloud queue process */
bool InitQueue(LidarDataQueue *queue, uint32_t queue_size) {
if (queue == nullptr) {
// ROS_WARN("RosDriver Queue: Initialization failed - invalid queue.");
return false;
}
if (!IsPowerOf2(queue_size)) {
queue_size = RoundupPowerOf2(queue_size);
printf("Init queue, real query size:%u.\n", queue_size);
}
if (queue->storage_packet) {
delete[] queue->storage_packet;
queue->storage_packet = nullptr;
}
queue->storage_packet = new StoragePacket[queue_size];
if (queue->storage_packet == nullptr) {
// ROS_WARN("RosDriver Queue: Initialization failed - failed to allocate memory.");
return false;
}
queue->rd_idx = 0;
queue->wr_idx = 0;
queue->size = queue_size;
queue->mask = queue_size - 1;
return true;
}
bool DeInitQueue(LidarDataQueue *queue) {
if (queue == nullptr) {
// ROS_WARN("RosDriver Queue: Deinitialization failed - invalid queue.");
return false;
}
if (queue->storage_packet) {
delete[] queue->storage_packet;
}
queue->rd_idx = 0;
queue->wr_idx = 0;
queue->size = 0;
queue->mask = 0;
return true;
}
void ResetQueue(LidarDataQueue *queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
}
bool QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet) {
if (queue == nullptr || storage_packet == nullptr) {
// ROS_WARN("RosDriver Queue: Invalid pointer parameters.");
return false;
}
if (QueueIsEmpty(queue)) {
// ROS_WARN("RosDriver Queue: Pop failed, since the queue is empty.");
return false;
}
uint32_t rd_idx = queue->rd_idx & queue->mask;
storage_packet->base_time = queue->storage_packet[rd_idx].base_time;
storage_packet->points_num = queue->storage_packet[rd_idx].points_num;
storage_packet->points.resize(queue->storage_packet[rd_idx].points_num);
memcpy(storage_packet->points.data(), queue->storage_packet[rd_idx].points.data(), (storage_packet->points_num) * sizeof(PointXyzlt));
return true;
}
void QueuePopUpdate(LidarDataQueue *queue) {
queue->rd_idx++;
}
bool QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet) {
if (!QueuePrePop(queue, storage_packet)) {
return false;
}
QueuePopUpdate(queue);
return true;
}
uint32_t QueueUsedSize(LidarDataQueue *queue) {
return queue->wr_idx - queue->rd_idx;
}
uint32_t QueueUnusedSize(LidarDataQueue *queue) {
return (queue->size - QueueUsedSize(queue));
}
bool QueueIsFull(LidarDataQueue *queue) {
return ((queue->wr_idx - queue->rd_idx) > queue->mask);
}
bool QueueIsEmpty(LidarDataQueue *queue) {
return (queue->rd_idx == queue->wr_idx);
}
uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, const uint64_t base_time) {
uint32_t wr_idx = queue->wr_idx & queue->mask;
PointPacket* lidar_point_data = reinterpret_cast<PointPacket*>(data);
queue->storage_packet[wr_idx].base_time = base_time;
queue->storage_packet[wr_idx].points_num = lidar_point_data->points_num;
queue->storage_packet[wr_idx].points.clear();
queue->storage_packet[wr_idx].points.resize(lidar_point_data->points_num);
memcpy(queue->storage_packet[wr_idx].points.data(), lidar_point_data->points, sizeof(PointXyzlt) * (lidar_point_data->points_num));
queue->wr_idx++;
return 1;
}
} // namespace livox_ros

66
src/rm_driver/livox_ros_driver2/src/src/comm/ldq.h Normal file
View File

@ -0,0 +1,66 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_ROS_DRIVER_LDQ_H_
#define LIVOX_ROS_DRIVER_LDQ_H_
#include <stdint.h>
#include <vector>
#include "comm/comm.h"
namespace livox_ros {
inline static bool IsPowerOf2(uint32_t size) {
return (size != 0) && ((size & (size - 1)) == 0);
}
inline static uint32_t RoundupPowerOf2(uint32_t size) {
uint32_t power2_val = 0;
for (int i = 0; i < 32; i++) {
power2_val = ((uint32_t)1) << i;
if (size <= power2_val) {
break;
}
}
return power2_val;
}
/** queue operate function */
bool InitQueue(LidarDataQueue *queue, uint32_t queue_size);
bool DeInitQueue(LidarDataQueue *queue);
void ResetQueue(LidarDataQueue *queue);
bool QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet);
void QueuePopUpdate(LidarDataQueue *queue);
bool QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet);
uint32_t QueueUsedSize(LidarDataQueue *queue);
uint32_t QueueUnusedSize(LidarDataQueue *queue);
bool QueueIsFull(LidarDataQueue *queue);
bool QueueIsEmpty(LidarDataQueue *queue);
uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, const uint64_t base_time);
} // namespace livox_ros
#endif // LIVOX_ROS_DRIVER_LDQ_H_

70
src/rm_driver/livox_ros_driver2/src/src/comm/lidar_imu_data_queue.cpp Normal file
View File

@ -0,0 +1,70 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "lidar_imu_data_queue.h"
namespace livox_ros {
void LidarImuDataQueue::Push(ImuData* imu_data) {
ImuData data;
data.lidar_type = imu_data->lidar_type;
data.handle = imu_data->handle;
data.time_stamp = imu_data->time_stamp;
data.gyro_x = imu_data->gyro_x;
data.gyro_y = imu_data->gyro_y;
data.gyro_z = imu_data->gyro_z;
data.acc_x = imu_data->acc_x;
data.acc_y = imu_data->acc_y;
data.acc_z = imu_data->acc_z;
std::lock_guard<std::mutex> lock(mutex_);
imu_data_queue_.push_back(std::move(data));
}
bool LidarImuDataQueue::Pop(ImuData& imu_data) {
std::lock_guard<std::mutex> lock(mutex_);
if (imu_data_queue_.empty()) {
return false;
}
imu_data = imu_data_queue_.front();
imu_data_queue_.pop_front();
return true;
}
bool LidarImuDataQueue::Empty() {
std::lock_guard<std::mutex> lock(mutex_);
return imu_data_queue_.empty();
}
void LidarImuDataQueue::Clear() {
std::list<ImuData> tmp_imu_data_queue;
{
std::lock_guard<std::mutex> lock(mutex_);
imu_data_queue_.swap(tmp_imu_data_queue);
}
}
} // namespace livox_ros

77
src/rm_driver/livox_ros_driver2/src/src/comm/lidar_imu_data_queue.h Normal file
View File

@ -0,0 +1,77 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_
#define LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_
#include <list>
#include <mutex>
#include <cstdint>
namespace livox_ros {
// Based on the IMU Data Type in Livox communication protocol
// TODO: add a link to the protocol
typedef struct {
float gyro_x; /**< Gyroscope X axis, Unit:rad/s */
float gyro_y; /**< Gyroscope Y axis, Unit:rad/s */
float gyro_z; /**< Gyroscope Z axis, Unit:rad/s */
float acc_x; /**< Accelerometer X axis, Unit:g */
float acc_y; /**< Accelerometer Y axis, Unit:g */
float acc_z; /**< Accelerometer Z axis, Unit:g */
} RawImuPoint;
typedef struct {
uint8_t lidar_type;
uint32_t handle;
uint8_t slot;
// union {
// uint8_t handle;
// uint8_t slot;
// };
uint64_t time_stamp;
float gyro_x; /**< Gyroscope X axis, Unit:rad/s */
float gyro_y; /**< Gyroscope Y axis, Unit:rad/s */
float gyro_z; /**< Gyroscope Z axis, Unit:rad/s */
float acc_x; /**< Accelerometer X axis, Unit:g */
float acc_y; /**< Accelerometer Y axis, Unit:g */
float acc_z; /**< Accelerometer Z axis, Unit:g */
} ImuData;
class LidarImuDataQueue {
public:
void Push(ImuData* imu_data);
bool Pop(ImuData& imu_data);
bool Empty();
void Clear();
private:
std::mutex mutex_;
std::list<ImuData> imu_data_queue_;
};
} // namespace
#endif // LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_

531
src/rm_driver/livox_ros_driver2/src/src/comm/pub_handler.cpp Normal file
View File

@ -0,0 +1,531 @@
//
// 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 <chrono>
#include <cstdlib>
#include <iostream>
#include <limits>
namespace livox_ros {
std::atomic<bool> PubHandler::is_timestamp_sync_;
PubHandler &pub_handler() {
static PubHandler handler;
return handler;
}
static ExtParameterDetailed extrinsic_global = {
{0, 0, 0},
{
{1, 0, 0},
{0, 1, 0},
{0, 0, 1}
}
};
void PubHandler::Init() {}
void PubHandler::Uninit() {
if (lidar_listen_id_ > 0) {
LivoxLidarRemovePointCloudObserver(lidar_listen_id_);
lidar_listen_id_ = 0;
}
RequestExit();
if (point_process_thread_ && point_process_thread_->joinable()) {
point_process_thread_->join();
point_process_thread_ = nullptr;
} else {
/* */
}
}
void PubHandler::RequestExit() { is_quit_.store(true); }
void PubHandler::SetPointCloudConfig(const double publish_freq) {
publish_interval_ = (kNsPerSecond / (publish_freq * 10)) * 10;
publish_interval_tolerance_ =
publish_interval_ - kNsTolerantFrameTimeDeviation;
publish_interval_ms_ = publish_interval_ / kRatioOfMsToNs;
if (!point_process_thread_) {
point_process_thread_ =
std::make_shared<std::thread>(&PubHandler::RawDataProcess, this);
}
return;
}
void PubHandler::SetImuDataCallback(ImuDataCallback cb, void *client_data) {
imu_client_data_ = client_data;
imu_callback_ = cb;
}
void PubHandler::AddLidarsExtParam(LidarExtParameter &lidar_param) {
std::unique_lock<std::mutex> lock(packet_mutex_);
uint32_t id = 0;
GetLidarId(lidar_param.lidar_type, lidar_param.handle, id);
lidar_extrinsics_[id] = lidar_param;
}
void PubHandler::ClearAllLidarsExtrinsicParams() {
std::unique_lock<std::mutex> lock(packet_mutex_);
lidar_extrinsics_.clear();
}
void PubHandler::SetPointCloudsCallback(PointCloudsCallback cb,
void *client_data) {
pub_client_data_ = client_data;
points_callback_ = cb;
lidar_listen_id_ =
LivoxLidarAddPointCloudObserver(OnLivoxLidarPointCloudCallback, this);
}
void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle,
const uint8_t dev_type,
LivoxLidarEthernetPacket *data,
void *client_data) {
PubHandler *self = (PubHandler *)client_data;
if (!self) {
return;
}
if (data->time_type != kTimestampTypeNoSync) {
is_timestamp_sync_.store(true);
} else {
is_timestamp_sync_.store(false);
}
if (data->data_type == kLivoxLidarImuData) {
if (self->imu_callback_) {
RawImuPoint *imu = (RawImuPoint *)data->data;
ImuData imu_data;
imu_data.lidar_type =
static_cast<uint8_t>(LidarProtoType::kLivoxLidarType);
imu_data.handle = handle;
imu_data.time_stamp = GetEthPacketTimestamp(
data->time_type, data->timestamp, sizeof(data->timestamp));
imu_data.gyro_x = imu->gyro_x * extrinsic_global.rotation[0][0] +
imu->gyro_y * extrinsic_global.rotation[0][1] +
imu->gyro_z * extrinsic_global.rotation[0][2];
// * this->extrinsic_.rotation[0][0] + imu->gyro_x *
// extrinsic_.rotation[0][1] + imu->gyro_x * extrinsic_.rotation[0][2];
imu_data.gyro_y = imu->gyro_x * extrinsic_global.rotation[1][0] +
imu->gyro_y * extrinsic_global.rotation[1][1] +
imu->gyro_z * extrinsic_global.rotation[1][2];
imu_data.gyro_z = imu->gyro_x * extrinsic_global.rotation[2][0] +
imu->gyro_y * extrinsic_global.rotation[2][1] +
imu->gyro_z * extrinsic_global.rotation[2][2];
imu_data.acc_x = imu->acc_x * extrinsic_global.rotation[0][0] +
imu->acc_y * extrinsic_global.rotation[0][1] +
imu->acc_z * extrinsic_global.rotation[0][2];
imu_data.acc_y = imu->acc_x * extrinsic_global.rotation[1][0] +
imu->acc_y * extrinsic_global.rotation[1][1] +
imu->acc_z * extrinsic_global.rotation[1][2];
imu_data.acc_z = imu->acc_x * extrinsic_global.rotation[2][0] +
imu->acc_y * extrinsic_global.rotation[2][1] +
imu->acc_z * extrinsic_global.rotation[2][2];
self->imu_callback_(&imu_data, self->imu_client_data_);
}
return;
}
RawPacket packet = {};
packet.handle = handle;
packet.lidar_type = LidarProtoType::kLivoxLidarType;
packet.extrinsic_enable = false;
if (dev_type == LivoxLidarDeviceType::kLivoxLidarTypeIndustrialHAP) {
packet.line_num = kLineNumberHAP;
} else if (dev_type == LivoxLidarDeviceType::kLivoxLidarTypeMid360) {
packet.line_num = kLineNumberMid360;
} else {
packet.line_num = kLineNumberDefault;
}
packet.data_type = data->data_type;
packet.point_num = data->dot_num;
packet.point_interval = data->time_interval * 100 / data->dot_num; // ns
packet.time_stamp = GetEthPacketTimestamp(data->time_type, data->timestamp,
sizeof(data->timestamp));
uint32_t length = data->length - sizeof(LivoxLidarEthernetPacket) + 1;
packet.raw_data.insert(packet.raw_data.end(), data->data,
data->data + length);
{
std::unique_lock<std::mutex> lock(self->packet_mutex_);
self->raw_packet_queue_.push_back(packet);
}
self->packet_condition_.notify_one();
return;
}
void PubHandler::PublishPointCloud() {
// publish point
if (points_callback_) {
points_callback_(&frame_, pub_client_data_);
}
return;
}
void PubHandler::CheckTimer(uint32_t id) {
if (PubHandler::is_timestamp_sync_.load()) { // Enable time synchronization
auto &process_handler = lidar_process_handlers_[id];
uint64_t recent_time_ms =
process_handler->GetRecentTimeStamp() / kRatioOfMsToNs;
if ((recent_time_ms % publish_interval_ms_ != 0) || recent_time_ms == 0) {
return;
}
uint64_t diff = process_handler->GetRecentTimeStamp() -
process_handler->GetLidarBaseTime();
if (diff < publish_interval_tolerance_) {
return;
}
frame_.base_time[frame_.lidar_num] = process_handler->GetLidarBaseTime();
points_[id].clear();
process_handler->GetLidarPointClouds(points_[id]);
if (points_[id].empty()) {
return;
}
PointPacket &lidar_point = frame_.lidar_point[frame_.lidar_num];
lidar_point.lidar_type = LidarProtoType::kLivoxLidarType; // TODO:
lidar_point.handle = id;
lidar_point.points_num = points_[id].size();
lidar_point.points = points_[id].data();
frame_.lidar_num++;
if (frame_.lidar_num != 0) {
PublishPointCloud();
frame_.lidar_num = 0;
}
} else { // Disable time synchronization
auto now_time = std::chrono::high_resolution_clock::now();
// First Set
static bool first = true;
if (first) {
last_pub_time_ = now_time;
first = false;
return;
}
if (now_time - last_pub_time_ <
std::chrono::nanoseconds(publish_interval_)) {
return;
}
last_pub_time_ += std::chrono::nanoseconds(publish_interval_);
for (auto &process_handler : lidar_process_handlers_) {
frame_.base_time[frame_.lidar_num] =
process_handler.second->GetLidarBaseTime();
uint32_t handle = process_handler.first;
points_[handle].clear();
process_handler.second->GetLidarPointClouds(points_[handle]);
if (points_[handle].empty()) {
continue;
}
PointPacket &lidar_point = frame_.lidar_point[frame_.lidar_num];
lidar_point.lidar_type = LidarProtoType::kLivoxLidarType; // TODO:
lidar_point.handle = handle;
lidar_point.points_num = points_[handle].size();
lidar_point.points = points_[handle].data();
frame_.lidar_num++;
}
PublishPointCloud();
frame_.lidar_num = 0;
}
return;
}
void PubHandler::RawDataProcess() {
RawPacket raw_data;
while (!is_quit_.load()) {
{
std::unique_lock<std::mutex> lock(packet_mutex_);
if (raw_packet_queue_.empty()) {
packet_condition_.wait_for(lock, std::chrono::milliseconds(500));
if (raw_packet_queue_.empty()) {
continue;
}
}
raw_data = raw_packet_queue_.front();
raw_packet_queue_.pop_front();
if (raw_packet_queue_.size() < 3) { // reduce CPU usage
std::this_thread::sleep_for(std::chrono::microseconds(50));
}
}
uint32_t id = 0;
GetLidarId(raw_data.lidar_type, raw_data.handle, id);
if (lidar_process_handlers_.find(id) == lidar_process_handlers_.end()) {
lidar_process_handlers_[id].reset(new LidarPubHandler());
}
auto &process_handler = lidar_process_handlers_[id];
if (lidar_extrinsics_.find(id) != lidar_extrinsics_.end()) {
lidar_process_handlers_[id]->SetLidarsExtParam(lidar_extrinsics_[id]);
}
process_handler->PointCloudProcess(raw_data);
CheckTimer(id);
}
}
bool PubHandler::GetLidarId(LidarProtoType lidar_type, uint32_t handle,
uint32_t &id) {
if (lidar_type == kLivoxLidarType) {
id = handle;
return true;
}
return false;
}
uint64_t PubHandler::GetEthPacketTimestamp(uint8_t timestamp_type,
uint8_t *time_stamp, uint8_t size) {
LdsStamp time;
memcpy(time.stamp_bytes, time_stamp, size);
if (timestamp_type == kTimestampTypeGptpOrPtp ||
timestamp_type == kTimestampTypeGps) {
return time.stamp;
}
return std::chrono::high_resolution_clock::now().time_since_epoch().count();
}
/*******************************/
/* LidarPubHandler Definitions*/
LidarPubHandler::LidarPubHandler() : is_set_extrinsic_params_(false) {}
uint64_t LidarPubHandler::GetLidarBaseTime() {
if (points_clouds_.empty()) {
return 0;
}
return points_clouds_.at(0).offset_time;
}
void LidarPubHandler::GetLidarPointClouds(
std::vector<PointXyzlt> &points_clouds) {
std::lock_guard<std::mutex> lock(mutex_);
points_clouds.swap(points_clouds_);
}
uint64_t LidarPubHandler::GetRecentTimeStamp() {
if (points_clouds_.empty()) {
return 0;
}
return points_clouds_.back().offset_time;
}
uint32_t LidarPubHandler::GetLidarPointCloudsSize() {
std::lock_guard<std::mutex> lock(mutex_);
return points_clouds_.size();
}
// convert to standard format and extrinsic compensate
void LidarPubHandler::PointCloudProcess(RawPacket &pkt) {
if (pkt.lidar_type == LidarProtoType::kLivoxLidarType) {
LivoxLidarPointCloudProcess(pkt);
} else {
static bool flag = false;
if (!flag) {
std::cout << "error, unsupported protocol type: "
<< static_cast<int>(pkt.lidar_type) << std::endl;
flag = true;
}
}
}
void LidarPubHandler::LivoxLidarPointCloudProcess(RawPacket &pkt) {
switch (pkt.data_type) {
case kLivoxLidarCartesianCoordinateHighData:
ProcessCartesianHighPoint(pkt);
break;
case kLivoxLidarCartesianCoordinateLowData:
ProcessCartesianLowPoint(pkt);
break;
case kLivoxLidarSphericalCoordinateData:
ProcessSphericalPoint(pkt);
break;
default:
std::cout << "unknown data type: " << static_cast<int>(pkt.data_type)
<< " !!" << std::endl;
break;
}
}
void LidarPubHandler::SetLidarsExtParam(LidarExtParameter lidar_param) {
if (is_set_extrinsic_params_) {
return;
}
extrinsic_.trans[0] = lidar_param.param.x;
extrinsic_.trans[1] = lidar_param.param.y;
extrinsic_.trans[2] = lidar_param.param.z;
double cos_roll =
cos(static_cast<double>(lidar_param.param.roll * PI / 180.0));
double cos_pitch =
cos(static_cast<double>(lidar_param.param.pitch * PI / 180.0));
double cos_yaw = cos(static_cast<double>(lidar_param.param.yaw * PI / 180.0));
double sin_roll =
sin(static_cast<double>(lidar_param.param.roll * PI / 180.0));
double sin_pitch =
sin(static_cast<double>(lidar_param.param.pitch * PI / 180.0));
double sin_yaw = sin(static_cast<double>(lidar_param.param.yaw * PI / 180.0));
std::cout<<"extrinsic rpy"<<" "<<lidar_param.param.roll<<" "<<lidar_param.param.pitch<<" "<<lidar_param.param.yaw<<"\n";
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;
extrinsic_global.rotation[0][0] = cos_pitch * cos_yaw;
extrinsic_global.rotation[0][1] =
sin_roll * sin_pitch * cos_yaw - cos_roll * sin_yaw;
extrinsic_global.rotation[0][2] =
cos_roll * sin_pitch * cos_yaw + sin_roll * sin_yaw;
extrinsic_global.rotation[1][0] = cos_pitch * sin_yaw;
extrinsic_global.rotation[1][1] =
sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw;
extrinsic_global.rotation[1][2] =
cos_roll * sin_pitch * sin_yaw - sin_roll * cos_yaw;
extrinsic_global.rotation[2][0] = -sin_pitch;
extrinsic_global.rotation[2][1] = sin_roll * cos_pitch;
extrinsic_global.rotation[2][2] = cos_roll * cos_pitch;
is_set_extrinsic_params_ = true;
}
void LidarPubHandler::ProcessCartesianHighPoint(RawPacket &pkt) {
LivoxLidarCartesianHighRawPoint *raw =
(LivoxLidarCartesianHighRawPoint *)pkt.raw_data.data();
PointXyzlt point = {};
for (uint32_t i = 0; i < pkt.point_num; i++) {
if (pkt.extrinsic_enable) {
point.x = raw[i].x / 1000.0;
point.y = raw[i].y / 1000.0;
point.z = raw[i].z / 1000.0;
} else {
point.x = (raw[i].x * extrinsic_.rotation[0][0] +
raw[i].y * extrinsic_.rotation[0][1] +
raw[i].z * extrinsic_.rotation[0][2] + extrinsic_.trans[0]) /
1000.0;
point.y = (raw[i].x * extrinsic_.rotation[1][0] +
raw[i].y * extrinsic_.rotation[1][1] +
raw[i].z * extrinsic_.rotation[1][2] + extrinsic_.trans[1]) /
1000.0;
point.z = (raw[i].x * extrinsic_.rotation[2][0] +
raw[i].y * extrinsic_.rotation[2][1] +
raw[i].z * extrinsic_.rotation[2][2] + extrinsic_.trans[2]) /
1000.0;
}
point.intensity = raw[i].reflectivity;
point.line = i % pkt.line_num;
point.tag = raw[i].tag;
point.offset_time = pkt.time_stamp + i * pkt.point_interval;
std::lock_guard<std::mutex> lock(mutex_);
points_clouds_.push_back(point);
}
}
void LidarPubHandler::ProcessCartesianLowPoint(RawPacket &pkt) {
LivoxLidarCartesianLowRawPoint *raw =
(LivoxLidarCartesianLowRawPoint *)pkt.raw_data.data();
PointXyzlt point = {};
for (uint32_t i = 0; i < pkt.point_num; i++) {
if (pkt.extrinsic_enable) {
point.x = raw[i].x / 100.0;
point.y = raw[i].y / 100.0;
point.z = raw[i].z / 100.0;
} else {
point.x = (raw[i].x * extrinsic_.rotation[0][0] +
raw[i].y * extrinsic_.rotation[0][1] +
raw[i].z * extrinsic_.rotation[0][2] + extrinsic_.trans[0]) /
100.0;
point.y = (raw[i].x * extrinsic_.rotation[1][0] +
raw[i].y * extrinsic_.rotation[1][1] +
raw[i].z * extrinsic_.rotation[1][2] + extrinsic_.trans[1]) /
100.0;
point.z = (raw[i].x * extrinsic_.rotation[2][0] +
raw[i].y * extrinsic_.rotation[2][1] +
raw[i].z * extrinsic_.rotation[2][2] + extrinsic_.trans[2]) /
100.0;
}
point.intensity = raw[i].reflectivity;
point.line = i % pkt.line_num;
point.tag = raw[i].tag;
point.offset_time = pkt.time_stamp + i * pkt.point_interval;
std::lock_guard<std::mutex> lock(mutex_);
points_clouds_.push_back(point);
}
}
void LidarPubHandler::ProcessSphericalPoint(RawPacket &pkt) {
LivoxLidarSpherPoint *raw = (LivoxLidarSpherPoint *)pkt.raw_data.data();
PointXyzlt point = {};
for (uint32_t i = 0; i < pkt.point_num; i++) {
double radius = raw[i].depth / 1000.0;
double theta = raw[i].theta / 100.0 / 180 * PI;
double phi = raw[i].phi / 100.0 / 180 * PI;
double src_x = radius * sin(theta) * cos(phi);
double src_y = radius * sin(theta) * sin(phi);
double src_z = radius * cos(theta);
if (pkt.extrinsic_enable) {
point.x = src_x;
point.y = src_y;
point.z = src_z;
} else {
point.x = src_x * extrinsic_.rotation[0][0] +
src_y * extrinsic_.rotation[0][1] +
src_z * extrinsic_.rotation[0][2] +
(extrinsic_.trans[0] / 1000.0);
point.y = src_x * extrinsic_.rotation[1][0] +
src_y * extrinsic_.rotation[1][1] +
src_z * extrinsic_.rotation[1][2] +
(extrinsic_.trans[1] / 1000.0);
point.z = src_x * extrinsic_.rotation[2][0] +
src_y * extrinsic_.rotation[2][1] +
src_z * extrinsic_.rotation[2][2] +
(extrinsic_.trans[2] / 1000.0);
}
point.intensity = raw[i].reflectivity;
point.line = i % pkt.line_num;
point.tag = raw[i].tag;
point.offset_time = pkt.time_stamp + i * pkt.point_interval;
std::lock_guard<std::mutex> lock(mutex_);
points_clouds_.push_back(point);
}
}
} // namespace livox_ros

138
src/rm_driver/livox_ros_driver2/src/src/comm/pub_handler.h Normal file
View File

@ -0,0 +1,138 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_DRIVER_PUB_HANDLER_H_
#define LIVOX_DRIVER_PUB_HANDLER_H_
#include <atomic>
#include <cstring>
#include <condition_variable> // std::condition_variable
#include <deque>
#include <functional>
#include <map>
#include <memory>
#include <mutex> // std::mutex
#include <thread>
#include "livox_lidar_def.h"
#include "livox_lidar_api.h"
#include "comm/comm.h"
namespace livox_ros {
class LidarPubHandler {
public:
LidarPubHandler();
~ LidarPubHandler() {}
void PointCloudProcess(RawPacket& pkt);
void SetLidarsExtParam(LidarExtParameter param);
void GetLidarPointClouds(std::vector<PointXyzlt>& points_clouds);
uint64_t GetRecentTimeStamp();
uint32_t GetLidarPointCloudsSize();
uint64_t GetLidarBaseTime();
private:
void LivoxLidarPointCloudProcess(RawPacket & pkt);
void ProcessCartesianHighPoint(RawPacket & pkt);
void ProcessCartesianLowPoint(RawPacket & pkt);
void ProcessSphericalPoint(RawPacket & pkt);
std::vector<PointXyzlt> points_clouds_;
ExtParameterDetailed extrinsic_ = {
{0, 0, 0},
{
{1, 0, 0},
{0, 1, 0},
{0, 0, 1}
}
};
std::mutex mutex_;
std::atomic_bool is_set_extrinsic_params_;
};
class PubHandler {
public:
using PointCloudsCallback = std::function<void(PointFrame*, void *)>;
using ImuDataCallback = std::function<void(ImuData*, void*)>;
using TimePoint = std::chrono::high_resolution_clock::time_point;
PubHandler() {}
~ PubHandler() { Uninit(); }
void Uninit();
void RequestExit();
void Init();
void SetPointCloudConfig(const double publish_freq);
void SetPointCloudsCallback(PointCloudsCallback cb, void* client_data);
void AddLidarsExtParam(LidarExtParameter& extrinsic_params);
void ClearAllLidarsExtrinsicParams();
void SetImuDataCallback(ImuDataCallback cb, void* client_data);
private:
//thread to process raw data
void RawDataProcess();
std::atomic<bool> is_quit_{false};
std::shared_ptr<std::thread> point_process_thread_;
std::mutex packet_mutex_;
std::condition_variable packet_condition_;
//publish callback
void CheckTimer(uint32_t id);
void PublishPointCloud();
static void OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t dev_type,
LivoxLidarEthernetPacket *data, void *client_data);
static bool GetLidarId(LidarProtoType lidar_type, uint32_t handle, uint32_t& id);
static uint64_t GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size);
PointCloudsCallback points_callback_;
void* pub_client_data_ = nullptr;
ImuDataCallback imu_callback_;
void* imu_client_data_ = nullptr;
PointFrame frame_;
std::deque<RawPacket> raw_packet_queue_;
//pub config
uint64_t publish_interval_ = 100000000; //100 ms
uint64_t publish_interval_tolerance_ = 100000000; //100 ms
uint64_t publish_interval_ms_ = 100; //100 ms
TimePoint last_pub_time_;
std::map<uint32_t, std::unique_ptr<LidarPubHandler>> lidar_process_handlers_;
std::map<uint32_t, std::vector<PointXyzlt>> points_;
std::map<uint32_t, LidarExtParameter> lidar_extrinsics_;
static std::atomic<bool> is_timestamp_sync_;
uint16_t lidar_listen_id_ = 0;
};
PubHandler &pub_handler();
} // namespace livox_ros
#endif // LIVOX_DRIVER_PUB_HANDLER_H_

41
src/rm_driver/livox_ros_driver2/src/src/comm/semaphore.cpp Normal file
View File

@ -0,0 +1,41 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "semaphore.h"
namespace livox_ros {
void Semaphore::Signal() {
std::unique_lock<std::mutex> lock(mutex_);
++count_;
cv_.notify_one();
}
void Semaphore::Wait() {
std::unique_lock<std::mutex> lock(mutex_);
cv_.wait(lock, [=] { return count_ > 0; });
--count_;
}
} // namespace livox_ros

51
src/rm_driver/livox_ros_driver2/src/src/comm/semaphore.h Normal file
View File

@ -0,0 +1,51 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_ROS_DRIVER_SEMAPHORE_H_
#define LIVOX_ROS_DRIVER_SEMAPHORE_H_
#include <mutex>
#include <condition_variable>
namespace livox_ros {
class Semaphore {
public:
explicit Semaphore(int count = 0) : count_(count) {
}
void Signal();
void Wait();
int GetCount() {
return count_;
}
private:
std::mutex mutex_;
std::condition_variable cv_;
volatile int count_;
};
} // namespace livox_ros
#endif // LIVOX_ROS_DRIVER_SEMAPHORE_H_

46
src/rm_driver/livox_ros_driver2/src/src/driver_node.cpp Normal file
View File

@ -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

56
src/rm_driver/livox_ros_driver2/src/src/driver_node.h Normal file
View File

@ -0,0 +1,56 @@
//
// 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;
class DriverNode final : public rclcpp::Node {
public:
explicit DriverNode(const rclcpp::NodeOptions& options);
DriverNode(const DriverNode &) = delete;
~DriverNode();
DriverNode &operator=(const DriverNode &) = delete;
DriverNode& GetNode() noexcept;
private:
void PointCloudDataPollThread();
void ImuDataPollThread();
std::unique_ptr<Lddc> lddc_ptr_;
std::shared_ptr<std::thread> pointclouddata_poll_thread_;
std::shared_ptr<std::thread> imudata_poll_thread_;
std::shared_future<void> future_;
std::promise<void> exit_signal_;
};
} // namespace livox_ros
#endif // LIVOX_DRIVER_NODE_H

40
src/rm_driver/livox_ros_driver2/src/src/include/livox_ros_driver2.h Normal file
View File

@ -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 0
#define LIVOX_ROS_DRIVER2_VER_PATCH 0
#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_

52
src/rm_driver/livox_ros_driver2/src/src/include/ros2_headers.h Normal file
View File

@ -0,0 +1,52 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
// Denoting headers specifically used for building ROS1 Driver.
#ifndef ROS2_HEADERS_H_
#define ROS2_HEADERS_H_
#include <thread>
#include <future>
#include <rclcpp/rclcpp.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include "livox_ros_driver2/msg/custom_point.hpp"
#include "livox_ros_driver2/msg/custom_msg.hpp"
#define DRIVER_DEBUG(node, ...) RCLCPP_DEBUG((node).get_logger(), __VA_ARGS__)
#define DRIVER_INFO(node, ...) RCLCPP_INFO((node).get_logger(), __VA_ARGS__)
#define DRIVER_WARN(node, ...) RCLCPP_WARN((node).get_logger(), __VA_ARGS__)
#define DRIVER_ERROR(node, ...) RCLCPP_ERROR((node).get_logger(), __VA_ARGS__)
#define DRIVER_FATAL(node, ...) RCLCPP_FATAL((node).get_logger(), __VA_ARGS__)
#define DRIVER_DEBUG_EXTRA(node, EXTRA, ...) RCLCPP_DEBUG_##EXTRA((node).get_logger(), __VA_ARGS__)
#define DRIVER_INFO_EXTRA(node, EXTRA, ...) RCLCPP_INFO_##EXTRA((node).get_logger(), __VA_ARGS__)
#define DRIVER_WARN_EXTRA(node, EXTRA, ...) RCLCPP_WARN_##EXTRA((node).get_logger(), __VA_ARGS__)
#define DRIVER_ERROR_EXTRA(node, EXTRA, ...) RCLCPP_ERROR_##EXTRA((node).get_logger(), __VA_ARGS__)
#define DRIVER_FATAL_EXTRA(node, EXTRA, ...) RCLCPP_FATAL_##EXTRA((node).get_logger(), __VA_ARGS__)
#endif // ROS2_HEADERS_H_

30
src/rm_driver/livox_ros_driver2/src/src/include/ros_headers.h Normal file
View File

@ -0,0 +1,30 @@
//
// 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_
#include "ros2_headers.h"
#endif // ROS_HEADERS_H_

551
src/rm_driver/livox_ros_driver2/src/src/lddc.cpp Normal file
View File

@ -0,0 +1,551 @@
//
// 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/comm.h"
#include "comm/ldq.h"
#include <cstdint>
#include <inttypes.h>
#include <iomanip>
#include <iostream>
#include <math.h>
#include <stdint.h>
#include "include/ros_headers.h"
#include "driver_node.h"
#include "lds_lidar.h"
namespace livox_ros {
/** Lidar Data Distribute Control--------------------------------------------*/
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;
}
Lddc::~Lddc() {
PrepareExit();
std::cout << "lddc destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
<< std::endl;
}
int Lddc::RegisterLds(Lds *lds) {
if (lds_ == nullptr) {
lds_ = lds;
return 0;
} else {
return -1;
}
}
void Lddc::DistributePointCloudData(void) {
if (!lds_) {
std::cout << "lds is not registered" << std::endl;
return;
}
if (lds_->IsRequestExit()) {
std::cout << "DistributePointCloudData is RequestExit" << std::endl;
return;
}
lds_->pcd_semaphore_.Wait();
for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
uint32_t lidar_id = i;
LidarDevice *lidar = &lds_->lidars_[lidar_id];
LidarDataQueue *p_queue = &lidar->data;
if ((kConnectStateSampling != lidar->connect_state) ||
(p_queue == nullptr)) {
continue;
}
PollingLidarPointCloudData(lidar_id, lidar);
}
}
void Lddc::DistributeImuData(void) {
if (!lds_) {
std::cout << "lds is not registered" << std::endl;
return;
}
if (lds_->IsRequestExit()) {
std::cout << "DistributeImuData is RequestExit" << std::endl;
return;
}
lds_->imu_semaphore_.Wait();
for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
uint32_t lidar_id = i;
LidarDevice *lidar = &lds_->lidars_[lidar_id];
LidarImuDataQueue *p_queue = &lidar->imu_data;
if ((kConnectStateSampling != lidar->connect_state) ||
(p_queue == nullptr)) {
continue;
}
PollingLidarImuData(lidar_id, lidar);
}
}
void Lddc::PollingLidarPointCloudData(uint8_t index, LidarDevice *lidar) {
LidarDataQueue *p_queue = &lidar->data;
if (p_queue == nullptr || p_queue->storage_packet == nullptr) {
return;
}
while (!lds_->IsRequestExit() && !QueueIsEmpty(p_queue)) {
if (kPointCloud2Msg == transfer_format_) {
PublishPointcloud2(p_queue, index);
} else if (kLivoxCustomMsg == transfer_format_) {
// PublishPointcloud2AndCustomMsg(p_queue, index);
PublishCustomPointcloud(p_queue, index);
} else if (kPclPxyziMsg == transfer_format_) {
PublishPclMsg(p_queue, index);
} else if (kAllMsg == transfer_format_) {
PublishPointcloud2AndCustomMsg(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) {
if (lds_) {
lds_->PrepareExit();
lds_ = nullptr;
}
}
void Lddc::PublishPointcloud2(LidarDataQueue *queue, uint8_t index) {
while (!QueueIsEmpty(queue)) {
StoragePacket pkg;
QueuePop(queue, &pkg);
if (pkg.points.empty()) {
printf("Publish point cloud2 failed, the pkg points is empty.\n");
continue;
}
PointCloud2 cloud;
uint64_t timestamp = 0;
InitPointcloud2Msg(pkg, cloud, timestamp);
PublishPointcloud2Data(index, timestamp, cloud);
}
}
void Lddc::PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index) {
while (!QueueIsEmpty(queue)) {
StoragePacket pkg;
QueuePop(queue, &pkg);
if (pkg.points.empty()) {
printf("Publish custom point cloud failed, the pkg points is empty.\n");
continue;
}
CustomMsg livox_msg;
InitCustomMsg(livox_msg, pkg, index);
FillPointsToCustomMsg(livox_msg, pkg);
PublishCustomPointData(livox_msg, index);
}
}
void Lddc::PublishPointcloud2AndCustomMsg(LidarDataQueue *queue,
uint8_t index) {
// printf("12312312321\n");
while (!QueueIsEmpty(queue)) {
StoragePacket pkg;
QueuePop(queue, &pkg);
if (pkg.points.empty()) {
printf("Publish point cloud failed, the pkg points is empty.\n");
continue;
}
PointCloud2 cloud;
uint64_t timestamp = 0;
InitPointcloud2Msg(pkg, cloud, timestamp);
PublishPointcloud2Data(index, timestamp, cloud);
CustomMsg livox_msg;
InitCustomMsg(livox_msg, pkg, index);
FillPointsToCustomMsg(livox_msg, pkg);
PublishCustomPointData(livox_msg, index);
}
}
/* for pcl::pxyzi */
void Lddc::PublishPclMsg(LidarDataQueue *queue, uint8_t index) {
#ifdef BUILDING_ROS2
static bool first_log = true;
if (first_log) {
std::cout
<< "error: message type 'pcl::PointCloud' is NOT supported in ROS2, "
<< "please modify the 'xfer_format' field in the launch file"
<< std::endl;
}
first_log = false;
return;
#endif
while (!QueueIsEmpty(queue)) {
StoragePacket pkg;
QueuePop(queue, &pkg);
if (pkg.points.empty()) {
printf("Publish point cloud failed, the pkg points is empty.\n");
continue;
}
PointCloud cloud;
uint64_t timestamp = 0;
InitPclMsg(pkg, cloud, timestamp);
FillPointsToPclMsg(pkg, cloud);
PublishPclData(index, timestamp, cloud);
}
return;
}
void Lddc::InitPointcloud2MsgHeader(PointCloud2 &cloud) {
cloud.header.frame_id.assign(frame_id_);
cloud.height = 1;
cloud.width = 0;
cloud.fields.resize(7);
cloud.fields[0].offset = 0;
cloud.fields[0].name = "x";
cloud.fields[0].count = 1;
cloud.fields[0].datatype = PointField::FLOAT32;
cloud.fields[1].offset = 4;
cloud.fields[1].name = "y";
cloud.fields[1].count = 1;
cloud.fields[1].datatype = PointField::FLOAT32;
cloud.fields[2].offset = 8;
cloud.fields[2].name = "z";
cloud.fields[2].count = 1;
cloud.fields[2].datatype = PointField::FLOAT32;
cloud.fields[3].offset = 12;
cloud.fields[3].name = "intensity";
cloud.fields[3].count = 1;
cloud.fields[3].datatype = PointField::FLOAT32;
cloud.fields[4].offset = 16;
cloud.fields[4].name = "tag";
cloud.fields[4].count = 1;
cloud.fields[4].datatype = PointField::UINT8;
cloud.fields[5].offset = 17;
cloud.fields[5].name = "line";
cloud.fields[5].count = 1;
cloud.fields[5].datatype = PointField::UINT8;
cloud.fields[6].offset = 18;
cloud.fields[6].name = "timestamp";
cloud.fields[6].count = 1;
cloud.fields[6].datatype = PointField::FLOAT64;
cloud.point_step = sizeof(LivoxPointXyzrtlt);
}
void Lddc::InitPointcloud2Msg(const StoragePacket &pkg, PointCloud2 &cloud,
uint64_t &timestamp) {
InitPointcloud2MsgHeader(cloud);
cloud.point_step = sizeof(LivoxPointXyzrtlt);
cloud.width = pkg.points_num;
cloud.row_step = cloud.width * cloud.point_step;
cloud.is_bigendian = false;
cloud.is_dense = true;
if (!pkg.points.empty()) {
timestamp = pkg.base_time;
}
cloud.header.stamp = rclcpp::Time(timestamp);
std::vector<LivoxPointXyzrtlt> points;
for (size_t i = 0; i < pkg.points_num; ++i) {
LivoxPointXyzrtlt point;
point.x = pkg.points[i].x;
point.y = pkg.points[i].y;
point.z = pkg.points[i].z;
point.reflectivity = pkg.points[i].intensity;
point.tag = pkg.points[i].tag;
point.line = pkg.points[i].line;
point.timestamp = static_cast<double>(pkg.points[i].offset_time);
points.push_back(std::move(point));
}
cloud.data.resize(pkg.points_num * sizeof(LivoxPointXyzrtlt));
memcpy(cloud.data.data(), points.data(),
pkg.points_num * sizeof(LivoxPointXyzrtlt));
}
void Lddc::PublishPointcloud2Data(const uint8_t index, const uint64_t timestamp,
const PointCloud2 &cloud) {
Publisher<PointCloud2>::SharedPtr publisher_ptr;
if (kAllMsg == transfer_format_) {
publisher_ptr = std::dynamic_pointer_cast<Publisher<PointCloud2>>(
GetCurrentPublisher2(index));
} else {
publisher_ptr = std::dynamic_pointer_cast<Publisher<PointCloud2>>(
GetCurrentPublisher(index));
}
if (kOutputToRos == output_type_) {
publisher_ptr->publish(cloud);
} else {
}
}
void Lddc::InitCustomMsg(CustomMsg &livox_msg, const StoragePacket &pkg,
uint8_t index) {
livox_msg.header.frame_id.assign(frame_id_);
uint64_t timestamp = 0;
if (!pkg.points.empty()) {
timestamp = pkg.base_time;
}
livox_msg.timebase = timestamp;
livox_msg.header.stamp = rclcpp::Time(timestamp);
livox_msg.point_num = pkg.points_num;
if (lds_->lidars_[index].lidar_type == kLivoxLidarType) {
livox_msg.lidar_id = lds_->lidars_[index].handle;
} else {
printf("Init custom msg lidar id failed, the index:%u.\n", index);
livox_msg.lidar_id = 0;
}
}
void Lddc::FillPointsToCustomMsg(CustomMsg &livox_msg,
const StoragePacket &pkg) {
uint32_t points_num = pkg.points_num;
const std::vector<PointXyzlt> &points = pkg.points;
for (uint32_t i = 0; i < points_num; ++i) {
CustomPoint point;
point.x = points[i].x;
point.y = points[i].y;
point.z = points[i].z;
point.reflectivity = points[i].intensity;
point.tag = points[i].tag;
point.line = points[i].line;
point.offset_time =
static_cast<uint32_t>(points[i].offset_time - pkg.base_time);
livox_msg.points.push_back(std::move(point));
}
}
void Lddc::PublishCustomPointData(const CustomMsg &livox_msg,
const uint8_t index) {
Publisher<CustomMsg>::SharedPtr publisher_ptr =
std::dynamic_pointer_cast<Publisher<CustomMsg>>(
GetCurrentPublisher(index));
if (kOutputToRos == output_type_) {
publisher_ptr->publish(livox_msg);
} else {
}
}
void Lddc::InitPclMsg(const StoragePacket &pkg, PointCloud &cloud,
uint64_t &timestamp) {
std::cout << "warning: pcl::PointCloud is not supported in ROS2, "
<< "please check code logic" << std::endl;
return;
}
void Lddc::FillPointsToPclMsg(const StoragePacket &pkg, PointCloud &pcl_msg) {
std::cout << "warning: pcl::PointCloud is not supported in ROS2, "
<< "please check code logic" << std::endl;
return;
}
void Lddc::PublishPclData(const uint8_t index, const uint64_t timestamp,
const PointCloud &cloud) {
std::cout << "warning: pcl::PointCloud is not supported in ROS2, "
<< "please check code logic" << std::endl;
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;
imu_msg.header.stamp = rclcpp::Time(timestamp); // to ros time stamp
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);
Publisher<ImuMsg>::SharedPtr publisher_ptr =
std::dynamic_pointer_cast<Publisher<ImuMsg>>(
GetCurrentImuPublisher(index));
if (kOutputToRos == output_type_) {
publisher_ptr->publish(imu_msg);
} else {
}
}
std::shared_ptr<rclcpp::PublisherBase>
Lddc::CreatePublisher(uint8_t msg_type, std::string &topic_name,
uint32_t queue_size) {
if (kPointCloud2Msg == msg_type) {
DRIVER_INFO(*cur_node_, "%s publish use PointCloud2 format",
topic_name.c_str());
return cur_node_->create_publisher<PointCloud2>(topic_name, queue_size);
} else if (kLivoxCustomMsg == msg_type) {
DRIVER_INFO(*cur_node_, "%s publish use livox custom format",
topic_name.c_str());
return cur_node_->create_publisher<CustomMsg>(topic_name, queue_size);
} else if (kLivoxImuMsg == msg_type) {
DRIVER_INFO(*cur_node_, "%s publish use imu format", topic_name.c_str());
return cur_node_->create_publisher<ImuMsg>(topic_name, queue_size);
} else {
PublisherPtr null_publisher(nullptr);
return null_publisher;
}
}
std::shared_ptr<rclcpp::PublisherBase>
Lddc::GetCurrentPublisher(uint8_t handle) {
uint32_t queue_size = kMinEthPacketQueueSize;
if (use_multi_topic_) {
if (!private_pub_[handle]) {
char name_str[48];
memset(name_str, 0, sizeof(name_str));
std::string ip_string = IpNumToString(lds_->lidars_[handle].handle);
snprintf(name_str, sizeof(name_str), "livox/lidar_%s",
ReplacePeriodByUnderline(ip_string).c_str());
std::string topic_name(name_str);
queue_size = queue_size * 2; // queue size is 64 for only one lidar
private_pub_[handle] =
CreatePublisher(transfer_format_, topic_name, queue_size);
}
return private_pub_[handle];
} else {
if (!global_pub_) {
std::string topic_name("livox/lidar");
queue_size = queue_size * 8; // shared queue size is 256, for all lidars
if (kAllMsg == transfer_format_) {
global_pub_ =
cur_node_->create_publisher<CustomMsg>(topic_name, queue_size);
global_pub_2_ = cur_node_->create_publisher<PointCloud2>(
"livox/lidar/pointcloud", queue_size);
DRIVER_INFO(*cur_node_, "%s publish use pointcloud2 and custom format",
topic_name.c_str());
} else {
global_pub_ = CreatePublisher(transfer_format_, topic_name, queue_size);
}
}
return global_pub_;
}
}
std::shared_ptr<rclcpp::PublisherBase>
Lddc::GetCurrentPublisher2(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
if (kAllMsg == transfer_format_) {
global_pub_ =
cur_node_->create_publisher<CustomMsg>(topic_name, queue_size);
global_pub_2_ = cur_node_->create_publisher<PointCloud2>(
"livox/lidar/pointcloud", queue_size);
DRIVER_INFO(*cur_node_, "%s publish use pointcloud2 and custom format",
topic_name.c_str());
} else {
global_pub_ = CreatePublisher(transfer_format_, topic_name, queue_size);
}
}
return global_pub_2_;
}
}
std::shared_ptr<rclcpp::PublisherBase>
Lddc::GetCurrentImuPublisher(uint8_t handle) {
uint32_t queue_size = kMinEthPacketQueueSize;
if (use_multi_topic_) {
if (!private_imu_pub_[handle]) {
char name_str[48];
memset(name_str, 0, sizeof(name_str));
std::string ip_string = IpNumToString(lds_->lidars_[handle].handle);
snprintf(name_str, sizeof(name_str), "livox/imu_%s",
ReplacePeriodByUnderline(ip_string).c_str());
std::string topic_name(name_str);
queue_size = queue_size * 2; // queue size is 64 for only one lidar
private_imu_pub_[handle] =
CreatePublisher(kLivoxImuMsg, topic_name, queue_size);
}
return private_imu_pub_[handle];
} else {
if (!global_imu_pub_) {
std::string topic_name("livox/imu");
queue_size = queue_size * 8; // shared queue size is 256, for all lidars
global_imu_pub_ = CreatePublisher(kLivoxImuMsg, topic_name, queue_size);
}
return global_imu_pub_;
}
}
void Lddc::CreateBagFile(const std::string &file_name) {}
} // namespace livox_ros

153
src/rm_driver/livox_ros_driver2/src/src/lddc.h Normal file
View File

@ -0,0 +1,153 @@
//
// 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 "comm/comm.h"
#include "include/livox_ros_driver2.h"
#include "driver_node.h"
#include "lds.h"
#include <cstdint>
namespace livox_ros {
/** Send pointcloud message Data to ros subscriber or save them in rosbag file
*/
typedef enum {
kOutputToRos = 0,
kOutputToRosBagFile = 1,
} DestinationOfMessageOutput;
/** The message type of transfer */
typedef enum {
kPointCloud2Msg = 0,
kLivoxCustomMsg = 1,
kPclPxyziMsg = 2,
kLivoxImuMsg = 3,
kAllMsg = 4,
} TransferType;
/** Type-Definitions based on ROS versions */
template <typename MessageT> using Publisher = rclcpp::Publisher<MessageT>;
using PublisherPtr = std::shared_ptr<rclcpp::PublisherBase>;
using PointCloud2 = sensor_msgs::msg::PointCloud2;
using PointField = sensor_msgs::msg::PointField;
using CustomMsg = livox_ros_driver2::msg::CustomMsg;
using CustomPoint = livox_ros_driver2::msg::CustomPoint;
using ImuMsg = sensor_msgs::msg::Imu;
using PointCloud = pcl::PointCloud<pcl::PointXYZI>;
class DriverNode;
class Lddc final {
public:
Lddc(int format, int multi_topic, int data_src, int output_type, double frq,
std::string &frame_id);
~Lddc();
int RegisterLds(Lds *lds);
void DistributePointCloudData(void);
void DistributeImuData(void);
void CreateBagFile(const std::string &file_name);
void PrepareExit(void);
uint8_t GetTransferFormat(void) { return transfer_format_; }
uint8_t IsMultiTopic(void) { return use_multi_topic_; }
void SetRosNode(livox_ros::DriverNode *node) { cur_node_ = node; }
// void SetRosPub(ros::Publisher *pub) { global_pub_ = pub; }; // NOT USED
void SetPublishFrq(uint32_t frq) { publish_frq_ = frq; }
public:
Lds *lds_;
private:
void PollingLidarPointCloudData(uint8_t index, LidarDevice *lidar);
void PollingLidarImuData(uint8_t index, LidarDevice *lidar);
void PublishPointcloud2(LidarDataQueue *queue, uint8_t index);
void PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index);
void PublishPointcloud2AndCustomMsg(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);
PublisherPtr CreatePublisher(uint8_t msg_type, std::string &topic_name,
uint32_t queue_size);
PublisherPtr GetCurrentPublisher(uint8_t index);
PublisherPtr GetCurrentPublisher2(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_;
PublisherPtr private_pub_[kMaxSourceLidar];
PublisherPtr global_pub_;
PublisherPtr global_pub_2_;
PublisherPtr private_imu_pub_[kMaxSourceLidar];
PublisherPtr global_imu_pub_;
livox_ros::DriverNode *cur_node_;
};
} // namespace livox_ros
#endif // LIVOX_ROS_DRIVER2_LDDC_H_

200
src/rm_driver/livox_ros_driver2/src/src/lds.cpp Normal file
View File

@ -0,0 +1,200 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <chrono>
#include <algorithm>
#include "lds.h"
#include "comm/ldq.h"
namespace livox_ros {
CacheIndex Lds::cache_index_;
/* Member function --------------------------------------------------------- */
Lds::Lds(const double publish_freq, const uint8_t data_src)
: lidar_count_(kMaxSourceLidar),
pcd_semaphore_(0),
imu_semaphore_(0),
publish_freq_(publish_freq),
data_src_(data_src),
request_exit_(false) {
ResetLds(data_src_);
}
Lds::~Lds() {
lidar_count_ = 0;
ResetLds(0);
printf("lds destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
}
void Lds::ResetLidar(LidarDevice *lidar, uint8_t data_src) {
//cache_index_.ResetIndex(lidar);
DeInitQueue(&lidar->data);
lidar->imu_data.Clear();
lidar->data_src = data_src;
lidar->connect_state = kConnectStateOff;
}
void Lds::SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src) {
lidar->data_src = data_src;
}
void Lds::ResetLds(uint8_t data_src) {
lidar_count_ = kMaxSourceLidar;
for (uint32_t i = 0; i < kMaxSourceLidar; i++) {
ResetLidar(&lidars_[i], data_src);
}
}
void Lds::RequestExit() {
request_exit_ = true;
}
bool Lds::IsAllQueueEmpty() {
for (int i = 0; i < lidar_count_; i++) {
if (!QueueIsEmpty(&lidars_[i].data)) {
return false;
}
}
return true;
}
bool Lds::IsAllQueueReadStop() {
for (int i = 0; i < lidar_count_; i++) {
uint32_t data_size = QueueUsedSize(&lidars_[i].data);
if (data_size) {
return false;
}
}
return true;
}
void Lds::StorageImuData(ImuData* imu_data) {
uint32_t device_num = 0;
if (imu_data->lidar_type == kLivoxLidarType) {
device_num = imu_data->handle;
} else {
printf("Storage imu data failed, unknown lidar type:%u.\n", imu_data->lidar_type);
return;
}
uint8_t index = 0;
int ret = cache_index_.GetIndex(imu_data->lidar_type, device_num, index);
if (ret != 0) {
printf("Storage point data failed, can not get index, lidar type:%u, device_num:%u.\n", imu_data->lidar_type, device_num);
return;
}
LidarDevice *p_lidar = &lidars_[index];
LidarImuDataQueue* imu_queue = &p_lidar->imu_data;
imu_queue->Push(imu_data);
if (!imu_queue->Empty()) {
if (imu_semaphore_.GetCount() <= 0) {
imu_semaphore_.Signal();
}
}
}
void Lds::StorageLvxPointData(PointFrame* frame) {
if (frame == nullptr) {
return;
}
uint8_t lidar_number = frame->lidar_num;
for (uint i = 0; i < lidar_number; ++i) {
PointPacket& lidar_point = frame->lidar_point[i];
uint64_t base_time = frame->base_time[i];
uint8_t index = 0;
int8_t ret = cache_index_.LvxGetIndex(lidar_point.lidar_type, lidar_point.handle, index);
if (ret != 0) {
printf("Storage lvx point data failed, lidar type:%u, device num:%u.\n", lidar_point.lidar_type, lidar_point.handle);
continue;
}
lidars_[index].connect_state = kConnectStateSampling;
PushLidarData(&lidar_point, index, base_time);
}
}
void Lds::StoragePointData(PointFrame* frame) {
if (frame == nullptr) {
return;
}
uint8_t lidar_number = frame->lidar_num;
for (uint i = 0; i < lidar_number; ++i) {
PointPacket& lidar_point = frame->lidar_point[i];
//printf("StoragePointData, lidar_type:%u, point_num:%lu.\n", lidar_point.lidar_type, lidar_point.points_num);
uint64_t base_time = frame->base_time[i];
uint8_t index = 0;
int8_t ret = cache_index_.GetIndex(lidar_point.lidar_type, lidar_point.handle, index);
if (ret != 0) {
printf("Storage point data failed, lidar type:%u, handle:%u.\n", lidar_point.lidar_type, lidar_point.handle);
continue;
}
PushLidarData(&lidar_point, index, base_time);
}
}
void Lds::PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint64_t base_time) {
if (lidar_data == nullptr) {
return;
}
LidarDevice *p_lidar = &lidars_[index];
LidarDataQueue *queue = &p_lidar->data;
if (nullptr == queue->storage_packet) {
uint32_t queue_size = CalculatePacketQueueSize(publish_freq_);
InitQueue(queue, queue_size);
printf("Lidar[%u] storage queue size: %u\n", index, queue_size);
}
if (!QueueIsFull(queue)) {
QueuePushAny(queue, (uint8_t *)lidar_data, base_time);
if (!QueueIsEmpty(queue)) {
if (pcd_semaphore_.GetCount() <= 0) {
pcd_semaphore_.Signal();
}
}
} else {
if (pcd_semaphore_.GetCount() <= 0) {
pcd_semaphore_.Signal();
}
}
}
void Lds::PrepareExit(void) {}
} // namespace livox_ros

83
src/rm_driver/livox_ros_driver2/src/src/lds.h Normal file
View File

@ -0,0 +1,83 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
// livox lidar data source
#ifndef LIVOX_ROS_DRIVER_LDS_H_
#define LIVOX_ROS_DRIVER_LDS_H_
#include <map>
#include "comm/semaphore.h"
#include "comm/comm.h"
#include "comm/cache_index.h"
namespace livox_ros {
/**
* Lidar data source abstract.
*/
class Lds {
public:
Lds(const double publish_freq, const uint8_t data_src);
virtual ~Lds();
void StorageImuData(ImuData* imu_data);
void StoragePointData(PointFrame* frame);
void StorageLvxPointData(PointFrame* frame);
int8_t GetHandle(const uint8_t lidar_type, const PointPacket* lidar_point);
void PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint64_t base_time);
static void ResetLidar(LidarDevice *lidar, uint8_t data_src);
static void SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src);
void ResetLds(uint8_t data_src);
void RequestExit();
bool IsAllQueueEmpty();
bool IsAllQueueReadStop();
void CleanRequestExit() { request_exit_ = false; }
bool IsRequestExit() { return request_exit_; }
virtual void PrepareExit(void);
// get publishing frequency
double GetLdsFrequency() { return publish_freq_; }
public:
uint8_t lidar_count_; /**< Lidar access handle. */
LidarDevice lidars_[kMaxSourceLidar]; /**< The index is the handle */
Semaphore pcd_semaphore_;
Semaphore imu_semaphore_;
static CacheIndex cache_index_;
protected:
double publish_freq_;
uint8_t data_src_;
private:
volatile bool request_exit_;
};
} // namespace livox_ros
#endif // LIVOX_ROS_DRIVER_LDS_H_

213
src/rm_driver/livox_ros_driver2/src/src/lds_lidar.cpp Normal file
View File

@ -0,0 +1,213 @@
//
// 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 <memory>
#include <mutex>
#include <stdio.h>
#include <string.h>
#include <thread>
#ifdef WIN32
#include <winsock2.h>
#include <ws2def.h>
#pragma comment(lib, "Ws2_32.lib")
#else
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
#endif // WIN32
#include "comm/comm.h"
#include "comm/pub_handler.h"
#include "parse_cfg_file/parse_cfg_file.h"
#include "parse_cfg_file/parse_livox_lidar_cfg.h"
#include "call_back/lidar_common_callback.h"
#include "call_back/livox_lidar_callback.h"
using namespace std;
namespace livox_ros {
/** Const varible ------------------------------------------------------------*/
/** For callback use only */
LdsLidar *g_lds_ldiar = nullptr;
/** Global function for common use -------------------------------------------*/
/** Lds lidar function -------------------------------------------------------*/
LdsLidar::LdsLidar(double publish_freq)
: Lds(publish_freq, kSourceRawLidar), auto_connect_mode_(true),
whitelist_count_(0), is_initialized_(false) {
memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
ResetLdsLidar();
}
LdsLidar::~LdsLidar() {}
void LdsLidar::ResetLdsLidar(void) { ResetLds(kSourceRawLidar); }
bool LdsLidar::InitLdsLidar(const std::string &path_name) {
if (is_initialized_) {
printf("Lds is already inited!\n");
return false;
}
if (g_lds_ldiar == nullptr) {
g_lds_ldiar = this;
}
path_ = path_name;
if (!InitLidars()) {
return false;
}
SetLidarPubHandle();
if (!Start()) {
return false;
}
is_initialized_ = true;
return true;
}
bool LdsLidar::InitLidars() {
if (!ParseSummaryConfig()) {
return false;
}
std::cout << "config lidar type: "
<< static_cast<int>(lidar_summary_info_.lidar_type) << std::endl;
if (lidar_summary_info_.lidar_type & kLivoxLidarType) {
if (!InitLivoxLidar()) {
return false;
}
}
return true;
}
bool LdsLidar::Start() {
if (lidar_summary_info_.lidar_type & kLivoxLidarType) {
if (!LivoxLidarStart()) {
return false;
}
}
return true;
}
bool LdsLidar::ParseSummaryConfig() {
return ParseCfgFile(path_).ParseSummaryInfo(lidar_summary_info_);
}
bool LdsLidar::InitLivoxLidar() {
DisableLivoxSdkConsoleLogger();
// parse user config
LivoxLidarConfigParser parser(path_);
std::vector<UserLivoxLidarConfig> user_configs;
if (!parser.Parse(user_configs)) {
std::cout << "failed to parse user-defined config" << std::endl;
}
// SDK initialization
if (!LivoxLidarSdkInit(path_.c_str())) {
std::cout << "Failed to init livox lidar sdk." << std::endl;
return false;
}
// fill in lidar devices
for (auto &config : user_configs) {
uint8_t index = 0;
int8_t ret = g_lds_ldiar->cache_index_.GetFreeIndex(kLivoxLidarType,
config.handle, index);
if (ret != 0) {
std::cout << "failed to get free index, lidar ip: "
<< IpNumToString(config.handle) << std::endl;
continue;
}
LidarDevice *p_lidar = &(g_lds_ldiar->lidars_[index]);
p_lidar->lidar_type = kLivoxLidarType;
p_lidar->livox_config = config;
p_lidar->handle = config.handle;
LidarExtParameter lidar_param;
lidar_param.handle = config.handle;
lidar_param.lidar_type = kLivoxLidarType;
if (config.pcl_data_type == kLivoxLidarCartesianCoordinateLowData) {
// temporary resolution
lidar_param.param.roll = config.extrinsic_param.roll;
lidar_param.param.pitch = config.extrinsic_param.pitch;
lidar_param.param.yaw = config.extrinsic_param.yaw;
lidar_param.param.x = config.extrinsic_param.x / 10;
lidar_param.param.y = config.extrinsic_param.y / 10;
lidar_param.param.z = config.extrinsic_param.z / 10;
} else {
lidar_param.param.roll = config.extrinsic_param.roll;
lidar_param.param.pitch = config.extrinsic_param.pitch;
lidar_param.param.yaw = config.extrinsic_param.yaw;
lidar_param.param.x = config.extrinsic_param.x;
lidar_param.param.y = config.extrinsic_param.y;
lidar_param.param.z = config.extrinsic_param.z;
}
pub_handler().AddLidarsExtParam(lidar_param);
}
SetLivoxLidarInfoChangeCallback(LivoxLidarCallback::LidarInfoChangeCallback,
g_lds_ldiar);
return true;
}
void LdsLidar::SetLidarPubHandle() {
pub_handler().SetPointCloudsCallback(LidarCommonCallback::OnLidarPointClounCb,
g_lds_ldiar);
pub_handler().SetImuDataCallback(LidarCommonCallback::LidarImuDataCallback,
g_lds_ldiar);
double publish_freq = Lds::GetLdsFrequency();
pub_handler().SetPointCloudConfig(publish_freq);
}
bool LdsLidar::LivoxLidarStart() { return true; }
int LdsLidar::DeInitLdsLidar(void) {
if (!is_initialized_) {
printf("LiDAR data source is not exit");
return -1;
}
if (lidar_summary_info_.lidar_type & kLivoxLidarType) {
LivoxLidarSdkUninit();
printf("Livox Lidar SDK Deinit completely!\n");
}
return 0;
}
void LdsLidar::PrepareExit(void) { DeInitLdsLidar(); }
} // namespace livox_ros

95
src/rm_driver/livox_ros_driver2/src/src/lds_lidar.h Normal file
View File

@ -0,0 +1,95 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
/** Livox LiDAR data source, data from dependent lidar */
#ifndef LIVOX_ROS_DRIVER_LDS_LIDAR_H_
#define LIVOX_ROS_DRIVER_LDS_LIDAR_H_
#include <memory>
#include <mutex>
#include <vector>
#include "lds.h"
#include "comm/comm.h"
#include "livox_lidar_api.h"
#include "livox_lidar_def.h"
#include "rapidjson/document.h"
namespace livox_ros {
class LdsLidar final : public Lds {
public:
static LdsLidar *GetInstance(double publish_freq) {
printf("LdsLidar *GetInstance\n");
static LdsLidar lds_lidar(publish_freq);
return &lds_lidar;
}
bool InitLdsLidar(const std::string& path_name);
bool Start();
int DeInitLdsLidar(void);
private:
LdsLidar(double publish_freq);
LdsLidar(const LdsLidar &) = delete;
~LdsLidar();
LdsLidar &operator=(const LdsLidar &) = delete;
bool ParseSummaryConfig();
bool InitLidars();
bool InitLivoxLidar(); // for new SDK
bool LivoxLidarStart();
void ResetLdsLidar(void);
void SetLidarPubHandle();
// auto connect mode
void EnableAutoConnectMode(void) { auto_connect_mode_ = true; }
void DisableAutoConnectMode(void) { auto_connect_mode_ = false; }
bool IsAutoConnectMode(void) { return auto_connect_mode_; }
virtual void PrepareExit(void);
public:
std::mutex config_mutex_;
private:
std::string path_;
LidarSummaryInfo lidar_summary_info_;
bool auto_connect_mode_;
uint32_t whitelist_count_;
volatile bool is_initialized_;
char broadcast_code_whitelist_[kMaxLidarCount][kBroadcastCodeSize];
};
} // namespace livox_ros
#endif // LIVOX_ROS_DRIVER_LDS_LIDAR_H_

137
src/rm_driver/livox_ros_driver2/src/src/livox_ros_driver2.cpp Normal file
View File

@ -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.
//
#include <chrono>
#include <csignal>
#include <iostream>
#include <thread>
#include <vector>
#include "driver_node.h"
#include "include/livox_ros_driver2.h"
#include "include/ros_headers.h"
#include "lddc.h"
#include "lds_lidar.h"
using namespace livox_ros;
namespace livox_ros {
DriverNode::DriverNode(const rclcpp::NodeOptions &node_options)
: Node("livox_driver_node", node_options) {
DRIVER_INFO(*this, "Livox Ros Driver2 Version: %s",
LIVOX_ROS_DRIVER2_VERSION_STRING);
/** Init default system parameter */
int xfer_format = kPointCloud2Msg;
int multi_topic = 0;
int data_src = kSourceRawLidar;
double publish_freq = 10.0; /* Hz */
int output_type = kOutputToRos;
std::string frame_id;
this->declare_parameter("xfer_format", xfer_format);
this->declare_parameter("multi_topic", 0);
this->declare_parameter("data_src", data_src);
this->declare_parameter("publish_freq", 10.0);
this->declare_parameter("output_data_type", output_type);
this->declare_parameter("frame_id", "frame_default");
this->declare_parameter("user_config_path", "path_default");
this->declare_parameter("cmdline_input_bd_code", "000000000000001");
this->declare_parameter("lvx_file_path", "/home/livox/livox_test.lvx");
this->get_parameter("xfer_format", xfer_format);
this->get_parameter("multi_topic", multi_topic);
this->get_parameter("data_src", data_src);
this->get_parameter("publish_freq", publish_freq);
this->get_parameter("output_data_type", output_type);
this->get_parameter("frame_id", frame_id);
if (publish_freq > 100.0) {
publish_freq = 100.0;
} else if (publish_freq < 0.5) {
publish_freq = 0.5;
} else {
publish_freq = publish_freq;
}
future_ = exit_signal_.get_future();
/** Lidar data distribute control and lidar data source set */
lddc_ptr_ = std::make_unique<Lddc>(xfer_format, multi_topic, data_src,
output_type, publish_freq, frame_id);
lddc_ptr_->SetRosNode(this);
if (data_src == kSourceRawLidar) {
DRIVER_INFO(*this, "Data Source is raw lidar.");
std::string user_config_path;
this->get_parameter("user_config_path", user_config_path);
DRIVER_INFO(*this, "Config file : %s", user_config_path.c_str());
std::string cmdline_bd_code;
this->get_parameter("cmdline_input_bd_code", cmdline_bd_code);
LdsLidar *read_lidar = LdsLidar::GetInstance(publish_freq);
lddc_ptr_->RegisterLds(static_cast<Lds *>(read_lidar));
if ((read_lidar->InitLdsLidar(user_config_path))) {
DRIVER_INFO(*this, "Init lds lidar success!");
} else {
DRIVER_ERROR(*this, "Init lds lidar fail!");
}
} else {
DRIVER_ERROR(*this, "Invalid data src (%d), please check the launch file",
data_src);
}
pointclouddata_poll_thread_ = std::make_shared<std::thread>(
&DriverNode::PointCloudDataPollThread, this);
imudata_poll_thread_ =
std::make_shared<std::thread>(&DriverNode::ImuDataPollThread, this);
}
} // namespace livox_ros
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(livox_ros::DriverNode)
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);
}

67
src/rm_driver/livox_ros_driver2/src/src/parse_cfg_file/parse_cfg_file.cpp Normal file
View File

@ -0,0 +1,67 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "parse_cfg_file.h"
#include <iostream>
#include <cstdio>
#include <arpa/inet.h>
namespace livox_ros {
ParseCfgFile::ParseCfgFile(const std::string& path) : path_(path) {}
bool ParseCfgFile::ParseSummaryInfo(LidarSummaryInfo& lidar_summary_info) {
FILE* raw_file = std::fopen(path_.c_str(), "rb");
if (!raw_file) {
std::cout << "parse summary info failed, can not open file: " << path_ << std::endl;
return false;
}
char read_buffer[kMaxBufferSize];
rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer));
rapidjson::Document doc;
do {
if (doc.ParseStream(config_file).HasParseError()) {
break;
}
if (!doc.HasMember("lidar_summary_info") || !doc["lidar_summary_info"].IsObject()) {
break;
}
const rapidjson::Value &object = doc["lidar_summary_info"];
if (!object.HasMember("lidar_type") || !object["lidar_type"].IsUint()) {
break;
}
lidar_summary_info.lidar_type = static_cast<uint8_t>(object["lidar_type"].GetUint());
std::fclose(raw_file);
return true;
} while (false);
std::cout << "parse lidar type failed." << std::endl;
std::fclose(raw_file);
return false;
}
} // namespace livox_ros

52
src/rm_driver/livox_ros_driver2/src/src/parse_cfg_file/parse_cfg_file.h Normal file
View File

@ -0,0 +1,52 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_
#define LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_
#include "../comm/comm.h"
#include "rapidjson/document.h"
#include "rapidjson/filereadstream.h"
#include "rapidjson/stringbuffer.h"
#include <string>
#include <vector>
namespace livox_ros {
class ParseCfgFile {
public:
explicit ParseCfgFile(const std::string& path);
~ParseCfgFile() {}
bool ParseSummaryInfo(LidarSummaryInfo& lidar_summary_info);
private:
const std::string path_;
};
} // namespace livox_ros
#endif // LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_

156
src/rm_driver/livox_ros_driver2/src/src/parse_cfg_file/parse_livox_lidar_cfg.cpp Normal file
View File

@ -0,0 +1,156 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "parse_livox_lidar_cfg.h"
#include <iostream>
namespace livox_ros {
bool LivoxLidarConfigParser::Parse(std::vector<UserLivoxLidarConfig> &lidar_configs) {
FILE* raw_file = std::fopen(path_.c_str(), "rb");
if (!raw_file) {
std::cout << "failed to open config file: " << path_ << std::endl;
return false;
}
lidar_configs.clear();
char read_buffer[kMaxBufferSize];
rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer));
rapidjson::Document doc;
do {
if (doc.ParseStream(config_file).HasParseError()) {
std::cout << "failed to parse config jason" << std::endl;
break;
}
if (!doc.HasMember("lidar_configs") ||
!doc["lidar_configs"].IsArray() ||
0 == doc["lidar_configs"].Size()) {
std::cout << "there is no user-defined config" << std::endl;
break;
}
if (!ParseUserConfigs(doc, lidar_configs)) {
std::cout << "failed to parse basic configs" << std::endl;
break;
}
return true;
} while (false);
std::fclose(raw_file);
return false;
}
bool LivoxLidarConfigParser::ParseUserConfigs(const rapidjson::Document &doc,
std::vector<UserLivoxLidarConfig> &user_configs) {
const rapidjson::Value &lidar_configs = doc["lidar_configs"];
for (auto &config : lidar_configs.GetArray()) {
if (!config.HasMember("ip")) {
continue;
}
UserLivoxLidarConfig user_config;
// parse user configs
user_config.handle = IpStringToNum(std::string(config["ip"].GetString()));
if (!config.HasMember("pcl_data_type")) {
user_config.pcl_data_type = -1;
} else {
user_config.pcl_data_type = static_cast<int8_t>(config["pcl_data_type"].GetInt());
}
if (!config.HasMember("pattern_mode")) {
user_config.pattern_mode = -1;
} else {
user_config.pattern_mode = static_cast<int8_t>(config["pattern_mode"].GetInt());
}
if (!config.HasMember("blind_spot_set")) {
user_config.blind_spot_set = -1;
} else {
user_config.blind_spot_set = static_cast<int8_t>(config["blind_spot_set"].GetInt());
}
if (!config.HasMember("dual_emit_en")) {
user_config.dual_emit_en = -1;
} else {
user_config.dual_emit_en = static_cast<uint8_t>(config["dual_emit_en"].GetInt());
}
if (!config.HasMember("extrinsic_parameter")) {
memset(&user_config.extrinsic_param, 0, sizeof(user_config.extrinsic_param));
} else {
auto &value = config["extrinsic_parameter"];
if (!ParseExtrinsics(value, user_config.extrinsic_param)) {
memset(&user_config.extrinsic_param, 0, sizeof(user_config.extrinsic_param));
std::cout << "failed to parse extrinsic parameters, ip: "
<< IpNumToString(user_config.handle) << std::endl;
}
}
user_config.set_bits = 0;
user_config.get_bits = 0;
user_configs.push_back(user_config);
}
if (0 == user_configs.size()) {
std::cout << "no valid base configs" << std::endl;
return false;
}
std::cout << "successfully parse base config, counts: "
<< user_configs.size() << std::endl;
return true;
}
bool LivoxLidarConfigParser::ParseExtrinsics(const rapidjson::Value &value,
ExtParameter &param) {
if (!value.HasMember("roll")) {
param.roll = 0.0f;
} else {
param.roll = static_cast<float>(value["roll"].GetFloat());
}
if (!value.HasMember("pitch")) {
param.pitch = 0.0f;
} else {
param.pitch = static_cast<float>(value["pitch"].GetFloat());
}
if (!value.HasMember("yaw")) {
param.yaw = 0.0f;
} else {
param.yaw = static_cast<float>(value["yaw"].GetFloat());
}
if (!value.HasMember("x")) {
param.x = 0;
} else {
param.x = static_cast<int32_t>(value["x"].GetInt());
}
if (!value.HasMember("y")) {
param.y = 0;
} else {
param.y = static_cast<int32_t>(value["y"].GetInt());
}
if (!value.HasMember("z")) {
param.z = 0;
} else {
param.z = static_cast<int32_t>(value["z"].GetInt());
}
return true;
}
} // namespace livox_ros

58
src/rm_driver/livox_ros_driver2/src/src/parse_cfg_file/parse_livox_lidar_cfg.h Normal file
View File

@ -0,0 +1,58 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_
#define LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_
#include "comm/comm.h"
#include "livox_lidar_def.h"
#include "rapidjson/document.h"
#include "rapidjson/filereadstream.h"
#include "rapidjson/stringbuffer.h"
#include <iostream>
#include <string>
#include <vector>
namespace livox_ros {
class LivoxLidarConfigParser {
public:
explicit LivoxLidarConfigParser(const std::string& path) : path_(path) {}
~LivoxLidarConfigParser() {}
bool Parse(std::vector<UserLivoxLidarConfig> &lidar_configs);
private:
bool ParseUserConfigs(const rapidjson::Document &doc,
std::vector<UserLivoxLidarConfig> &user_configs);
bool ParseExtrinsics(const rapidjson::Value &value, ExtParameter &param);
const std::string path_;
};
} // namespace livox_ros
#endif // LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_

47
src/rm_driver/rm_serial_driver/CMakeLists.txt Normal file
View File

@ -0,0 +1,47 @@
cmake_minimum_required(VERSION 3.5)
project(rm_serial_driver)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(std_msgs REQUIRED)
find_package(serial REQUIRED)
find_package(rm_msgs REQUIRED)
# 构建共享库作为组件节点
add_library(rm_serial_driver SHARED
src/rm_serial_driver.cpp
src/crc.cpp
)
target_include_directories(rm_serial_driver PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
ament_target_dependencies(rm_serial_driver
rclcpp
rclcpp_components
std_msgs
serial
rm_msgs
)
# 注册节点
rclcpp_components_register_nodes(rm_serial_driver "rm_serial_driver::RMSerialDriver")
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/
)
install(DIRECTORY config
DESTINATION share/${PROJECT_NAME}/
)
install(TARGETS rm_serial_driver
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
ament_package()

21
src/rm_driver/rm_serial_driver/LICENSE Normal file
View File

@ -0,0 +1,21 @@
MIT License
Copyright (c) 2025 zucheng Lv
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.

70
src/rm_driver/rm_serial_driver/README.md Normal file
View File

@ -0,0 +1,70 @@
# rm_serial_driver
ROS2: 适用于RM的简易上位机串口通信程序。
## 使用方式
### 1. 安装依赖
首先，安装所需的依赖包：
```bash
sudo apt install ros-humble-serial-driver
```
### 2. 编译并安装串口库
克隆串口库源码并编译安装：
```bash
git clone https://github.com/ZhaoXiangBox/serial.git
cd serial
mkdir build
cd build
cmake ..
make
sudo make install
```
**注意**：安装完成后可能需要重启系统。
### 3. 下载并编译项目
将项目克隆到你的ROS2工作空间中：
```bash
cd ~/ros2_ws/src
git clone https://github.com/goldenfishs/rm_serial_driver.git
git clone https://github.com/goldenfishs/rm_msg.git # 包含所需的自定义话题消息，必须一起使用
```
回到工作空间并编译：
```bash
cd ~/ros2_ws
colcon build
source install/setup.bash
```
### 4. 运行程序
使用以下命令启动串口驱动：
```bash
ros2 launch rm_serial_driver rm_serial_driver.launch.py
```
使用时请将上位机控制命令发送到data_ai话题，即可将数据传给下位机，下位机发送上来的数据会发布到data_mcu和data_ref话题，其他程序可直接哪去
### 5. 自定义通信协议
如果需要自定义通信协议，可以参考 `protocol.h` 文件，修改对应的协议。同时，修改 `rm_msgs` 包中的自定义消息，注意数据顺序和数据类型。修改完成后，重新编译即可。
```bash
colcon build
source install/setup.bash
```
## 注意事项
- 确保在修改协议和消息后重新编译项目。
- 如果遇到问题，请检查串口连接和权限设置。

4
src/rm_driver/rm_serial_driver/config/serial_config.yaml Normal file
View File

@ -0,0 +1,4 @@
/rm_serial_driver:
ros__parameters:
device_name: /dev/ttyACM1
baud_rate: 115200

14
src/rm_driver/rm_serial_driver/include/rm_serial_driver/crc.hpp Normal file
View File

@ -0,0 +1,14 @@
#pragma once
#include <cstddef>
#include <cstdint>
#define CRC16_INIT 0xFFFF
namespace crc16 {
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc);
bool CRC16_Verify(const uint8_t *buf, size_t len);
} // namespace crc16

68
src/rm_driver/rm_serial_driver/include/rm_serial_driver/protocol.hpp Normal file
View File

@ -0,0 +1,68 @@
#pragma once
#define ID_MCU (0xC4)
#define ID_REF (0xA8)
#include <cstdint>
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float q0;
float q1;
float q2;
float q3;
} quat; /* 四元数 */
struct __attribute__((packed))
{
float yaw;
float pit;
float rol;
} gimbal; /* 欧拉角 */
uint8_t notice; /* 控制命令 */
} DataMCU_t;
typedef struct __attribute__((packed))
{
uint8_t id;
DataMCU_t data;
uint16_t crc16;
} PackageMCU_t;
typedef struct __attribute__((packed))
{
uint16_t remain_hp; /* 剩余血量 */
uint8_t game_progress : 4; /* 比赛进度 */
uint16_t stage_remain_time; /* 比赛剩余时间 */
} DataReferee_t;
typedef struct __attribute__((packed))
{
uint8_t id;
DataReferee_t data;
uint16_t crc16;
} PackageReferee_t;
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float yaw;
float pit;
float rol;
} delta_eulr;
struct __attribute__((packed))
{
float vx;
float vy;
float wz;
} delta_pos;
uint8_t notice; /* 控制命令 */
} DataAI_t;
typedef struct __attribute__((packed))
{
DataAI_t data;
uint16_t crc16;
} PackageAI_t;

42
src/rm_driver/rm_serial_driver/include/rm_serial_driver/rm_serial_driver.hpp Normal file
View File

@ -0,0 +1,42 @@
#pragma once
#include <serial/serial.h>
#include <atomic>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <string>
#include <thread>
#include "rm_msgs/msg/data_mcu.hpp"
#include "rm_msgs/msg/data_ref.hpp"
#include "rm_msgs/msg/data_ai.hpp"
#include "rm_serial_driver/crc.hpp"
#include "rm_serial_driver/protocol.hpp"
namespace rm_serial_driver
{
class RMSerialDriver : public rclcpp::Node
{
public:
explicit RMSerialDriver(const rclcpp::NodeOptions &options);
~RMSerialDriver() override;
private:
size_t get_expected_length(uint8_t id);
void process_packet(uint8_t id, const char *data, size_t packet_size);
void receive_data();
void reopen_port();
void data_ai_callback(const rm_msgs::msg::DataAI::SharedPtr msg);
std::unique_ptr<serial::Serial> serial_port_;
rclcpp::Publisher<rm_msgs::msg::DataMCU>::SharedPtr data_mcu_pub_;
rclcpp::Publisher<rm_msgs::msg::DataRef>::SharedPtr data_ref_pub_;
rclcpp::Subscription<rm_msgs::msg::DataAI>::SharedPtr data_ai_sub_;
std::thread read_thread_; // 串口数据读取线程
std::atomic<bool> running_; // 原子操作: 保证多线程操作时的数据一致性
std::string serial_buffer_; // 串口数据缓冲区
};
} // namespace rm_serial_driver

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