ublox_driver 使用解读
发布时间：2024年01月10日
ublox_driver
Authors/Maintainers: CAO Shaozu (shaozu.cao AT gmail.com)
The ublox_driver provides essential functionalities for u-blox GNSS receivers. This package is originally designed for u-blox ZED-F9P module according to the specification UBX-18010854, but should also be compatible to other 8-series or 9-series u-blox receivers as long as the interface is the same.
这段话描述的是
ublox_driver软件包，它是为u-blox GNSS（全球导航卫星系统）接收器提供基本功能的ROS（机器人操作系统）驱动程序。关键信息如下：
- 专为u-blox ZED-F9P设计：
- 这个驱动程序最初是为u-blox ZED-F9P模块设计的，根据u-blox的规格文件UBX-18010854来实现。
- 兼容性：
- 尽管
ublox_driver是针对ZED-F9P模块开发的，但它也应该与其他u-blox 8系列或9系列的接收器兼容，前提是这些接收器的接口与ZED-F9P模块相同。
The following diagram shows all possible input and output options supported by ublox_driver.
这张图说明了
ublox_driver在ROS（机器人操作系统）环境中的数据流和功能。它展示了驱动程序如何处理输入和输出数据：
- 输入：
- Serial Port：这表明
ublox_driver可以从串行端口接收数据。这通常是GNSS模块（如u-blox模块）通过UART（通用异步接收/发送）连接发送的数据。- File Record：驱动程序也可以从一个文件中读取数据。这对于播放之前记录的数据或使用静态数据进行测试和开发很有用。
- 处理：
- ublox_driver：这是核心的驱动程序，它处理输入数据并生成输出。在ROS环境中，它可能将原始数据转换为ROS消息格式，或者执行其他必要的处理，如数据解析、格式转换或发布到ROS主题。
- 输出：
- RTCM Stream：这是指向上的单行箭头，表明
ublox_driver能够输出RTCM数据流。这通常是为了发送差分校正信息到GNSS模块，用于RTK定位。- ROS Message：
ublox_driver能够发布ROS消息，这意味着它可以将GNSS数据发布到ROS主题，供其他ROS节点订阅和使用。- File Record：驱动程序可以将数据记录到文件中。这对于记录原始数据以备后用，如调试、分析或作为记录的一部分很有用。
- Serial Port：同样，
ublox_driver可以将数据发送回串行端口。这可能用于与GNSS模块进行双向通信，或将处理过的数据发送给其他设备。总的来说，这张图展示了
ublox_driver如何作为数据的中心枢纽，在ROS系统中与GNSS硬件设备以及其他软件组件进行交互。
1. Prerequisites
1.1 C++11 Compiler
This package requires some features of C++11.
1.2 ROS
This package is developed under ROS Kinetic environment.
1.3 Eigen
We use Eigen 3.3.3 for matrix manipulation.
sudo apt-get install libeigen3-dev
这句话说明了在某个项目或软件中，用于矩阵操作的库是Eigen，具体版本为3.3.3。
- Eigen库：
- Eigen是一个高水平的C++库，用于线性代数、矩阵和向量运算，数值解算以及相关的数学运算。它是开源的，并且被广泛应用于需要高性能数学运算的软件中。
- 矩阵操作：
- 矩阵操作包括但不限于矩阵的加法、减法、乘法、转置、逆、行列式计算和特征值计算等。Eigen库提供了快速且高效的方法来执行这些操作。
- 版本3.3.3：
- 软件库通常会有多个版本，每个版本都可能增加新功能或修复先前版本的bug。在这种情况下，使用的是Eigen库的3.3.3版本。
- 应用场景：
- Eigen库在许多需要数学运算的应用中都有广泛应用，例如计算机视觉、机器学习、机器人学以及物理模拟等领域。
总结来说，这句话表明开发者选择了Eigen这个库作为他们的数学和矩阵操作工具，这是因为其功能强大且性能出色。选择3.3.3版本可能是出于稳定性、兼容性或其他特定需求的考虑。
1.4 Boost
Our software utilizes Boost library for serial and socket manipulation. Using command sudo apt-get install libboost-all-dev to install Boost.
sudo apt-get install libboost-all-dev
这段话描述的是软件开发中的一个常见实践，即使用Boost库来进行串行端口和网络套接字的操作。Boost库是一个广泛使用的C++库集合，它提供了许多用于包括系统输入输出、线程管理和网络通信在内的任务的组件。
- Boost库的用途：
- 在这个上下文中，Boost库被用于“串行和套接字操作”。具体来说，这可能涉及到与串行端口通信（例如，与连接到计算机的外部设备交换数据）以及执行网络套接字操作（例如，处理TCP/IP连接和数据传输）。
- 安装Boost库：
- 为了在基于Debian或Ubuntu的系统上安装Boost库，提供了一个Linux命令：
sudo apt-get install libboost-all-dev。- 这个命令使用
apt-get，这是Debian及其衍生系统（如Ubuntu）的包管理工具，用于安装和管理软件包。sudo是一个允许普通用户执行需要管理员权限（例如，安装软件到系统区域）的命令的程序。libboost-all-dev是一个元包，包含了Boost库所有开发组件的集合。安装这个包会安装Boost库的所有可用开发文件，包括头文件和库文件，这对于编译和链接使用Boost的程序是必需的。
- Boost库的选择：
- Boost库因其高质量、广泛测试和文档完善而受到C++开发者的青睐。它被设计为易于使用，且与标准C++库高度兼容。
通过使用这个命令安装Boost库，软件开发者可以确保他们的应用程序有权访问Boost提供的各种功能，这是开发现代C++应用程序的重要基础。
1.5 gnss_comm
This package also requires gnss_comm for ROS message definitions and some utility functions. Follow those instructions to build the gnss_comm package.
cd /path/to/your/catkin_workspace/src
git clone https://github.com/your_username/gnss_comm.git
cd /path/to/your/catkin_workspace
catkin_make
source devel/setup.bash
这段话提到了在某个软件包的构建或运行过程中对另一个软件包的依赖关系。这里的上下文是ROS（机器人操作系统），一个广泛用于机器人编程的中间件框架。具体内容解释如下：
- 依赖项：
gnss_comm是被提及的依赖包，它可能包含了一系列为GNSS（全球导航卫星系统）相关数据定义的ROS消息类型和一些实用工具函数。
- ROS消息定义：
- 在ROS中，消息定义（通常保存在
.msg文件中）是一种数据结构描述，它定义了可以在不同ROS节点之间传递的数据类型。gnss_comm包中包含的消息定义可能特别适用于GNSS应用，例如，包含经纬度、海拔、速度等信息的消息。
- 实用工具函数：
gnss_comm包还提供了一些实用工具函数，这些函数可能用于常见的GNSS数据处理任务，比如坐标转换、数据解析或校验计算。
- 构建说明：
- “按照那些指示构建
gnss_comm包”意味着要获取gnss_comm的源代码，并且按照提供的指示来编译和安装这个包。这可能包括克隆一个Git仓库、运行catkin_make或colcon等ROS构建工具，以及设置适当的环境变量。
- 包的构建：
- 构建过程是将源代码转换为可执行程序和/或库的过程，这通常还涉及到将消息定义转换为可在程序中使用的源代码。
在ROS生态系统中，构建并安装包是在使用特定功能之前的常见步骤。
gnss_comm包为ROS提供了关于GNSS通信所需的基本组件，是在这一特定环境中进行GNSS数据处理和通信的基础。
2. Build ublox_driver
Clone the repository to your catkin workspace (for example ~/catkin_ws/):
cd ~/catkin_ws/src/
git clone https://github.com/HKUST-Aerial-Robotics/ublox_driver.git
Then build the package with:
cd ~/catkin_ws/
catkin_make
source ~/catkin_ws/devel/setup.bash
3. Run with your u-blox receiver(主要)
Our software can take the serial stream from the u-blox receiver as an input source. Before running the package, you need to configure your receiver using u-center to output at least UBX-RXM-RAWX, UBX-RXM-SFRBX and UBX-NAV-PVT messages to a specific serial port (a sample config used in our system can be found at config/ucenter_config_f9p_gvins.txt). Then connecting your computer(Linux) with the receiver, make sure the serial port appears as a file in the /dev/ directory. Then add your account to dialout group to obtain permission on serial r/w operation via (no need to substitute $USER):
这段话详细说明了如何准备和配置u-blox GNSS接收器以便与特定软件一起使用，这通常是在基于Linux的系统上运行机器人操作系统（ROS）的上下文中：
- 输入源：
- 提到的软件能够接受来自u-blox接收器的串行数据流作为输入。
- 使用u-center配置接收器：
- u-center是u-blox提供的配置软件，用于设置接收器的输出消息类型和参数。
- 需要配置接收器以至少输出三种类型的消息：UBX-RXM-RAWX（原始测量数据），UBX-RXM-SFRBX（卫星导航信息），以及UBX-NAV-PVT（导航定位信息）。
- 这些消息被发送到指定的串行端口，这样软件才能够读取它们。
- 接收器与计算机的连接：
- 连接接收器和计算机，通常通过USB转串行适配器完成。
- 确保在Linux的
/dev/目录下出现了表示串行端口的文件，例如/dev/ttyUSB0。
- Linux用户权限：
- 为了从串行端口读写数据，Linux用户需要有相应的权限。
- 通过将用户添加到
dialout组，可以授予对串行端口的读写权限。这可以通过执行命令sudo usermod -a -G dialout $USER来完成，其中$USER是当前用户的用户名。
- 示例配置文件：
- 代码库中的
config/ucenter_config_f9p_gvins.txt文件包含了在他们的系统中使用的样例配置，这可能是一个文本文件，列出了通过u-center设置接收器时使用的配置参数。这段话总体上是在描述如何通过u-center设置u-blox GNSS接收器，以确保它输出正确的数据类型，随后确保Linux系统能够正确识别接收器，并且当前用户有足够的权限来读写串行端口。这是在运行需要这些数据的软件包之前的必要准备工作。
sudo usermod -aG dialout $USER
Open config/driver_config.yaml, set online and to_ros to 1, and adjust input_serial_port and serial_baud_rate according to your setting. Run the package with:
这段话提供了如何配置和运行依赖于串行通信的ROS软件包的指示：
- 编辑配置文件：
- 指引用户打开配置文件
driver_config.yaml，这是一个YAML（YAML Ain’t Markup Language）格式的文件，通常用于配置软件参数。- YAML是一种数据序列化格式，易于人类阅读和编辑，被广泛用于配置文件中。
- 设置在线和ROS输出：
- 用户被指导将
online和to_ros参数设置为1。这通常表示驱动程序应该在线运行（可能意味着实时接收数据）并且将数据发布到ROS话题。
- 调整串行端口设置：
- 用户需要根据自己的设置调整
input_serial_port（输入串行端口）和serial_baud_rate（串行波特率）的值。input_serial_port参数需要设置为接收器连接到计算机的串行端口，如/dev/ttyUSB0。serial_baud_rate是串行通信的速率，需要与接收器的配置匹配。
- 运行软件包：
- 之后的内容（未提供）可能包含了如何运行软件包的具体指令，通常这会是一个ROS命令，如
roslaunch或rosrun，这两个命令分别用于启动整个ROS节点集和单个节点。这个过程确保了软件包能够正确连接到GNSS接收器，并以适当的格式和速率接收数据，然后在ROS环境中进行处理和分发。
roslaunch ublox_driver ublox_driver.launch
Open another terminal, echo the ROS message by:
rostopic echo /ublox_driver/receiver_lla
If everything goes smoothly, there should be some ROS messages coming out. Note that some ROS topics remain inactive until the receiver gets GNSS signals.
Besides the ROS message, you can record the serial stream to a file(to_file option) or forward the stream to another serial port(to_serial option). All output options can be turned on simultaneously.
这段话提供了有关ROS（机器人操作系统）环境中GNSS（全球导航卫星系统）接收器数据流的操作和期望的行为：
- ROS消息输出：
- 如果系统设置正确且运行顺利，用户应该期望在ROS中看到消息发布。这意味着
ublox_driver会开始发布包含GNSS数据的ROS话题消息。
- ROS主题活跃性：
- 一些ROS主题可能在接收器获取GNSS信号之前保持不活跃状态。也就是说，直到GNSS接收器开始接收有效的卫星信号后，某些ROS话题才会开始发布消息。
- 记录串行数据流：
- 用户可以选择将串行数据流记录到文件中。这在配置文件中通常由一个名为
to_file的选项控制。这对于调试或记录原始数据以供后期分析非常有用。
- 串行数据流转发：
- 用户还可以选择将串行数据流转发到另一个串行端口。这通常由配置文件中名为
to_serial的选项控制。这可能用于直接将数据传输到另一个需要这些数据的硬件设备。
- 同时启用多个输出选项：
- 所有的输出选项，包括ROS消息发布、文件记录和串行端口转发，都可以同时被激活。这为用户提供了灵活性，允许他们根据需要配置数据流的处理方式。
这段话总结了ROS环境中与GNSS接收器相关的一系列输出配置选项，以及在GNSS信号接收和处理过程中的期望行为。
Obtain RTK Solution (Optional)
If your receiver owns an internal RTK engine (for example, ZED-F9P), you can input RTCM messages from the GNSS base station to the receiver in order to obtain the cm-level accurate RTK localization result. Our software can forward the RTCM stream from a local socket to the receiver’s serial port. Nowadays many GNSS stations distribute their RTCM streams via NTRIP protocol and you can easily fetch the NTRIP data and map it to a local socket via RTKLIB. Following those commands to build RTKLIB and setup the RTCM stream:
这段话介绍了如何使用具有内置RTK（实时动态运动定位技术）引擎的GNSS（全球导航卫星系统）接收器（例如u-blox的ZED-F9P模块）进行高精度定位，并且如何通过软件将RTCM（无线电技术委员会海事服务）校正数据流转发到接收器。关键步骤包括：
- 内置RTK引擎的接收器：
- 某些GNSS接收器，如ZED-F9P，内置了RTK引擎，可以处理RTCM校正信号以实现厘米级的定位精度。
- 输入RTCM消息：
- 从GNSS基站接收RTCM消息，并输入到接收器中，接收器会使用这些校正数据计算出高精度的RTK定位结果。
- 软件转发RTCM流：
- 所提到的软件可以将RTCM数据流从本地网络套接字（socket）转发到接收器的串行端口。在网络编程中，socket是两个程序（通常运行在不同计算机上）之间进行数据交换的端点。
- 使用NTRIP协议获取RTCM流：
- 许多GNSS基站通过NTRIP（网络传输实时运动定位信息系统）协议分发它们的RTCM数据流。NTRIP是一种用于实时数据流传输的通用协议。
- 通过使用RTKLIB（一个开源的GNSS软件库），可以轻松地获取NTRIP数据并将其映射到本地socket。
- 构建RTKLIB和设置RTCM流：
- 提供了一些命令用于编译和安装RTKLIB，以及设置RTCM数据流的说明。这些命令具体是什么没有在这段话中提及，但通常包括下载RTKLIB的源码，使用编译器进行编译，并配置必要的参数以连接到NTRIP服务。
总结来说，这段话讲述了使用带有内置RTK引擎的GNSS接收器来实现高精度定位的过程，以及如何使用特定的软件和库（如RTKLIB）来管理和转发为实现这一过程所需的RTCM数据流。
git clone https://github.com/tomojitakasu/RTKLIB.git
cd RTKLIB/
git checkout rtklib_2.4.3
cd app/consapp/str2str/gcc/
make
./str2str -in ntrip://${NTRIP_SITE}:${NTRIP_PORT}/${MOUNT_POINT} -out tcpsvr://:3503
Then set the input_rtcm option to 1 in config/driver_config.yaml and launch the ros node with:
这段话是关于如何配置和运行ROS（机器人操作系统）节点来处理RTCM（无线电技术委员会海事服务）数据的说明：
- 配置
input_rtcm选项：
- 在配置文件
driver_config.yaml中，你需要找到input_rtcm这一选项，并将其设置为1。这通常意味着启用或激活了RTCM数据的输入。- 在ROS包的上下文中，配置文件通常用YAML（YAML Ain’t Markup Language）格式编写，这种格式易于阅读和编辑。
- 启动ROS节点：
- 配置完成后，你将使用特定的命令启动ROS节点。这个命令可能是
roslaunch或rosrun，后面跟着包名和节点名。具体的命令没有在这段话中给出，但它将启动配置文件中指定的功能。
- 使用配置文件：
driver_config.yaml文件包含了ROS节点的配置参数。当ROS节点启动时，它将读取这个文件，并根据文件中的设置执行操作。通过设置
input_rtcm为1，你告诉ublox_driver期望接收RTCM数据流。这对于进行实时动态运动定位（RTK）来说是必需的，因为RTCM流包含了进行高精度定位所需的差分校正信号。
roslaunch ublox_driver ublox_driver.launch
If the field carr_soln in /ublox_driver/receiver_pvt message becomes 2, the RTK is in fix status. If you find the location of the GNSS base station reported in the RTCM message is somehow biased, you can apply correction via the variable rtk_correction_ecef in the config file.
这段话讲述了如何判断RTK（实时动态运动定位技术）系统是否已经达到固定（fix）状态，并且如何对基站的位置偏差进行校正：
- RTK状态的判断：
- 在ROS（机器人操作系统）环境中，
/ublox_driver/receiver_pvt消息是由ublox_driver发布的一个话题，该话题携带GNSS（全球导航卫星系统）接收器的定位信息。carr_soln字段是这个ROS消息的一部分，用于指示RTK定位的状态。当carr_soln的值变为2时，表示RTK系统已经达到了固定解状态。在RTK定位中，“固定解”意味着系统已经锁定了最精确的位置计算，通常精度在厘米级别。
- 基站位置偏差的校正：
- 如果发现通过RTCM消息报告的GNSS基站位置存在偏差，可以在配置文件中使用变量
rtk_correction_ecef进行校正。- ECEF（Earth-Centered, Earth-Fixed）坐标系是一种地心坐标系，其中的坐标点是相对于地球的中心且固定不动的，不随地球旋转而变化。
- 通过调整这个变量，可以对基站报告的位置进行偏差校正，以确保RTK系统提供更加准确的定位结果。
- 配置文件的调整：
- 通常在ROS的工作环境中，配置文件是用YAML（YAML Ain’t Markup Language）格式编写的，允许用户配置一系列参数，包括这里提到的
rtk_correction_ecef。- 用户需要根据实际观测到的偏差，适当地调整这个变量的值以修正位置信息。
总之，这段话涉及到监测和校正RTK系统的状态，以及如何对系统进行细微的调整，确保定位数据的准确性。
4. Playback Log Files
Our package can also take an log file as the input. The log file can be recorded via u-center, RTKLIB or our package itself. To playback the log file, you need to set online to 0 and point ubx_filepath to your log file in the config file. Then launch the ros node with:
这段话解释了ROS（机器人操作系统）软件包如何使用日志文件作为输入，并提供了回放日志文件的步骤：
- 日志文件作为输入：
- 提到的软件包可以使用一个日志文件作为输入源。这个日志文件包含了先前记录的GNSS（全球导航卫星系统）数据。
- 日志文件的记录方法：
- 这些日志文件可以通过多种方式记录，如使用u-center（u-blox提供的软件），RTKLIB（一个开源的GNSS软件库），或者软件包本身。
- 回放日志文件的设置：
- 为了回放日志文件，需要在配置文件中进行几项设置：
- 将
online选项设置为0，这通常表示软件将不会从实时的GNSS设备获取数据，而是从文件中读取。- 将
ubx_filepath设置为日志文件的路径。这告诉软件包从指定位置读取记录的GNSS数据。
- 启动ROS节点：
- 完成设置后，可以使用特定的命令来启动ROS节点。这个命令可能是
roslaunch或rosrun，后面跟着包名和节点名。具体命令没有在这段话中给出，但它会根据配置文件中的设置启动节点并开始处理日志文件中的数据。这种配置允许用户在不连接到实际GNSS设备的情况下分析和测试以前记录的数据。这对于调试、数据分析或模拟特定场景的定位行为非常有用。
roslaunch ublox_driver ublox_driver.launch
Similar to the online receiver manner, all three output options are also supported in the playback mode. Note that the playback speed is controlled by the serial_baud_rate variable in the config file.
这段话解释了在回放模式下，与在线接收器模式类似，软件包支持的三种输出选项，以及如何控制回放速度：
- 三种输出选项：
- 提到的软件包在回放模式下支持三种输出选项，这可能包括：
- 将数据发布到ROS话题。
- 将数据记录到文件。
- 将数据转发到另一个串行端口。
这些输出选项允许用户灵活地处理从日志文件中回放的数据，就像它们是从实时GNSS接收器中直接获得的一样。
- 回放速度控制：
回放速度由配置文件中的
serial_baud_rate变量控制。虽然这个变量通常用于定义串行通信的波特率，但在回放模式下，它可能被用来模拟数据流的速率。调整
serial_baud_rate可以加快或减慢从日志文件中读取数据的速度，这对于测试软件包在不同数据速率下的行为非常有用。
- 配置文件的作用：
- 配置文件中的设置决定了软件包的运行方式，包括如何处理输入数据和输出数据。在这种情况下，通过调整配置文件中的参数，用户可以控制数据的回放速度和处理方式。
总之，这段话指出，无论是在线接收器模式还是回放模式，软件包都提供了相同的输出选项，并且用户可以通过配置文件调整回放速度，以适应不同的测试和分析需求。
5. Synchronize System Time
In addition to message parsing and delivery, ublox_driver can also synchronize the local system time to the global time without the need of internet connection. Note that such synchronization is only in a coarse level and the accuracy is not guaranteed. To perform such synchronization, you need to set UTC_OFFSET macro in src/sync_system_time.cpp according to your timezone. Recompile and launch the driver with:
这段话解释了
ublox_driver软件包除了解析和传递GNSS（全球导航卫星系统）消息之外的另一个功能：同步本地系统时间到全球时间，而且这一过程不需要互联网连接。具体内容如下：
- 时间同步功能：
ublox_driver能够将本地系统时间与从GNSS接收器获得的全球时间进行同步。- 这种同步对于确保系统时间的准确性很有用，尤其是在没有互联网连接的环境中。
- 同步精度说明：
- 需要注意的是，这种时间同步只是在一个较粗糙的水平上进行，不能保证极高的精度。这意味着它足以确保大致的时间一致性，但可能不适合需要高精度时间标记的应用。
- 时区设置：
- 为了进行时间同步，需要在
src/sync_system_time.cpp文件中设置UTC_OFFSET宏，以匹配你的时区。UTC_OFFSET宏定义了本地时间和协调世界时（UTC）之间的差值。
- 重新编译和启动驱动程序：
- 在修改了代码后，需要重新编译
ublox_driver，这通常涉及到在ROS工作空间中运行编译命令（如catkin_make或colcon build）。- 修改和重新编译完成后，可以通过特定的命令启动驱动程序，但具体命令在这段话中没有给出。
综上所述，这段话说明了
ublox_driver如何提供本地系统时间与全球时间同步的功能，并且指出了这一功能的一些限制和配置要求。这种时间同步功能在没有网络访问的环境中尤其有用，可以用于确保系统时间的一般一致性。
roslaunch ublox_driver ublox_driver.launch
Then open another terminal and run commands:
sudo su
source /opt/ros/kinetic/setup.bash
source ${YOUR_CATKIN_WORKSPACE}/devel/setup.bash
rosrun ublox_driver sync_system_time
The system time will be synchronized to the global time when the receiver gets a valid PVT solution.
这句话说明了系统时间同步到全球时间的具体条件：当接收器获得有效的PVT（Position, Velocity, Time - 位置、速度、时间）解决方案时。
- 系统时间同步：
- "系统时间"通常指的是运行软件的计算机或设备的内部时钟。
- "同步到全球时间"意味着将这个系统时间调整为与全球标准时间（如协调世界时，UTC）一致。
- 接收器获得有效的PVT解：
- GNSS接收器的主要功能之一是提供PVT信息，即给出当前的位置（经纬度和高度）、速度和时间信息。
- 有效的PVT解指的是接收器从卫星信号中计算出准确且可靠的位置、速度和时间数据。
- 同步时机：
- 当接收器从卫星信号中成功地计算出这些信息并确定它们是准确可靠的，系统会利用这些信息来校准或同步内部时钟。
- 这种同步通常在接收器第一次获取有效PVT数据时自动发生，也可能在接收器每次启动或在特定时间间隔内进行。
这句话的关键点在于，利用GNSS接收器提供的精确时间信息来校准系统时间，从而无需依赖外部网络时间源，提高了系统的独立性和准确性。这在许多应用中非常重要，尤其是在那些对时间同步有严格要求的场景，如某些科学实验、时间敏感的数据采集或位置追踪系统等。
6. Todo List
- Config u-blox receiver when the driver gets launched
- Infer timezone from geodetic location when sync time
- Optimize time-sync function to improve precision
这段话描述了一些与配置u-blox GNSS接收器和优化时间同步功能相关的操作：
- 启动驱动时配置u-blox接收器：
- 当提到的驱动程序（可能是
ublox_driver）启动时，会自动配置连接的u-blox GNSS接收器。- 这可能包括设置接收器输出特定的数据格式（如UBX-RXM-RAWX, UBX-RXM-SFRBX, UBX-NAV-PVT等），设置波特率，或配置其他相关参数以确保接收器正常工作和与驱动程序的兼容性。
- 根据地理位置推断时区并同步时间：
- 驱动程序可能具有根据接收器获取的地理位置（经纬度）来自动推断当前时区的功能。
- 利用这个推断出的时区信息，驱动程序可以同步本地系统时间与全球时间（如协调世界时UTC）。
- 这对于确保系统时间的准确性非常重要，尤其是在移动设备或变更地理位置的场景中。
- 优化时间同步功能以提高精度：
- 这可能意味着在软件层面对时间同步的算法或流程进行改进，以提高同步精度。
- 时间同步的优化可能包括减少延迟、提高同步频率，或更精确地处理来自GNSS接收器的时间信号。
总的来说，这段话强调了在使用u-blox GNSS接收器进行高精度定位和时间同步操作时的自动化和优化措施。自动配置接收器确保了系统的即插即用能力，而时间同步的优化则对于那些需要高精度时间信息的应用至关重要。
7. Acknowledgements
Many of the ephemeris parsing functions in our package are adapted from RTKLIB. We use mini-yaml for config parsing.
这段话描述了某个软件包中使用的两种重要的库或工具，用于解析星历数据和配置文件：
- 星历解析功能源自RTKLIB：
- “星历”（ephemeris）是描述卫星位置和轨道的数据，对于GNSS（全球导航卫星系统）接收器来说至关重要，因为它们使用这些数据来确定自身的位置。
- 这段话中提到的软件包中的许多解析星历数据的功能是基于RTKLIB进行适配的。RTKLIB是一个开源的GNSS应用和研究软件库，提供了广泛的GNSS数据处理工具，包括对星历数据的解析。
- 这意味着开发者利用了RTKLIB中的现有代码或算法，并将其调整以适合他们自己的软件包的需求。
- 使用mini-yaml进行配置解析：
- YAML（YAML Ain’t Markup Language）是一种用于配置文件的数据序列化格式，因其易读性和简洁性而广受欢迎。
- “mini-yaml”可能是YAML解析器的一个变种或精简版本，用于解析配置文件。这表示该软件包使用这个工具来读取和解析其配置文件，这些文件可能包含了软件运行所需的参数和设置。
- 使用mini-yaml解析器可以方便地处理配置数据，使得用户或开发者能够以一种简洁直观的方式配置软件。
总体而言，这段话强调了软件包在星历数据处理和配置文件解析方面的技术选型。它表明开发者在创建自己的软件包时借鉴了成熟的开源解决方案（RTKLIB），并选择了适合其需求的配置文件解析工具（mini-yaml）。
8. License
The source code is released under GPLv3 license.
file_dumper.cpp
/**
* This file is part of ublox-driver.
*
* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology
* Author: CAO Shaozu (shaozu.cao@gmail.com)
*
* ublox-driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ublox-driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ublox-driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include "file_dumper.hpp"
FileDumper::FileDumper(const std::string &filepath, uint32_t buf_len) :
filepath_(filepath), buf_len_(buf_len), buf_pos_(0), data_buf_(new uint8_t[buf_len_])
{
// create file at first
const size_t last_slash_pos = filepath_.rfind('/');
if (last_slash_pos == std::string::npos)
{
std::cerr << "filepath error: " << filepath_ << '\n';
return;
}
const std::string folder_path = filepath_.substr(0, last_slash_pos);
if (FileDumper::createDirectoryIfNotExists(folder_path.c_str()))
{
std::cerr << "Failed to create " << folder_path << '\n';
return;
}
std::ofstream ofs(filepath_, std::ios::out | std::ios::binary);
if (!ofs.good())
std::cerr << "Unable to open file " << filepath_ << ".\n";
ofs.close();
}
FileDumper::~FileDumper()
{
if (buf_pos_ > 0)
{
std::ofstream ofs(filepath_, std::ios::out | std::ios::app | std::ios::binary);
ofs.write((const char*)(data_buf_.get()), buf_pos_);
ofs.close();
}
}
void FileDumper::process_data(const uint8_t *data, size_t len)
{
if (buf_pos_+len > buf_len_)
{
std::ofstream ofs(filepath_, std::ios::out | std::ios::app | std::ios::binary);
ofs.write((const char*)(data_buf_.get()), buf_pos_);
ofs.write((const char*)data, len);
ofs.close();
buf_pos_ = 0;
}
else
{
memcpy(data_buf_.get()+buf_pos_, data, len);
buf_pos_ += len;
}
}
/******************************************************************************
* Recursively create directory if `path` not exists.
* Return 0 if success.
*****************************************************************************/
int FileDumper::createDirectoryIfNotExists(const char *path)
{
struct stat info;
int statRC = stat(path, &info);
if( statRC != 0 )
{
if (errno == ENOENT)
{
printf("%s not exists, trying to create it \n", path);
if (! createDirectoryIfNotExists(dirname(strdupa(path))))
{
if (mkdir(path, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) != 0)
{
fprintf(stderr, "Failed to create folder %s \n", path);
return 1;
}
else
return 0;
}
else
return 1;
} // directory not exists
if (errno == ENOTDIR)
{
fprintf(stderr, "%s is not a directory path \n", path);
return 1;
} // something in path prefix is not a dir
return 1;
}
return ( info.st_mode & S_IFDIR ) ? 0 : 1;
}
这段代码是一个C++类FileDumper，用于处理数据的存储。以下是该代码的功能概述：
-
版权声明：
- 代码文件属于
ublox-driver项目，作者为香港科技大学航空机器人组的曹绍祖。 - 代码遵循GNU通用公共许可证（GPLv3），允许自由分发和修改，但没有任何明示的保证。
- 代码文件属于
-
类定义：
FileDumper类用于将数据写入指定的文件路径。它使用缓冲区来存储数据，并在必要时将数据写入文件。
-
构造函数：
- 在创建实例时，构造函数尝试创建文件路径所指示的目录，如果路径错误或文件无法打开则输出错误信息。
-
析构函数：
- 在对象销毁时，如果缓冲区内有数据未写入文件，则将它们写入文件。
-
数据处理函数：
process_data函数将传入的数据添加到内部缓冲区。如果缓冲区填满或数据块太大，它将缓冲区的内容以及新数据写入文件，并清空缓冲区。
-
创建目录函数：
createDirectoryIfNotExists函数递归地创建目录路径，如果目录不存在则创建它。如果路径是一个非目录文件或创建目录失败，则返回错误。
整体上，这个类实现了数据的缓冲写入，使得频繁的磁盘写入操作更加高效。这在处理大量的数据流（如从GNSS接收器接收的数据）时非常有用。
在FileDumper类的构造函数中，用户可以配置以下参数：
const std::string &filepath：文件路径，定义了数据将被写入的文件的路径。uint32_t buf_len：缓冲区长度，定义了内存中用于临时存储数据的缓冲区的大小。
这些参数允许用户根据他们想要存储数据的特定位置和预期的数据量来设置。其他代码部分主要处理数据的写入操作和内部逻辑，而不是用户可配置的参数。
file_loader.cpp
/**
* This file is part of ublox-driver.
*
* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology
* Author: CAO Shaozu (shaozu.cao@gmail.com)
*
* ublox-driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ublox-driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ublox-driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include "file_loader.hpp"
// data members are initialized base on their decleration order in header file
FileLoader::FileLoader(const std::string &filepath, uint32_t baud_rate) :
filepath_(filepath), baud_rate_(baud_rate), paused_(true), stopped_(false),
finished_(false), read_thread_(std::bind(&FileLoader::read_handler, this)),
MSG_HEADER_LEN(ParameterManager::getInstance().MSG_HEADER_LEN)
{
// open file
ifs_.open(filepath_, std::ios::in | std::ios::binary);
if (!ifs_.good())
{
std::cerr << "Unable to open file " << filepath_ << ".\n";
finished_ = true;
}
}
FileLoader::~FileLoader()
{
close();
finished_ = true;
read_thread_.join();
if (ifs_.is_open())
ifs_.close();
}
void FileLoader::startRead()
{
paused_ = false;
}
void FileLoader::close()
{
stopped_ = true;
}
void FileLoader::addCallback(std::function<void (const uint8_t*, size_t)> callback)
{
callbacks_.push_back(callback);
}
void FileLoader::read_handler()
{
uint32_t buf_size = 8192;
std::unique_ptr<uint8_t[]> data_buf(new uint8_t[buf_size]);
while(!finished_ && !stopped_)
{
if (paused_)
{
std::this_thread::sleep_for(std::chrono::milliseconds(20));
continue;
}
ifs_.read((char*)(data_buf.get()), MSG_HEADER_LEN);
uint32_t header_remains = MSG_HEADER_LEN;
while (ifs_.good() && header_remains != 0)
{
uint32_t pream_idx = 0;
// search for u-blox preamble sequence
for (; pream_idx < MSG_HEADER_LEN-1; ++pream_idx)
if (data_buf[pream_idx] == 0xB5 && data_buf[pream_idx+1] == 0x62)
break;
header_remains = pream_idx;
// if more data needed by header, should only happen at boot phase
if (pream_idx != 0)
{
// copy the valid header / remain unchecked part to the front of data buffer
for (uint32_t i = pream_idx; i < MSG_HEADER_LEN; ++i)
data_buf[i-pream_idx] = data_buf[i];
ifs_.read((char*)(data_buf.get()+MSG_HEADER_LEN-pream_idx), header_remains);
}
}
if (!ifs_.good())
{
finished_ = true;
continue;
}
const uint16_t payload_len = *reinterpret_cast<uint16_t*>(data_buf.get()+4);
const uint16_t msg_len = MSG_HEADER_LEN+payload_len+2;
// if message is too large, reallocate data_buf
if (msg_len > buf_size)
{
const uint32_t new_buf_size = buf_size*2;
uint8_t *new_data_buf = new uint8_t[new_buf_size];
memcpy((char*)new_data_buf, (char*)(data_buf.get()), MSG_HEADER_LEN);
data_buf.reset(new_data_buf);
buf_size = new_buf_size;
}
// read message data and CRC
ifs_.read((char*)(data_buf.get()+MSG_HEADER_LEN), payload_len+2);
if (!ifs_.good())
{
finished_ = true;
continue;
}
// invoke all callbacks
for (auto &f : callbacks_)
f(data_buf.get(), msg_len);
int64_t sleep_time_us = static_cast<int64_t>(msg_len*8.0/baud_rate_*1.0e6);
std::this_thread::sleep_for(std::chrono::microseconds(sleep_time_us));
}
if (finished_)
std::cout << "Done with file " << filepath_ << std::endl;
ifs_.close();
}
这段代码是C++编写的FileLoader类，用于从文件中读取数据，并将读取的数据通过回调函数发送出去。以下是它的主要功能：
-
构造函数（
FileLoader::FileLoader）打开指定路径的文件，如果文件无法打开，会将finished_标志设为true并报错。 -
析构函数（
FileLoader::~FileLoader）关闭文件并等待读取线程结束。 -
startRead函数用于开始读取数据。 -
close函数用于停止读取数据。 -
addCallback函数允许外部添加处理数据的回调函数。 -
read_handler函数是读取数据的主要函数，它会循环读取文件中的数据，并调用添加的回调函数处理这些数据。读取的速度通过baud_rate变量控制，以模拟实际串行通信中的数据流速度。 -
在
read_handler中，它会检查UBX消息的前导码，并根据消息长度读取完整的消息，包括消息头、有效载荷和CRC校验。消息读取完成后，通过回调函数将消息发送出去，并根据波特率计算的时间暂停，以模拟真实的串行数据流速率。
这个类可用于模拟串行设备，以测试和开发需要处理串行数据流的应用程序。
在FileLoader类的构造函数中，用户可以配置以下参数：
const std::string &filepath：文件路径，指定了数据将从中读取的文件。uint32_t baud_rate：波特率，用于模拟实时数据流的速度。
这些参数允许用户根据需要读取的文件路径和模拟串口通信速度来定制行为。其他代码部分主要处理文件的读取操作和内部逻辑，而不是用户可配置的参数。
serial_handler.cpp
/**
* This file is part of ublox-driver.
*
* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology
* Author: CAO Shaozu (shaozu.cao@gmail.com)
*
* ublox-driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ublox-driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ublox-driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include "serial_handler.hpp"
SerialHandler::SerialHandler(std::string port, unsigned int baud_rate, unsigned int buf_size) : port_(port),
data_buf_(new uint8_t[buf_size]), serial_(io_service_, port), keep_working_task_(io_service_),
write_timeout_(timer_io_service_), MSG_HEADER_LEN(ParameterManager::MSG_HEADER_LEN)
{
serial_.set_option(boost::asio::serial_port_base::baud_rate(baud_rate));
if (!serial_.is_open())
{
std::cerr << "Cannot open serial port " << port << ".\n";
return;
}
boost::thread bt(boost::bind(&boost::asio::io_service::run, &io_service_));
}
SerialHandler::~SerialHandler()
{
close();
serial_.close();
}
void SerialHandler::addCallback(std::function<void (const uint8_t*, size_t)> callback)
{
callbacks_.push_back(callback);
}
void SerialHandler::startRead()
{
boost::asio::async_read(serial_, boost::asio::buffer(data_buf_.get(), MSG_HEADER_LEN),
boost::asio::transfer_all(),
boost::bind(&SerialHandler::read_handler, this, boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
}
void SerialHandler::stop_read()
{
serial_.cancel();
}
void SerialHandler::write(const std::string &str, uint32_t timeout_ms)
{
writeRaw((const uint8_t*)(str.c_str()), str.size(), timeout_ms);
}
void SerialHandler::writeRaw(const uint8_t *data, size_t len, uint32_t timeout_ms)
{
boost::asio::async_write(serial_, boost::asio::buffer(data, len),
boost::bind(&SerialHandler::write_callback, this,
boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
/* // unblock these lines will cause write timeout for unknown reason
write_timeout_.expires_from_now(boost::posix_time::milliseconds(timeout_ms));
write_timeout_.async_wait(boost::bind(&SerialHandler::wait_callback, this,
boost::asio::placeholders::error));
timer_io_service_.run();
timer_io_service_.reset();
*/
}
void SerialHandler::close()
{
serial_.cancel();
write_timeout_.cancel();
if (!io_service_.stopped())
io_service_.stop();
if (!timer_io_service_.stopped())
timer_io_service_.stop();
}
void SerialHandler::read_handler(const boost::system::error_code& error, std::size_t bytes_transferred)
{
if (!error)
{
if (bytes_transferred != MSG_HEADER_LEN)
{
std::cerr << "Not received enough bytes: " << bytes_transferred << " while expect " << MSG_HEADER_LEN << '\n';
return;
}
uint32_t header_remains = MSG_HEADER_LEN;
while (header_remains != 0)
{
uint32_t pream_idx = 0;
void config(const std::vector<std::string> &receiver_configs);
// search for u-blox preamble sequence
for (; pream_idx < MSG_HEADER_LEN-1; ++pream_idx)
if (data_buf_[pream_idx] == 0xB5 && data_buf_[pream_idx+1] == 0x62)
break;
header_remains = pream_idx;
// if more data needed by header, should only happen at boot phase
if (header_remains != 0)
{
// copy the valid header / remain unchecked part to the front of data buffer
memmove(data_buf_.get(), data_buf_.get()+pream_idx, MSG_HEADER_LEN-header_remains);
boost::system::error_code receive_error;
unsigned int bytes_num = boost::asio::read(serial_,
boost::asio::buffer(data_buf_.get()+MSG_HEADER_LEN-pream_idx, header_remains),
boost::asio::transfer_all(), receive_error); // read until buffer is full
if (bytes_num != header_remains)
{
std::cerr << "Not received enough bytes: " << bytes_num << " while expect " << header_remains << '\n';
return;
}
}
}
uint16_t *msg_len_ptr = reinterpret_cast<uint16_t*>(data_buf_.get()+4);
// buffer for message data and CRC
boost::system::error_code receive_error;
unsigned int bytes_num = boost::asio::read(serial_,
boost::asio::buffer(data_buf_.get()+MSG_HEADER_LEN, *msg_len_ptr+2),
boost::asio::transfer_all(), receive_error); // read until buffer is full
if (bytes_num != static_cast<unsigned int>(*msg_len_ptr+2))
{
std::cerr << "Not received enough bytes: " << bytes_num << " while expect " << (*msg_len_ptr+2) << '\n';
return;
}
// message received completely, invoke callback functions
for (auto &f : callbacks_)
f(data_buf_.get(), MSG_HEADER_LEN+(*msg_len_ptr)+2);
// start another read session
startRead();
}
else if (error == boost::asio::error::operation_aborted || error != boost::asio::error::bad_descriptor)
{
// std::cerr << error.message() << '\n';
std::cerr << "Serial " << port_ << " read handler aborted due to serial shutdown.\n";
return;
}
else
std::cerr << "Connection to serial " << port_ << " lost.\n";
}
void SerialHandler::write_callback(const boost::system::error_code& error, std::size_t bytes_transferred)
{
if (error || !bytes_transferred)
{
// no data was written
std::cerr << "Serial write failed or operation timeout: " << port_ << '\n';
return;
}
write_timeout_.cancel();
}
void SerialHandler::wait_callback(const boost::system::error_code& error)
{
if (error) return; // data was written and this timer was cancelled
serial_.cancel(); // will cause write_callback to fire with an error
}
这段代码定义了一个SerialHandler类，它使用Boost Asio库来异步处理串行端口通信。这个类可以打开串行端口，发送和接收数据，并在数据到达时调用回调函数。这里的关键功能包括：
- 初始化串行端口并启动读取线程。
- 提供开始读取(
startRead)和停止读取(stop_read)的方法。 - 提供写入串行端口的方法(
write和writeRaw)。 - 提供关闭串行端口的方法(
close)。 - 异步读取处理函数(
read_handler)，用于处理接收到的数据并调用回调。 - 异步写入回调函数(
write_callback)，用于确认数据是否成功写入。
此外，代码还包含了处理写入超时(wait_callback)的功能。整个类的设计旨在以非阻塞方式处理串行端口数据，这对于需要实时处理串行通信的应用程序非常有用。
在SerialHandler类的构造函数中，用户可以配置以下参数：
std::string port：串口名称（如/dev/ttyUSB0），定义了与串行设备通信的端口。unsigned int baud_rate：波特率，定义了串行通信的速度。unsigned int buf_size：数据缓冲区大小，定义了内部数据处理的缓冲区大小。
这些参数通常根据连接的具体设备和系统的需求进行设置。其他部分的代码主要涉及类的内部逻辑和串行通信的处理，而不是作为用户可配置的参数。
socket_handler.cpp
/**
* This file is part of ublox-driver.
*
* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology
* Author: CAO Shaozu (shaozu.cao@gmail.com)
*
* ublox-driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ublox-driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ublox-driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include "socket_handler.hpp"
SocketHandler::SocketHandler(const std::string &host, uint64_t port, unsigned int buf_size) : host_(host),
port_(port), buf_size_(buf_size), data_buf_(new uint8_t[buf_size_]), data_socket_(io_service_),
keep_working_task_(io_service_), write_timeout_(timer_io_service_)
{
boost::asio::ip::tcp::resolver resolver(io_service_);
try
{
boost::asio::ip::tcp::resolver::query query_config(host, std::to_string(port));
boost::asio::ip::tcp::resolver::iterator endpoint_config = resolver.resolve(query_config);
data_socket_.connect(*endpoint_config);
}
catch (boost::system::system_error const &e)
{
std::cerr << "Could not connect to " << host << " at port " << port << '\n';
return;
}
if (!data_socket_.is_open())
{
std::cerr << "Cannot open socket " << host << ':' << port << '\n';
return;
}
boost::thread bt(boost::bind(&boost::asio::io_service::run, &io_service_));
}
SocketHandler::~SocketHandler()
{
close();
data_socket_.close();
}
void SocketHandler::addCallback(std::function<void(const uint8_t*, size_t)> callback)
{
callbacks_.push_back(callback);
}
void SocketHandler::startRead()
{
boost::asio::async_read(data_socket_, boost::asio::buffer(data_buf_.get(), buf_size_),
boost::asio::transfer_at_least(1),
boost::bind(&SocketHandler::read_handler, this, boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
}
void SocketHandler::write(const std::string &str, uint32_t timeout_ms)
{
writeRaw((const uint8_t*)(str.c_str()), str.size(), timeout_ms);
}
void SocketHandler::writeRaw(const uint8_t *data, size_t len, uint32_t timeout_ms)
{
boost::asio::async_write(data_socket_, boost::asio::buffer(data, len),
boost::bind(&SocketHandler::write_callback, this,
boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
write_timeout_.expires_from_now(boost::posix_time::milliseconds(timeout_ms));
write_timeout_.async_wait(boost::bind(&SocketHandler::wait_callback, this,
boost::asio::placeholders::error));
timer_io_service_.run();
timer_io_service_.reset();
}
void SocketHandler::close()
{
data_socket_.cancel();
write_timeout_.cancel();
if (!io_service_.stopped())
io_service_.stop();
if (!timer_io_service_.stopped())
timer_io_service_.stop();
}
void SocketHandler::read_handler(const boost::system::error_code& error, std::size_t bytes_transferred)
{
if (!error)
{
// simply invoke callback functions
for (auto &f : callbacks_)
f(data_buf_.get(), bytes_transferred);
// start another read session
startRead();
}
else if (error == boost::asio::error::operation_aborted || error != boost::asio::error::bad_descriptor)
{
std::cerr << "Socket " << host_ << ':' << port_ << " read handler aborted due to shutdown.\n";
return;
}
else
std::cerr << "Connection to socket" << host_ << ':' << port_ << " lost.\n";
}
void SocketHandler::write_callback(const boost::system::error_code& error, std::size_t bytes_transferred)
{
if (error || !bytes_transferred)
{
// no data was written
std::cerr << "Socket write failed or operation timeout: " << host_ << ':' << port_ << '\n';
return;
}
write_timeout_.cancel();
}
void SocketHandler::wait_callback(const boost::system::error_code& error)
{
if (error) return; // data was written and this timer was cancelled
data_socket_.cancel(); // will cause write_callback to fire with an error
}
这段代码定义了一个SocketHandler类，它使用Boost Asio库来异步处理TCP套接字通信。该类负责与指定的主机和端口上的服务建立和维护连接。以下是它的主要功能：
- 构造函数：初始化并尝试连接到指定的主机和端口。
- 析构函数：关闭套接字并清理资源。
- 回调函数添加：允许外部函数注册回调，以便在接收到数据时被调用。
- 读取启动：开始异步读取操作。
- 写入函数：提供将数据写入套接字的方法。
- 关闭连接：用于停止读取和写入操作，并关闭套接字。
- 读取处理程序：处理接收到的数据，并调用回调函数。
- 写入回调：在写入操作完成后被调用。
- 超时处理：处理写入操作超时的情况。
此类提供了TCP网络通信的基本构件，可以用于需要网络功能的任何应用程序中。它封装了套接字的创建、数据传输和异步操作的复杂性，使得在应用程序中处理网络通信变得更加容易。
在SocketHandler类的构造函数中，用户可以配置以下参数：
std::string &host：主机地址，定义了要连接的服务器的地址。uint64_t port：端口号，定义了要连接的服务器的端口。unsigned int buf_size：数据缓冲区大小，定义了内部数据处理的缓冲区大小。
这些参数允许用户根据他们想要连接的特定服务器和网络通信的需求来设置。其他部分的代码主要处理类的内部逻辑和网络通信，而不是用户可配置的参数。
sync_system_time.cpp
/**
* This file is part of ublox-driver.
*
* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology
* Author: CAO Shaozu (shaozu.cao@gmail.com)
*
* ublox-driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ublox-driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ublox-driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
#include <unistd.h>
#include <atomic>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <gnss_comm/gnss_utility.hpp>
#include <gnss_comm/gnss_ros.hpp>
constexpr int UTC_OFFSET = 8;
using namespace gnss_comm;
static std::atomic<bool> done(false);
void set_datetime(const gtime_t t, const int timezone)
{
gtime_t local_t = time_add(t, timezone*3600);
gtime_t gtime_utc = gpst2utc(local_t);
std::vector<double> utc_t;
utc_t.resize(6);
utc_t.clear();
time2epoch(gtime_utc, &(utc_t[0]));
std::cout << "set system time to: ";
for (size_t i = 0; i< utc_t.size(); ++i)
std::cout << utc_t[i] << " ";
std::cout << std::endl;
char buf[100];
sprintf(buf, "%4d-%02d-%02d,%02d:%02d:%02d", int(utc_t[0]), int(utc_t[1]),
int(utc_t[2]), int(utc_t[3]), int(utc_t[4]), int(utc_t[5]));
struct tm s_tm;
strptime(buf, "%Y-%m-%d,%H:%M:%S", &s_tm);
struct timespec s_timespec;
s_timespec.tv_sec = mktime(&s_tm);
s_timespec.tv_nsec = static_cast<long>(t.sec*1e9+0.5);
// LOG(INFO) << "tv_sec is " << s_timespec.tv_sec << ", and nsec is " << s_timespec.tv_nsec;
if(clock_settime(CLOCK_REALTIME, &s_timespec) == -1)
std::cerr << "clock setting error: " << strerror(errno);
}
void receiver_lla_callback(const sensor_msgs::NavSatFixConstPtr &lla_msg)
{
const gtime_t time = sec2time(lla_msg->header.stamp.toSec());
if (time.time != 0)
{
set_datetime(time, UTC_OFFSET);
done = true;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "set_system_time");
ros::NodeHandle n("~");
ros::Subscriber sub_lla = n.subscribe("/ublox_driver/receiver_lla", 10, receiver_lla_callback);
ros::Rate rate(1);
while(!done)
{
ros::spinOnce();
rate.sleep();
}
std::cout << "System time updated" << std::endl;
return 0;
}
这段代码是一个ROS（机器人操作系统）程序，用于根据GNSS（全球导航卫星系统）数据同步系统时间：
-
主要功能：
- 程序订阅了名为
/ublox_driver/receiver_lla的ROS主题，该主题提供GNSS接收器的纬度、经度和高度信息。 - 当收到消息时，它会调用
receiver_lla_callback函数，该函数将GNSS时间转换为本地时间，并尝试将系统时间设置为此时间。
- 程序订阅了名为
-
时间转换和设置：
set_datetime函数接收GNSS时间和时区偏移量（UTC_OFFSET），将GNSS时间转换为UTC时间，并尝试将系统时间设置为此UTC时间。
-
程序流程：
- 程序在一个循环中运行，直到系统时间更新（由
done标志控制）。 - 使用ROS的订阅-发布机制，当接收到新的GNSS数据时，会触发回调函数，从而更新系统时间。
- 程序在一个循环中运行，直到系统时间更新（由
这个程序主要用于在没有互联网连接的情况下根据GNSS数据同步系统时间，特别适用于依赖精确时间的应用，如某些导航和定位系统。
在这段代码中，用户可配置的参数主要集中在set_datetime函数中对系统时间设置的部分，以及main函数中与ROS节点订阅相关的部分。具体的参数包括：
UTC_OFFSET：这是一个编译时常量，用于定义从GPS时间到本地时间的时区偏移量（以小时为单位）。"/ublox_driver/receiver_lla"：在main函数中，这是ROS订阅者用于接收GPS数据的话题名称。
这些参数允许用户根据自己的需要调整时区偏移和ROS话题名称。代码的其余部分主要涉及程序逻辑，而不是用户可配置的参数。
ublox_driver.cpp
/**
* This file is part of ublox-driver.
*
* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology
* Author: CAO Shaozu (shaozu.cao@gmail.com)
*
* ublox-driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ublox-driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ublox-driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include <vector>
#include <string>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <stdlib.h>
#include <time.h>
// #include <sys/procmgr.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <ros/ros.h>
#include "parameter_manager.hpp"
#include "serial_handler.hpp"
#include "socket_handler.hpp"
#include "file_dumper.hpp"
#include "file_loader.hpp"
#include "ublox_message_processor.hpp"
static std::atomic<bool> interrupted(false);
// variables for receiver config at start
std::mutex ack_m;
std::condition_variable ack_cv;
int ack_flag = 0;
void ctrl_c_handler(int s)
{
interrupted = true;
}
std::string time_str()
{
std::stringstream ss;
std::time_t time_ptr;
time_ptr = time(NULL);
tm *tm_local = localtime(&time_ptr);
ss << tm_local->tm_year + 1900 << '_' << tm_local->tm_mon + 1 << '_'
<< tm_local->tm_mday << '_' << tm_local->tm_hour << '_'
<< tm_local->tm_min << '_' << tm_local->tm_sec;
return ss.str();
}
void config_ack_callback(const uint8_t *data, size_t len)
{
int ack_result = UbloxMessageProcessor::check_ack(data, len);
if (ack_result != 0)
{
std::lock_guard<std::mutex> ack_lk(ack_m);
ack_flag = ack_result;
}
ack_cv.notify_one();
}
bool config_receiver(std::shared_ptr<SerialHandler> serial, std::vector<RcvConfigRecord> &rcv_configs)
{
const uint32_t rcv_config_buff_capacity = 8192;
std::unique_ptr<uint8_t[]> rcv_config_buff(new uint8_t[rcv_config_buff_capacity]);
memset(rcv_config_buff.get(), 0, rcv_config_buff_capacity);
uint32_t msg_len = 0;
UbloxMessageProcessor::build_config_msg(rcv_configs, rcv_config_buff.get(), msg_len);
std::unique_lock<std::mutex> ack_lk(ack_m);
ack_flag = 0;
serial->addCallback(std::bind(&config_ack_callback,
std::placeholders::_1, std::placeholders::_2));
serial->writeRaw(rcv_config_buff.get(), msg_len);
serial->startRead();
// block, wait ack
ack_cv.wait(ack_lk, []{return ack_flag != 0;});
serial->stop_read();
ack_lk.unlock();
// resume
if (ack_flag == 1)
return true;
return false;
}
int main(int argc, char **argv)
{
::google::InitGoogleLogging(argv[0]);
FLAGS_logtostderr = 1;
if (!interrupted.is_lock_free()) return 10;
ros::init(argc, argv, "ublox_driver");
ros::NodeHandle nh("~");
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = ctrl_c_handler;
sigemptyset(&sigIntHandler.sa_mask);
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
// procmgr_ability( 0, PROCMGR_AID_CLOCKSET );
std::string config_filepath;
nh.getParam("config_file", config_filepath);
// config_filepath = argv[1];
ParameterManager &pm(ParameterManager::getInstance());
pm.read_parameter(config_filepath);
std::shared_ptr<SerialHandler> serial;
std::shared_ptr<SocketHandler> socket;
std::shared_ptr<FileLoader> file_loader;
std::shared_ptr<FileDumper> file_dumper;
std::shared_ptr<UbloxMessageProcessor> ublox_msg_processor;
std::shared_ptr<SerialHandler> output_serial;
if (pm.to_ros)
ublox_msg_processor.reset(new UbloxMessageProcessor(nh));
if (pm.to_file)
{
const std::string t_str = time_str();
const std::string dump_filepath = pm.dump_dir + "/" + t_str + ".ubx";
file_dumper.reset(new FileDumper(dump_filepath));
}
if (pm.to_serial)
{
output_serial.reset(new SerialHandler(pm.output_serial_port, pm.serial_baud_rate));
}
if (pm.online)
{
serial.reset(new SerialHandler(pm.input_serial_port, pm.serial_baud_rate));
if (pm.config_receiver_at_start)
{
if (config_receiver(serial, pm.receiver_configs))
LOG(ERROR) << "Successfully configured the receiver.";
else
LOG(FATAL) << "Error occurs when configuring the receiver.";
}
if (pm.input_rtcm)
{
socket.reset(new SocketHandler("localhost", pm.rtcm_tcp_port));
socket->addCallback(std::bind(&SerialHandler::writeRaw, serial.get(),
std::placeholders::_1, std::placeholders::_2, pm.IO_TIMEOUT_MS));
socket->startRead();
}
if (pm.to_ros)
serial->addCallback(std::bind(&UbloxMessageProcessor::process_data,
ublox_msg_processor.get(), std::placeholders::_1, std::placeholders::_2));
if (pm.to_file)
serial->addCallback(std::bind(&FileDumper::process_data, file_dumper.get(),
std::placeholders::_1, std::placeholders::_2));
if (pm.to_serial)
serial->addCallback(std::bind(&SerialHandler::writeRaw, output_serial.get(),
std::placeholders::_1, std::placeholders::_2, pm.IO_TIMEOUT_MS));
serial->startRead();
}
else
{
file_loader.reset(new FileLoader(pm.ubx_filepath, pm.serial_baud_rate));
if (pm.to_ros)
file_loader->addCallback(std::bind(&UbloxMessageProcessor::process_data,
ublox_msg_processor.get(), std::placeholders::_1, std::placeholders::_2));
if (pm.to_file)
file_loader->addCallback(std::bind(&FileDumper::process_data, file_dumper.get(),
std::placeholders::_1, std::placeholders::_2));
if (pm.to_serial)
file_loader->addCallback(std::bind(&SerialHandler::writeRaw, output_serial.get(),
std::placeholders::_1, std::placeholders::_2, pm.IO_TIMEOUT_MS));
file_loader->startRead();
}
ros::Rate loop(50);
while (ros::ok() && !interrupted)
{
ros::spinOnce();
loop.sleep();
}
if (serial)
serial->close();
if (file_loader)
file_loader->close();
return 0;
}
这段代码是一个ROS（机器人操作系统）程序，用于控制和管理u-blox GNSS接收器的数据流。代码的主要功能包括：
-
信号处理：定义了一个
ctrl_c_handler函数来处理中断信号（例如Ctrl+C），以安全地关闭程序。 -
ROS节点初始化：程序作为一个ROS节点启动，从参数服务器读取配置。
-
串行和套接字处理：根据配置文件，程序可以通过串行端口或TCP套接字接收GNSS数据。
-
GNSS数据处理：使用
UbloxMessageProcessor类处理接收到的数据，并可选择将数据发布到ROS话题。 -
数据录制：如果启用，程序可以将接收到的数据保存到文件中。
-
循环和退出：主循环检查ROS节点状态和中断标志，并在必要时关闭串行端口和文件加载器。
此程序提供了灵活的方式来接收和处理GNSS数据，适用于需要实时定位和追踪的应用。
在这段代码中，可以由用户配置的参数包括：
config_file：配置文件的路径，用于初始化参数管理器。pm.to_ros：决定是否将数据发送到ROS。pm.to_file：决定是否将数据保存到文件。pm.to_serial：决定是否将数据通过串口发送。pm.online：决定程序是在线处理（实时从串口读取）还是离线处理（从文件加载）。pm.config_receiver_at_start：是否在启动时配置接收器。pm.input_rtcm：是否输入RTCM数据。pm.dump_dir：文件转储目录。pm.ubx_filepath：UBX文件的路径。pm.serial_baud_rate：串口波特率。pm.input_serial_port和pm.output_serial_port：输入和输出的串口端口。pm.rtcm_tcp_port：RTCM TCP端口。pm.IO_TIMEOUT_MS：输入/输出超时设置。
这些参数允许用户根据他们的需求定制ublox GNSS接收器的行为。
在提供的代码中，串行和套接字处理主要体现在以下几个方面：
-
初始化串行处理器（SerialHandler）：程序创建了一个名为
serial的SerialHandler对象，用于通过串行端口接收GNSS数据。这通过调用new SerialHandler(pm.input_serial_port, pm.serial_baud_rate)实现，其中pm.input_serial_port和pm.serial_baud_rate是配置文件中指定的串行端口和波特率。 -
初始化套接字处理器（SocketHandler）：如果启用了RTCM输入（
pm.input_rtcm），程序会创建一个名为socket的SocketHandler对象，用于通过TCP套接字接收RTCM数据。这是通过new SocketHandler("localhost", pm.rtcm_tcp_port)实现的，其中pm.rtcm_tcp_port是配置文件中指定的TCP端口。 -
处理器回调：对于
serial对象，程序添加了多个回调函数，以处理接收到的数据（例如，发布到ROS、写入文件或通过另一个串行端口输出）。 -
读取数据：通过调用
serial->startRead()和socket->startRead()，程序开始从串行端口和TCP套接字读取数据。
总的来说，代码通过配置文件参数来决定是否使用串行端口或TCP套接字来接收GNSS数据，并根据需求将接收到的数据进行相应处理。
ublox_message_processor.cpp
/**
* This file is part of ublox-driver.
*
* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology
* Author: CAO Shaozu (shaozu.cao@gmail.com)
*
* ublox-driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ublox-driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ublox-driver. If not, see <http://www.gnu.org/licenses/>.
*
* As most of the ephemeris parsing functions are adapted from RTKLIB,
* the license for those functions is claimed as follows:
*
* The RTKLIB software package is distributed under the following BSD 2-clause
* license (http://opensource.org/licenses/BSD-2-Clause) and additional two
* exclusive clauses. Users are permitted to develop, produce or sell their own
* non-commercial or commercial products utilizing, linking or including RTKLIB as
* long as they comply with the license.
*
* Copyright (c) 2007-2020, T. Takasu, All rights reserved.
*/
#include "ublox_message_processor.hpp"
constexpr double UbloxMessageProcessor::lam_carr[];
constexpr unsigned int UbloxMessageProcessor::tbl_CRC24Q[];
UbloxMessageProcessor::UbloxMessageProcessor(ros::NodeHandle& nh) :
nh_(nh),
MSG_HEADER_LEN(ParameterManager::getInstance().MSG_HEADER_LEN)
{
pub_pvt_ = nh_.advertise<GnssPVTSolnMsg>("receiver_pvt", 100);
pub_lla_ = nh_.advertise<sensor_msgs::NavSatFix>("receiver_lla", 100);
pub_tp_info_ = nh_.advertise<GnssTimePulseInfoMsg>("time_pulse_info", 100);
pub_range_meas_ = nh_.advertise<GnssMeasMsg>("range_meas", 100);
pub_ephem_ = nh_.advertise<GnssEphemMsg>("ephem", 100);
pub_glo_ephem_ = nh_.advertise<GnssGloEphemMsg>("glo_ephem", 100);
pub_iono_ = nh_.advertise<StampedFloat64Array>("iono_params", 100);
}
void UbloxMessageProcessor::process_data(const uint8_t *data, size_t len)
{
if (!verify_msg(data, len))
return;
const uint16_t msg_type = (data[2]<<8) | data[3];
if (msg_type == UBX_RXMRAWX_ID)
{
std::vector<ObsPtr> meas = parse_meas_msg(data, len);
if (meas.empty()) return;
GnssMeasMsg meas_msg = meas2msg(meas);
pub_range_meas_.publish(meas_msg);
return;
}
else if (msg_type == UBX_RXMSFRBX_ID)
{
std::vector<double> iono_params;
EphemBasePtr ephem = parse_subframe(data, len, iono_params);
if (ephem && ephem->ttr.time!=0)
{
if (satsys(ephem->sat, NULL) == SYS_GLO)
{
GnssGloEphemMsg glo_ephem_msg = glo_ephem2msg(std::dynamic_pointer_cast<GloEphem>(ephem));
pub_glo_ephem_.publish(glo_ephem_msg);
}
else
{
GnssEphemMsg ephem_msg = ephem2msg(std::dynamic_pointer_cast<Ephem>(ephem));
pub_ephem_.publish(ephem_msg);
}
}
if (iono_params.size() == 8)
{
// publish ionosphere parameters
StampedFloat64Array iono_msg;
if (ephem && ephem->ttr.time!=0)
iono_msg.header.stamp = ros::Time(time2sec(ephem->ttr));
std::copy(iono_params.begin(), iono_params.end(), std::back_inserter(iono_msg.data));
pub_iono_.publish(iono_msg);
}
return;
}
else if (msg_type == UBX_TIM_TP_ID)
{
TimePulseInfoPtr tp_info = parse_time_pulse(data, len);
if (tp_info && tp_info->time.time != 0)
{
GnssTimePulseInfoMsg tp_info_msg = tp_info2msg(tp_info);
pub_tp_info_.publish(tp_info_msg);
}
return;
}
else if (msg_type == UBX_NAVPOS_ID)
{
PVTSolutionPtr pvt_soln = parse_pvt(data, len);
if (!pvt_soln || pvt_soln->time.time == 0) return;
// add RTK station offset if appliable
if (pvt_soln->carr_soln == 1 || pvt_soln->carr_soln == 2)
{
Eigen::Vector3d pvt_lla(pvt_soln->lat, pvt_soln->lon, pvt_soln->hgt);
Eigen::Vector3d pvt_ecef = geo2ecef(pvt_lla);
pvt_ecef += ParameterManager::getInstance().rtk_correction_ecef;
pvt_lla = ecef2geo(pvt_ecef);
pvt_soln->lat = pvt_lla.x();
pvt_soln->lon = pvt_lla.y();
pvt_soln->hgt = pvt_lla.z();
}
GnssPVTSolnMsg pvt_msg = pvt2msg(pvt_soln);
pub_pvt_.publish(pvt_msg);
sensor_msgs::NavSatFix lla_msg;
lla_msg.header.stamp = ros::Time(time2sec(pvt_soln->time));
lla_msg.latitude = pvt_soln->lat;
lla_msg.longitude = pvt_soln->lon;
lla_msg.altitude = pvt_soln->hgt;
lla_msg.status.status = static_cast<int8_t>(pvt_soln->fix_type);
lla_msg.status.service = static_cast<uint16_t>(pvt_soln->carr_soln);
pub_lla_.publish(lla_msg);
return;
}
// unsupported message reach here
return;
}
int UbloxMessageProcessor::check_ack(const uint8_t *data, size_t len)
{
if (!verify_msg(data, len))
return 0;
const uint16_t msg_type = (data[2]<<8) | data[3];
if (msg_type == UBX_ACK_ACK_ID)
return 1;
else if (msg_type == UBX_ACK_NAK_ID)
return -1;
// ignore other messages
return 0;
}
bool UbloxMessageProcessor::verify_msg(const uint8_t *data, const size_t len)
{
if (len < 8) // ubx header length is 6, checksum length is 2
{
LOG(ERROR) << "Invalid message length ";
return false;
}
if (data[0] != 0xB5 || data[1] != 0x62)
{
LOG(ERROR) << "Invalid ublox message preamble.";
return false;
}
uint16_t payload_len = *reinterpret_cast<const uint16_t*>(data+4);
if (len != payload_len+ParameterManager::getInstance().MSG_HEADER_LEN+2)
{
LOG(ERROR) << "Invalid message length.";
return false;
}
if (!check_checksum(data, len))
{
LOG(ERROR) << "Invalid checksum.\n";
return false;
}
return true;
}
void UbloxMessageProcessor::parse_ion_utc(const uint8_t *data, const size_t header_len)
{
// TODO: parse ionosphere and utc parameters
return;
}
TimePulseInfoPtr UbloxMessageProcessor::parse_time_pulse(const uint8_t *msg_data, const uint32_t msg_len)
{
TimePulseInfoPtr tp_info;
if (msg_len != 24) // header(6) + payload(16) + checksum(2)
{
LOG(ERROR) << "UBX-TIM-TP message length error. len=" << msg_len << " rather than " << 24;
return tp_info;
}
tp_info.reset(new TimePulseInfo());
const uint8_t *p = msg_data + 6; // skip header
double tow_ms = *reinterpret_cast<const uint32_t*>(p);
double tow_sub_ms = *reinterpret_cast<const uint32_t*>(p+4);
double tow = tow_ms * 1.0e-3 + tow_sub_ms * 1.0e-3 * std::pow(2, -32);
uint16_t week = *reinterpret_cast<const uint16_t*>(p+12);
tp_info->time = gpst2time(week, tow);
uint8_t flags = *(p+14);
tp_info->utc_based = flags & 0x01;
uint8_t ref_info = *(p+15);
uint8_t ref_system = ref_info & 0x0F;
switch (ref_system)
{
case SYS_GPS:
tp_info->time_sys = SYS_GPS;
break;
case SYS_BDS:
tp_info->time_sys = SYS_BDS;
break;
case SYS_GLO:
tp_info->time_sys = SYS_GLO;
break;
case SYS_GAL:
tp_info->time_sys = SYS_GAL;
break;
default:
tp_info->time_sys = SYS_NONE;
break;
}
return tp_info;
}
PVTSolutionPtr UbloxMessageProcessor::parse_pvt(const uint8_t *msg_data, const uint32_t msg_len)
{
PVTSolutionPtr pvt_soln;
if (msg_len != MSG_HEADER_LEN+UBX_PVT_PAYLOAD_LEN+2) // header(6) + payload(92) + checksum(2)
{
LOG(ERROR) << "ubx nav-pvt message length error. len=" << msg_len;
return pvt_soln;
}
if (curr_time.time == 0) return pvt_soln;
const uint8_t *p = msg_data+6;
uint32_t itow = *reinterpret_cast<const uint32_t*>(p);
uint32_t week = 0;
double tow = time2gpst(curr_time, &week);
if (itow*1e-3 > tow+302400.0) --week;
else if (itow*1e-3 < tow-302400.0) ++week;
gtime_t time = gpst2time(week, itow*1e-3);
if (false && (p[11]&0x04) && ((p[22]&0xF0) == 0xE0)) // skip UTC time check
{
// check time
double ep[6];
ep[0] = static_cast<double>((p[5]<<8)+p[4]);
ep[1] = static_cast<double>(p[6]);
ep[2] = static_cast<double>(p[7]);
ep[3] = static_cast<double>(p[8]);
ep[4] = static_cast<double>(p[9]);
ep[5] = static_cast<double>(p[10]) + (*reinterpret_cast<const int32_t*>(p+16))*1e-9;
gtime_t time_utc = epoch2time(ep);
gtime_t time_gpst = utc2gpst(time_utc);
LOG(INFO) << "time_diff is " << std::setprecision(20) << time_diff(time_gpst, time);
LOG_IF(FATAL, time2sec(time) != time2sec(time_gpst)) << "Time mismatch";
}
pvt_soln.reset(new PVTSolution());
pvt_soln->fix_type = p[20];
uint8_t flags = p[21];
pvt_soln->valid_fix = static_cast<bool>(flags&0x01);
pvt_soln->diff_soln = static_cast<bool>((flags&0x02)>>1);
pvt_soln->carr_soln = (flags&0xC0)>>6;
pvt_soln->num_sv = p[23];
pvt_soln->lat = (*reinterpret_cast<const int32_t*>(p+28)) * 1e-7;
pvt_soln->lon = (*reinterpret_cast<const int32_t*>(p+24)) * 1e-7;
pvt_soln->hgt = (*reinterpret_cast<const int32_t*>(p+32)) * 1e-3;
pvt_soln->hgt_msl = (*reinterpret_cast<const int32_t*>(p+36)) * 1e-3;
pvt_soln->h_acc = (*reinterpret_cast<const uint32_t*>(p+40)) * 1e-3;
pvt_soln->v_acc = (*reinterpret_cast<const uint32_t*>(p+44)) * 1e-3;
pvt_soln->p_dop = (*reinterpret_cast<const uint16_t*>(p+76)) * 1e-2;
pvt_soln->vel_n = (*reinterpret_cast<const int32_t*>(p+48)) * 1e-3;
pvt_soln->vel_e = (*reinterpret_cast<const int32_t*>(p+52)) * 1e-3;
pvt_soln->vel_d = (*reinterpret_cast<const int32_t*>(p+56)) * 1e-3;
pvt_soln->vel_acc = (*reinterpret_cast<const uint32_t*>(p+68)) * 1e-3;
pvt_soln->time = time;
return pvt_soln;
}
std::vector<ObsPtr> UbloxMessageProcessor::parse_meas_msg(const uint8_t *msg_data, const uint32_t msg_len)
{
std::vector<ObsPtr> obs_meas;
if (msg_len < 24)
{
LOG(ERROR) << "ubx rxmrawx length error: len=" << msg_len;
return obs_meas;
}
const uint8_t *p = msg_data + 6; // skip header
double tow = *reinterpret_cast<const double*>(p);
uint16_t week = *reinterpret_cast<const uint16_t*>(p+8);
uint8_t num_meas = p[11];
if (msg_len < static_cast<uint32_t>(24+32*num_meas))
{
LOG(ERROR) << "ubx rxmrawx length error: len=" << msg_len << " num_meas=" << num_meas;
return obs_meas;
}
if (week == 0)
{
LOG(ERROR) << "ubx rxmrawx week=0 error";
return obs_meas;
}
gtime_t time = gpst2time(week, tow);
std::map<uint32_t, uint32_t> sat2idx;
p += 16;
for (size_t i=0; i < num_meas; ++i, p+=32)
{
double psr = *reinterpret_cast<const double*>(p);
double cp = *reinterpret_cast<const double*>(p+8);
float dopp = *reinterpret_cast<const float*>(p+16);
uint8_t gnss_id = p[20];
uint8_t svid = p[21];
uint8_t sig_id = p[22];
uint8_t freq_id = p[23];
uint16_t lock_time = *reinterpret_cast<const uint16_t*>(p+24);
uint8_t cn0 = p[26];
uint8_t psr_std = p[27] & 0x0F;
uint8_t cp_std = p[28] & 0x0F;
uint8_t dopp_std = p[29] & 0x0F;
uint8_t tck_sta = p[30];
if (!(tck_sta & 0x01)) psr = 0.0; // pseudo-range invalid
if (!(tck_sta & 0x02) || cp == -0.5 || cp_std > CPSTD_VALID) cp = 0.0; // carrier phase invalid
uint32_t sys;
if (!(sys = ubx_sys(gnss_id)))
{
// LOG(WARNING) << "ubx rxmrawx stallite system not supported. GNSS id: " << static_cast<int>(gnss_id);
continue;
}
uint32_t sat;
if (!(sat = sat_no(sys, svid)))
{
LOG(ERROR) << "ubx rxmrawx satellite number error. sys: " << sys << " prn: " << static_cast<int>(svid);
continue;
}
int code = ubx_sig(sys, sig_id);
int sid = sig_idx(sys, code); // signal index in obs data
if (sid == 0 || sid > N_FREQ)
{
LOG(ERROR) << "ubx rxmrawx signal error: sat=" << sat << " signal_id=" << sid;
continue;
}
uint8_t halfv = (tck_sta & 0x04) ? 1 : 0;
uint8_t halfc = (tck_sta & 0x08) ? 1 : 0;
uint8_t slip = lock_time == 0 || lock_time*1e-3 < lock_time_rec[sat-1][sid-1] ||
halfc != halfc_rec[sat-1][sid-1];
lock_time_rec[sat-1][sid-1] = lock_time * 1e-3;
halfc_rec[sat-1][sid-1] = halfc;
uint8_t LLI = (slip ? LLI_SLIP : 0) | (!halfv ? LLI_HALFC : 0);
if (sat2idx.count(sat) == 0)
{
ObsPtr obs(new Obs());
obs->time = time;
obs->sat = sat;
obs_meas.emplace_back(obs);
sat2idx.emplace(sat, obs_meas.size()-1);
}
ObsPtr obs = obs_meas[sat2idx.at(sat)];
double freq = sig_freq(sys, sid, static_cast<int>(freq_id)-7);
obs->freqs.push_back(freq);
obs->CN0.push_back(cn0);
obs->LLI.push_back(LLI);
obs->code.push_back(code);
obs->psr.push_back(psr);
obs->psr_std.push_back(0.01*(1<<psr_std));
obs->cp.push_back(cp);
obs->cp_std.push_back(0.004*cp_std);
obs->dopp.push_back(dopp);
obs->dopp_std.push_back(0.002*(1<<dopp_std));
obs->status.push_back(tck_sta&0x0F);
}
if (!obs_meas.empty()) curr_time = time; // update current time
return obs_meas;
}
EphemBasePtr UbloxMessageProcessor::parse_subframe(const uint8_t *msg_data, const uint32_t msg_len, std::vector<double> &iono_params)
{
EphemBasePtr ephem;
uint32_t sys = 0;
if (!(sys=ubx_sys(msg_data[6])))
{
// LOG(ERROR) << "ubx rxmsfrbx not supported satellite system : " << int(msg_data[6]);
return ephem;
}
uint8_t prn = msg_data[7];
uint32_t sat = 0;
if (!(sat=sat_no(sys, prn)))
{
LOG(ERROR) << "ubx rxmsfrbx sat number error: sys=" << sys << " prn=" << int(prn);
return ephem;
}
switch (sys)
{
case SYS_GPS:
ephem.reset(new Ephem());
ephem->sat = sat;
ephem->ttr.time = 0;
decode_GPS_subframe(msg_data, msg_len, std::dynamic_pointer_cast<Ephem>(ephem));
break;
case SYS_BDS:
ephem.reset(new Ephem());
ephem->sat = sat;
ephem->ttr.time = 0;
decode_BDS_subframe(msg_data, msg_len, std::dynamic_pointer_cast<Ephem>(ephem), iono_params);
break;
case SYS_GAL:
ephem.reset(new Ephem());
ephem->sat = sat;
ephem->ttr.time = 0;
decode_GAL_subframe(msg_data, msg_len, std::dynamic_pointer_cast<Ephem>(ephem));
break;
case SYS_GLO:
ephem.reset(new GloEphem());
ephem->sat = sat;
ephem->ttr.time = 0;
decode_GLO_subframe(msg_data, msg_len, std::dynamic_pointer_cast<GloEphem>(ephem));
break;
}
return ephem;
}
int UbloxMessageProcessor::decode_GLO_subframe(const uint8_t *msg_data, const uint32_t msg_len, GloEphemPtr glo_ephem)
{
int i,j,k,m;
const uint8_t *p=msg_data+14;
uint8_t buff[64],*fid;
if (msg_len<32)
{
LOG(ERROR) << "ubx rawsfrbx gnav length error: len=" << msg_len;
return -1;
}
glo_ephem->freqo = msg_data[9]-7;
for (i=k=0;i<4;i++,p+=4) for (j=0;j<4;j++) {
buff[k++]=p[3-j];
}
/* test hamming of glonass string */
if (!test_glostr(buff))
{
LOG(ERROR) << "ubx rawsfrbx glo string hamming error: sat=" << glo_ephem->sat;
return -1;
}
m=getbitu(buff,1,4);
if (m<1||15<m)
{
LOG(ERROR) << "ubx rawsfrbx glo string no error: sat=" << glo_ephem->sat;
return -1;
}
/* flush frame buffer if frame-id changed */
fid=subfrm[glo_ephem->sat-1]+150;
if (fid[0]!=buff[12]||fid[1]!=buff[13]) {
for (i=0;i<4;i++) memset(subfrm[glo_ephem->sat-1]+i*10,0,10);
memcpy(fid,buff+12,2); /* save frame-id */
}
memcpy(subfrm[glo_ephem->sat-1]+(m-1)*10,buff,10);
if (m!=4) return 0;
/* decode glonass ephemeris strings */
decode_GLO_ephem(glo_ephem);
return 0;
}
int UbloxMessageProcessor::decode_GLO_ephem(GloEphemPtr glo_ephem)
{
double tow,tod,tof,toe;
int tk_h,tk_m,tk_s,tb,slot;
uint32_t week;
int i=1,frn1,frn2,frn3,frn4;
// LOG(INFO) << "decode_glostr";
uint8_t *buff = subfrm[glo_ephem->sat-1];
/* frame 1 */
frn1 =getbitu(buff,i, 4); i+= 4+2;
getbitu(buff,i, 2); i+= 2; // skip
tk_h =getbitu(buff,i, 5); i+= 5;
tk_m =getbitu(buff,i, 6); i+= 6;
tk_s =getbitu(buff,i, 1)*30; i+= 1;
glo_ephem->vel[0] =getbitg(buff,i,24)*P2_20*1E3; i+=24;
glo_ephem->acc[0] =getbitg(buff,i, 5)*P2_30*1E3; i+= 5;
glo_ephem->pos[0] =getbitg(buff,i,27)*P2_11*1E3; i+=27+4;
/* frame 2 */
frn2 =getbitu(buff,i, 4); i+= 4;
glo_ephem->health =getbitu(buff,i, 3); i+= 3;
getbitu(buff,i, 1); i+= 1; // skip
tb =getbitu(buff,i, 7); i+= 7+5;
glo_ephem->vel[1] =getbitg(buff,i,24)*P2_20*1E3; i+=24;
glo_ephem->acc[1] =getbitg(buff,i, 5)*P2_30*1E3; i+= 5;
glo_ephem->pos[1] =getbitg(buff,i,27)*P2_11*1E3; i+=27+4;
/* frame 3 */
frn3 =getbitu(buff,i, 4); i+= 4;
getbitu(buff,i, 1); i+= 1; // skip
glo_ephem->gamma =getbitg(buff,i,11)*P2_40; i+=11+1;
getbitu(buff,i, 2); i+= 2; // skip
getbitu(buff,i, 1); i+= 1; // skip
glo_ephem->vel[2] =getbitg(buff,i,24)*P2_20*1E3; i+=24;
glo_ephem->acc[2] =getbitg(buff,i, 5)*P2_30*1E3; i+= 5;
glo_ephem->pos[2] =getbitg(buff,i,27)*P2_11*1E3; i+=27+4;
/* frame 4 */
frn4 =getbitu(buff,i, 4); i+= 4;
glo_ephem->tau_n =getbitg(buff,i,22)*P2_30; i+=22;
glo_ephem->delta_tau_n =getbitg(buff,i, 5)*P2_30; i+= 5;
glo_ephem->age =getbitu(buff,i, 5); i+= 5+14;
getbitu(buff,i, 1); i+= 1; // skip
uint32_t ft =getbitu(buff,i, 4); i+= 4+3;
getbitu(buff,i,11); i+=11; // skip
slot =getbitu(buff,i, 5); i+= 5;
getbitu(buff,i, 2); // skip
if (ft == 2) glo_ephem->ura = 2.5;
else if (ft < 5) glo_ephem->ura = ft+1.0;
else if (ft == 5) glo_ephem->ura = 7.0;
else if (ft < 10) glo_ephem->ura = 10.0+(ft-6)*2.0;
else if (ft < 15) glo_ephem->ura = 1 << (ft-5);
else glo_ephem->ura = -1.0;
if (frn1!=1||frn2!=2||frn3!=3||frn4!=4)
{
// LOG(ERROR) << "decode_GLO_ephem error: frn=" << frn1 << ' '
// << frn2 << ' ' << frn3 << ' ' << frn4;
return -1;
}
if (glo_ephem->sat != sat_no(SYS_GLO,slot))
{
LOG(ERROR) << "decode_GLO_ephem error: slot=" << slot;
return -1;
}
glo_ephem->iode=tb;
tow=time2gpst(gpst2utc(curr_time),&week);
tod=fmod(tow,86400.0); tow-=tod;
tof=tk_h*3600.0+tk_m*60.0+tk_s-10800.0; /* lt->utc */
if (tof<tod-43200.0) tof+=86400.0;
else if (tof>tod+43200.0) tof-=86400.0;
glo_ephem->ttr=utc2gpst(gpst2time(week,tow+tof));
toe=tb*900.0-10800.0; /* lt->utc */
if (toe<tod-43200.0) toe+=86400.0;
else if (toe>tod+43200.0) toe-=86400.0;
glo_ephem->toe=utc2gpst(gpst2time(week,tow+toe)); /* utc->gpst */
return 0;
}
int UbloxMessageProcessor::decode_BDS_subframe(const uint8_t *msg_data, const uint32_t msg_len, EphemPtr ephem, std::vector<double> &iono_params)
{
uint32_t words[10];
uint32_t prn;
int i,id,pgn;
const uint8_t *p=msg_data+14;
if (msg_len < 56)
{
LOG(INFO) << "ubx rawsfrbx length error: sat=" << ephem->sat << " len=" << msg_len;
return -1;
}
for (i=0;i<10;i++,p+=4) words[i]=(*reinterpret_cast<const uint32_t*>(p))&0x3FFFFFFF; /* 30 bits */
satsys(ephem->sat,&prn);
id=(words[0]>>12)&0x07; /* subframe id (3bit) */
if (id<1||5<id)
{
LOG(INFO) << "ubx rawsfrbx subfrm id error: sat=" << ephem->sat;
return -1;
}
if (prn>5&&prn<59) { /* IGSO/MEO */
for (i=0;i<10;i++) {
setbitu(subfrm[ephem->sat-1]+(id-1)*38,i*30,30,words[i]);
}
if (id!=3) return -1;
/* decode beidou D1 ephemeris */
decode_BDS_D1_ephem(ephem, iono_params);
}
else { /* GEO (C01-05, C59-63) */
if (id!=1) return -1;
/* subframe 1 */
pgn=(words[1]>>14)&0x0F; /* page number (4bit) */
if (pgn<1||10<pgn) {
LOG(INFO) << "ubx rawsfrbx page number error: sat=" << ephem->sat;
return -1;
}
for (i=0;i<10;i++) {
setbitu(subfrm[ephem->sat-1]+(pgn-1)*38,i*30,30,words[i]);
}
if (pgn!=10) return -1;
/* decode beidou D2 ephemeris */
decode_BDS_D2_ephem(ephem);
}
return 0;
}
int UbloxMessageProcessor::decode_BDS_D2_ephem(EphemPtr ephem)
{
double toc_bds,sqrtA;
uint32_t f1p4,cucp5,ep6,cicp7,i0p8,OMGdp9,omgp10;
uint32_t bdt_week,toes,sow1,sow3,sow4,sow5,sow6,sow7,sow8,sow9,sow10;
int i,f1p3,cucp4,ep5,cicp6,i0p7,OMGdp8,omgp9;
int pgn1,pgn3,pgn4,pgn5,pgn6,pgn7,pgn8,pgn9,pgn10;
// LOG(INFO) << "decode_bds_d2";
uint8_t *buff = subfrm[ephem->sat-1];
i=8*38*0; /* page 1 */
pgn1 =getbitu (buff,i+ 42, 4);
sow1 =getbitu2(buff,i+ 18, 8,i+ 30,12);
ephem->health =getbitu (buff,i+ 46, 1); /* SatH1 */
ephem->iodc =getbitu (buff,i+ 47, 5); /* AODC */
uint8_t urai =getbitu (buff,i+ 60, 4);
ephem->ura = (urai < 6) ? pow(2, urai/2.0+1) : (1<<(urai-2));
bdt_week =getbitu (buff,i+ 64,13); /* week in BDT */
toc_bds =getbitu2(buff,i+ 77, 5,i+ 90,12)*8.0;
ephem->tgd[0] =getbits (buff,i+102,10)*0.1*1E-9;
ephem->tgd[1] =getbits (buff,i+120,10)*0.1*1E-9;
i=8*38*2; /* page 3 */
pgn3 =getbitu (buff,i+ 42, 4);
sow3 =getbitu2(buff,i+ 18, 8,i+ 30,12);
ephem->af0 =getbits2(buff,i+100,12,i+120,12)*P2_33;
f1p3 =getbits (buff,i+132,4);
i=8*38*3; /* page 4 */
pgn4 =getbitu (buff,i+ 42, 4);
sow4 =getbitu2(buff,i+ 18, 8,i+ 30,12);
f1p4 =getbitu2(buff,i+ 46, 6,i+ 60,12);
ephem->af2 =getbits2(buff,i+ 72,10,i+ 90, 1)*P2_66;
ephem->iode =getbitu (buff,i+ 91, 5); /* AODE */
ephem->delta_n =getbits (buff,i+ 96,16)*P2_43*SC2RAD;
cucp4 =getbits (buff,i+120,14);
i=8*38*4; /* page 5 */
pgn5 =getbitu (buff,i+ 42, 4);
sow5 =getbitu2(buff,i+ 18, 8,i+ 30,12);
cucp5 =getbitu (buff,i+ 46, 4);
ephem->M0 =getbits3(buff,i+ 50, 2,i+ 60,22,i+ 90, 8)*P2_31*SC2RAD;
ephem->cus =getbits2(buff,i+ 98,14,i+120, 4)*P2_31;
ep5 =getbits (buff,i+124,10);
i=8*38*5; /* page 6 */
pgn6 =getbitu (buff,i+ 42, 4);
sow6 =getbitu2(buff,i+ 18, 8,i+ 30,12);
ep6 =getbitu2(buff,i+ 46, 6,i+ 60,16);
sqrtA =getbitu3(buff,i+ 76, 6,i+ 90,22,i+120,4)*P2_19;
cicp6 =getbits (buff,i+124,10);
ephem->A =sqrtA*sqrtA;
i=8*38*6; /* page 7 */
pgn7 =getbitu (buff,i+ 42, 4);
sow7 =getbitu2(buff,i+ 18, 8,i+ 30,12);
cicp7 =getbitu2(buff,i+ 46, 6,i+ 60, 2);
ephem->cis =getbits (buff,i+ 62,18)*P2_31;
toes =getbitu2(buff,i+ 80, 2,i+ 90,15)*8.0;
i0p7 =getbits2(buff,i+105, 7,i+120,14);
i=8*38*7; /* page 8 */
pgn8 =getbitu (buff,i+ 42, 4);
sow8 =getbitu2(buff,i+ 18, 8,i+ 30,12);
i0p8 =getbitu2(buff,i+ 46, 6,i+ 60, 5);
ephem->crc =getbits2(buff,i+ 65,17,i+ 90, 1)*P2_6;
ephem->crs =getbits (buff,i+ 91,18)*P2_6;
OMGdp8 =getbits2(buff,i+109, 3,i+120,16);
i=8*38*8; /* page 9 */
pgn9 =getbitu (buff,i+ 42, 4);
sow9 =getbitu2(buff,i+ 18, 8,i+ 30,12);
OMGdp9 =getbitu (buff,i+ 46, 5);
ephem->OMG0 =getbits3(buff,i+ 51, 1,i+ 60,22,i+ 90, 9)*P2_31*SC2RAD;
omgp9 =getbits2(buff,i+ 99,13,i+120,14);
i=8*38*9; /* page 10 */
pgn10 =getbitu (buff,i+ 42, 4);
sow10 =getbitu2(buff,i+ 18, 8,i+ 30,12);
omgp10 =getbitu (buff,i+ 46, 5);
ephem->i_dot =getbits2(buff,i+ 51, 1,i+ 60,13)*P2_43*SC2RAD;
/* check consistency of page numbers, sows and toe/toc */
if (pgn1!=1||pgn3!=3||pgn4!=4||pgn5!=5||pgn6!=6||pgn7!=7||pgn8!=8||pgn9!=9||
pgn10!=10) {
LOG(ERROR) << "decode_bds_d2 error: pgn=" << pgn1 << ' ' << pgn3 << ' '
<< pgn4 << ' ' << pgn5 << ' ' << pgn6 << ' ' << pgn7 << ' '
<< pgn8 << ' ' << pgn9 << ' ' << pgn10;
return -1;
}
if (sow3!=sow1+6||sow4!=sow3+3||sow5!=sow4+3||sow6!=sow5+3||
sow7!=sow6+3||sow8!=sow7+3||sow9!=sow8+3||sow10!=sow9+3) {
LOG(ERROR) << "decode_bds_d2 error: sow=" << sow1 << ' ' << sow3 << ' '
<< sow4 << ' ' << sow5 << ' ' << sow6 << ' ' << sow7 << ' '
<< sow8 << ' ' << sow9 << ' ' << sow10;
return -1;
}
if (toc_bds!=toes) {
LOG(ERROR) << "decode_bds_d2 error: toe=" << toes << " toc=" << toc_bds;
return -1;
}
ephem->af1 = merge_two_s(f1p3 ,f1p4 ,18)*P2_50;
ephem->cuc = merge_two_s(cucp4 ,cucp5 , 4)*P2_31;
ephem->e = merge_two_s(ep5 ,ep6 ,22)*P2_33;
ephem->cic = merge_two_s(cicp6 ,cicp7 , 8)*P2_31;
ephem->i0 = merge_two_s(i0p7 ,i0p8 ,11)*P2_31*SC2RAD;
ephem->OMG_dot = merge_two_s(OMGdp8,OMGdp9, 5)*P2_43*SC2RAD;
ephem->omg = merge_two_s(omgp9 ,omgp10, 5)*P2_31*SC2RAD;
ephem->ttr = bdt2time(bdt_week,sow1);
ephem->ttr = time_add(ephem->ttr, 14.0);
if (toes>sow1+302400.0) bdt_week++;
else if (toes<sow1-302400.0) bdt_week--;
ephem->toe = bdt2time(bdt_week,toes);
ephem->toe = time_add(ephem->toe, 14.0);
ephem->toe_tow = time2gpst(ephem->toe, &ephem->week);
ephem->toc = bdt2time(bdt_week,toc_bds);
ephem->toc = time_add(ephem->toc, 14.0);
ephem->code = 0; /* data source = unknown */
curr_time = ephem->ttr; // update current time
return 0;
}
int UbloxMessageProcessor::decode_BDS_D1_ephem(EphemPtr ephem, std::vector<double> &iono_params)
{
std::vector<double> tmp_iono_params;
double toc_bds,sqrtA;
uint32_t bdt_week,toes,toe1,toe2,sow1,sow2,sow3;
int i,frn1,frn2,frn3;
// LOG(INFO) << "decode_bds_d1";
uint8_t *buff = subfrm[ephem->sat-1];
i=8*38*0; /* subframe 1 */
frn1 =getbitu (buff,i+ 15, 3);
sow1 =getbitu2(buff,i+ 18, 8,i+30,12);
ephem->health =getbitu (buff,i+ 42, 1); /* SatH1 */
ephem->iodc =getbitu (buff,i+ 43, 5); /* AODC */
uint8_t urai =getbitu (buff,i+ 48, 4);
ephem->ura = (urai < 6) ? pow(2, urai/2.0+1) : (1<<(urai-2));
bdt_week =getbitu (buff,i+ 60,13); /* week in BDT */
toc_bds =getbitu2(buff,i+ 73, 9,i+ 90, 8)*8.0;
ephem->tgd[0] =getbits (buff,i+ 98,10)*0.1*1E-9;
ephem->tgd[1] =getbits2(buff,i+108, 4,i+120, 6)*0.1*1E-9;
tmp_iono_params.push_back(getbits (buff, i+126, 8)*P2_30);
tmp_iono_params.push_back(getbits (buff, i+134, 8)*P2_27);
tmp_iono_params.push_back(getbits (buff, i+150, 8)*P2_24);
tmp_iono_params.push_back(getbits (buff, i+158, 8)*P2_24);
tmp_iono_params.push_back(getbits2(buff,i+166, 6,i+180, 2)*(1<<11));
tmp_iono_params.push_back(getbits (buff, i+182, 8)*(1<<14));
tmp_iono_params.push_back(getbits (buff, i+190, 8)*(1<<16));
tmp_iono_params.push_back(getbits2(buff,i+198, 4,i+210, 4)*(1<<16));
ephem->af2 =getbits (buff,i+214,11)*P2_66;
ephem->af0 =getbits2(buff,i+225, 7,i+240,17)*P2_33;
ephem->af1 =getbits2(buff,i+257, 5,i+270,17)*P2_50;
ephem->iode =getbitu (buff,i+287, 5); /* AODE */
i=8*38*1; /* subframe 2 */
frn2 =getbitu (buff,i+ 15, 3);
sow2 =getbitu2(buff,i+ 18, 8,i+30,12);
ephem->delta_n =getbits2(buff,i+ 42,10,i+ 60, 6)*P2_43*SC2RAD;
ephem->cuc =getbits2(buff,i+ 66,16,i+ 90, 2)*P2_31;
ephem->M0 =getbits2(buff,i+ 92,20,i+120,12)*P2_31*SC2RAD;
ephem->e =getbitu2(buff,i+132,10,i+150,22)*P2_33;
ephem->cus =getbits (buff,i+180,18)*P2_31;
ephem->crc =getbits2(buff,i+198, 4,i+210,14)*P2_6;
ephem->crs =getbits2(buff,i+224, 8,i+240,10)*P2_6;
sqrtA =getbitu2(buff,i+250,12,i+270,20)*P2_19;
toe1 =getbitu (buff,i+290, 2); /* TOE 2-MSB */
ephem->A =sqrtA*sqrtA;
i=8*38*2; /* subframe 3 */
frn3 =getbitu (buff,i+ 15, 3);
sow3 =getbitu2(buff,i+ 18, 8,i+30,12);
toe2 =getbitu2(buff,i+ 42,10,i+ 60, 5); /* TOE 5-LSB */
ephem->i0 =getbits2(buff,i+ 65,17,i+ 90,15)*P2_31*SC2RAD;
ephem->cic =getbits2(buff,i+105, 7,i+120,11)*P2_31;
ephem->OMG_dot =getbits2(buff,i+131,11,i+150,13)*P2_43*SC2RAD;
ephem->cis =getbits2(buff,i+163, 9,i+180, 9)*P2_31;
ephem->i_dot =getbits2(buff,i+189,13,i+210, 1)*P2_43*SC2RAD;
ephem->OMG0 =getbits2(buff,i+211,21,i+240,11)*P2_31*SC2RAD;
ephem->omg =getbits2(buff,i+251,11,i+270,21)*P2_31*SC2RAD;
toes =merge_two_u(toe1,toe2,15)*8.0;
/* check consistency of subframe numbers, sows and toe/toc */
if (frn1!=1||frn2!=2||frn3!=3)
{
LOG(ERROR) << "decode_bds_d1 error: frn=" << frn1 << ' ' << frn2 << ' ' << frn3;
return -1;
}
if (sow2!=sow1+6||sow3!=sow2+6)
{
// LOG(ERROR) << "decode_bds_d1 error: sow=" << sow1 << ' ' << sow2 << ' ' << sow3;
return -1;
}
if (toc_bds!=toes)
{
LOG(ERROR) << "decode_bds_d1 error: toe=" << toes << " toc=" << toc_bds;
return -1;
}
ephem->ttr = bdt2time(bdt_week,sow1);
ephem->ttr = time_add(ephem->ttr, 14.0);
if (toes>sow1+302400.0) bdt_week--;
else if (toes<sow1-302400.0) bdt_week++;
ephem->toe = bdt2time(bdt_week,toes);
ephem->toe = time_add(ephem->toe, 14.0);
ephem->toe_tow = time2gpst(ephem->toe, &(ephem->week));
ephem->toc = bdt2time(bdt_week,toc_bds);
ephem->toc = time_add(ephem->toc, 14.0);
ephem->code = 0; /* data source = unknown */
curr_time = ephem->ttr; // update current time
iono_params.swap(tmp_iono_params);
return 0;
}
int UbloxMessageProcessor::decode_GAL_subframe(const uint8_t *msg_data, const uint32_t msg_len, EphemPtr ephem)
{
const uint8_t *p = msg_data + 14;
uint8_t buff[32],crc_buff[26]={0};
int i,j,k,part1,page1,part2,page2,type;
if (msg_len < 52) // header(6) + payload_1(8) + payload_2(36) + checksum(2)
{
LOG(ERROR) << "ubx rxmsfrbx length error: sat=" << sat2str(ephem->sat) << " len=" << msg_len;
return -1;
}
for (i=k=0;i<8;i++,p+=4)
for (j=0;j<4;j++)
buff[k++]=p[3-j];
part1=getbitu(buff ,0,1);
page1=getbitu(buff ,1,1);
part2=getbitu(buff+16,0,1);
page2=getbitu(buff+16,1,1);
/* skip alert page */
if (page1==1||page2==1) return 0;
/* test even-odd parts */
if (part1!=0||part2!=1)
{
LOG(ERROR) << "ubx rawsfrbx gal page even/odd error: sat=" << ephem->sat;
return -1;
}
/* test crc (4(pad) + 114 + 82 bits) */
for (i=0,j= 4;i<15;i++,j+=8) setbitu(crc_buff,j,8,getbitu(buff ,i*8,8));
for (i=0,j=118;i<11;i++,j+=8) setbitu(crc_buff,j,8,getbitu(buff+16,i*8,8));
if (crc24q(crc_buff,25)!=getbitu(buff+16,82,24))
{
LOG(ERROR) << "ubx rawsfrbx gal page crc error: sat=" << ephem->sat;
return -1;
}
type=getbitu(buff,2,6); /* word type */
/* skip word except for ephemeris, iono, utc parameters */
if (type>6) return 0;
/* clear word 0-6 flags */
if (type==2) subfrm[ephem->sat-1][112]=0;
/* save page data (112 + 16 bits) to frame buffer */
k=type*16;
for (i=0,j=2;i<14;i++,j+=8) subfrm[ephem->sat-1][k++]=getbitu(buff ,j,8);
for (i=0,j=2;i< 2;i++,j+=8) subfrm[ephem->sat-1][k++]=getbitu(buff+16,j,8);
/* test word 0-6 flags */
subfrm[ephem->sat-1][112]|=(1<<type);
if (subfrm[ephem->sat-1][112]!=0x7F) return 0;
decode_GAL_ephem(ephem);
return 0;
}
int UbloxMessageProcessor::decode_GAL_ephem(EphemPtr ephem)
{
double tow,toc,tt,sqrtA, toes;
int i,time_f,week,svid,e5b_hs,e1b_hs,e5b_dvs,e1b_dvs,type[6],iod_nav[4];
uint8_t *buff = subfrm[ephem->sat-1];
i=0; /* word type 0 */
type[0] =getbitu(buff,i, 6); i+= 6;
time_f =getbitu(buff,i, 2); i+= 2+88;
week =getbitu(buff,i,12); i+=12; /* gst-week */
tow =getbitu(buff,i,20);
i=128; /* word type 1 */
type[1] =getbitu(buff,i, 6); i+= 6;
iod_nav[0] =getbitu(buff,i,10); i+=10;
toes =getbitu(buff,i,14)*60.0; i+=14;
ephem->M0 =getbits(buff,i,32)*P2_31*SC2RAD; i+=32;
ephem->e =getbitu(buff,i,32)*P2_33; i+=32;
sqrtA =getbitu(buff,i,32)*P2_19;
i=128*2; /* word type 2 */
type[2] =getbitu(buff,i, 6); i+= 6;
iod_nav[1] =getbitu(buff,i,10); i+=10;
ephem->OMG0=getbits(buff,i,32)*P2_31*SC2RAD; i+=32;
ephem->i0 =getbits(buff,i,32)*P2_31*SC2RAD; i+=32;
ephem->omg =getbits(buff,i,32)*P2_31*SC2RAD; i+=32;
ephem->i_dot=getbits(buff,i,14)*P2_43*SC2RAD;
i=128*3; /* word type 3 */
type[3] =getbitu(buff,i, 6); i+= 6;
iod_nav[2] =getbitu(buff,i,10); i+=10;
ephem->OMG_dot =getbits(buff,i,24)*P2_43*SC2RAD; i+=24;
ephem->delta_n =getbits(buff,i,16)*P2_43*SC2RAD; i+=16;
ephem->cuc =getbits(buff,i,16)*P2_29; i+=16;
ephem->cus =getbits(buff,i,16)*P2_29; i+=16;
ephem->crc =getbits(buff,i,16)*P2_5; i+=16;
ephem->crs =getbits(buff,i,16)*P2_5; i+=16;
uint32_t sisa =getbitu(buff,i, 8);
if (sisa < 50) ephem->ura = 0.01*sisa;
else if (sisa < 75) ephem->ura = 0.5+0.02*(sisa-50);
else if (sisa < 100) ephem->ura = 1.0+0.04*(sisa-75);
else if (sisa < 125) ephem->ura = 2.0+0.16*(sisa-100);
else ephem->ura = -1.0;
i=128*4; /* word type 4 */
type[4] =getbitu(buff,i, 6); i+= 6;
iod_nav[3] =getbitu(buff,i,10); i+=10;
svid =getbitu(buff,i, 6); i+= 6;
ephem->cic =getbits(buff,i,16)*P2_29; i+=16;
ephem->cis =getbits(buff,i,16)*P2_29; i+=16;
toc =getbitu(buff,i,14)*60.0; i+=14;
ephem->af0 =getbits(buff,i,31)*P2_34; i+=31;
ephem->af1 =getbits(buff,i,21)*P2_46; i+=21;
ephem->af2 =getbits(buff,i, 6)*P2_59;
i=128*5; /* word type 5 */
type[5] =getbitu(buff,i, 6); i+= 6+41;
ephem->tgd[0] =getbits(buff,i,10)*P2_32; i+=10; /* BGD E5a/E1 */
ephem->tgd[1] =getbits(buff,i,10)*P2_32; i+=10; /* BGD E5b/E1 */
e5b_hs =getbitu(buff,i, 2); i+= 2;
e1b_hs =getbitu(buff,i, 2); i+= 2;
e5b_dvs =getbitu(buff,i, 1); i+= 1;
e1b_dvs =getbitu(buff,i, 1);
/* test word types */
if (type[0]!=0||type[1]!=1||type[2]!=2||type[3]!=3||type[4]!=4||type[5]!=5)
{
LOG(ERROR) << "decode gal inav error: type=" << type[0] << ' ' << type[1]
<< ' ' << type[2] << ' ' << ' ' << type[3] << ' ' << type[4]
<< ' ' << type[5];
return -1;
}
/* test word type 0 time field */
if (time_f!=2)
{
LOG(ERROR) << "decode_gal_inav error: word0-time=" << time_f;
return -1;
}
/* test consistency of iod_nav */
if (iod_nav[0]!=iod_nav[1]||iod_nav[0]!=iod_nav[2]||iod_nav[0]!=iod_nav[3]) {
LOG(ERROR) << "decode_gal_inav error: ionav=" << iod_nav[0] << ' '
<< iod_nav[1] << ' ' << iod_nav[2] << ' ' << iod_nav[3];
return 0;
}
if (ephem->sat != sat_no(SYS_GAL,svid))
{
LOG(ERROR) << "decode_gal_inav svid error: svid=" << svid;
return 0;
}
ephem->A=sqrtA*sqrtA;
ephem->iode=ephem->iodc=iod_nav[0];
ephem->health=(e5b_hs<<7)|(e5b_dvs<<6)|(e1b_hs<<1)|e1b_dvs;
ephem->ttr=gst2time(week,tow);
tt=time_diff(gst2time(week,toes),ephem->ttr); /* week complient to toe */
if (tt> 302400.0) week--;
else if (tt<-302400.0) week++;
ephem->toe=gst2time(week,toes);
ephem->toc=gst2time(week,toc);
ephem->toe_tow = time2gpst(ephem->toe, &ephem->week);
ephem->code =(1<<0)|(1<<9); /* data source = i/nav e1b, af0-2,toc,sisa for e5b-e1 */
curr_time = ephem->ttr; // update current time
memset(subfrm[ephem->sat-1], 0, 380);
return 0;
}
int UbloxMessageProcessor::decode_GPS_subframe(const uint8_t *msg_data, const uint32_t msg_len, EphemPtr ephem)
{
uint32_t words[10];
if (msg_len < 56) // header(6) + payload_1(8) + payload_2(40) + checksum(2)
{
LOG(ERROR) << "ubx rxmsfrbx length error: sat=" << ephem->sat << " len=" << msg_len;
return -1;
}
const uint8_t *p = msg_data + 14;
if ((*reinterpret_cast<const uint32_t*>(p))>>24 == PREAMB_CNAV)
{
// LOG(INFO) << "ubx rxmsfrbx CNAV not supported sat=" << ephem->sat; // suppress info
return -1;
}
for (size_t i = 0; i < 10; i++, p+=4)
words[i] = (*reinterpret_cast<const uint32_t*>(p)) >> 6; // 24 bits without parity
uint32_t subframe_id = (words[1]>>2) & 0x07;
if (subframe_id < 1 || subframe_id > 5)
{
LOG(ERROR) << "ubx rxmsfrbx subframe id error: sat=" << ephem->sat << " id=" << subframe_id;
return -1;
}
// LOG(INFO) << "sat is " << ephem->sat << ", GPS subframe id is " << subframe_id;
uint32_t subframe_pos = (subframe_id-1) * 30; // 240 bits for each subframe
// now set bits to subframe buffer
for (size_t i = 0; i < 10; i++)
setbitu(subfrm[ephem->sat]+subframe_pos, 24*i, 24, words[i]);
if (subframe_id == 3)
decode_GPS_ephem(ephem);
return 0;
}
int UbloxMessageProcessor::decode_GPS_ephem(EphemPtr ephem)
{
uint8_t *frame_buf = subfrm[ephem->sat];
// check if subframe1,2,3 are received or not
for (uint32_t i = 0; i < 3; ++i)
{
if (getbitu(frame_buf+30*i, 43, 3) != i+1)
return static_cast<int>(i+1);
if (i != 0 && (getbitu(frame_buf+30*i-30, 24, 17)+1) != getbitu(frame_buf+30*i, 24, 17))
return -2; // z count discontinue
}
// LOG(FATAL) << "decoding GPS ephem";
// decode subframe 1
double tow = getbitu(frame_buf, 24, 17) * 6.0;
uint32_t off = 48;
uint32_t week = getbitu(frame_buf, off, 10); off += 10;
off += 2; // skip signal code
uint8_t urai = getbitu(frame_buf, off, 4); off += 4;
ephem->ura = (urai < 6) ? pow(2, urai/2.0+1) : (1<<(urai-2));
ephem->health = getbitu(frame_buf, off, 6); off += 6;
uint32_t iodc0 = getbitu(frame_buf, off, 2); off += 2;
off += 1+87;
int tgd = getbits(frame_buf, off, 8); off += 8;
uint32_t iodc1 = getbitu(frame_buf, off, 8); off += 8;
double toc = getbitu(frame_buf, off, 16) * 16.0; off += 16;
ephem->af2 = getbits(frame_buf, off, 8) * P2_55; off += 8;
ephem->af1 = getbits(frame_buf, off, 16) * P2_43; off += 16;
ephem->af0 = getbits(frame_buf, off, 22) * P2_31;
ephem->tgd[0] = (tgd == -128 ? 0.0 : tgd*P2_31);
ephem->iodc = (iodc0<<8) + iodc1;
// ephem->week = adjgpsweek(week);
ephem->week = week + 1024 * GPS_WEEK_ROLLOVER_N; // adjust GPS week
ephem->ttr = gpst2time(ephem->week, tow);
ephem->toc = gpst2time(ephem->week, toc);
// decode subframe 2
frame_buf += 30;
off = 48;
ephem->iode = getbitu(frame_buf, off, 8); off += 8;
ephem->crs = getbits(frame_buf, off, 16) * P2_5; off += 16;
ephem->delta_n = getbits(frame_buf, off, 16) * P2_43*SC2RAD; off += 16;
ephem->M0 = getbits(frame_buf, off, 32) * P2_31*SC2RAD; off += 32;
ephem->cuc = getbits(frame_buf, off, 16) * P2_29; off += 16;
ephem->e = getbitu(frame_buf, off, 32) * P2_33; off += 32;
ephem->cus = getbits(frame_buf, off, 16) * P2_29; off += 16;
double sqrtA = getbitu(frame_buf, off, 32) * P2_19; off += 32;
ephem->toe_tow = getbitu(frame_buf, off, 16) * 16.0; off += 16;
ephem->A = sqrtA * sqrtA;
// decode subframe 3
frame_buf += 30;
off = 48;
ephem->cic = getbits(frame_buf, off, 16) * P2_29; off += 16;
ephem->OMG0 = getbits(frame_buf, off, 32) * P2_31*SC2RAD; off += 32;
ephem->cis = getbits(frame_buf, off, 16) * P2_29; off += 16;
ephem->i0 = getbits(frame_buf, off, 32) * P2_31*SC2RAD; off += 32;
ephem->crc = getbits(frame_buf, off, 16) * P2_5; off += 16;
ephem->omg = getbits(frame_buf, off, 32) * P2_31*SC2RAD; off += 32;
ephem->OMG_dot = getbits(frame_buf, off, 24) * P2_43*SC2RAD; off += 24;
uint32_t iode = getbitu(frame_buf, off, 8); off += 8;
ephem->i_dot = getbits(frame_buf, off, 14) * P2_43*SC2RAD;
/* check iode and iodc consistency */
if (iode != ephem->iode || iode != (ephem->iodc & 0xFF)) return -1;
/* adjustment for week handover */
tow = time2gpst(ephem->ttr, &ephem->week);
toc = time2gpst(ephem->toc, NULL);
if (ephem->toe_tow < tow-302400.0) {ephem->week++; tow -= 604800.0;}
else if (ephem->toe_tow > tow+302400.0) {ephem->week--; tow += 604800.0;}
ephem->toe = gpst2time(ephem->week, ephem->toe_tow);
ephem->toc = gpst2time(ephem->week, toc);
ephem->ttr = gpst2time(ephem->week, tow);
curr_time = ephem->ttr; // update current time
return 0;
}
int UbloxMessageProcessor::build_config_msg(const std::vector<RcvConfigRecord> &rcv_configs,
uint8_t *buff, uint32_t &msg_len)
{
const std::map<std::string, std::pair<uint32_t, std::string>> rcv_config_item2code =
{
{"CFG-MSGOUT-UBX_NAV_HPPOSECEF" , {0x2091002f, "U1"}},
{"CFG-MSGOUT-UBX_NAV_HPPOSLLH" , {0x20910034, "U1"}},
{"CFG-MSGOUT-UBX_NAV_POSECEF" , {0x20910025, "U1"}},
{"CFG-MSGOUT-UBX_NAV_POSLLH" , {0x2091002A, "U1"}},
{"CFG-MSGOUT-UBX_NAV_PVT" , {0x20910007, "U1"}},
{"CFG-MSGOUT-UBX_NAV_SAT" , {0x20910016, "U1"}},
{"CFG-MSGOUT-UBX_NAV_STATUS" , {0x2091001B, "U1"}},
{"CFG-MSGOUT-UBX_NAV_VELECEF" , {0x2091003E, "U1"}},
{"CFG-MSGOUT-UBX_NAV_VELNED" , {0x20910043, "U1"}},
{"CFG-MSGOUT-UBX_RXM_RAWX" , {0x209102A5, "U1"}},
{"CFG-MSGOUT-UBX_RXM_SFRBX" , {0x20910232, "U1"}},
{"CFG-NAVSPG-DYNMODEL" , {0x20110021, "E1"}},
{"CFG-RATE-MEAS" , {0x30210001, "U2"}},
{"CFG-RATE-NAV" , {0x30210002, "U2"}},
{"CFG-RATE-TIMEREF" , {0x20210003, "E1"}},
{"CFG-SIGNAL-GPS_ENA" , {0x1031001F, "L" }},
{"CFG-SIGNAL-GPS_L1CA_ENA" , {0x10310001, "L" }},
{"CFG-SIGNAL-GPS_L2CA_ENA" , {0x10310003, "L" }},
{"CFG-SIGNAL-GAL_ENA" , {0x10310021, "L" }},
{"CFG-SIGNAL-GAL_E1_ENA" , {0x10310007, "L" }},
{"CFG-SIGNAL-GAL_E5B_ENA" , {0x1031000A, "L" }},
{"CFG-SIGNAL-BDS_ENA" , {0x10310022, "L" }},
{"CFG-SIGNAL-BDS_B1_ENA" , {0x1031000D, "L" }},
{"CFG-SIGNAL-BDS_B2_ENA" , {0x1031000E, "L" }},
{"CFG-SIGNAL-QZSS_ENA" , {0x10310024, "L" }},
{"CFG-SIGNAL-QZSS_L1CA_ENA" , {0x10310012, "L" }},
{"CFG-SIGNAL-QZSS_L2C_ENA" , {0x10310015, "L" }},
{"CFG-SIGNAL-GLO_ENA" , {0x10310025, "L" }},
{"CFG-SIGNAL-GLO_L1_ENA" , {0x10310018, "L" }},
{"CFG-SIGNAL-GLO_L2_ENA" , {0x1031001A, "L" }},
{"CFG-UART1-BAUDRATE" , {0x40520001, "U4"}},
{"CFG-UART1-STOPBITS" , {0x20520002, "E1"}},
{"CFG-UART1-DATABITS" , {0x20520003, "E1"}},
{"CFG-UART1-PARITY" , {0x20520004, "E1"}},
{"CFG-UART1-ENABLED" , {0x10520005, "L" }},
{"CFG-UART1INPROT-UBX" , {0x10730001, "L" }},
{"CFG-UART1INPROT-NMEA" , {0x10730002, "L" }},
{"CFG-UART1INPROT-RTCM3X" , {0x10730004, "L" }},
{"CFG-UART1OUTPROT-UBX" , {0x10740001, "L" }},
{"CFG-UART1OUTPROT-NMEA" , {0x10740002, "L" }},
{"CFG-UART1OUTPROT-RTCM3X" , {0x10740004, "L" }},
{"CFG-UART2-BAUDRATE" , {0x40530001, "U4"}},
{"CFG-UART2-STOPBITS" , {0x20530002, "E1"}},
{"CFG-UART2-DATABITS" , {0x20530003, "E1"}},
{"CFG-UART2-PARITY" , {0x20530004, "E1"}},
{"CFG-UART2-ENABLED" , {0x10530005, "L" }},
{"CFG-UART2INPROT-UBX" , {0x10750001, "L" }},
{"CFG-UART2INPROT-NMEA" , {0x10750002, "L" }},
{"CFG-UART2INPROT-RTCM3X" , {0x10750004, "L" }},
{"CFG-UART2OUTPROT-UBX" , {0x10760001, "L" }},
{"CFG-UART2OUTPROT-NMEA" , {0x10760002, "L" }},
{"CFG-UART2OUTPROT-RTCM3X" , {0x10760004, "L" }},
{"CFG-USB-ENABLED" , {0x10650001, "L" }},
{"CFG-USBINPROT-UBX" , {0x10770001, "L" }},
{"CFG-USBINPROT-NMEA" , {0x10770002, "L" }},
{"CFG-USBINPROT-RTCM3X" , {0x10770004, "L" }},
{"CFG-USBOUTPROT-UBX" , {0x10780001, "L" }},
{"CFG-USBOUTPROT-NMEA" , {0x10780002, "L" }},
{"CFG-USBOUTPROT-RTCM3X" , {0x10780004, "L" }},
{"CFG-TP-PULSE_DEF" , {0x20050023, "E1" }},
};
const std::map<std::string, uint8_t> rcv_config_type2code =
{
{"U1", 1 },
{"U2", 2 },
{"U4", 3 },
{"U8", 4 },
{"E1", 5 },
{"E2", 6 },
{"E4", 7 },
{"X1", 8 },
{"X2", 9 },
{"X4", 10},
{"X8", 11},
{"L" , 12},
{"I1", 13},
{"I2", 14},
{"I4", 15},
{"I8", 16},
{"R4", 17},
{"R8", 18},
};
buff[0] = UBX_SYNC_1;
buff[1] = UBX_SYNC_2;
buff[2] = 0x06;
buff[3] = 0x8A; // UBX-CFG-VALSET
buff[6] = 0; // Message version, set to 0 for transactionless, 1 for transaction
buff[7] = 1; // apply config to RAM(0) layer. NOTE: RAM(1<<0), BBR(1<<1), Flash(1<<2)
buff[8] = 0;
buff[9] = 0;
uint8_t *p = buff + 10;
uint16_t payload_len = 4;
for(const RcvConfigRecord &record : rcv_configs) // for each config record
{
const std::string &record_key = record.first;
const std::string &record_value_str = record.second;
if (record_key.empty())
{
LOG(ERROR) << "Empty record key.";
return -1;
}
uint32_t code = 0;
std::string data_type;
if (rcv_config_item2code.count(record_key) != 0)
{
code = rcv_config_item2code.at(record_key).first;
data_type = rcv_config_item2code.at(record_key).second;
}
else if (record_key.rfind("CFG-MSGOUT-UBX_NAV_", 0) != std::string::npos)
{
size_t delimiter_pos = record_key.find_last_of('_');
std::string msgout_type = record_key.substr(0, delimiter_pos);
if (rcv_config_item2code.count(msgout_type) == 0)
{
LOG(ERROR) << "Not supported command type: " << record_key;
return -1;
}
if (record_key.substr(delimiter_pos+1) == "UART1")
code = rcv_config_item2code.at(msgout_type).first;
else if (record_key.substr(delimiter_pos+1) == "UART2")
code = rcv_config_item2code.at(msgout_type).first + 1;
else if (record_key.substr(delimiter_pos+1) == "USB")
code = rcv_config_item2code.at(msgout_type).first + 2;
data_type = rcv_config_item2code.at(msgout_type).second;
}
else
{
LOG(ERROR) << "Not supported command type: " << record_key;
return -1;
}
uint8_t value_size = static_cast<uint8_t>((code>>28)&0x07);
uint8_t value_num_bytes = (value_size>2 ? (1<<(value_size-2)) : 1);
uint8_t *value_bytes;
int64_t value_u;
int64_t value_i;
float value_f;
double value_d;
switch(rcv_config_type2code.at(data_type))
{
case 1 :
case 2 :
case 3 :
case 4 :
case 5 :
case 6 :
case 7 :
case 8 :
case 9 :
case 10:
case 11:
case 12:
value_u = stoull(record_value_str);
value_bytes = reinterpret_cast<uint8_t*>(&value_u);
break;
case 13:
case 14:
case 15:
case 16:
value_i = stoll(record_value_str);
value_bytes = reinterpret_cast<uint8_t*>(&value_i);
break;
case 17:
value_f = stof(record_value_str);
value_bytes = reinterpret_cast<uint8_t*>(&value_f);
break;
case 18:
value_d = stod(record_value_str);
value_bytes = reinterpret_cast<uint8_t*>(&value_d);
break;
default:
LOG(INFO) << "Unrecognized date type: " << data_type;
break;
}
memcpy(p, reinterpret_cast<uint8_t*>(&code), 4); // write config bytes
p += 4;
payload_len += 4;
memcpy(p, value_bytes, value_num_bytes);
p += value_num_bytes;
payload_len += value_num_bytes;
}
memcpy(buff+4, reinterpret_cast<uint8_t*>(&payload_len), 2);
msg_len = 6 + payload_len + 2;
set_checksum(buff, msg_len);
return 0;
}
double UbloxMessageProcessor::sig_freq(const int sys, const int sid, const int fcn)
{
static const double freq_glo[8]={FREQ1_GLO,FREQ2_GLO,FREQ3_GLO};
static const double dfrq_glo[8]={DFRQ1_GLO,DFRQ2_GLO};
static const double freq_bds[8]={FREQ1_BDS,FREQ2_BDS,FREQ3_BDS,FREQ2_BDS};
if (sys == SYS_GLO) {
return freq_glo[sid-1]+dfrq_glo[sid-1]*fcn;
}
else if (sys == SYS_BDS) {
return freq_bds[sid-1];
}
return LIGHT_SPEED/lam_carr[sid-1];
}
int UbloxMessageProcessor::sig_idx(const int sys, const int code)
{
if (sys == SYS_GPS) {
if (code==CODE_L1C) return 1;
if (code==CODE_L2L) return 2;
if (code==CODE_L2M) return N_FREQ+1;
}
else if (sys == SYS_GLO) {
if (code==CODE_L1C) return 1;
if (code==CODE_L2C) return 2;
}
else if (sys == SYS_GAL) {
if (code==CODE_L1C) return 1;
if (code==CODE_L1B) return N_FREQ+1;
if (code==CODE_L7I) return 2; /* E5bI */
if (code==CODE_L7Q) return 2; /* E5bQ */
}
else if (sys == SYS_QZS) {
if (code==CODE_L1C) return 1;
if (code==CODE_L2L) return 2;
}
else if (sys == SYS_BDS) {
if (code==CODE_L1I) return 1;
if (code==CODE_L7I) return 2;
}
else if (sys == SYS_SBS) {
if (code==CODE_L1C) return 1;
}
return 0;
}
int UbloxMessageProcessor::ubx_sig(const int sys, const int sigid)
{
if (sys == SYS_GPS) {
if (sigid == 0) return CODE_L1C; /* L1C/A */
if (sigid == 3) return CODE_L2L; /* L2CL */
if (sigid == 4) return CODE_L2M; /* L2CM */
}
else if (sys == SYS_GLO) {
if (sigid == 0) return CODE_L1C; /* G1C/A (GLO L1 OF) */
if (sigid == 2) return CODE_L2C; /* G2C/A (GLO L2 OF) */
}
else if (sys == SYS_GAL) {
if (sigid == 0) return CODE_L1C; /* E1C */
if (sigid == 1) return CODE_L1B; /* E1B */
if (sigid == 5) return CODE_L7I; /* E5bI */
if (sigid == 6) return CODE_L7Q; /* E5bQ */
}
else if (sys == SYS_QZS) {
if (sigid == 0) return CODE_L1C; /* L1C/A */
if (sigid == 5) return CODE_L2L; /* L2CL (not specified in [5]) */
}
else if (sys == SYS_BDS) {
if (sigid == 0) return CODE_L1I; /* B1I D1 (rinex 3.02) */
if (sigid == 1) return CODE_L1I; /* B1I D2 (rinex 3.02) */
if (sigid == 2) return CODE_L7I; /* B2I D1 */
if (sigid == 3) return CODE_L7I; /* B2I D2 */
}
else if (sys == SYS_SBS) {
return CODE_L1C; /* L1C/A (not in [5]) */
}
return CODE_NONE;
}
int UbloxMessageProcessor::ubx_sys(const int gnssid)
{
switch (gnssid) {
case 0: return SYS_GPS;
case 2: return SYS_GAL;
case 3: return SYS_BDS;
case 6: return SYS_GLO;
}
return 0;
}
int UbloxMessageProcessor::test_glostr(const uint8_t *data)
{
static const uint8_t xor_8bit[256]={ /* xor of 8 bits */
0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,
1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,
0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,
0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0
};
static const uint8_t mask_hamming[][12]={ /* mask of hamming codes */
{0x55,0x55,0x5A,0xAA,0xAA,0xAA,0xB5,0x55,0x6A,0xD8,0x08},
{0x66,0x66,0x6C,0xCC,0xCC,0xCC,0xD9,0x99,0xB3,0x68,0x10},
{0x87,0x87,0x8F,0x0F,0x0F,0x0F,0x1E,0x1E,0x3C,0x70,0x20},
{0x07,0xF8,0x0F,0xF0,0x0F,0xF0,0x1F,0xE0,0x3F,0x80,0x40},
{0xF8,0x00,0x0F,0xFF,0xF0,0x00,0x1F,0xFF,0xC0,0x00,0x80},
{0x00,0x00,0x0F,0xFF,0xFF,0xFF,0xE0,0x00,0x00,0x01,0x00},
{0xFF,0xFF,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x00},
{0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xF8}
};
uint8_t cs = 0;
int n = 0;
for (uint32_t i = 0; i < 8; ++i)
{
for (uint32_t j = 0, cs = 0; j < 11; ++j)
cs ^= xor_8bit[data[j]&mask_hamming[i][j]];
if (cs) n++;
}
return n==0 || (n==2 && cs);
}
uint32_t UbloxMessageProcessor::crc24q(const uint8_t *data, const uint32_t size) const
{
uint32_t crc = 0;
for (uint32_t i = 0; i < size; ++i)
crc = ( (crc<<8) & 0xFFFFFF) ^ tbl_CRC24Q[ (crc>>16) ^ data[i]];
return crc;
}
bool UbloxMessageProcessor::check_checksum(const uint8_t *data, const uint32_t size)
{
uint8_t cka = 0, ckb = 0;
for (uint32_t i = 2;i < size-2; ++i)
{
cka += data[i];
ckb += cka;
}
return cka == data[size-2] && ckb == data[size-1];
}
void UbloxMessageProcessor::set_checksum(uint8_t *data, uint32_t size)
{
uint8_t cka = 0, ckb = 0;
for (uint32_t i = 2; i < size-2; ++i)
{
cka += data[i];
ckb += cka;
}
data[size-2] = cka;
data[size-1] = ckb;
}
void UbloxMessageProcessor::setbitu(uint8_t *buff, const uint32_t pos, const uint32_t len, uint32_t data) const
{
if (len == 0 || len > 32) return;
uint32_t mask = 1u << (len-1);
for (uint32_t i = pos; i < pos+len; ++i, mask>>=1)
{
if (data & mask)
buff[i/8] |= (1u<<(7-i%8));
else
buff[i/8] &= ~(1u<<(7-i%8));
}
}
uint32_t UbloxMessageProcessor::getbitu(const uint8_t *buff, const uint32_t pos, const uint32_t len) const
{
uint32_t bits = 0;
for (size_t i = pos; i < pos+len; ++i)
bits = (bits<<1) + ((buff[i/8]>>(7-i%8))&1u);
return bits;
}
int UbloxMessageProcessor::getbits(const uint8_t *buff, const uint32_t pos, const uint32_t len) const
{
uint32_t bits = getbitu(buff, pos, len);
if (len == 0 || len >= 32 || !(bits&(1u<<(len-1)))) return (int)bits;
return (int)(bits|(~0u<<len)); // extend sign
}
uint32_t UbloxMessageProcessor::getbitu2(const uint8_t *buff, const uint32_t p1, const uint32_t l1,
const uint32_t p2, const uint32_t l2) const
{
return (getbitu(buff,p1,l1)<<l2)+getbitu(buff,p2,l2);
}
int UbloxMessageProcessor::getbits2(const uint8_t *buff, const uint32_t p1, const uint32_t l1,
const uint32_t p2, const uint32_t l2) const
{
if (getbitu(buff,p1,1))
return (int)((getbits(buff,p1,l1)<<l2)+getbitu(buff,p2,l2));
else
return (int)getbitu2(buff,p1,l1,p2,l2);
}
uint32_t UbloxMessageProcessor::getbitu3(const uint8_t *buff, const uint32_t p1, const uint32_t l1,
const uint32_t p2, const uint32_t l2, const uint32_t p3, const uint32_t l3) const
{
return (getbitu(buff,p1,l1)<<(l2+l3))+(getbitu(buff,p2,l2)<<l3)+ getbitu(buff,p3,l3);
}
int UbloxMessageProcessor::getbits3(const uint8_t *buff, const uint32_t p1, const uint32_t l1, const uint32_t p2,
const uint32_t l2, const uint32_t p3, const uint32_t l3) const
{
if (getbitu(buff,p1,1))
return (int)((getbits(buff,p1,l1)<<(l2+l3))+
(getbitu(buff,p2,l2)<<l3)+getbitu(buff,p3,l3));
else
return (int)getbitu3(buff,p1,l1,p2,l2,p3,l3);
}
double UbloxMessageProcessor::getbitg(const uint8_t *buff, const int pos, const int len) const
{
double value=getbitu(buff,pos+1,len-1);
return getbitu(buff,pos,1)?-value:value;
}
uint32_t UbloxMessageProcessor::merge_two_u(const uint32_t a, const uint32_t b, const uint32_t n) const
{
return (a<<n)+b;
}
int UbloxMessageProcessor::merge_two_s(const int a, const uint8_t b, const int n) const
{
return (int)((a<<n)+b);
}
这段代码是ublox-driver软件包的一部分，特别是处理u-blox GNSS接收器数据的UbloxMessageProcessor类。代码主要功能如下：
-
处理和解析GNSS数据：代码可以处理和解析多种GNSS数据格式，包括GPS、GLONASS、Galileo和BeiDou系统的数据。
-
GNSS数据解码：能够解码各种GNSS信号，如GPS L1C/A、GLONASS L1 OF等，以及不同系统的导航信息。
-
校验和计算：实现了校验和的计算，确保接收到的数据是正确无误的。
-
配置信息构建：能夠构建和处理u-blox接收器的配置信息，例如更改波特率、使能某个频段的信号等。
-
支持多种协议：代码支持UBX、NMEA、RTCM3等多种通信协议，以及各种消息类型。
总体而言，这段代码是一个复杂且功能丰富的GNSS数据处理库，用于接收和处理来自u-blox接收器的多种类型的GNSS数据。
在代码中，可供用户灵活配置的参数主要通过RcvConfigRecord变量传递，这个变量是std::vector容器，其中包含配置记录，每个记录是一个std::pair<std::string, std::string>。第一个string是配置项的名称，第二个string是配置项的值。配置项的名称遵循Ublox配置协议的标准，例如：
CFG-MSGOUT-UBX_NAV_PVT：控制导航位置速度时间解的输出。CFG-RATE-MEAS：设置测量速率。CFG-SIGNAL-GPS_ENA：启用或禁用GPS信号。
这些参数允许用户定义如何设置接收器以及控制消息输出和信号处理的具体行为。配置值的具体内容应符合Ublox接收器的规格。
在提供的代码中，支持多种协议主要体现在以下几个方面：
-
配置消息输出：代码通过配置项
CFG-MSGOUT-UBX_NAV_*、CFG-MSGOUT-UBX_RXM_*等，可以控制UBX协议下的不同消息类型的输出。 -
多协议输入输出配置：代码包含了对UART1和UART2端口的配置，可以设定哪些协议的消息可以通过这些端口输入或输出。这些配置项包括
CFG-UART1INPROT-*和CFG-UART1OUTPROT-*，分别用于控制输入和输出协议，协议类型包括UBX、NMEA和RTCM3X。 -
对不同卫星系统的处理：代码中包含了对不同卫星系统（如GPS、GLONASS、Galileo、BDS等）的处理逻辑，包括信号频率的计算、消息的解析等。
这些元素表明，代码被设计为支持多种通信协议，能够处理和输出各种类型的导航和定位相关数据。
这段代码是
UbloxMessageProcessor类的一个核心部分，名为process_data。它处理从U-blox GPS接收器接收到的数据，并根据数据类型发布不同的消息。以下是对这个函数的逐步解释：
- 验证接收到的消息:
verify_msg(data, len)：这个函数用来验证接收到的数据是否有效。它检查数据的完整性和格式。如果数据无效，函数会直接返回。
- 消息类型的解析:
const uint16_t msg_type = (data[2]<<8) | data[3];：这行代码提取消息类型。它使用位操作从数据的第3和第4个字节中提取消息类型。
- 处理不同类型的消息:
根据
msg_type的值，函数执行不同的操作：
RXM-RAWX (原始测量数据): 如果消息类型是
UBX_RXMRAWX_ID，函数解析原始测量数据，并发布测量结果。RXM-SFRBX (星历数据): 如果消息类型是
UBX_RXMSFRBX_ID，函数解析星历数据。它可能会发布格洛纳斯星历信息GnssGloEphemMsg或其他系统的星历信息GnssEphemMsg。如果数据中包含电离层参数，它还会发布这些参数。TIM-TP (时间脉冲信息): 如果消息类型是
UBX_TIM_TP_ID，函数解析时间脉冲信息，并发布相关的消息。NAV-POSLLH (经纬高信息): 如果消息类型是
UBX_NAVPOS_ID，函数解析位置解算结果。它可能会调整位置信息（如果适用），并发布两种类型的消息：一种是PVT解算结果，另一种是导航卫星定位信息。
- 发布消息:
- 通过
publish方法，函数根据解析的数据类型发布不同的ROS消息。这些消息包括原始GPS测量值、星历信息、时间脉冲信息和位置信息。
- 未支持的消息类型:
- 如果数据类型不属于上述任何一种，函数将不执行任何操作并返回。
这个函数的主要作用是将接收到的原始GPS数据转换为对应的信息（如测量值、星历、时间脉冲和位置信息），并通过ROS话题发布这些信息，以便其他系统或节点使用。
解析PVT信息
在代码片段中，当接收到的消息类型是UBX_NAVPOS_ID，表示这是一个NAV-POSLLH消息，即关于位置（经度、纬度和高度）的信息。下面是对这部分代码的详细解释，特别关注于PVT（Position, Velocity, Time，即位置、速度、时间）解析函数：
-
解析PVT数据:
PVTSolutionPtr pvt_soln = parse_pvt(data, len);: 这一行调用parse_pvt函数来解析PVT数据。这个函数接收原始的二进制数据和长度作为参数，然后解析出GPS的PVT解算结果。
-
PVT解析函数的工作原理:
parse_pvt函数通常解析下列数据：- GPS时间
- 经纬度坐标
- 海拔高度
- 地面速度
- 位置的准确度估计
- 解算类型（如单点定位、差分GPS等）
-
数据有效性检查:
- 函数首先检查解析出来的PVT解是否有效。如果无效（如时间戳为0），函数会直接返回。
-
应用RTK站偏移:
- 如果解算结果显示使用了RTK（实时动态差分定位），那么会根据配置调整解算结果。这通常涉及将RTK站的坐标偏移应用于原始PVT解。
-
发布PVT消息:
GnssPVTSolnMsg pvt_msg = pvt2msg(pvt_soln);: 这一行将PVT解算结果转换为适合发布的消息格式，并通过pub_pvt_话题发布。
-
发布导航卫星定位信息:
- 接下来的几行代码创建并填充
NavSatFix消息，这是ROS标准消息类型，包含经纬度、高度以及定位的状态和服务类型。然后，通过pub_lla_话题发布这个消息。
- 接下来的几行代码创建并填充
-
返回:
- 函数在处理完PVT消息后返回，不再执行任何其他操作。
parse_pvt函数是GPS数据解析过程中的关键部分，它提取并解释从卫星接收到的定位数据，然后以一种格式化的方式提供这些信息，使得其他系统或应用程序可以利用这些定位数据。
解析RAWX信息
在UbloxMessageProcessor::process_data函数中，当接收到的消息类型是UBX_RXMRAWX_ID，代表这是一个RXM-RAWX消息，即关于原始测量数据的信息。这部分代码主要涉及解析原始测量数据并发布这些数据。以下是对这部分代码的详细解释，尤其是关于RAWX数据的解析：
-
解析RAWX数据:
std::vector<ObsPtr> meas = parse_meas_msg(data, len);: 这一行调用parse_meas_msg函数来解析RAWX数据。这个函数接收原始的二进制数据和长度作为参数，然后解析出GPS的原始测量结果。
-
RAWX解析函数的工作原理:
parse_meas_msg函数通常解析以下数据：- 测量时刻
- 每颗卫星的伪距、载波相位、多普勒频移和信噪比
- 接收到的卫星的信息，包括卫星系统（如GPS、GLONASS等）、卫星编号等
-
数据有效性检查:
- 解析函数首先检查解析出来的测量数据是否有效。如果无效（如没有测量数据），函数会直接返回。
-
构造测量消息并发布:
GnssMeasMsg meas_msg = meas2msg(meas);: 这一行将原始测量数据转换为适合发布的消息格式。pub_range_meas_.publish(meas_msg);: 然后通过pub_range_meas_话题发布这个消息。
-
返回:
- 函数在处理完RAWX消息后返回，不再执行任何其他操作。
parse_meas_msg函数是关键的数据解析部分，它提取并解释从卫星接收到的原始测量数据。这些数据是了解卫星信号状态的重要资源，对于精确定位和高级GNSS应用至关重要。通过解析并发布这些数据，UbloxMessageProcessor类使得其他系统或应用程序可以访问和使用这些原始测量信息。
解析SFRBX信息
在UbloxMessageProcessor::process_data函数中，当接收到的消息类型是UBX_RXMSFRBX_ID，表示这是一个RXM-SFRBX消息，即关于星历数据的信息。以下是对这部分代码的详细解释，尤其是关于SFRBX数据的解析：
-
解析SFRBX数据:
EphemBasePtr ephem = parse_subframe(data, len, iono_params);: 这一行调用parse_subframe函数来解析SFRBX数据。这个函数接收原始的二进制数据和长度作为参数，然后解析出卫星星历数据。
-
SFRBX解析函数的工作原理:
parse_subframe函数根据接收到的数据来解析不同GNSS系统（如GPS、GLONASS、Galileo、BDS）的星历数据。- 函数还可能解析电离层参数，这些参数用于卫星信号的电离层延迟校正。
-
发布星历消息:
- 根据解析出的星历所属的卫星系统，函数会构造并发布相应的星历消息：
- 对于GLONASS系统的星历，发布
GnssGloEphemMsg消息。 - 对于GPS、Galileo、BDS等其他系统的星历，发布
GnssEphemMsg消息。
- 对于GLONASS系统的星历，发布
- 这些消息通过
pub_glo_ephem_或pub_ephem_话题进行发布。
- 根据解析出的星历所属的卫星系统，函数会构造并发布相应的星历消息：
-
发布电离层参数:
- 如果解析出电离层参数，且星历数据有效，则构造并发布电离层参数的消息。
- 这些参数通过
pub_iono_话题发布，它们对于理解和校正卫星信号在电离层中的行为非常重要。
-
返回:
- 函数在处理完SFRBX消息后返回，不再执行任何其他操作。
parse_subframe函数是GPS数据处理中的重要部分，它不仅提取星历数据，这些数据对于精确导航至关重要，还可能提取电离层参数，这对于提高定位精度和可靠性至关重要。通过解析并发布这些数据，UbloxMessageProcessor类使得其他系统或应用程序可以访问和使用这些重要的GNSS信息。
Yaml.cpp
/*
* MIT License
*
* Copyright(c) 2018 Jimmie Bergmann
*
* 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 "Yaml.hpp"
#include <memory>
#include <fstream>
#include <sstream>
#include <vector>
#include <list>
#include <cstdio>
#include <stdarg.h>
// Implementation access definitions.
#define NODE_IMP static_cast<NodeImp*>(m_pImp)
#define NODE_IMP_EXT(node) static_cast<NodeImp*>(node.m_pImp)
#define TYPE_IMP static_cast<NodeImp*>(m_pImp)->m_pImp
#define IT_IMP static_cast<IteratorImp*>(m_pImp)
namespace Yaml
{
class ReaderLine;
// Exception message definitions.
static const std::string g_ErrorInvalidCharacter = "Invalid character found.";
static const std::string g_ErrorKeyMissing = "Missing key.";
static const std::string g_ErrorKeyIncorrect = "Incorrect key.";
static const std::string g_ErrorValueIncorrect = "Incorrect value.";
static const std::string g_ErrorTabInOffset = "Tab found in offset.";
static const std::string g_ErrorBlockSequenceNotAllowed = "Sequence entries are not allowed in this context.";
static const std::string g_ErrorUnexpectedDocumentEnd = "Unexpected document end.";
static const std::string g_ErrorDiffEntryNotAllowed = "Different entry is not allowed in this context.";
static const std::string g_ErrorIncorrectOffset = "Incorrect offset.";
static const std::string g_ErrorSequenceError = "Error in sequence node.";
static const std::string g_ErrorCannotOpenFile = "Cannot open file.";
static const std::string g_ErrorIndentation = "Space indentation is less than 2.";
static const std::string g_ErrorInvalidBlockScalar = "Invalid block scalar.";
static const std::string g_ErrorInvalidQuote = "Invalid quote.";
static const std::string g_EmptyString = "";
static Yaml::Node g_NoneNode;
// Global function definitions. Implemented at end of this source file.
static std::string ExceptionMessage(const std::string & message, ReaderLine & line);
static std::string ExceptionMessage(const std::string & message, ReaderLine & line, const size_t errorPos);
static std::string ExceptionMessage(const std::string & message, const size_t errorLine, const size_t errorPos);
static std::string ExceptionMessage(const std::string & message, const size_t errorLine, const std::string & data);
static bool FindQuote(const std::string & input, size_t & start, size_t & end, size_t searchPos = 0);
static size_t FindNotCited(const std::string & input, char token, size_t & preQuoteCount);
static size_t FindNotCited(const std::string & input, char token);
static bool ValidateQuote(const std::string & input);
static void CopyNode(const Node & from, Node & to);
static bool ShouldBeCited(const std::string & key);
static void AddEscapeTokens(std::string & input, const std::string & tokens);
static void RemoveAllEscapeTokens(std::string & input);
// Exception implementations
Exception::Exception(const std::string & message, const eType type) :
std::runtime_error(message),
m_Type(type)
{
}
Exception::eType Exception::Type() const
{
return m_Type;
}
const char * Exception::Message() const
{
return what();
}
InternalException::InternalException(const std::string & message) :
Exception(message, InternalError)
{
}
ParsingException::ParsingException(const std::string & message) :
Exception(message, ParsingError)
{
}
OperationException::OperationException(const std::string & message) :
Exception(message, OperationError)
{
}
class TypeImp
{
public:
virtual ~TypeImp()
{
}
virtual const std::string & GetData() const = 0;
virtual bool SetData(const std::string & data) = 0;
virtual size_t GetSize() const = 0;
virtual Node * GetNode(const size_t index) = 0;
virtual Node * GetNode(const std::string & key) = 0;
virtual Node * Insert(const size_t index) = 0;
virtual Node * PushFront() = 0;
virtual Node * PushBack() = 0;
virtual void Erase(const size_t index) = 0;
virtual void Erase(const std::string & key) = 0;
};
class SequenceImp : public TypeImp
{
public:
~SequenceImp()
{
for(auto it = m_Sequence.begin(); it != m_Sequence.end(); it++)
{
delete it->second;
}
}
virtual const std::string & GetData() const
{
return g_EmptyString;
}
virtual bool SetData(const std::string & data)
{
return false;
}
virtual size_t GetSize() const
{
return m_Sequence.size();
}
virtual Node * GetNode(const size_t index)
{
auto it = m_Sequence.find(index);
if(it != m_Sequence.end())
{
return it->second;
}
return nullptr;
}
virtual Node * GetNode(const std::string & key)
{
return nullptr;
}
virtual Node * Insert(const size_t index)
{
if(m_Sequence.size() == 0)
{
Node * pNode = new Node;
m_Sequence.insert({0, pNode});
return pNode;
}
if(index >= m_Sequence.size())
{
auto it = m_Sequence.end();
--it;
Node * pNode = new Node;
m_Sequence.insert({it->first, pNode});
return pNode;
}
auto it = m_Sequence.cbegin();
while(it != m_Sequence.cend())
{
m_Sequence[it->first+1] = it->second;
if(it->first == index)
{
break;
}
}
Node * pNode = new Node;
m_Sequence.insert({index, pNode});
return pNode;
}
virtual Node * PushFront()
{
for(auto it = m_Sequence.cbegin(); it != m_Sequence.cend(); it++)
{
m_Sequence[it->first+1] = it->second;
}
Node * pNode = new Node;
m_Sequence.insert({0, pNode});
return pNode;
}
virtual Node * PushBack()
{
size_t index = 0;
if(m_Sequence.size())
{
auto it = m_Sequence.end();
--it;
index = it->first + 1;
}
Node * pNode = new Node;
m_Sequence.insert({index, pNode});
return pNode;
}
virtual void Erase(const size_t index)
{
auto it = m_Sequence.find(index);
if(it == m_Sequence.end())
{
return;
}
delete it->second;
m_Sequence.erase(index);
}
virtual void Erase(const std::string & key)
{
}
std::map<size_t, Node*> m_Sequence;
};
class MapImp : public TypeImp
{
public:
~MapImp()
{
for(auto it = m_Map.begin(); it != m_Map.end(); it++)
{
delete it->second;
}
}
virtual const std::string & GetData() const
{
return g_EmptyString;
}
virtual bool SetData(const std::string & data)
{
return false;
}
virtual size_t GetSize() const
{
return m_Map.size();
}
virtual Node * GetNode(const size_t index)
{
return nullptr;
}
virtual Node * GetNode(const std::string & key)
{
auto it = m_Map.find(key);
if(it == m_Map.end())
{
Node * pNode = new Node;
m_Map.insert({key, pNode});
return pNode;
}
return it->second;
}
virtual Node * Insert(const size_t index)
{
return nullptr;
}
virtual Node * PushFront()
{
return nullptr;
}
virtual Node * PushBack()
{
return nullptr;
}
virtual void Erase(const size_t index)
{
}
virtual void Erase(const std::string & key)
{
auto it = m_Map.find(key);
if(it == m_Map.end())
{
return;
}
delete it->second;
m_Map.erase(key);
}
std::map<std::string, Node*> m_Map;
};
class ScalarImp : public TypeImp
{
public:
~ScalarImp()
{
}
virtual const std::string & GetData() const
{
return m_Value;
}
virtual bool SetData(const std::string & data)
{
m_Value = data;
return true;
}
virtual size_t GetSize() const
{
return 0;
}
virtual Node * GetNode(const size_t index)
{
return nullptr;
}
virtual Node * GetNode(const std::string & key)
{
return nullptr;
}
virtual Node * Insert(const size_t index)
{
return nullptr;
}
virtual Node * PushFront()
{
return nullptr;
}
virtual Node * PushBack()
{
return nullptr;
}
virtual void Erase(const size_t index)
{
}
virtual void Erase(const std::string & key)
{
}
std::string m_Value;
};
// Node implementations.
class NodeImp
{
public:
NodeImp() :
m_Type(Node::None),
m_pImp(nullptr)
{
}
~NodeImp()
{
Clear();
}
void Clear()
{
if(m_pImp != nullptr)
{
delete m_pImp;
m_pImp = nullptr;
}
m_Type = Node::None;
}
void InitSequence()
{
if(m_Type != Node::SequenceType || m_pImp == nullptr)
{
if(m_pImp)
{
delete m_pImp;
}
m_pImp = new SequenceImp;
m_Type = Node::SequenceType;
}
}
void InitMap()
{
if(m_Type != Node::MapType || m_pImp == nullptr)
{
if(m_pImp)
{
delete m_pImp;
}
m_pImp = new MapImp;
m_Type = Node::MapType;
}
}
void InitScalar()
{
if(m_Type != Node::ScalarType || m_pImp == nullptr)
{
if(m_pImp)
{
delete m_pImp;
}
m_pImp = new ScalarImp;
m_Type = Node::ScalarType;
}
}
Node::eType m_Type; ///< Type of node.
TypeImp * m_pImp; ///< Imp of type.
};
// Iterator implementation class
class IteratorImp
{
public:
virtual ~IteratorImp()
{
}
virtual Node::eType GetType() const = 0;
virtual void InitBegin(SequenceImp * pSequenceImp) = 0;
virtual void InitEnd(SequenceImp * pSequenceImp) = 0;
virtual void InitBegin(MapImp * pMapImp) = 0;
virtual void InitEnd(MapImp * pMapImp) = 0;
};
class SequenceIteratorImp : public IteratorImp
{
public:
virtual Node::eType GetType() const
{
return Node::SequenceType;
}
virtual void InitBegin(SequenceImp * pSequenceImp)
{
m_Iterator = pSequenceImp->m_Sequence.begin();
}
virtual void InitEnd(SequenceImp * pSequenceImp)
{
m_Iterator = pSequenceImp->m_Sequence.end();
}
virtual void InitBegin(MapImp * pMapImp)
{
}
virtual void InitEnd(MapImp * pMapImp)
{
}
void Copy(const SequenceIteratorImp & it)
{
m_Iterator = it.m_Iterator;
}
std::map<size_t, Node *>::iterator m_Iterator;
};
class MapIteratorImp : public IteratorImp
{
public:
virtual Node::eType GetType() const
{
return Node::MapType;
}
virtual void InitBegin(SequenceImp * pSequenceImp)
{
}
virtual void InitEnd(SequenceImp * pSequenceImp)
{
}
virtual void InitBegin(MapImp * pMapImp)
{
m_Iterator = pMapImp->m_Map.begin();
}
virtual void InitEnd(MapImp * pMapImp)
{
m_Iterator = pMapImp->m_Map.end();
}
void Copy(const MapIteratorImp & it)
{
m_Iterator = it.m_Iterator;
}
std::map<std::string, Node *>::iterator m_Iterator;
};
class SequenceConstIteratorImp : public IteratorImp
{
public:
virtual Node::eType GetType() const
{
return Node::SequenceType;
}
virtual void InitBegin(SequenceImp * pSequenceImp)
{
m_Iterator = pSequenceImp->m_Sequence.begin();
}
virtual void InitEnd(SequenceImp * pSequenceImp)
{
m_Iterator = pSequenceImp->m_Sequence.end();
}
virtual void InitBegin(MapImp * pMapImp)
{
}
virtual void InitEnd(MapImp * pMapImp)
{
}
void Copy(const SequenceConstIteratorImp & it)
{
m_Iterator = it.m_Iterator;
}
std::map<size_t, Node *>::const_iterator m_Iterator;
};
class MapConstIteratorImp : public IteratorImp
{
public:
virtual Node::eType GetType() const
{
return Node::MapType;
}
virtual void InitBegin(SequenceImp * pSequenceImp)
{
}
virtual void InitEnd(SequenceImp * pSequenceImp)
{
}
virtual void InitBegin(MapImp * pMapImp)
{
m_Iterator = pMapImp->m_Map.begin();
}
virtual void InitEnd(MapImp * pMapImp)
{
m_Iterator = pMapImp->m_Map.end();
}
void Copy(const MapConstIteratorImp & it)
{
m_Iterator = it.m_Iterator;
}
std::map<std::string, Node *>::const_iterator m_Iterator;
};
// Iterator class
Iterator::Iterator() :
m_Type(None),
m_pImp(nullptr)
{
}
Iterator::~Iterator()
{
if(m_pImp)
{
switch(m_Type)
{
case SequenceType:
delete static_cast<SequenceIteratorImp*>(m_pImp);
break;
case MapType:
delete static_cast<MapIteratorImp*>(m_pImp);
break;
default:
break;
}
}
}
Iterator::Iterator(const Iterator & it)
{
*this = it;
}
Iterator & Iterator::operator = (const Iterator & it)
{
if(m_pImp)
{
switch(m_Type)
{
case SequenceType:
delete static_cast<SequenceIteratorImp*>(m_pImp);
break;
case MapType:
delete static_cast<MapIteratorImp*>(m_pImp);
break;
default:
break;
}
m_pImp = nullptr;
m_Type = None;
}
IteratorImp * pNewImp = nullptr;
switch(it.m_Type)
{
case SequenceType:
m_Type = SequenceType;
pNewImp = new SequenceIteratorImp;
static_cast<SequenceIteratorImp*>(pNewImp)->m_Iterator = static_cast<SequenceIteratorImp*>(it.m_pImp)->m_Iterator;
break;
case MapType:
m_Type = MapType;
pNewImp = new MapIteratorImp;
static_cast<MapIteratorImp*>(pNewImp)->m_Iterator = static_cast<MapIteratorImp*>(it.m_pImp)->m_Iterator;
break;
default:
break;
}
m_pImp = pNewImp;
return *this;
}
std::pair<const std::string &, Node &> Iterator::operator *()
{
switch(m_Type)
{
case SequenceType:
return {"", *(static_cast<SequenceIteratorImp*>(m_pImp)->m_Iterator->second)};
break;
case MapType:
return {static_cast<MapIteratorImp*>(m_pImp)->m_Iterator->first,
*(static_cast<MapIteratorImp*>(m_pImp)->m_Iterator->second)};
break;
default:
break;
}
g_NoneNode.Clear();
return {"", g_NoneNode};
}
Iterator & Iterator::operator ++ (int dummy)
{
switch(m_Type)
{
case SequenceType:
static_cast<SequenceIteratorImp*>(m_pImp)->m_Iterator++;
break;
case MapType:
static_cast<MapIteratorImp*>(m_pImp)->m_Iterator++;
break;
default:
break;
}
return *this;
}
Iterator & Iterator::operator -- (int dummy)
{
switch(m_Type)
{
case SequenceType:
static_cast<SequenceIteratorImp*>(m_pImp)->m_Iterator--;
break;
case MapType:
static_cast<MapIteratorImp*>(m_pImp)->m_Iterator--;
break;
default:
break;
}
return *this;
}
bool Iterator::operator == (const Iterator & it)
{
if(m_Type != it.m_Type)
{
return false;
}
switch(m_Type)
{
case SequenceType:
return static_cast<SequenceIteratorImp*>(m_pImp)->m_Iterator == static_cast<SequenceIteratorImp*>(it.m_pImp)->m_Iterator;
break;
case MapType:
return static_cast<MapIteratorImp*>(m_pImp)->m_Iterator == static_cast<MapIteratorImp*>(it.m_pImp)->m_Iterator;
break;
default:
break;
}
return false;
}
bool Iterator::operator != (const Iterator & it)
{
return !(*this == it);
}
// Const Iterator class
ConstIterator::ConstIterator() :
m_Type(None),
m_pImp(nullptr)
{
}
ConstIterator::~ConstIterator()
{
if(m_pImp)
{
switch(m_Type)
{
case SequenceType:
delete static_cast<SequenceConstIteratorImp*>(m_pImp);
break;
case MapType:
delete static_cast<MapConstIteratorImp*>(m_pImp);
break;
default:
break;
}
}
}
ConstIterator::ConstIterator(const ConstIterator & it)
{
*this = it;
}
ConstIterator & ConstIterator::operator = (const ConstIterator & it)
{
if(m_pImp)
{
switch(m_Type)
{
case SequenceType:
delete static_cast<SequenceConstIteratorImp*>(m_pImp);
break;
case MapType:
delete static_cast<MapConstIteratorImp*>(m_pImp);
break;
default:
break;
}
m_pImp = nullptr;
m_Type = None;
}
IteratorImp * pNewImp = nullptr;
switch(it.m_Type)
{
case SequenceType:
m_Type = SequenceType;
pNewImp = new SequenceConstIteratorImp;
static_cast<SequenceConstIteratorImp*>(pNewImp)->m_Iterator = static_cast<SequenceConstIteratorImp*>(it.m_pImp)->m_Iterator;
break;
case MapType:
m_Type = MapType;
pNewImp = new MapConstIteratorImp;
static_cast<MapConstIteratorImp*>(pNewImp)->m_Iterator = static_cast<MapConstIteratorImp*>(it.m_pImp)->m_Iterator;
break;
default:
break;
}
m_pImp = pNewImp;
return *this;
}
std::pair<const std::string &, const Node &> ConstIterator::operator *()
{
switch(m_Type)
{
case SequenceType:
return {"", *(static_cast<SequenceConstIteratorImp*>(m_pImp)->m_Iterator->second)};
break;
case MapType:
return {static_cast<MapConstIteratorImp*>(m_pImp)->m_Iterator->first,
*(static_cast<MapConstIteratorImp*>(m_pImp)->m_Iterator->second)};
break;
default:
break;
}
g_NoneNode.Clear();
return {"", g_NoneNode};
}
ConstIterator & ConstIterator::operator ++ (int dummy)
{
switch(m_Type)
{
case SequenceType:
static_cast<SequenceConstIteratorImp*>(m_pImp)->m_Iterator++;
break;
case MapType:
static_cast<MapConstIteratorImp*>(m_pImp)->m_Iterator++;
break;
default:
break;
}
return *this;
}
ConstIterator & ConstIterator::operator -- (int dummy)
{
switch(m_Type)
{
case SequenceType:
static_cast<SequenceConstIteratorImp*>(m_pImp)->m_Iterator--;
break;
case MapType:
static_cast<MapConstIteratorImp*>(m_pImp)->m_Iterator--;
break;
default:
break;
}
return *this;
}
bool ConstIterator::operator == (const ConstIterator & it)
{
if(m_Type != it.m_Type)
{
return false;
}
switch(m_Type)
{
case SequenceType:
return static_cast<SequenceConstIteratorImp*>(m_pImp)->m_Iterator == static_cast<SequenceConstIteratorImp*>(it.m_pImp)->m_Iterator;
break;
case MapType:
return static_cast<MapConstIteratorImp*>(m_pImp)->m_Iterator == static_cast<MapConstIteratorImp*>(it.m_pImp)->m_Iterator;
break;
default:
break;
}
return false;
}
bool ConstIterator::operator != (const ConstIterator & it)
{
return !(*this == it);
}
// Node class
Node::Node() :
m_pImp(new NodeImp)
{
}
Node::Node(const Node & node) :
Node()
{
*this = node;
}
Node::Node(const std::string & value) :
Node()
{
*this = value;
}
Node::Node(const char * value) :
Node()
{
*this = value;
}
Node::~Node()
{
delete static_cast<NodeImp*>(m_pImp);
}
Node::eType Node::Type() const
{
return NODE_IMP->m_Type;
}
bool Node::IsNone() const
{
return NODE_IMP->m_Type == Node::None;
}
bool Node::IsSequence() const
{
return NODE_IMP->m_Type == Node::SequenceType;
}
bool Node::IsMap() const
{
return NODE_IMP->m_Type == Node::MapType;
}
bool Node::IsScalar() const
{
return NODE_IMP->m_Type == Node::ScalarType;
}
void Node::Clear()
{
NODE_IMP->Clear();
}
size_t Node::Size() const
{
if(TYPE_IMP == nullptr)
{
return 0;
}
return TYPE_IMP->GetSize();
}
Node & Node::Insert(const size_t index)
{
NODE_IMP->InitSequence();
return *TYPE_IMP->Insert(index);
}
Node & Node::PushFront()
{
NODE_IMP->InitSequence();
return *TYPE_IMP->PushFront();
}
Node & Node::PushBack()
{
NODE_IMP->InitSequence();
return *TYPE_IMP->PushBack();
}
Node & Node::operator[](const size_t index)
{
NODE_IMP->InitSequence();
Node * pNode = TYPE_IMP->GetNode(index);
if(pNode == nullptr)
{
g_NoneNode.Clear();
return g_NoneNode;
}
return *pNode;
}
Node & Node::operator[](const std::string & key)
{
NODE_IMP->InitMap();
return *TYPE_IMP->GetNode(key);
}
void Node::Erase(const size_t index)
{
if(TYPE_IMP == nullptr || NODE_IMP->m_Type != Node::SequenceType)
{
return;
}
return TYPE_IMP->Erase(index);
}
void Node::Erase(const std::string & key)
{
if(TYPE_IMP == nullptr || NODE_IMP->m_Type != Node::MapType)
{
return;
}
return TYPE_IMP->Erase(key);
}
Node & Node::operator = (const Node & node)
{
NODE_IMP->Clear();
CopyNode(node, *this);
return *this;
}
Node & Node::operator = (const std::string & value)
{
NODE_IMP->InitScalar();
TYPE_IMP->SetData(value);
return *this;
}
Node & Node::operator = (const char * value)
{
NODE_IMP->InitScalar();
TYPE_IMP->SetData(value ? std::string(value) : "");
return *this;
}
Iterator Node::Begin()
{
Iterator it;
if(TYPE_IMP != nullptr)
{
IteratorImp * pItImp = nullptr;
switch(NODE_IMP->m_Type)
{
case Node::SequenceType:
it.m_Type = Iterator::SequenceType;
pItImp = new SequenceIteratorImp;
pItImp->InitBegin(static_cast<SequenceImp*>(TYPE_IMP));
break;
case Node::MapType:
it.m_Type = Iterator::MapType;
pItImp = new MapIteratorImp;
pItImp->InitBegin(static_cast<MapImp*>(TYPE_IMP));
break;
default:
break;
}
it.m_pImp = pItImp;
}
return it;
}
ConstIterator Node::Begin() const
{
ConstIterator it;
if(TYPE_IMP != nullptr)
{
IteratorImp * pItImp = nullptr;
switch(NODE_IMP->m_Type)
{
case Node::SequenceType:
it.m_Type = ConstIterator::SequenceType;
pItImp = new SequenceConstIteratorImp;
pItImp->InitBegin(static_cast<SequenceImp*>(TYPE_IMP));
break;
case Node::MapType:
it.m_Type = ConstIterator::MapType;
pItImp = new MapConstIteratorImp;
pItImp->InitBegin(static_cast<MapImp*>(TYPE_IMP));
break;
default:
break;
}
it.m_pImp = pItImp;
}
return it;
}
Iterator Node::End()
{
Iterator it;
if(TYPE_IMP != nullptr)
{
IteratorImp * pItImp = nullptr;
switch(NODE_IMP->m_Type)
{
case Node::SequenceType:
it.m_Type = Iterator::SequenceType;
pItImp = new SequenceIteratorImp;
pItImp->InitEnd(static_cast<SequenceImp*>(TYPE_IMP));
break;
case Node::MapType:
it.m_Type = Iterator::MapType;
pItImp = new MapIteratorImp;
pItImp->InitEnd(static_cast<MapImp*>(TYPE_IMP));
break;
default:
break;
}
it.m_pImp = pItImp;
}
return it;
}
ConstIterator Node::End() const
{
ConstIterator it;
if(TYPE_IMP != nullptr)
{
IteratorImp * pItImp = nullptr;
switch(NODE_IMP->m_Type)
{
case Node::SequenceType:
it.m_Type = ConstIterator::SequenceType;
pItImp = new SequenceConstIteratorImp;
pItImp->InitEnd(static_cast<SequenceImp*>(TYPE_IMP));
break;
case Node::MapType:
it.m_Type = ConstIterator::MapType;
pItImp = new MapConstIteratorImp;
pItImp->InitEnd(static_cast<MapImp*>(TYPE_IMP));
break;
default:
break;
}
it.m_pImp = pItImp;
}
return it;
}
const std::string & Node::AsString() const
{
if(TYPE_IMP == nullptr)
{
return g_EmptyString;
}
return TYPE_IMP->GetData();
}
// Reader implementations
/**
* @breif Line information structure.
*
*/
class ReaderLine
{
public:
/**
* @breif Constructor.
*
*/
ReaderLine(const std::string & data = "",
const size_t no = 0,
const size_t offset = 0,
const Node::eType type = Node::None,
const unsigned char flags = 0) :
Data(data),
No(no),
Offset(offset),
Type(type),
Flags(flags),
NextLine(nullptr)
{
}
enum eFlag
{
LiteralScalarFlag, ///< Literal scalar type, defined as "|".
FoldedScalarFlag, ///< Folded scalar type, defined as "<".
ScalarNewlineFlag ///< Scalar ends with a newline.
};
/**
* @breif Set flag.
*
*/
void SetFlag(const eFlag flag)
{
Flags |= FlagMask[static_cast<size_t>(flag)];
}
/**
* @breif Set flags by mask value.
*
*/
void SetFlags(const unsigned char flags)
{
Flags |= flags;
}
/**
* @breif Unset flag.
*
*/
void UnsetFlag(const eFlag flag)
{
Flags &= ~FlagMask[static_cast<size_t>(flag)];
}
/**
* @breif Unset flags by mask value.
*
*/
void UnsetFlags(const unsigned char flags)
{
Flags &= ~flags;
}
/**
* @breif Get flag value.
*
*/
bool GetFlag(const eFlag flag) const
{
return Flags & FlagMask[static_cast<size_t>(flag)];
}
/**
* @breif Copy and replace scalar flags from another ReaderLine.
*
*/
void CopyScalarFlags(ReaderLine * from)
{
if (from == nullptr)
{
return;
}
unsigned char newFlags = from->Flags & (FlagMask[0] | FlagMask[1] | FlagMask[2]);
Flags |= newFlags;
}
static const unsigned char FlagMask[3];
std::string Data; ///< Data of line.
size_t No; ///< Line number.
size_t Offset; ///< Offset to first character in data.
Node::eType Type; ///< Type of line.
unsigned char Flags; ///< Flags of line.
ReaderLine * NextLine; ///< Pointer to next line.
};
const unsigned char ReaderLine::FlagMask[3] = { 0x01, 0x02, 0x04 };
/**
* @breif Implementation class of Yaml parsing.
* Parsing incoming stream and outputs a root node.
*
*/
class ParseImp
{
public:
/**
* @breif Default constructor.
*
*/
ParseImp()
{
}
/**
* @breif Destructor.
*
*/
~ParseImp()
{
ClearLines();
}
/**
* @breif Run full parsing procedure.
*
*/
void Parse(Node & root, std::iostream & stream)
{
try
{
root.Clear();
ReadLines(stream);
PostProcessLines();
//Print();
ParseRoot(root);
}
catch(Exception e)
{
root.Clear();
throw;
}
}
private:
/**
* @breif Copy constructor.
*
*/
ParseImp(const ParseImp & copy)
{
}
/**
* @breif Read all lines.
* Ignoring:
* - Empty lines.
* - Comments.
* - Document start/end.
*
*/
void ReadLines(std::iostream & stream)
{
std::string line = "";
size_t lineNo = 0;
bool documentStartFound = false;
bool foundFirstNotEmpty = false;
std::streampos streamPos = 0;
// Read all lines, as long as the stream is ok.
while (!stream.eof() && !stream.fail())
{
// Read line
streamPos = stream.tellg();
std::getline(stream, line);
lineNo++;
// Remove comment
const size_t commentPos = FindNotCited(line, '#');
if(commentPos != std::string::npos)
{
line.resize(commentPos);
}
// Start of document.
if (documentStartFound == false && line == "---")
{
// Erase all lines before this line.
ClearLines();
documentStartFound = true;
continue;
}
// End of document.
if (line == "...")
{
break;
}
else if(line == "---")
{
stream.seekg(streamPos);
break;
}
// Remove trailing return.
if (line.size())
{
if (line[line.size() - 1] == '\r')
{
line.resize(line.size() - 1);
}
}
// Validate characters.
for (size_t i = 0; i < line.size(); i++)
{
if (line[i] != '\t' && (line[i] < 32 || line[i] > 126))
{
throw ParsingException(ExceptionMessage(g_ErrorInvalidCharacter, lineNo, i + 1));
}
}
// Validate tabs
const size_t firstTabPos = line.find_first_of('\t');
size_t startOffset = line.find_first_not_of(" \t");
// Make sure no tabs are in the very front.
if (startOffset != std::string::npos)
{
if(firstTabPos < startOffset)
{
throw ParsingException(ExceptionMessage(g_ErrorTabInOffset, lineNo, firstTabPos));
}
// Remove front spaces.
line = line.substr(startOffset);
}
else
{
startOffset = 0;
line = "";
}
// Add line.
if(foundFirstNotEmpty == false)
{
if(line.size())
{
foundFirstNotEmpty = true;
}
else
{
continue;
}
}
ReaderLine * pLine = new ReaderLine(line, lineNo, startOffset);
m_Lines.push_back(pLine);
}
}
/**
* @breif Run post-processing on all lines.
* Basically split lines into multiple lines if needed, to follow the parsing algorithm.
*
*/
void PostProcessLines()
{
for (auto it = m_Lines.begin(); it != m_Lines.end();)
{
// Sequence.
if (PostProcessSequenceLine(it) == true)
{
continue;
}
// Mapping.
if (PostProcessMappingLine(it) == true)
{
continue;
}
// Scalar.
PostProcessScalarLine(it);
}
// Set next line of all lines.
if (m_Lines.size())
{
if (m_Lines.back()->Type != Node::ScalarType)
{
throw ParsingException(ExceptionMessage(g_ErrorUnexpectedDocumentEnd, *m_Lines.back()));
}
if (m_Lines.size() > 1)
{
auto prevEnd = m_Lines.end();
--prevEnd;
for (auto it = m_Lines.begin(); it != prevEnd; it++)
{
auto nextIt = it;
++nextIt;
(*it)->NextLine = *nextIt;
}
}
}
}
/**
* @breif Run post-processing and check for sequence.
* Split line into two lines if sequence token is not on it's own line.
*
* @return true if line is sequence, else false.
*
*/
bool PostProcessSequenceLine(std::list<ReaderLine *>::iterator & it)
{
ReaderLine * pLine = *it;
// Sequence split
if (IsSequenceStart(pLine->Data) == false)
{
return false;
}
pLine->Type = Node::SequenceType;
ClearTrailingEmptyLines(++it);
const size_t valueStart = pLine->Data.find_first_not_of(" \t", 1);
if (valueStart == std::string::npos)
{
return true;
}
// Create new line and insert
std::string newLine = pLine->Data.substr(valueStart);
it = m_Lines.insert(it, new ReaderLine(newLine, pLine->No, pLine->Offset + valueStart));
pLine->Data = "";
return false;
}
/**
* @breif Run post-processing and check for mapping.
* Split line into two lines if mapping value is not on it's own line.
*
* @return true if line is mapping, else move on to scalar parsing.
*
*/
bool PostProcessMappingLine(std::list<ReaderLine *>::iterator & it)
{
ReaderLine * pLine = *it;
// Find map key.
size_t preKeyQuotes = 0;
size_t tokenPos = FindNotCited(pLine->Data, ':', preKeyQuotes);
if (tokenPos == std::string::npos)
{
return false;
}
if(preKeyQuotes > 1)
{
throw ParsingException(ExceptionMessage(g_ErrorKeyIncorrect, *pLine));
}
pLine->Type = Node::MapType;
// Get key
std::string key = pLine->Data.substr(0, tokenPos);
const size_t keyEnd = key.find_last_not_of(" \t");
if (keyEnd == std::string::npos)
{
throw ParsingException(ExceptionMessage(g_ErrorKeyMissing, *pLine));
}
key.resize(keyEnd + 1);
// Handle cited key.
if(preKeyQuotes == 1)
{
if(key.front() != '"' || key.back() != '"')
{
throw ParsingException(ExceptionMessage(g_ErrorKeyIncorrect, *pLine));
}
key = key.substr(1, key.size() - 2);
}
RemoveAllEscapeTokens(key);
// Get value
std::string value = "";
size_t valueStart = std::string::npos;
if (tokenPos + 1 != pLine->Data.size())
{
valueStart = pLine->Data.find_first_not_of(" \t", tokenPos + 1);
if (valueStart != std::string::npos)
{
value = pLine->Data.substr(valueStart);
}
}
// Make sure the value is not a sequence start.
if (IsSequenceStart(value) == true)
{
throw ParsingException(ExceptionMessage(g_ErrorBlockSequenceNotAllowed, *pLine, valueStart));
}
pLine->Data = key;
// Remove all empty lines after map key.
ClearTrailingEmptyLines(++it);
// Add new empty line?
size_t newLineOffset = valueStart;
if(newLineOffset == std::string::npos)
{
if(it != m_Lines.end() && (*it)->Offset > pLine->Offset)
{
return true;
}
newLineOffset = tokenPos + 2;
}
else
{
newLineOffset += pLine->Offset;
}
// Add new line with value.
unsigned char dummyBlockFlags = 0;
if(IsBlockScalar(value, pLine->No, dummyBlockFlags) == true)
{
newLineOffset = pLine->Offset;
}
ReaderLine * pNewLine = new ReaderLine(value, pLine->No, newLineOffset, Node::ScalarType);
it = m_Lines.insert(it, pNewLine);
// Return false in order to handle next line(scalar value).
return false;
}
/**
* @breif Run post-processing and check for scalar.
* Checking for multi-line scalars.
*
* @return true if scalar search should continue, else false.
*
*/
void PostProcessScalarLine(std::list<ReaderLine *>::iterator & it)
{
ReaderLine * pLine = *it;
pLine->Type = Node::ScalarType;
size_t parentOffset = pLine->Offset;
if(pLine != m_Lines.front())
{
std::list<ReaderLine *>::iterator lastIt = it;
--lastIt;
parentOffset = (*lastIt)->Offset;
}
std::list<ReaderLine *>::iterator lastNotEmpty = it++;
// Find last empty lines
while(it != m_Lines.end())
{
pLine = *it;
pLine->Type = Node::ScalarType;
if(pLine->Data.size())
{
if(pLine->Offset <= parentOffset)
{
break;
}
else
{
lastNotEmpty = it;
}
}
++it;
}
ClearTrailingEmptyLines(++lastNotEmpty);
}
/**
* @breif Process root node and start of document.
*
*/
void ParseRoot(Node & root)
{
// Get first line and start type.
auto it = m_Lines.begin();
if(it == m_Lines.end())
{
return;
}
Node::eType type = (*it)->Type;
ReaderLine * pLine = *it;
// Handle next line.
switch(type)
{
case Node::SequenceType:
ParseSequence(root, it);
break;
case Node::MapType:
ParseMap(root, it);
break;
case Node::ScalarType:
ParseScalar(root, it);
break;
default:
break;
}
if(it != m_Lines.end())
{
throw InternalException(ExceptionMessage(g_ErrorUnexpectedDocumentEnd, *pLine));
}
}
/**
* @breif Process sequence node.
*
*/
void ParseSequence(Node & node, std::list<ReaderLine *>::iterator & it)
{
ReaderLine * pNextLine = nullptr;
while(it != m_Lines.end())
{
ReaderLine * pLine = *it;
Node & childNode = node.PushBack();
// Move to next line, error check.
++it;
if(it == m_Lines.end())
{
throw InternalException(ExceptionMessage(g_ErrorUnexpectedDocumentEnd, *pLine));
}
// Handle value of map
Node::eType valueType = (*it)->Type;
switch(valueType)
{
case Node::SequenceType:
ParseSequence(childNode, it);
break;
case Node::MapType:
ParseMap(childNode, it);
break;
case Node::ScalarType:
ParseScalar(childNode, it);
break;
default:
break;
}
// Check next line. if sequence and correct level, go on, else exit.
// If same level but but of type map = error.
if(it == m_Lines.end() || ((pNextLine = *it)->Offset < pLine->Offset))
{
break;
}
if(pNextLine->Offset > pLine->Offset)
{
throw ParsingException(ExceptionMessage(g_ErrorIncorrectOffset, *pNextLine));
}
if(pNextLine->Type != Node::SequenceType)
{
throw InternalException(ExceptionMessage(g_ErrorDiffEntryNotAllowed, *pNextLine));
}
}
}
/**
* @breif Process map node.
*
*/
void ParseMap(Node & node, std::list<ReaderLine *>::iterator & it)
{
ReaderLine * pNextLine = nullptr;
while(it != m_Lines.end())
{
ReaderLine * pLine = *it;
Node & childNode = node[pLine->Data];
// Move to next line, error check.
++it;
if(it == m_Lines.end())
{
throw InternalException(ExceptionMessage(g_ErrorUnexpectedDocumentEnd, *pLine));
}
// Handle value of map
Node::eType valueType = (*it)->Type;
switch(valueType)
{
case Node::SequenceType:
ParseSequence(childNode, it);
break;
case Node::MapType:
ParseMap(childNode, it);
break;
case Node::ScalarType:
ParseScalar(childNode, it);
break;
default:
break;
}
// Check next line. if map and correct level, go on, else exit.
// if same level but but of type map = error.
if(it == m_Lines.end() || ((pNextLine = *it)->Offset < pLine->Offset))
{
break;
}
if(pNextLine->Offset > pLine->Offset)
{
throw ParsingException(ExceptionMessage(g_ErrorIncorrectOffset, *pNextLine));
}
if(pNextLine->Type != pLine->Type)
{
throw InternalException(ExceptionMessage(g_ErrorDiffEntryNotAllowed, *pNextLine));
}
}
}
/**
* @breif Process scalar node.
*
*/
void ParseScalar(Node & node, std::list<ReaderLine *>::iterator & it)
{
std::string data = "";
ReaderLine * pFirstLine = *it;
ReaderLine * pLine = *it;
// Check if current line is a block scalar.
unsigned char blockFlags = 0;
bool isBlockScalar = IsBlockScalar(pLine->Data, pLine->No, blockFlags);
const bool newLineFlag = static_cast<bool>(blockFlags & ReaderLine::FlagMask[static_cast<size_t>(ReaderLine::ScalarNewlineFlag)]);
const bool foldedFlag = static_cast<bool>(blockFlags & ReaderLine::FlagMask[static_cast<size_t>(ReaderLine::FoldedScalarFlag)]);
const bool literalFlag = static_cast<bool>(blockFlags & ReaderLine::FlagMask[static_cast<size_t>(ReaderLine::LiteralScalarFlag)]);
size_t parentOffset = 0;
// Find parent offset
if(it != m_Lines.begin())
{
std::list<ReaderLine *>::iterator parentIt = it;
--parentIt;
parentOffset = (*parentIt)->Offset;
}
// Move to next iterator/line if current line is a block scalar.
if(isBlockScalar)
{
++it;
if(it == m_Lines.end() || (pLine = *it)->Type != Node::ScalarType)
{
return;
}
}
// Not a block scalar, cut end spaces/tabs
if(isBlockScalar == false)
{
while(1)
{
pLine = *it;
if(parentOffset != 0 && pLine->Offset <= parentOffset)
{
throw ParsingException(ExceptionMessage(g_ErrorIncorrectOffset, *pLine));
}
const size_t endOffset = pLine->Data.find_last_not_of(" \t");
if(endOffset == std::string::npos)
{
data += "\n";
}
else
{
data += pLine->Data.substr(0, endOffset + 1);
}
// Move to next line
++it;
if(it == m_Lines.end() || (*it)->Type != Node::ScalarType)
{
break;
}
data += " ";
}
if(ValidateQuote(data) == false)
{
throw ParsingException(ExceptionMessage(g_ErrorInvalidQuote, *pFirstLine));
}
}
// Block scalar
else
{
pLine = *it;
size_t blockOffset = pLine->Offset;
if(blockOffset <= parentOffset)
{
throw ParsingException(ExceptionMessage(g_ErrorIncorrectOffset, *pLine));
}
bool addedSpace = false;
while(it != m_Lines.end() && (*it)->Type == Node::ScalarType)
{
pLine = *it;
const size_t endOffset = pLine->Data.find_last_not_of(" \t");
if(endOffset != std::string::npos && pLine->Offset < blockOffset)
{
throw ParsingException(ExceptionMessage(g_ErrorIncorrectOffset, *pLine));
}
if(endOffset == std::string::npos)
{
if(addedSpace)
{
data[data.size() - 1] = '\n';
addedSpace = false;
}
else
{
data += "\n";
}
++it;
continue;
}
else
{
if(blockOffset != pLine->Offset && foldedFlag)
{
if(addedSpace)
{
data[data.size() - 1] = '\n';
addedSpace = false;
}
else
{
data += "\n";
}
}
data += std::string(pLine->Offset - blockOffset, ' ');
data += pLine->Data;
}
// Move to next line
++it;
if(it == m_Lines.end() || (*it)->Type != Node::ScalarType)
{
if(newLineFlag)
{
data += "\n";
}
break;
}
if(foldedFlag)
{
data += " ";
addedSpace = true;
}
else if(literalFlag && endOffset != std::string::npos)
{
data += "\n";
}
}
}
if(data.size() && (data[0] == '"' || data[0] == '\''))
{
data = data.substr(1, data.size() - 2 );
}
node = data;
}
/**
* @breif Debug printing.
*
*/
void Print()
{
for (auto it = m_Lines.begin(); it != m_Lines.end(); it++)
{
ReaderLine * pLine = *it;
// Print type
if (pLine->Type == Node::SequenceType)
{
std::cout << "seq ";
}
else if (pLine->Type == Node::MapType)
{
std::cout << "map ";
}
else if (pLine->Type == Node::ScalarType)
{
std::cout << "sca ";
}
else
{
std::cout << " ";
}
// Print flags
if (pLine->GetFlag(ReaderLine::FoldedScalarFlag))
{
std::cout << "f";
}
else
{
std::cout << "-";
}
if (pLine->GetFlag(ReaderLine::LiteralScalarFlag))
{
std::cout << "l";
}
else
{
std::cout << "-";
}
if (pLine->GetFlag(ReaderLine::ScalarNewlineFlag))
{
std::cout << "n";
}
else
{
std::cout << "-";
}
if (pLine->NextLine == nullptr)
{
std::cout << "e";
}
else
{
std::cout << "-";
}
std::cout << "| ";
std::cout << pLine->No << " ";
std::cout << std::string(pLine->Offset, ' ');
if (pLine->Type == Node::ScalarType)
{
std::string scalarValue = pLine->Data;
for (size_t i = 0; (i = scalarValue.find("\n", i)) != std::string::npos;)
{
scalarValue.replace(i, 1, "\\n");
i += 2;
}
std::cout << scalarValue << std::endl;
}
else if (pLine->Type == Node::MapType)
{
std::cout << pLine->Data + ":" << std::endl;
}
else if (pLine->Type == Node::SequenceType)
{
std::cout << "-" << std::endl;
}
else
{
std::cout << "> UNKOWN TYPE <" << std::endl;
}
}
}
/**
* @breif Clear all read lines.
*
*/
void ClearLines()
{
for (auto it = m_Lines.begin(); it != m_Lines.end(); it++)
{
delete *it;
}
m_Lines.clear();
}
void ClearTrailingEmptyLines(std::list<ReaderLine *>::iterator & it)
{
while(it != m_Lines.end())
{
ReaderLine * pLine = *it;
if(pLine->Data.size() == 0)
{
delete *it;
it = m_Lines.erase(it);
}
else
{
return;
}
}
}
static bool IsSequenceStart(const std::string & data)
{
if (data.size() == 0 || data[0] != '-')
{
return false;
}
if (data.size() >= 2 && data[1] != ' ')
{
return false;
}
return true;
}
static bool IsBlockScalar(const std::string & data, const size_t line, unsigned char & flags)
{
flags = 0;
if(data.size() == 0)
{
return false;
}
if(data[0] == '|')
{
if(data.size() >= 2)
{
if(data[1] != '-' && data[1] != ' ' && data[1] != '\t')
{
throw ParsingException(ExceptionMessage(g_ErrorInvalidBlockScalar, line, data));
}
}
else
{
flags |= ReaderLine::FlagMask[static_cast<size_t>(ReaderLine::ScalarNewlineFlag)];
}
flags |= ReaderLine::FlagMask[static_cast<size_t>(ReaderLine::LiteralScalarFlag)];
return true;
}
if(data[0] == '>')
{
if(data.size() >= 2)
{
if(data[1] != '-' && data[1] != ' ' && data[1] != '\t')
{
throw ParsingException(ExceptionMessage(g_ErrorInvalidBlockScalar, line, data));
}
}
else
{
flags |= ReaderLine::FlagMask[static_cast<size_t>(ReaderLine::ScalarNewlineFlag)];
}
flags |= ReaderLine::FlagMask[static_cast<size_t>(ReaderLine::FoldedScalarFlag)];
return true;
}
return false;
}
std::list<ReaderLine *> m_Lines; ///< List of lines.
};
// Parsing functions
void Parse(Node & root, const char * filename)
{
std::ifstream f(filename, std::ifstream::binary);
if (f.is_open() == false)
{
throw OperationException(g_ErrorCannotOpenFile);
}
f.seekg(0, f.end);
size_t fileSize = static_cast<size_t>(f.tellg());
f.seekg(0, f.beg);
std::unique_ptr<char[]> data(new char[fileSize]);
f.read(data.get(), fileSize);
f.close();
Parse(root, data.get(), fileSize);
}
void Parse(Node & root, std::iostream & stream)
{
ParseImp * pImp = nullptr;
try
{
pImp = new ParseImp;
pImp->Parse(root, stream);
delete pImp;
}
catch (const Exception e)
{
delete pImp;
throw;
}
}
void Parse(Node & root, const std::string & string)
{
std::stringstream ss(string);
Parse(root, ss);
}
void Parse(Node & root, const char * buffer, const size_t size)
{
std::stringstream ss(std::string(buffer, size));
Parse(root, ss);
}
// Serialize configuration structure.
SerializeConfig::SerializeConfig(const size_t spaceIndentation,
const size_t scalarMaxLength,
const bool sequenceMapNewline,
const bool mapScalarNewline) :
SpaceIndentation(spaceIndentation),
ScalarMaxLength(scalarMaxLength),
SequenceMapNewline(sequenceMapNewline),
MapScalarNewline(mapScalarNewline)
{
}
// Serialization functions
void Serialize(const Node & root, const char * filename, const SerializeConfig & config)
{
std::stringstream stream;
Serialize(root, stream, config);
std::ofstream f(filename);
if (f.is_open() == false)
{
throw OperationException(g_ErrorCannotOpenFile);
}
f.write(stream.str().c_str(), stream.str().size());
f.close();
}
size_t LineFolding(const std::string & input, std::vector<std::string> & folded, const size_t maxLength)
{
folded.clear();
if(input.size() == 0)
{
return 0;
}
size_t currentPos = 0;
size_t lastPos = 0;
size_t spacePos = std::string::npos;
while(currentPos < input.size())
{
currentPos = lastPos + maxLength;
if(currentPos < input.size())
{
spacePos = input.find_first_of(' ', currentPos);
}
if(spacePos == std::string::npos || currentPos >= input.size())
{
const std::string endLine = input.substr(lastPos);
if(endLine.size())
{
folded.push_back(endLine);
}
return folded.size();
}
folded.push_back(input.substr(lastPos, spacePos - lastPos));
lastPos = spacePos + 1;
}
return folded.size();
}
static void SerializeLoop(const Node & node, std::iostream & stream, bool useLevel, const size_t level, const SerializeConfig & config)
{
const size_t indention = config.SpaceIndentation;
switch(node.Type())
{
case Node::SequenceType:
{
for(auto it = node.Begin(); it != node.End(); it++)
{
const Node & value = (*it).second;
if(value.IsNone())
{
continue;
}
stream << std::string(level, ' ') << "- ";
useLevel = false;
if(value.IsSequence() || (value.IsMap() && config.SequenceMapNewline == true))
{
useLevel = true;
stream << "\n";
}
SerializeLoop(value, stream, useLevel, level + 2, config);
}
}
break;
case Node::MapType:
{
size_t count = 0;
for(auto it = node.Begin(); it != node.End(); it++)
{
const Node & value = (*it).second;
if(value.IsNone())
{
continue;
}
if(useLevel || count > 0)
{
stream << std::string(level, ' ');
}
std::string key = (*it).first;
AddEscapeTokens(key, "\\\"");
if(ShouldBeCited(key))
{
stream << "\"" << key << "\"" << ": ";
}
else
{
stream << key << ": ";
}
useLevel = false;
if(value.IsScalar() == false || (value.IsScalar() && config.MapScalarNewline))
{
useLevel = true;
stream << "\n";
}
SerializeLoop(value, stream, useLevel, level + indention, config);
useLevel = true;
count++;
}
}
break;
case Node::ScalarType:
{
const std::string value = node.As<std::string>();
// Empty scalar
if(value.size() == 0)
{
stream << "\n";
break;
}
// Get lines of scalar.
std::string line = "";
std::vector<std::string> lines;
std::istringstream iss(value);
while (iss.eof() == false)
{
std::getline(iss, line);
lines.push_back(line);
}
// Block scalar
const std::string & lastLine = lines.back();
const bool endNewline = lastLine.size() == 0;
if(endNewline)
{
lines.pop_back();
}
// Literal
if(lines.size() > 1)
{
stream << "|";
}
// Folded/plain
else
{
const std::string frontLine = lines.front();
if(config.ScalarMaxLength == 0 || lines.front().size() <= config.ScalarMaxLength ||
LineFolding(frontLine, lines, config.ScalarMaxLength) == 1)
{
if(useLevel)
{
stream << std::string(level, ' ');
}
if(ShouldBeCited(value))
{
stream << "\"" << value << "\"\n";
break;
}
stream << value << "\n";
break;
}
else
{
stream << ">";
}
}
if(endNewline == false)
{
stream << "-";
}
stream << "\n";
for(auto it = lines.begin(); it != lines.end(); it++)
{
stream << std::string(level, ' ') << (*it) << "\n";
}
}
break;
default:
break;
}
}
void Serialize(const Node & root, std::iostream & stream, const SerializeConfig & config)
{
if(config.SpaceIndentation < 2)
{
throw OperationException(g_ErrorIndentation);
}
SerializeLoop(root, stream, false, 0, config);
}
void Serialize(const Node & root, std::string & string, const SerializeConfig & config)
{
std::stringstream stream;
Serialize(root, stream, config);
string = stream.str();
}
// Static function implementations
std::string ExceptionMessage(const std::string & message, ReaderLine & line)
{
return message + std::string(" Line ") + std::to_string(line.No) + std::string(": ") + line.Data;
}
std::string ExceptionMessage(const std::string & message, ReaderLine & line, const size_t errorPos)
{
return message + std::string(" Line ") + std::to_string(line.No) + std::string(" column ") + std::to_string(errorPos + 1) + std::string(": ") + line.Data;
}
std::string ExceptionMessage(const std::string & message, const size_t errorLine, const size_t errorPos)
{
return message + std::string(" Line ") + std::to_string(errorLine) + std::string(" column ") + std::to_string(errorPos);
}
std::string ExceptionMessage(const std::string & message, const size_t errorLine, const std::string & data)
{
return message + std::string(" Line ") + std::to_string(errorLine) + std::string(": ") + data;
}
bool FindQuote(const std::string & input, size_t & start, size_t & end, size_t searchPos)
{
start = end = std::string::npos;
size_t qPos = searchPos;
bool foundStart = false;
while(qPos != std::string::npos)
{
// Find first quote.
qPos = input.find_first_of("\"'", qPos);
if(qPos == std::string::npos)
{
return false;
}
const char token = input[qPos];
if(token == '"' && (qPos == 0 || input[qPos-1] != '\\'))
{
// Found start quote.
if(foundStart == false)
{
start = qPos;
foundStart = true;
}
// Found end quote
else
{
end = qPos;
return true;
}
}
// Check if it's possible for another loop.
if(qPos + 1 == input.size())
{
return false;
}
qPos++;
}
return false;
}
size_t FindNotCited(const std::string & input, char token, size_t & preQuoteCount)
{
preQuoteCount = 0;
size_t tokenPos = input.find_first_of(token);
if(tokenPos == std::string::npos)
{
return std::string::npos;
}
// Find all quotes
std::vector<std::pair<size_t, size_t>> quotes;
size_t quoteStart = 0;
size_t quoteEnd = 0;
while(FindQuote(input, quoteStart, quoteEnd, quoteEnd))
{
quotes.push_back({quoteStart, quoteEnd});
if(quoteEnd + 1 == input.size())
{
break;
}
quoteEnd++;
}
if(quotes.size() == 0)
{
return tokenPos;
}
size_t currentQuoteIndex = 0;
std::pair<size_t, size_t> currentQuote = {0, 0};
while(currentQuoteIndex < quotes.size())
{
currentQuote = quotes[currentQuoteIndex];
if(tokenPos < currentQuote.first)
{
return tokenPos;
}
preQuoteCount++;
if(tokenPos <= currentQuote.second)
{
// Find next token
if(tokenPos + 1 == input.size())
{
return std::string::npos;
}
tokenPos = input.find_first_of(token, tokenPos + 1);
if(tokenPos == std::string::npos)
{
return std::string::npos;
}
}
currentQuoteIndex++;
}
return tokenPos;
}
size_t FindNotCited(const std::string & input, char token)
{
size_t dummy = 0;
return FindNotCited(input, token, dummy);
}
bool ValidateQuote(const std::string & input)
{
if(input.size() == 0)
{
return true;
}
char token = 0;
size_t searchPos = 0;
if(input[0] == '\"' || input[0] == '\'')
{
if(input.size() == 1)
{
return false;
}
token = input[0];
searchPos = 1;
}
while(searchPos != std::string::npos && searchPos < input.size() - 1)
{
searchPos = input.find_first_of("\"'", searchPos + 1);
if(searchPos == std::string::npos)
{
break;
}
const char foundToken = input[searchPos];
if(input[searchPos] == '\"' || input[searchPos] == '\'')
{
if(token == 0 && input[searchPos-1] != '\\')
{
return false;
}
//if(foundToken == token)
//{
/*if(foundToken == token && searchPos == input.size() - 1 && input[searchPos-1] != '\\')
{
return true;
if(searchPos == input.size() - 1)
{
return true;
}
return false;
}
else */
if(foundToken == token && input[searchPos-1] != '\\')
{
if(searchPos == input.size() - 1)
{
return true;
}
return false;
}
//}
}
}
return token == 0;
}
void CopyNode(const Node & from, Node & to)
{
const Node::eType type = from.Type();
switch(type)
{
case Node::SequenceType:
for(auto it = from.Begin(); it != from.End(); it++)
{
const Node & currentNode = (*it).second;
Node & newNode = to.PushBack();
CopyNode(currentNode, newNode);
}
break;
case Node::MapType:
for(auto it = from.Begin(); it != from.End(); it++)
{
const Node & currentNode = (*it).second;
Node & newNode = to[(*it).first];
CopyNode(currentNode, newNode);
}
break;
case Node::ScalarType:
to = from.As<std::string>();
break;
case Node::None:
break;
}
}
bool ShouldBeCited(const std::string & key)
{
return key.find_first_of("\":{}[],&*#?|-<>=!%@") != std::string::npos;
}
void AddEscapeTokens(std::string & input, const std::string & tokens)
{
for(auto it = tokens.begin(); it != tokens.end(); it++)
{
const char token = *it;
const std::string replace = std::string("\\") + std::string(1, token);
size_t found = input.find_first_of(token);
while(found != std::string::npos)
{
input.replace(found, 1, replace);
found = input.find_first_of(token, found + 2);
}
}
}
void RemoveAllEscapeTokens(std::string & input)
{
size_t found = input.find_first_of("\\");
while(found != std::string::npos)
{
if(found + 1 == input.size())
{
return;
}
std::string replace(1, input[found + 1]);
input.replace(found, 2, replace);
found = input.find_first_of("\\", found + 1);
}
}
}
这段代码是一个用于处理和序列化YAML（YAML Ain’t Markup Language）数据的C++库。主要功能包括：
-
解析YAML数据：通过读取YAML格式的文件或字符串，将其内容解析成C++中的数据结构。
-
异常处理：定义了几种异常类型来处理在解析过程中可能出现的错误。
-
数据类型：支持YAML的主要数据类型，如序列（列表）、映射（键值对）和标量（单个值）。
-
迭代器实现：提供了迭代器来遍历YAML数据结构。
-
序列化：能够将C++中的数据结构转换回YAML格式，以便于存储和传输。
-
辅助功能：包括处理引用、转义字符等功能，以支持YAML的复杂结构。
这个库为处理YAML数据提供了全面而灵活的工具，使得在C++项目中读取和生成YAML文件变得简单高效。
ublox_driver.launch
<launch>
<arg name="config_path" default = "$(find ublox_driver)/config/driver_config.yaml" />
<node name="ublox_driver" pkg="ublox_driver" type="ublox_driver" output="screen">
<param name="config_file" type="string" value="$(arg config_path)" />
</node>
</launch>
这段代码是一个ROS（Robot Operating System）启动文件，通常用于启动ROS节点和配置它们的参数。下面是对这段代码的详细解释：
-
<launch>:<launch>是ROS启动文件的根元素，它包含了一系列ROS节点的配置和参数设置。 -
<arg name="config_path" default="$(find ublox_driver)/config/driver_config.yaml" />: 这是一个ROS参数配置，它定义了一个名为config_path的参数，并为其设置了默认值。这个参数用于存储ublox驱动程序的配置文件路径。default属性指定了默认值，它使用$(find ublox_driver)来查找ublox_driver软件包，并在其子目录config中寻找名为driver_config.yaml的配置文件。 -
<node name="ublox_driver" pkg="ublox_driver" type="ublox_driver" output="screen">: 这是一个ROS节点的定义。它启动了一个名为ublox_driver的节点，该节点来自名为ublox_driver的ROS软件包。output="screen"属性表示将该节点的输出信息显示在屏幕上。 -
<param name="config_file" type="string" value="$(arg config_path)" />: 这是一个ROS参数配置，它用于配置ublox_driver节点的参数。具体来说：name="config_file": 这指定了参数的名称，即config_file。type="string": 这指定了参数的数据类型，即字符串。value="$(arg config_path)": 这设置了参数的值，它使用之前定义的config_path参数的值作为config_file参数的值。这意味着ublox_driver节点将使用driver_config.yaml配置文件的路径作为参数。
综上所述，这段启动文件的作用是启动一个名为ublox_driver的ROS节点，并将名为config_file的参数配置为指向ublox_driver软件包中的config/driver_config.yaml配置文件的路径。这使得ublox_driver节点能够读取和使用该配置文件中的设置来执行其功能。这是一个常见的ROS启动文件示例，用于配置和启动ROS节点。
ublox_driver.yaml
%YAML:1.0
# input options
online: 1 # input from serial port(1) or file(0)
## if `online == 1` then following options may be set
input_serial_port: "/dev/ttyACM0"
serial_baud_rate: 921600
input_rtcm: 0 # whether input RTCM data to receiver for RTK solution
rtcm_tcp_port: 3503
config_receiver_at_start: 0 # whether to config receiver during booting, ** NOT SUPPORT YET **
receiver_config_filepath: "~/catkin_ws/src/ublox_driver/config/ublox_rcv_config.yaml"
## if `online == 0` then following options may be set, `serial_baud_rate` is used for speed control
ubx_filepath: "~/tmp/ublox_driver_test/2020_11_30_7_14_33.ubx"
rtk_correction_ecef: # apply RTK correction in case of the position of base station reported in RTCM is biased
rows: 3
cols: 1
dt: d
data: [ 0, 0, 0 ]
# output options
to_ros: 1 # publish as ros topic
to_file: 0 # dump data to file
dump_dir: "~/tmp/ublox_driver_test/" # dump data to this directory, if applicable
to_serial: 0 # output data to serial port, for debugging purpose
output_serial_port: "/dev/ttyACM0" # dump data to this serial port, if applicable
这段代码是一个 YAML 格式的配置文件，用于设置 ublox_driver，这是一个 ROS 驱动程序，用于处理来自 u-blox GNSS 接收器的数据。配置文件中的每个选项都用于控制驱动程序的不同方面。以下是对每个配置选项的详细解释：
输入选项
online: 指定输入源是来自串行端口（值为 1）还是文件（值为 0）。input_serial_port: 当online为 1 时，这个选项指定串行端口的路径，例如/dev/ttyACM0。serial_baud_rate: 串行通信的波特率，例如 921600。input_rtcm: 指定是否向接收器输入 RTCM 数据以获取 RTK 解决方案。值为 1 表示是，0 表示否。rtcm_tcp_port: RTCM 数据的 TCP 端口号，例如 3503。config_receiver_at_start: 启动时是否配置接收器。目前尚未支持，因此应设置为 0。receiver_config_filepath: 接收器配置文件的路径，仅当config_receiver_at_start为 1 时使用。ubx_filepath: 当online为 0 时，指定 UBX 文件的路径，用于从文件中读取数据。rtk_correction_ecef: 如果 RTCM 报告的基站位置有偏差，应用 RTK 修正。这是一个 3x1 的矩阵，包含 ECEF（地心地固坐标系）中的修正值。
输出选项
to_ros: 是否将数据发布为 ROS 话题。值为 1 表示是，0 表示否。to_file: 是否将数据转储到文件。值为 1 表示是，0 表示否。dump_dir: 如果to_file为 1，这个选项指定转储数据的目录路径。to_serial: 是否将数据输出到串行端口，用于调试目的。值为 1 表示是，0 表示否。output_serial_port: 如果to_serial为 1，这个选项指定输出数据的串行端口路径。
这个配置文件为 ublox_driver 提供了灵活的配置选项，允许用户根据自己的需求和设备设置来调整驱动程序的行为。
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(ublox_driver)
set(CMAKE_BUILD_TYPE "release")
set(CMAKE_CXX_FLAGS "-std=c++11 ")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC")
find_package(catkin REQUIRED COMPONENTS
nav_msgs
roscpp
rospy
sensor_msgs
std_msgs
gnss_comm
)
catkin_package(
)
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(Eigen3 REQUIRED)
find_package(Glog REQUIRED)
include_directories(
${EIGEN3_INCLUDE_DIR}
${GLOG_INCLUDE_DIR}
${catkin_INCLUDE_DIRS}
${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/
${PROJECT_SOURCE_DIR}/third_party/
)
add_executable(${PROJECT_NAME} ${PROJECT_SOURCE_DIR}/src/ublox_driver.cpp
${PROJECT_SOURCE_DIR}/src/ublox_message_processor.cpp
${PROJECT_SOURCE_DIR}/src/file_loader.cpp
${PROJECT_SOURCE_DIR}/src/file_dumper.cpp
${PROJECT_SOURCE_DIR}/src/serial_handler.cpp
${PROJECT_SOURCE_DIR}/src/socket_handler.cpp
${PROJECT_SOURCE_DIR}/third_party/yaml/Yaml.cpp)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GLOG_LIBRARIES})
add_executable(sync_system_time ${PROJECT_SOURCE_DIR}/src/sync_system_time.cpp)
target_link_libraries(sync_system_time ${catkin_LIBRARIES})
这段代码是一个CMakeLists.txt文件，用于配置和构建一个ROS（Robot Operating System）软件包，该软件包似乎是用于与u-blox GNSS设备进行通信的驱动程序。让我逐行详细解释这段代码：
-
cmake_minimum_required(VERSION 2.8.3)：指定所需的CMake的最低版本。 -
project(ublox_driver)：设置项目的名称为 “ublox_driver”。 -
set(CMAKE_BUILD_TYPE "release")：指定构建类型为 “release”，这意味着在编译时会应用优化选项。 -
set(CMAKE_CXX_FLAGS "-std=c++11 ")：设置C++编译标志，要求使用C++11标准。 -
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC")：设置Release构建类型下的C++编译标志，包括-O3优化级别、开启编译警告（-Wall）和生成位置无关代码（-fPIC）。 -
find_package(catkin REQUIRED COMPONENTS ...)：查找并配置catkin工作空间中所需的ROS包。这些包包括nav_msgs、roscpp、rospy、sensor_msgs、std_msgs和gnss_comm。 -
catkin_package()：定义一个catkin软件包。 -
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)：设置CMake模块路径，以便在后续的find_package中查找模块。 -
find_package(Eigen3 REQUIRED)：查找并配置Eigen3库，这是一个用于线性代数运算的C++库。 -
find_package(Glog REQUIRED)：查找并配置Google的日志库（Glog）。 -
include_directories(...)：添加包含目录到项目中，包括Eigen3和Glog的头文件，以及catkin工作空间中其他ROS包的头文件。 -
add_executable(${PROJECT_NAME} ...)：定义一个可执行文件，该文件名为${PROJECT_NAME}，并列出了要编译的源代码文件列表。 -
target_link_libraries(${PROJECT_NAME} ...)：将库链接到${PROJECT_NAME}可执行文件，包括catkin_LIBRARIES和GLOG_LIBRARIES。 -
add_executable(sync_system_time ...)：定义另一个可执行文件sync_system_time，并列出了要编译的源代码文件列表。 -
target_link_libraries(sync_system_time ...)：将库链接到sync_system_time可执行文件，包括catkin_LIBRARIES。
总之，这段代码主要是为一个ROS软件包配置构建环境，包括依赖项、编译标志和可执行文件，以便实现与u-blox GNSS设备通信的功能。其中的 add_executable 和 target_link_libraries 命令用于指定编译的源文件和依赖库。这个软件包还使用了Eigen3和Glog库以及其他的ROS包。用户可以根据自己的需求修改和扩展这个配置文件。
package.xml
<?xml version="1.0"?>
<package>
<name>ublox_driver</name>
<version>1.0.0</version>
<description>A driver for u-blox receiver (ZED-F9P) with ros support</description>
<maintainer email="shaozu.cao@gmail.com">CAO Shaozu</maintainer>
<license>GPLv3</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>gnss_comm</build_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>gnss_comm</run_depend>
</package>
这是一个ROS软件包的 package.xml 文件，它包含了有关软件包的元数据和依赖关系的信息。下面是对这段代码的详细解释：
-
<package>：<package>元素是XML文件的根元素，包含了整个package.xml文件。 -
<name>ublox_driver</name>：指定ROS软件包的名称，这个软件包的名称是 “ublox_driver”。 -
<version>1.0.0</version>：指定ROS软件包的版本号，这个软件包的版本号是 “1.0.0”。 -
<description>A driver for u-blox receiver (ZED-F9P) with ros support</description>：提供了对软件包的简短描述，说明了这个软件包是用于u-blox接收器（ZED-F9P）的驱动程序，并具有ROS支持。 -
<maintainer email="shaozu.cao@gmail.com">CAO Shaozu</maintainer>：指定了软件包的维护者信息，包括姓名和电子邮件地址。 -
<license>GPLv3</license>：指定了软件包的许可证，这个软件包使用GPLv3许可证。 -
<buildtool_depend>catkin</buildtool_depend>：指定了构建工具的依赖项，这里是指依赖于Catkin构建系统。 -
<build_depend>：指定了构建时的依赖项，这些依赖项是在编译软件包时需要的其他ROS软件包。在这个例子中，软件包依赖于nav_msgs、roscpp、rospy、sensor_msgs和gnss_comm软件包。 -
<run_depend>：指定了运行时的依赖项，这些依赖项是在运行软件包时需要的其他ROS软件包。在这个例子中，软件包依赖于nav_msgs、roscpp、rospy、std_msgs和gnss_comm软件包。
这个 package.xml 文件用于描述ROS软件包的元数据，以及它的构建和运行时依赖关系。这对于ROS系统中的软件包管理和构建过程非常重要，因为它们确定了软件包的编译和运行所需的其他软件包。这样，用户可以使用rosdep等工具轻松地解决和安装所有依赖项，以便构建和运行这个软件包。
/" # dump data to this directory, if applicable
to_serial: 0 # output data to serial port, for debugging purpose
output_serial_port: “/dev/ttyACM0” # dump data to this serial port, if applicable
这段代码是一个 YAML 格式的配置文件，用于设置 `ublox_driver`，这是一个 ROS 驱动程序，用于处理来自 u-blox GNSS 接收器的数据。配置文件中的每个选项都用于控制驱动程序的不同方面。以下是对每个配置选项的详细解释：
### 输入选项
- `online`: 指定输入源是来自串行端口（值为 1）还是文件（值为 0）。
- `input_serial_port`: 当 `online` 为 1 时，这个选项指定串行端口的路径，例如 `/dev/ttyACM0`。
- `serial_baud_rate`: 串行通信的波特率，例如 921600。
- `input_rtcm`: 指定是否向接收器输入 RTCM 数据以获取 RTK 解决方案。值为 1 表示是，0 表示否。
- `rtcm_tcp_port`: RTCM 数据的 TCP 端口号，例如 3503。
- `config_receiver_at_start`: 启动时是否配置接收器。目前尚未支持，因此应设置为 0。
- `receiver_config_filepath`: 接收器配置文件的路径，仅当 `config_receiver_at_start` 为 1 时使用。
- `ubx_filepath`: 当 `online` 为 0 时，指定 UBX 文件的路径，用于从文件中读取数据。
- `rtk_correction_ecef`: 如果 RTCM 报告的基站位置有偏差，应用 RTK 修正。这是一个 3x1 的矩阵，包含 ECEF（地心地固坐标系）中的修正值。
### 输出选项
- `to_ros`: 是否将数据发布为 ROS 话题。值为 1 表示是，0 表示否。
- `to_file`: 是否将数据转储到文件。值为 1 表示是，0 表示否。
- `dump_dir`: 如果 `to_file` 为 1，这个选项指定转储数据的目录路径。
- `to_serial`: 是否将数据输出到串行端口，用于调试目的。值为 1 表示是，0 表示否。
- `output_serial_port`: 如果 `to_serial` 为 1，这个选项指定输出数据的串行端口路径。
这个配置文件为 `ublox_driver` 提供了灵活的配置选项，允许用户根据自己的需求和设备设置来调整驱动程序的行为。
## CMakeLists.txt
```cmake
cmake_minimum_required(VERSION 2.8.3)
project(ublox_driver)
set(CMAKE_BUILD_TYPE "release")
set(CMAKE_CXX_FLAGS "-std=c++11 ")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC")
find_package(catkin REQUIRED COMPONENTS
nav_msgs
roscpp
rospy
sensor_msgs
std_msgs
gnss_comm
)
catkin_package(
)
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(Eigen3 REQUIRED)
find_package(Glog REQUIRED)
include_directories(
${EIGEN3_INCLUDE_DIR}
${GLOG_INCLUDE_DIR}
${catkin_INCLUDE_DIRS}
${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/
${PROJECT_SOURCE_DIR}/third_party/
)
add_executable(${PROJECT_NAME} ${PROJECT_SOURCE_DIR}/src/ublox_driver.cpp
${PROJECT_SOURCE_DIR}/src/ublox_message_processor.cpp
${PROJECT_SOURCE_DIR}/src/file_loader.cpp
${PROJECT_SOURCE_DIR}/src/file_dumper.cpp
${PROJECT_SOURCE_DIR}/src/serial_handler.cpp
${PROJECT_SOURCE_DIR}/src/socket_handler.cpp
${PROJECT_SOURCE_DIR}/third_party/yaml/Yaml.cpp)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GLOG_LIBRARIES})
add_executable(sync_system_time ${PROJECT_SOURCE_DIR}/src/sync_system_time.cpp)
target_link_libraries(sync_system_time ${catkin_LIBRARIES})
这段代码是一个CMakeLists.txt文件，用于配置和构建一个ROS（Robot Operating System）软件包，该软件包似乎是用于与u-blox GNSS设备进行通信的驱动程序。让我逐行详细解释这段代码：
-
cmake_minimum_required(VERSION 2.8.3)：指定所需的CMake的最低版本。 -
project(ublox_driver)：设置项目的名称为 “ublox_driver”。 -
set(CMAKE_BUILD_TYPE "release")：指定构建类型为 “release”，这意味着在编译时会应用优化选项。 -
set(CMAKE_CXX_FLAGS "-std=c++11 ")：设置C++编译标志，要求使用C++11标准。 -
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC")：设置Release构建类型下的C++编译标志，包括-O3优化级别、开启编译警告（-Wall）和生成位置无关代码（-fPIC）。 -
find_package(catkin REQUIRED COMPONENTS ...)：查找并配置catkin工作空间中所需的ROS包。这些包包括nav_msgs、roscpp、rospy、sensor_msgs、std_msgs和gnss_comm。 -
catkin_package()：定义一个catkin软件包。 -
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)：设置CMake模块路径，以便在后续的find_package中查找模块。 -
find_package(Eigen3 REQUIRED)：查找并配置Eigen3库，这是一个用于线性代数运算的C++库。 -
find_package(Glog REQUIRED)：查找并配置Google的日志库（Glog）。 -
include_directories(...)：添加包含目录到项目中，包括Eigen3和Glog的头文件，以及catkin工作空间中其他ROS包的头文件。 -
add_executable(${PROJECT_NAME} ...)：定义一个可执行文件，该文件名为${PROJECT_NAME}，并列出了要编译的源代码文件列表。 -
target_link_libraries(${PROJECT_NAME} ...)：将库链接到${PROJECT_NAME}可执行文件，包括catkin_LIBRARIES和GLOG_LIBRARIES。 -
add_executable(sync_system_time ...)：定义另一个可执行文件sync_system_time，并列出了要编译的源代码文件列表。 -
target_link_libraries(sync_system_time ...)：将库链接到sync_system_time可执行文件，包括catkin_LIBRARIES。
总之，这段代码主要是为一个ROS软件包配置构建环境，包括依赖项、编译标志和可执行文件，以便实现与u-blox GNSS设备通信的功能。其中的 add_executable 和 target_link_libraries 命令用于指定编译的源文件和依赖库。这个软件包还使用了Eigen3和Glog库以及其他的ROS包。用户可以根据自己的需求修改和扩展这个配置文件。
package.xml
<?xml version="1.0"?>
<package>
<name>ublox_driver</name>
<version>1.0.0</version>
<description>A driver for u-blox receiver (ZED-F9P) with ros support</description>
<maintainer email="shaozu.cao@gmail.com">CAO Shaozu</maintainer>
<license>GPLv3</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>gnss_comm</build_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>gnss_comm</run_depend>
</package>
这是一个ROS软件包的 package.xml 文件，它包含了有关软件包的元数据和依赖关系的信息。下面是对这段代码的详细解释：
-
<package>：<package>元素是XML文件的根元素，包含了整个package.xml文件。 -
<name>ublox_driver</name>：指定ROS软件包的名称，这个软件包的名称是 “ublox_driver”。 -
<version>1.0.0</version>：指定ROS软件包的版本号，这个软件包的版本号是 “1.0.0”。 -
<description>A driver for u-blox receiver (ZED-F9P) with ros support</description>：提供了对软件包的简短描述，说明了这个软件包是用于u-blox接收器（ZED-F9P）的驱动程序，并具有ROS支持。 -
<maintainer email="shaozu.cao@gmail.com">CAO Shaozu</maintainer>：指定了软件包的维护者信息，包括姓名和电子邮件地址。 -
<license>GPLv3</license>：指定了软件包的许可证，这个软件包使用GPLv3许可证。 -
<buildtool_depend>catkin</buildtool_depend>：指定了构建工具的依赖项，这里是指依赖于Catkin构建系统。 -
<build_depend>：指定了构建时的依赖项，这些依赖项是在编译软件包时需要的其他ROS软件包。在这个例子中，软件包依赖于nav_msgs、roscpp、rospy、sensor_msgs和gnss_comm软件包。 -
<run_depend>：指定了运行时的依赖项，这些依赖项是在运行软件包时需要的其他ROS软件包。在这个例子中，软件包依赖于nav_msgs、roscpp、rospy、std_msgs和gnss_comm软件包。
这个 package.xml 文件用于描述ROS软件包的元数据，以及它的构建和运行时依赖关系。这对于ROS系统中的软件包管理和构建过程非常重要，因为它们确定了软件包的编译和运行所需的其他软件包。这样，用户可以使用rosdep等工具轻松地解决和安装所有依赖项，以便构建和运行这个软件包。
文章来源:https://blog.csdn.net/m0_73640344/article/details/135499938
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：chenni525@qq.com进行投诉反馈，一经查实，立即删除！
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：chenni525@qq.com进行投诉反馈，一经查实，立即删除！
最新文章
- Python教程
- 深入理解 MySQL 中的 HAVING 关键字和聚合函数
- Qt之QChar编码（1）
- MyBatis入门基础篇
- 用Python脚本实现FFmpeg批量转换
- 计算机组成原理(万字爆肝整理)
- R3 下屏蔽 Windows 睡眠：拦截 Winlogon 调用（二）
- 实时传输协议（RTP）介绍
- Spring SpEL在Flink中的应用-与Filter结合实现数据动态分流
- IBM TS4500磁带库更换LCC及Driver驱动器后TSM中重新匹配带库及驱动器
- MyBatis初学者必看：快速上手，让你的项目事半功倍！
- yolov5s.yaml 文件解读
- 好消息！华为云时习知荣获IXDC AWARD国际体验奖
- 上海交通大学生存手册
- webGIS开发第四章